用ros publisher发布cmd_vel命令(python)_python给cmd-vel发指令

参考ros wiki的链接https://www.google.com/url?sa=t&rct=j&q=&esrc=s&source=web&cd=1&ved=0ahUKEwja9rnUjPPPAhXGI5QKHZgxDnsQFggcMAA&url=http%3A%2F%2Fwiki.ros.org%2FROS%2FTutorials%2FWritingPublisherSubscriber(c%252B%252B)&usg=AFQjCNGExFyKLatoS0PpIZyoWjvAqa62sQ&bvm=bv.136593572,d.cGw&cad=rja

本文实现的是将本地生成的一系列控制指令，从csv文件中读取，并以一定的频率发布给机器人。
首先介绍cmd_vel的数据类型
http://docs.ros.org/api/geometry_msgs/html/msg/Twist.html

 #This expresses velocity in free space broken into its linear and angular parts.
Vector3  linear
Vector3  angular
1
2
3

由三维的速度控制指令和三维的角度控制指令组成，一般就取linear.x和angular.z即可
我们需要写如下代码，才能正常使用该数据类型

from geometry_msgs.msg import Twist
1

接下来是读取csv文件，用csv.reader可以很方便的实现

read=csv.reader(csvfile)
for row in read:
            xxx=','.join(row)
            para =xxx.split(',') 
            vel=para[0]
            ang=para[1]
1
2
3
4
5
6

xxx返回了csv文件中每行的两个数据，以逗号分隔，para则是以逗号为分隔符将两个数据分别提取出来保存到数组中，下面用vel，ang两个变量分别保存。

有了数据，还需要一步

msg = Twist() #初始化
msg.linear.x=float(vel)
msg.angular.z=float(ang)
1
2
3

最后我们就可以发布了
pub.publish(msg)

完整代码如下

#!/usr/bin/env python
import rospy
import csv
from geometry_msgs.msg import Twist


csvfile=file('your/csv/file.csv','r')
read=csv.reader(csvfile)

def talker():
    pub = rospy.Publisher('/jackal_velocity_controller/cmd_vel', Twist,queue_size=10)
    rospy.init_node('talker', anonymous=True)
    rate = rospy.Rate(50) 
    msg = Twist()
    while not rospy.is_shutdown():
    for row in read:
            xxx=','.join(row)
            para =xxx.split(',') 
            vel=para[0]
            ang=para[1]
            msg.linear.x=float(vel)
            msg.angular.z=float(ang)
            pub.publish(msg)
            rate.sleep()

if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException:
        pass
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