ROS入门（二）：如何设计自定义消息

在上一篇博文中了解了如何编写一个简单的ROS收发程序。但是上次只涉及到标准消息库std_msg中的string类型。如果我们需要传送更复杂的信息如包含多个数据的结构化信息或图像等，就需要设计自定义消息。

本文通过学习rospy中002-004三部分的代码学习如何设计自定义消息、编写launch文件以及更新package.xml文件

更新：20180726 增加007示例中涉及消息的部分

---

接下来我们逐一分析002-004三份代码，三份代码都附有launch文件。
利用$ roslaunch <launchfile>进行实验能够加深对节点中具体操作的理解。

示例007中介绍了在消息和服务中加入字典形式的数据，这里将消息的部分并入本文档。

002_headers：学习rospy中的Header类

编写一个涉及自定义消息的收发程序主要有以下步骤：

• 新建msg文件夹并创建msg文件
  $ touch msg/HeaderString.msg
• 编写节点代码 ‘listener_header.py’ ，’talker_header.py’
• 编写headers.launch文件
• 修改package.xml

HeaderString.msg

Header header
string data

listener_header.py

#!/usr/bin/env python
import sys 
import rospy
from rospy_tutorials.msg import HeaderString

NAME = 'listener_header'

def callback(data):
    chatter = data.data
    header = data.header
    timestamp = header.stamp.to_sec()
    print rospy.get_caller_id(), header.seq, "I just heard that %s at %12f"%(chatter, timestamp)

def listener_header():
    rospy.Subscriber("chatter_header", HeaderString, callback)
    rospy.init_node(NAME, anonymous=True)
    rospy.spin()

if __name__ == '__main__':
    try:
        listener_header()
    except KeyboardInterrupt, e:
        pass
    print "exiting"

与001示例中的listener相比：

需要引用msg文件夹中的HeaderString类

自定义信息由（Header + string）组成

Header类是std_msg的继承类，包括seq,stamp,frame_id三个内容。其中stamp是创建类时传入参数或由genpy.Time生成的时间信息，通过stamp.sec 及 stamp.nsec（纳秒）可获得时间数据。
Header文件地址：/opt/ros/kinetic/lib/python2.7/dist-packages/std_msgs/msg/_Header.py

之前的listener是没有终止条件的

这里通过KeyboardInterrupt类实例e触发停止

talker_header.py

#! /usr/bin/env python
import sys

import rospy
from rospy_tutorials.msg import HeaderString

NAME = 'talker_header'

def talker_header():
    pub = rospy.Publisher("chatter_header", HeaderString, queue_size=10)

    rospy.init_node(NAME) 
    count = 0
    while not rospy.is_shutdown():
        str = 'hello world %s'%count
        print str
        pub.publish(HeaderString(None, str))
        count += 1
        rospy.sleep(0.1)

if __name__ == '__main__':
    talker_header()

通过Listener了解Header类之后talker的代码就没有什么疑问点了。差别主要是提示信息从loginfo改为直接print

headers.launch

<launch>
  <node name="listener_header" pkg="rospy_tutorials" type="listener_header.py" output="screen"/>
  <node name="talker_header" pkg="rospy_tutorials" type="talker_header.py" output="screen"/>
</launch>

package.xml

编译msg或src文件，需要修改package.xml。创建包的时候只是声明依赖std_msgs包，要生成消息或服务，需要增加如下内容：

<build_depend>messeage_generation</build_depend>
<run_depend>message_runtime</run_depend>

该launch文件启动两个节点。

003_listener_with_user_data：向接受反馈“callback”函数传参

该示例主要介绍了使用callback_args向callback函数传参。通常callback函数只是将消息推送到console或者日志文件，功能相似度很高。当callback函数可接收参数时，不同类型的主题调用同一个处理不同参数的callback函数，可大幅度简化代码。
由于只涉及callback函数中用{if，elif，else}处理参数，此处不粘帖py文件和launch文件的代码，感兴趣的朋友可以去看看。

004_listener_subscribe_notify：主题更新到达订阅户的反馈

该示例主要介绍了Publisher中subcriber_listener的用法。
首先，定义继承SubscribeListenser的类ChatterListener并对父类的两个函数进行复写，增加推送到console的提示信息。在定义publisher的时候将subscriber_listener定义为ChatterListener.

#！/usr/bin/env python
NAME = 'talker_callback'

import sys

import rospy
from std_msgs.msg import String

class ChatterListener(rospy.SubscribeListener):
    def peer_subscribe(self, topic_name, topic_publish, peer_publish):
        print("a peer subscribed to topic [%s]" % topic_name)

        str = "Hey everyone, we have a new friend!"
        print(str)
        topic_publish(String(str))
        str = "greetings. welcome to topic "+topic_name
        print(str)
        peer_publish(String(str))

    def peer_unsubscribe(self, topic_name, numPeers):
        print("a peer unsubscribed from topic [%s]" % topic_name)
        if numPeers == 0:
            print("I have no friends")

