ROS与Python入门教程-使用numpy

ROS与Python入门教程-使用numpy

说明

• 这一节介绍numpy，它是针对科学计算的python软件包, 用于处理传感器数据，数组数据等。
• 在本教程中，我们将使用rospy.numpy_msg模块。这个模块带有numpy_msg()的封装，它允许节点在适当的地方反序列化消息直接进入NumPy数组。numpy_msg()封装也可以用来发布包含NumPy数组数据信息。

创建包numpy_tutorial

• 创建新包，依赖numpy_tutorial，依赖rospy_tutorials的消息类型。

catkin_create_pkg numpy_tutorial rospy rospy_tutorials

• 查看rospy_tutorials的消息类型

$ rosmsg show rospy_tutorials/Floats
float32[] data

• 需要依赖python-numpy,添加下面两行代码到在package.xml文件里。

<build_depend>python-numpy</build_depend>
<run_depend>python-numpy</run_depend>

创建没使用numpy的listener节点

• 创建节点_listener.py, 用来订阅floats主题。

$ cd ~/catkin_ws/src/numpy_tutorial
$ mkdir scripts
$ cd scripts 
$ touch numpy_listener.py
$ chmod +x numpy_listener.py
$ vim numpy_listener.py

• 手工输入代码：

#!/usr/bin/env python

import rospy
from rospy_tutorials.msg import Floats

def callback(data):
    print rospy.get_name(), "I heard %s"%str(data.data)

def listener():
    rospy.init_node('listener')
    rospy.Subscriber("floats", Floats, callback)
    rospy.spin()

if __name__ == '__main__':
    listener()

测试listener节点

• 新开终端，启动roscore

$ roscore

• 新开终端，启动订阅节点

$ rosrun numpy_tutorial numpy_listener.py

• 新开终端，发布主题

$ rostopic pub -r 1 floats rospy_tutorials/Floats "[1.1, 2.2, 3.3, 4.4, 5.5]"

• 效果：

$ rosrun numpy_tutorial numpy_listener.py
/listener-977-1248226102144 I heard (1.1000000238418579, 2.2000000476837158, 3.2999999523162842, 4.4000000953674316, 5.5)
/listener-977-1248226102144 I heard (1.1000000238418579, 2.2000000476837158, 3.2999999523162842, 4.4000000953674316, 5.5)
... and so on

使用Numpy的listener节点

• 更改节点，需要变化: 导入numpy_msg， 在订阅指定numpy_msg函数处理的浮点值。

from rospy.numpy_msg import numpy_msg
...
    rospy.Subscriber("floats", numpy_msg(Floats), callback)

• 更改代码为：

#!/usr/bin/env python

import rospy
from rospy_tutorials.msg import Floats
from rospy.numpy_msg import numpy_msg

def callback(data):
    print rospy.get_name(), "I heard %s"%str(data.data)

def listener():
    rospy.init_node('listener')
    rospy.Subscriber("floats", numpy_msg(Floats), callback)
    rospy.spin()

if __name__ == '__main__':
    listener()

测试使用Numpy的listener节点

• 新开终端，启动roscore

$ roscore

• 新开终端，启动订阅节点

$ rosrun numpy_tutorial numpy_listener.py

• 发布主题

$ rostopic pub -r 1 floats rospy_tutorials/Floats "[1.1, 2.2, 3.3, 4.4, 5.5]"

• 效果：（跟之前稍有不同，就是位数变化，返回的是numpy数组格式）

$ rosrun rospy_tutorials listener_numpy.py
/listener-1243-1248226610835 I heard [ 1.10000002  2.20000005  3.29999995  4.4000001   5.5       ]
/listener-1243-1248226610835 I heard [ 1.10000002  2.20000005  3.29999995  4.4000001   5.5       ]
... and so on

使用Numpy的Talker节点

• Talker节点需要正确处理Numpy的数组的数据格式

• 在scripts新建numpy_talker.py

  $ cd ~/catkin_ws/src/numpy_tutorial
  $ mkdir scripts
  $ cd scripts
  $ touch numpy_talker.py
  $ chmod +x numpy_talker.py
  $ vim numpy_talker.py

• 手工输入如下代码：

#!/usr/bin/env python

import rospy
from rospy.numpy_msg import numpy_msg
from rospy_tutorials.msg import Floats

import numpy
def talker():
    pub = rospy.Publisher('floats', numpy_msg(Floats),queue_size=10)
    rospy.init_node('talker', anonymous=True)
    r = rospy.Rate(10) # 10hz
    while not rospy.is_shutdown():
        a = numpy.array([1.0, 2.1, 3.2, 4.3, 5.4, 6.5], dtype=numpy.float32)
        pub.publish(a)
        r.sleep()

if __name__ == '__main__':
    talker()

• 代码分析：

  • 代码：pub = rospy.Publisher('floats', numpy_msg(Floats))

  • 分析：发布经过numpy_msg函数来处理浮点值内容

  • 代码：a = numpy.array([1.0, 2.1, 3.2, 4.3, 5.4, 6.5], dtype=numpy.float32)

  • 分析：利用numpy.array来转换成numpy的数组结构，并要指定内容的数据格式，这是float32

测试使用numpy的Talker节点

• 新开终端，启动roscore

$ roscore

• 新开终端，启动订阅节点

$ rosrun numpy_tutorial numpy_listener.py

• 新开终端，启动发布节点

  $ rosrun numpy_tutorial numpy_talker.py

• 效果：

$ rosrun rospy_tutorials listener_numpy.py
/listener-1423-1248226794834 I heard [ 1.          2.0999999   3.20000005  4.30000019  5.4000001   6.5       ]
/listener-1423-1248226794834 I heard [ 1.          2.0999999   3.20000005  4.30000019  5.4000001   6.5       ]
... and so on