ROS--rospy
第七章 rospy
本章简介
上一章我们学习了C++语音的ROS接口roscpp，本章我们来学习ROS的另一个接口rospy，也即是Python语音的接口。客户端库（Client Libarary）不仅仅指的是C++、Python语言的接口，其实是各种语言的接口统称。rospy中函数的定义，函数的用法都和roscpp不相同。
本章依旧需要一定的Python编程基础，不熟悉该语言需要先行学习Python编程基础。本章的主要内容有rospy的一些常用的函数，以及一些重要的类。结合这些内容，本章还给出了topic demo和service demo的具体格式和写法，方便读者更直观的学习rospy的编写。
7.1 rospy与主要接口
7.1.1 rospy vs roscpp
rospy是Python版本的ROS客户端库，提供了Python编程需要的接口，你可以认为rospy就是一个Python的模块(Module)。这个模块位于/opt/ros/kineetic/lib/python2.7/dist-packages/rospy之中。
rospy包含的功能与roscpp相似，都有关于node、topic、service、param、time相关的操作。但同时rospy和roscpp也有一些区别：
- rospy没有一个NodeHandle，像创建publisher、subscriber等操作都被直接封装成了rospy中的函数或类，调用起来简单直观。
- rospy一些接口的命名和roscpp不一致，有些地方需要开发者注意，避免调用错误。
相比于C++的开发，用Python来写ROS程序开发效率大大提高，诸如显示、类型转换等细节不再需要我们注意，节省时间。但Python的执行效率较低，同样一个功能用Python运行的耗时会高于C++。因此我们开发SLAM、路径规划、机器视觉等方面的算法时，往往优先选择C++。
ROS中绝大多数基本指令，例如rostopic,roslaunch都是用python开发的，简单轻巧。
7.1.2 ROS中Python代码的组织方式
要介绍rospy，就不得不提Python代码在ROS中的组织方式。通常来说，Python代码有两种组织方式，一种是单独的一个Python脚本，适用于简单的程序，另一种是Python模块，适合体量较大的程序。
单独的Python脚本
对于一些小体量的ROS程序，一般就是一个Python文件，放在script/路径下，非常简单。
your_package
|- script/
|- your_script.py
|-...
Python模块
当程序的功能比较复杂，放在一个脚本里搞不定时，就需要把一些功能放到Python Module里，以便其他的脚本来调用。ROS建议我们按照以下规范来建立一个Python的模块：
your_package
|- src/
|-your_package/
|- _init_.py
|- modulefiles.py
|- scripts/
|- your_script.py
|- setup.py
在src下建立一个与你的package同名的路径，其中存放_init_.py以及你的模块文件。这样就建立好了ROS规范的Python模块，你可以在你的脚本中调用。 如果你不了解init.py的作用，可以参考这篇博客http://www.cnblogs.com/Lands-ljk/p/5880483.html ROS中的这种Python模块组织规范与标准的Python模块规范并不完全一致，你当然可以按照Python的标准去建立一个模块，然后在你的脚本中调用，但是我们还是建议按照ROS推荐的标准来写，这样方便别人去阅读。
通常我们常用的ROS命令，大多数其实都是一个个Python模块，源代码存放在ros_comm仓库的tools路径下：https://github.com/ros/ros_comm/tree/lunar-devel/tools 你可以看到每一个命令行工具（如rosbag、rosmsg）都是用模块的形式组织核心代码，然后在script/下建立一个脚本来调用模块。
7.1.3 常用rospy的API
这里分类整理了rospy常见的一些用法，请你浏览一遍，建立一个初步的影响。 具体API请查看http://docs.ros.org/api/rospy/html/rospy-module.html
Node相关
| 返回值 | 方法 | 作用 |
|---|---|---|
| rospy.init_node(name, argv=None, anonymous=False) | 注册和初始化node | |
| MasterProxy | rospy.get_master() | 获取master的句柄 |
| bool | rospy.is_shutdown() | 节点是否关闭 |
| rospy.on_shutdown(fn) | 在节点关闭时调用fn函数 | |
| str | get_node_uri() | 返回节点的URI |
| str | get_name() | 返回本节点的全名 |
| str | get_namespace() | 返回本节点的名字空间 |
| ... | ... | ... |
Topic相关
函数：
| 返回值 | 方法 | 作用 |
|---|---|---|
| [[str, str]] | get_published_topics() | 返回正在被发布的所有topic名称和类型 |
| Message | wait_for_message(topic, topic_type, time_out=None) | 等待某个topic的message |
| spin() | 触发topic或service的回调/处理函数，会阻塞直到关闭节点 | |
| ... | ... | ... |
Publisher类：
| 返回值 | 方法 | 作用 |
|---|---|---|
| init(self, name, data_class, queue_size=None) | 构造函数 | |
| publish(self, msg) | 发布消息 | |
| str | unregister(self) | 停止发布 |
| ... | ... | ... |
Subscriber类：
| 返回值 | 方法 | 作用 |
|---|---|---|
| init_(self, name, data_class, call_back=None, queue_size=None) | 构造函数 | |
| unregister(self, msg) | 停止订阅 | |
| ... | ... | ... |
Service相关
函数：
| 返回值 | 方法 | 作用 |
|---|---|---|
| wait_for_service(service, timeout=None) | 阻塞直到服务可用 | |
| ... | ... | ... |
Service类(server)：
| 返回值 | 方法 | 作用 |
|---|---|---|
| init(self, name, service_class, handler) | 构造函数，handler为处理函数，service_class为srv类型 | |
| shutdown(self) | 关闭服务的server | |
| ... | ... | ... |
ServiceProxy类(client)：
| 返回值 | 方法 | 作用 |
|---|---|---|
| init(self, name, service_class) | 构造函数，创建client | |
| call(self, args, *kwds) | 发起请求 | |
| call(self, args, *kwds) | 同上 | |
| close(self) | 关闭服务的client | |
| ... | ... | ... |
Param相关
函数：
| 返回值 | 方法 | 作用 |
|---|---|---|
| XmlRpcLegalValue | get_param(param_name, default=_unspecified) | 获取参数的值 |
| [str] | get_param_names() | 获取参数的名称 |
| set_param(param_name, param_value) | 设置参数的值 | |
| delete_param(param_name) | 删除参数 | |
| bool | has_param(param_name) | 参数是否存在于参数服务器上 |
| str | search_param() | 搜索参数 |
| ... | ... | ... |
时钟相关
函数：
| 返回值 | 方法 | 作用 |
|---|---|---|
| Time | get_rostime() | 获取当前时刻的Time对象 |
| float | get_time() | 返回当前时间，单位秒 |
| sleep(duration) | 执行挂起 | |
| ... | ... | ... |
Time类：
| 返回值 | 方法 | 作用 |
|---|---|---|
| init(self, secs=0, nsecs=0) | 构造函数 | |
| Time | now() | 静态方法 返回当前时刻的Time对象 |
| ... | ... | ... |
Duration类：
| 返回值 | 方法 | 作用 |
|---|---|---|
| init(self, secs=0, nsecs=0) | 构造函数 | |
| ... | ... | ... |
7.2 topic in rospy
与5.3节类似，我们用python来写一个节点间消息收发的demo，同样还是创建一个自定义的gps类型的消息，一个节点发布模拟的gps信息，另一个接收和计算距离原点的距离。
7.2.1 自定义消息的生成
gps.msg定义如下：
string state #工作状态
float32 x #x坐标
float32 y #y坐标
我们需要修改CMakeLists.txt文件，方法见5.3节，这里需要强调一点的就是，对创建的msg进行catkin_make会在~/catkin_ws/devel/lib/python2.7/dist-packages/topic_demo下生成msg模块（module）。 有了这个模块，我们就可以在python程序中from topic_demo.msg import gps,从而进行gps类型消息的读写。
7.2.2 消息发布节点
与C++的写法类似，我们来看topic用Python如何编写程序，见topic_demo/scripts/pytalker.py：
#!/usr/bin/env python
#coding=utf-8
import rospy
#导入自定义的数据类型
from topic_demo.msg import gps
def talker():
#Publisher 函数第一个参数是话题名称，第二个参数 数据类型，现在就是我们定义的msg 最后一个是缓冲区的大小
#queue_size: None（不建议） #这将设置为阻塞式同步收发模式！
#queue_size: 0（不建议）#这将设置为无限缓冲区模式，很危险！
#queue_size: 10 or more #一般情况下，设为10 。queue_size太大了会导致数据延迟不同步。
pub = rospy.Publisher('gps_info', gps , queue_size=10)
rospy.init_node('pytalker', anonymous=True)
#更新频率是1hz
rate = rospy.Rate(1)
x=1.0
y=2.0
state='working'
while not rospy.is_shutdown():
#计算距离
rospy.loginfo('Talker: GPS: x=%f ,y= %f',x,y)
pub.publish(gps(state,x,y))
x=1.03*x
y=1.01*y
rate.sleep()
if __name__ == '__main__':
talker()
以上代码与C++的区别体现在这几个方面：
- rospy创建和初始化一个node，不再需要用NodeHandle。rospy中没有设计NodeHandle这个句柄，我们创建topic、service等等操作都直接用rospy里对应的方法就行。
- rospy中节点的初始化并一定得放在程序的开头，在Publisher建立后再初始化也没问题。
- 消息的创建更加简单，比如gps类型的消息可以直接用类似于构造函数的方式
gps(state,x,y)来创建。 - 日志的输出方式不同，C++中是
ROS_INFO()，而Python中是rospy.loginfo() - 判断节点是否关闭的函数不同，C++用的是
ros::ok()而Python中的接口是rospy.is_shutdown()
通过以上的区别可以看出，roscpp和rospy的接口并不一致，在名称上要尽量避免混用。在实现原理上，两套客户端库也有各自的实现，并没有基于一个统一的核心库来开发。这也是ROS在设计上不足的地方。
ROS2就解决了这个问题，ROS2中的客户端库包括了rclcpp(ROS Clinet Library C++)、rclpy(ROS Client Library Python),以及其他语言的版本，他们都是基于一个共同的核心ROS客户端库rcl来开发的，这个核心库由C语言实现。
7.2.3 消息订阅节点
见topic_demo/scripts/pylistener.py：
#!/usr/bin/env python
#coding=utf-8
import rospy
import math
#导入mgs
from topic_demo.msg import gps
#回调函数输入的应该是msg
def callback(gps):
distance = math.sqrt(math.pow(gps.x, 2)+math.pow(gps.y, 2))
rospy.loginfo('Listener: GPS: distance=%f, state=%s', distance, gps.state)
def listener():
rospy.init_node('pylistener', anonymous=True)
#Subscriber函数第一个参数是topic的名称，第二个参数是接受的数据类型 第三个参数是回调函数的名称
rospy.Subscriber('gps_info', gps, callback)
rospy.spin()
if __name__ == '__main__':
listener()
在订阅节点的代码里，rospy与roscpp有一个不同的地方：rospy里没有spinOnce()，只有spin()。
建立完talker和listener之后，经过catkin_make，就完成了python版的topic通信模型。