def talker_callback():
    pub = rospy.Publisher("chatter", String, subscriber_listener=ChatterListener(), queue_size=10)
    rospy.init_node(NAME, anonymous=True)
    count = 0
    while not rospy.is_shutdown():
        str = "hello world %d"%count
        print(str)
        pub.publish(String(str))
        count += 1
        rospy.sleep(0.1)

if __name__ == '__main__':
    try:
        talker_callback()
    except rospy.ROSInterruptException: pass

看到这里，博主对于talker接收订阅者反馈的机制的作用还不是很理解，所以做了一个小实验：
- 运行listener_subscribe_notify.launch
- 查看节点名字
$ rosnode list
- 新开一个终端关闭listener_1节点
$rosnode kill /listener_1
回到原来的终端查看输出的信息：

首先talker节点在发布消息之前将信息文本打印在console上，接着4个listener接收到信息后报告收到信息。
在发送第330次时，ROS网络收到来自用户的关闭信息，应该是我kill掉listener1的时; 在第331次发送后，talker节点收到listener1的退订消息; 其后便不再接受来自listener1的反馈。

存疑：

进程关闭的时候显示的是listener1-2, 2-3,3-4,4-5 而且关闭次序是和生成次序相反的。难道chatter的发送是通过一条listener的链表逐一传递的？

tcp_nodelay

虽然示例中没有涉及这个参数的设定，但是博主认为tcp_nodelay是一个很值得学习的参数。
tcp_nodelay是Publisher的一个bool型参数，默认为false。当设置为True时，不进行Nagle algorithm.降低时延但可能也影响传输效率。

Nagle Algorithm
由于TCP/IP传输协议有40字节的头信息（20字节TCP，20字节IPv4），假设需要传输1字节的数据，就需要41字节的传输包。这无疑产生了资源浪费。Nagle算法将一些小容量的传出数据合并后一起发送。对于request-response这种场景，应该禁用Nagle算法以确保当传出数据大于一个packet的时候，request能接受完整的数据，不需要等待因拆分而停滞在管道的小包数据。对于窄带时间敏感网络（如NB-IoT）也应禁用Nagle算法。

007_connection_header: 传输信息的头部

就像文件有头信息说明如何解码数据，传输协议有头信息声明源地址等，ROS的Publisher也能定义一些必要的信息发送给订阅用户以说明自己的身份等，这些内容独立于每次传输的内容，且在定义Publisher时即给定。

talker和listener的代码和002示例大致相同，主要的区别在于talker节点中Publisher的定义以及listener接受信息后对头信息的查阅。

talker代码中Publisher在定义时向headers增加了一个字典，key为’cookies’，而value为’oreo’.

pub = rospy.Publisher('chatter', String, headers={'cookies': 'oreo'}, queue_size=10)

listener代码中在callback函数中增加了对头信息的获取。就像我们上网获取的数据包data，在data.data是我们向服务器请求的资源内容，而data._connection_header则存储了协议头信息。

def callback(data):
    chatter = data.data
    if 'callerid' in data._connection_header:
        who = data._connection_header['callerid']
    else:
        who = 'unknown'
    if 'cookies' in data._connection_header:
        print "%s just offered me %s cookies"%(who, data._connection_header['cookies'])
    else:
        print "I just heard %s from %s"%(chatter, who)

$ rosrun rospy_tutorials talker_connection_header.py

talker 节点持续向主题发布”hello world“的信息

$ rosrun rospy_tutorials listener_connection_header.py


listener 接受信息后的反馈

---

至此，我们在学习如何使用消息的同时，也基本了解了Publisher中各个参数的使用。

class Publisher(Topic)
 |  Class for registering as a publisher of a ROS topic.
 |  __init__(self, name, data_class, subscriber_listener=None, tcp_nodelay=False
, latch=False, headers=None, queue_size=None)

• name: 主题的名称
• data_class: 发布信息的类型（订阅户在登记时需要声明同样的数据类型）
• subscriber_listener: 信息达到订阅户时激活的函数
• tcp_nodelay: 传输时是否使用Nagle算法
• latch: 这个参数在示例中没有介绍; 当该值为真时，新的订阅户在建立连接时将接收到talker最近一条发出过的信息。
• headers: 和消息一起发送的头信息
• queue_size: 多线程下异步发送的队列长度。当未给定时，所有消息同步发出。帮助文档说这是一件很危险的事情，博主目前想不到例子介绍。后期填坑。

参考文献：
[1] rospy 官方文档：http://wiki.ros.org/rospy/
[2] Nagle算法维基百科： https://en.wikipedia.org/wiki/Nagle%27s_algorithm