一些简单代码--关于移动机器人

move_node.py

#! /usr/bin/env python

import rospy
from geometry_msgs.msg import Twist

rospy.init_node('stop_node')
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
rate = rospy.Rate(2)
move = Twist()
move.linear.x = 0.5
move.angular.z = 0.5

while not rospy.is_shutdown():
  pub.publish(move)
  rate.sleep()

stop_move.py

#! /usr/bin/env python

import rospy
from geometry_msgs.msg import Twist

rospy.init_node('stop_node')
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
rate = rospy.Rate(2)
move = Twist()
move.linear.x = 0
move.angular.z = 0

while not rospy.is_shutdown():
  pub.publish(move)
  rate.sleep()

service.py

#! /usr/bin/env python

import rospy
from std_srvs.srv import Empty, EmptyResponse 
# you import the service message python classes generated from Empty.srv.
from geometry_msgs.msg import Twist


def my_callback(request):

    pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
    rate = rospy.Rate(1)
    move = Twist()
    move.linear.x = 0.5
    move.angular.z = 1
    for i in xrange(4):
        pub.publish(move)
        rate.sleep()

    move.linear.x = 0
    move.angular.z = 0
    pub.publish(move)

    return EmptyResponse() # the service Response class, in this case EmptyResponse

rospy.init_node('service_test_node')
my_service = rospy.Service('/service_demo', Empty , my_callback)
# create the Service called my_service with the defined callback
rospy.spin() # mantain the service open.

调用服务

rosservice call /service_demo "{}"

action 含反馈
action_server.py

#! /usr/bin/env python
import rospy

import actionlib

from actionlib.msg import TestFeedback, TestResult, TestAction
from geometry_msgs.msg import Twist

class MoveClass(object):

  # create messages that are used to publish feedback/result
  _feedback = TestFeedback()
  _result   = TestResult()

  def __init__(self):
    # creates the action server
    self._as = actionlib.SimpleActionServer("action_demo", TestAction, self.goal_callback, False)
    self._as.start()

  def goal_callback(self, goal):
    # this callback is called when the action server is called.
    # this is the function that computes the Fibonacci sequence
    # and returns the sequence to the node that called the action server

    # helper variables
    r = rospy.Rate(1)
    success = True


    # append the seeds for the fibonacci sequence
    #self._feedback.sequence = []
    #self._feedback.sequence.append(0)
    #self._feedback.sequence.append(1)

    # publish info to the console for the user
    #rospy.loginfo('"fibonacci_as": Executing, creating fibonacci sequence of order %i with seeds %i, %i' % ( goal.order, self._feedback.sequence[0], self._feedback.sequence[1]))

    # starts calculating the Fibonacci sequence
    pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
    move = Twist()
    move.linear.x = 0.5
    move.angular.z = 1
    seconds = goal.goal
    for i in xrange(seconds):

      # check that preempt (cancelation) has not been requested by the action client
      if self._as.is_preempt_requested():
        rospy.loginfo('The goal has been cancelled/preempted')
        # the following line, sets the client in preempted state (goal cancelled)
        self._as.set_preempted()
        success = False
        # we end the calculation of the Fibonacci sequence
        break


      # builds the next feedback msg to be sent
      #self._feedback.sequence.append(self._feedback.sequence[i] + self._feedback.sequence[i-1])
      # publish the feedback
      #self._as.publish_feedback(self._feedback)
      # the sequence is computed at 1 Hz frequency
      pub.publish(move)
      r.sleep()

    move.linear.x = 0
    move.angular.z = 0
    pub.publish(move)
    # at this point, either the goal has been achieved (success==true)
    # or the client preempted the goal (success==false)
    # If success, then we publish the final result
    # If not success, we do not publish anything in the result
    if success:
      #self._result.sequence = self._feedback.sequence
      #rospy.loginfo('Succeeded calculating the Fibonacci of order %i' % fibonacciOrder )
      self._as.set_succeeded(self._result)

if __name__ == '__main__':
  rospy.init_node('action_test_node')
  MoveClass()
  rospy.spin()

client_demo.py

#! /usr/bin/env python
import rospy
import time
import actionlib
from actionlib.msg import TestAction, TestGoal, TestResult, TestFeedback


# definition of the feedback callback. This will be called when feedback
# is received from the action server
# it just prints a message indicating a new message has been received
def feedback_callback(feedback):

    print '[Feedback] Executing Action'


# initializes the action client node
rospy.init_node('action_client_node')

# create the connection to the action server
client = actionlib.SimpleActionClient('/action_demo', TestAction)
# waits until the action server is up and running
client.wait_for_server()

# creates a goal to send to the action server
goal = TestGoal()
goal.goal = 5 # indicates, take pictures along 10 seconds


# sends the goal to the action server, specifying which feedback function
# to call when feedback received
client.send_goal(goal, feedback_cb=feedback_callback)

# Uncomment these lines to test goal preemption:
#time.sleep(3.0)
#client.cancel_goal()  # would cancel the goal 3 seconds after starting

# wait until the result is obtained
# you can do other stuff here instead of waiting
# and check for status from time to time
# status = client.get_state()
# check the client API link below for more info

client.wait_for_result()

print('[Result] State: %d'%(client.get_state()))