一个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(http://www.my516.com))
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
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
需要做的修改
1.添加一个srv
由于需要递交“向前移动的距离”，所以需要一个srv,类型是：请求是浮点数（前进的距离），相应可以为空。下面是我新建的MoveTowards.srv，很简单的两行，第一行是请求，第二行是分隔线，响应为空。

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

2.对源程序进行的修改
首先import服务,注意格式：from “包名.srv” import “srv的名称”。虽然文件名为MoveTowards.srv但下面填import MoveTowards

from carry.srv import MoveTowards
1
然后是这一部分：

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):
1
2
3
4
以前“向前移动一米的代码段”在初始化内，我将其移动到了函数move内，而初始化只包含定义节点和service的语句。一旦有服务请求，将执行回调函数“move”。
注意以下一句话：

#self.odom_frame = rospy.get_param('~odom_frame', '/odom')
self.odom_frame = rospy.get_param('~odom_frame', '/odom_combined')
1
2
由于我们的底盘由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 [] ##
1
2
3
4
5
6
7
8
9
10
11
12
13
在底盘到达指定位置后，break跳出循环，源程序没有break，执行完后一直在while内，但我需要这个server返回一些信息，不然我的客户端程序一直在等待相应，程序一直阻塞没有往下执行。最后加了一行return，虽然返回的为“空”，但是你必须return一个东西，不然客户端不知道service是否执行完毕，导致程序阻塞。
---------------------