一个ROS的服务，使机器人向前移动指定距离

写在前面

为使机器人底座向前走指定距离，根据一段“向前走一米”的代码（这段代码应该很多ros参考书上都有）。为了方便程序引用，我直接把它改成了一个服务，方便以后程序内调用。

源代码

有点长，放文末链接里了。

服务描述及代码

现在的服务是：请求时携带要前进的距离，然后底盘前进相应距离。代码如下，改动很小：

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist, Point
from math import copysign, sqrt, pow
import tf
from carry.srv import MoveTowards


class CalibrateLinear():
    def __init__(self):
	    rospy.init_node('move_towards_server', anonymous=False)	
	    service= rospy.Service('move_towards', MoveTowards, self.move)
    def move(self,distanse_you_want):
        print distanse_you_want.a
        # Set rospy to execute a shutdown function when terminating the script
        rospy.on_shutdown(self.shutdown)
        
        # How fast will we check the odometry values?
        self.rate = rospy.get_param('~rate', 20)
        r = rospy.Rate(self.rate)
        
        # Set the distance to travel
        self.test_distance = rospy.get_param('~test_distance', distanse_you_want.a) # meters
        self.speed = rospy.get_param('~speed', 0.15) # meters per second
        self.tolerance = rospy.get_param('~tolerance', 0.01) # meters
        self.odom_linear_scale_correction = rospy.get_param('~odom_linear_scale_correction', 1.0)
        self.start_test = rospy.get_param('~start_test', True)
        
        # Publisher to control the robot's speed
        self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
        
 
        # The base frame is base_footprint for the TurtleBot but base_link for Pi Robot
        self.base_frame = rospy.get_param('~base_frame', '/base_footprint')

        # The odom frame is usually just /odom
        #self.odom_frame = rospy.get_param('~odom_frame', '/odom')          
        self.odom_frame = rospy.get_param('~odom_frame', '/odom_combined')

        # Initialize the tf listener
        self.tf_listener = tf.TransformListener()
        
        # Give tf some time to fill its buffer
        rospy.sleep(2)
        
        # Make sure we see the odom and base frames
        self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0))        
            
        rospy.loginfo("Bring up rqt_reconfigure to control the test.")
  
        self.position = Point()
        
        # Get the starting position from the tf transform between the odom and base frames
        self.position = self.get_position()
        
        x_start = self.position.x
        y_start = self.position.y
            
        move_cmd = Twist()
            
        while not rospy.is_shutdown():
            # Stop the robot by default
            move_cmd = Twist()
            
            if self.start_test:
                # Get the current position from the tf transform between the odom and base frames
                self.position = self.get_position()
                
                # Compute the Euclidean distance from the target point
                distance = sqrt(pow((self.position.x - x_start), 2) +
                                pow((self.position.y - y_start), 2))
                                
                # Correct the estimated distance by the correction factor
                distance *= self.odom_linear_scale_correction
                
                # How close are we?
                error =  distance - self.test_distance
                
                # Are we close enough?
                if not self.start_test or abs(error) <  self.tolerance:
                    self.start_test = False
                    params = {'start_test': False}
                    rospy.loginfo(params)
                else:
                    # If not, move in the appropriate direction
                    move_cmd.linear.x = copysign(self.speed, -1 * error)
            else:
                self.position = self.get_position()
                x_start = self.position.x
                y_start = self.position.y
		        break
                
            self.cmd_vel.publish(move_cmd)
            r.sleep()

        # Stop the robot
        self.cmd_vel.publish(Twist())
	print 'finish moving'
	return []

    def get_position(self):
        # Get the current transform between the odom and base frames
        try:
            (trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return

        return Point(*trans)
        
    def shutdown(self):
        # Always stop the robot when shutting down the node
        rospy.loginfo("Stopping the robot...")
        self.cmd_vel.publish(Twist())
        rospy.sleep(1)
 
if __name__ == '__main__':
    try:
        CalibrateLinear()
        rospy.spin()
    except:
        rospy.loginfo("Calibration terminated.")

需要做的修改

1.添加一个srv

由于需要递交“向前移动的距离”，所以需要一个srv,类型是：请求是浮点数（前进的距离），相应可以为空。下面是我新建的MoveTowards.srv，很简单的两行，第一行是请求，第二行是分隔线，响应为空。

float32 a
---

至于如何创建一个srv,我就不写了，参考ROS wiki的链接。创建ROS消息和ROS服务.

2.对源程序进行的修改

首先import服务,注意格式：from “包名.srv” import “srv的名称”。虽然文件名为MoveTowards.srv但下面填import MoveTowards

from carry.srv import MoveTowards

然后是这一部分：

    def __init__(self):
	    rospy.init_node('move_towards_server', anonymous=False)	
	    service= rospy.Service('move_towards', MoveTowards, self.move)
    def move(self,distanse_you_want):

以前“向前移动一米的代码段”在初始化内，我将其移动到了函数move内，而初始化只包含定义节点和service的语句。一旦有服务请求，将执行回调函数“move”。
注意以下一句话：

        #self.odom_frame = rospy.get_param('~odom_frame', '/odom')          
        self.odom_frame = rospy.get_param('~odom_frame', '/odom_combined')

由于我们的底盘由robot_pose_ekf这个功能包做了传感器信息融合，发布的frame是/odom_combined而不是/odom，所以需要非常注意。可以使用rosrun rqt_tf_tree rqt_tf_tree 来查看tf树，看到底是哪一个。这一行错了，机器人是肯定不能动的。
然后还改了两个小地方，一些小细节：（标注了两个#的两行）

            else:
                self.position = self.get_position()
                x_start = self.position.x
                y_start = self.position.y
		        break    ##
                
            self.cmd_vel.publish(move_cmd)
            r.sleep()

        # Stop the robot
        self.cmd_vel.publish(Twist())
	print 'finish moving'
	return []            ##

在底盘到达指定位置后，break跳出循环，源程序没有break，执行完后一直在while内，但我需要这个server返回一些信息，不然我的客户端程序一直在等待相应，程序一直阻塞没有往下执行。最后加了一行return，虽然返回的为“空”，但是你必须return一个东西，不然客户端不知道service是否执行完毕，导致程序阻塞。

附

好像代码复制上来有点缩进问题，还是附个下载链接吧link