经验首页 前端设计 程序设计 Java相关 移动开发 数据库/运维 软件/图像 大数据/云计算 其他经验
当前位置：技术经验 » 程序设计 » C++ » 查看文章
ROS应用层通信协议解析
来源：cnblogs　　作者：sherlock_lin　　时间：2022/11/19 17:13:50　　对本文有异议

参考：http://wiki.ros.org/ROS/Master_API

http://wiki.ros.org/ROS/Connection Header

说明

ROS本质上就是一个松耦合的通信框架，通信模式包括：远程调用（service-client）、订阅发布（topic）、持续通信（action）和全局参数（参数服务器），这四种模式基本已经能够涵盖百分之九十的应用场景了

本次针对订阅发布模式，探究一下ROS通信中的具体通信协议，读完本文后，你可以在不依赖ROS的情况下和ROS通信

本次通信采用从机订阅主机数据，通过wireshark抓包，得到具体xmlrpc协议数据内容，根据xmlrpc协议格式，找到对应代码

（因为时间有限，部分协议可能有跳过的地方）

1、registerPublisher

从机创建节点的第一步

这个方法用于注册一个发布者的caller

request报文body：

  1. <?xml version="1.0"?>
  2. <methodCall>
  3.     <methodName>registerPublisher</methodName>
  4.     <params>
  5.         <param>
  6.             <value>/test_sub</value>
  7.         </param>
  8.         <param>
  9.             <value>/rosout</value>
  10.         </param>
  11.         <param>
  12.             <value>rosgraph_msgs/Log</value>
  13.         </param>
  14.         <param>
  15.             <value>http://192.168.1.150:40209</value>
  16.         </param>
  17.     </params>
  18. </methodCall>

response报文body：

  1. <?xml version='1.0'?>
  2. <methodResponse>
  3.     <params>
  4.         <param>
  5.             <value>
  6.                 <array>
  7.                     <data>
  8.                         <value>
  9.                             <int>1</int>
  10.                         </value>
  11.                         <value>
  12.                             <string>Registered [/test_sub] as publisher of [/rosout]</string>
  13.                         </value>
  14.                         <value>
  15.                             <array>
  16.                                 <data>
  17.                                     <value>
  18.                                         <string>http://sherlock:35861/</string>
  19.                                     </value>
  20.                                 </data>
  21.                             </array>
  22.                         </value>
  23.                     </data>
  24.                 </array>
  25.             </value>
  26.         </param>
  27.     </params>
  28. </methodResponse>

先说结论：

ROS有一个日志相关的topic，名称是 /rosout，所有节点的日志信息都会通过这个 topic 发布出来

ROS还有一个日志相关的节点，名称是 /rosout，负责订阅 /rosout数据，然后使用名称为 /rosout_agg 的topic发布出来， /rosout_agg 的订阅者是rqt等调试工具

所以，结合上面的xml内容，我们可以大致推断，创建一个新的节点，那么这个节点必定会发布一个topic，就是/rosout，所以上面的XMLRPC协议内容，就是网rosmaster内注册一个publisher，用于发布/rosout

整体来说，就是调用接口

  1. registerPublisher("/test_sub", "/rosout", "rosgraph_msgs/Log", "http://192.168.1.150:40209")

返回值是：

  1. 1
  2. Registered [/test_sub] as publisher of [/rosout]
  3. http://sherlock:35861/

1是固定数据

第二行是message

最后一个返回值表示订阅者的uri列表，这里因为只有一个订阅者，所有只有一个uri

再看代码：

函数声明如下：

  1. registerPublisher(caller_id, topic, topic_type, caller_api)
  2. 	Register the caller as a publisher the topic.
  3. 	参数
  4. 		caller_id (str)
  5. 			ROS caller ID
  6. 		topic (str)
  7. 			Fully-qualified name of topic to register.
  8. 		topic_type (str)
  9. 			Datatype for topic. Must be a package-resource name, i.e. the .msg name.
  10. 		caller_api (str)
  11. 			API URI of publisher to register.
  12. 	返回值(int, str, [str])
  13. 		(code, statusMessage, subscriberApis)
  14. 		List of current subscribers of topic in the form of XMLRPC URIs.

找到 registerPublisher 接接口，位于ros_comm/rosmaster 包中，文件为：master_api.py（ROS主从机制利用python实现，拿掉python则无法实现主从）

  1. @apivalidate([], ( is_topic('topic'), valid_type_name('topic_type'), is_api('caller_api')))
  2. def registerPublisher(self, caller_id, topic, topic_type, caller_api):
  3. 	"""
  4.     Register the caller as a publisher the topic.
  5.     @param caller_id: ROS caller id
  6.     @type  caller_id: str
  7.     @param topic: Fully-qualified name of topic to register.
  8.     @type  topic: str
  9. 	@param topic_type: Datatype for topic. Must be a
  10.     package-resource name, i.e. the .msg name.
  11. 	@type  topic_type: str
  12.     @param caller_api str: ROS caller XML-RPC API URI
  13.     @type  caller_api: str
  14.     @return: (code, statusMessage, subscriberApis).
  15.     List of current subscribers of topic in the form of XMLRPC URIs.
  16.     @rtype: (int, str, [str])
  17.     """
  18.     #NOTE: we need topic_type for getPublishedTopics.
  19. 	try:
  20.         self.ps_lock.acquire()
  21.         self.reg_manager.register_publisher(topic, caller_id, caller_api)
  22.         # don't let '*' type squash valid typing
  23.         if topic_type != rosgraph.names.ANYTYPE or not topic in self.topics_types:
  24.             self.topics_types[topic] = topic_type
  25.         pub_uris = self.publishers.get_apis(topic)
  26.         sub_uris = self.subscribers.get_apis(topic)
  27.         self._notify_topic_subscribers(topic, pub_uris, sub_uris)
  28.         mloginfo("+PUB [%s] %s %s",topic, caller_id, caller_api)
  29.         sub_uris = self.subscribers.get_apis(topic)            
  30.     finally:
  31.     	self.ps_lock.release()
  32. 	return 1, "Registered [%s] as publisher of [%s]"%(caller_id, topic), sub_uris

registerPublisher 接口的注释：Register the caller as a publisher the topic，将调用者注册为一个topic发布者

可以对应xmlrpc中对应参数，加上猜测：

caller_id：调用者，可以认为是节点，/test_sub，从及创建的节点

topic：发布的topic name，/rosout

topic_type：发布的topic数据类型，rosgraph_msgs/Log

caller_api：调用者发布数据的API接口，http://192.168.1.150:40209

总上，我们大概有几点猜测：

  1. 接口在rosmaster中，接口是registerPublisher，表示，这是注册节点的
  2. 告诉master节点，我创建了一个节点，节点名是/test_sub
  3. 告诉master，这个节点需要发布topic，topic名是/rosout，数据类型是rosgraph_msgs/Log

registerPublisher 接口中有三个地方需要注意：

  1. register_publisher接口调用
  2. _notify_topic_subscribers接口调用，告知当前所有的subscriber，有新的publisher，他们需要再次到新的publisher中去订阅数据
  3. return 内容，最后会拼接成xmlrpc的报文，response 回去，这也就顺便解释了第二条xmlrpc报文（response）

register_publisher

先看 register_publisher，代码在rosmaster中的registrations.py文件中

  1. def register_publisher(self, topic, caller_id, caller_api):
  2.     """
  3. 	Register topic publisher
  4.     @return: None
  5.     """
  6.     self._register(self.publishers, topic, caller_id, caller_api)

_register 接口，这个节点做了三件事

  1. 调用内部接口保存节点信息
  2. 如果这个节点之前已经存在，就表明它是在更新，则发布数据的接口改变，且之前已经有订阅，则此时所有订阅该接口的所有subscriber解除订阅
  3. 调用register接口保存
  1. def _register(self, r, key, caller_id, caller_api, service_api=None):
  2.     # update node information
  3.     node_ref, changed = self._register_node_api(caller_id, caller_api)
  4.     node_ref.add(r.type, key)
  5.     # update pub/sub/service indicies
  6.     if changed:
  7.         self.publishers.unregister_all(caller_id)
  8.         self.subscribers.unregister_all(caller_id)
  9.         self.services.unregister_all(caller_id)
  10.         self.param_subscribers.unregister_all(caller_id)
  11. 	r.register(key, caller_id, caller_api, service_api)

_register_node_api 接口，我们可以看到，它主要做两件事

  1. 更新master中节点信息（节点名、节点发布数据的接口）
  2. 检查这个节点是不是已经存在，如果是，则告诉调用者
  1. def _register_node_api(self, caller_id, caller_api):
  2.     """
  3.     @param caller_id: caller_id of provider
  4.     @type  caller_id: str
  5.     @param caller_api: caller_api of provider
  6.     @type  caller_api: str
  7.     @return: (registration_information, changed_registration). changed_registration is true if 
  8.     caller_api is differet than the one registered with caller_id
  9.     @rtype: (NodeRef, bool)
  10.     """
  11.     node_ref = self.nodes.get(caller_id, None)
  13.     bumped_api = None
  14.     if node_ref is not None:
  15.         if node_ref.api == caller_api:
  16.             return node_ref, False
  17.         else:
  18.             bumped_api = node_ref.api
  19.             self.thread_pool.queue_task(bumped_api, shutdown_node_task,
  20.                                         (bumped_api, caller_id, "new node registered with same name"))
  22.     node_ref = NodeRef(caller_id, caller_api)
  23.     self.nodes[caller_id] = node_ref
  24.     return (node_ref, bumped_api != None)

_notify_topic_subscribers

_notify_topic_subscribers 代码，根据注释说明，接口的作用就是通知所有的subscriber，有新的publisher

  1. def _notify_topic_subscribers(self, topic, pub_uris, sub_uris):
  2.     """
  3.     Notify subscribers with new publisher list
  4.     @param topic: name of topic
  5.     @type  topic: str
  6.     @param pub_uris: list of URIs of publishers.
  7.     @type  pub_uris: [str]
  8.     """
  9.     self._notify(self.subscribers, publisher_update_task, topic, pub_uris, sub_uris)

_notify 代码，将更新的通知任务（publisher_update_task）放进事件队列中，等待执行：

  1. def _notify(self, registrations, task, key, value, node_apis):
  2.     """
  3.     Generic implementation of callback notification
  4.     @param registrations: Registrations
  5.     @type  registrations: L{Registrations}
  6.     @param task: task to queue
  7.     @type  task: fn
  8.     @param key: registration key
  9.     @type  key: str
  10.     @param value: value to pass to task
  11.     @type  value: Any
  12.     """
  13.     # cache thread_pool for thread safety
  14.     thread_pool = self.thread_pool
  15.     if not thread_pool:
  16.         return
  17.     
  18.     try:            
  19.         for node_api in node_apis:
  20.             # use the api as a marker so that we limit one thread per subscriber
  21.             thread_pool.queue_task(node_api, task, (node_api, key, value))
  22.     except KeyError:
  23.         _logger.warn('subscriber data stale (key [%s], listener [%s]): node API unknown'%(key, s))

publisher_update_task 代码，传入的三个参数分别是：新节点的接口、topic名称、订阅者的接口：

  1. def publisher_update_task(api, topic, pub_uris):
  2.     """
  3.     Contact api.publisherUpdate with specified parameters
  4.     @param api: XML-RPC URI of node to contact
  5.     @type  api: str
  6.     @param topic: Topic name to send to node
  7.     @type  topic: str
  8.     @param pub_uris: list of publisher APIs to send to node
  9.     @type  pub_uris: [str]
  10.     """
  11.     msg = "publisherUpdate[%s] -> %s %s" % (topic, api, pub_uris)
  12.     mloginfo(msg)
  13.     start_sec = time.time()
  14.     try:
  15.         #TODO: check return value for errors so we can unsubscribe if stale
  16.         ret = xmlrpcapi(api).publisherUpdate('/master', topic, pub_uris)
  17.         msg_suffix = "result=%s" % ret
  18.     except Exception as ex:
  19.         msg_suffix = "exception=%s" % ex
  20.         raise
  21.     finally:
  22.         delta_sec = time.time() - start_sec
  23.         mloginfo("%s: sec=%0.2f, %s", msg, delta_sec, msg_suffix)

publisherUpdate 接口在 rospy 模块的 masterslave.py 文件中，猜测是使用XMLRPC协议，通知所有的订阅者节点，发布者更新了

  1. @apivalidate(-1, (is_topic('topic'), is_publishers_list('publishers')))
  2. def publisherUpdate(self, caller_id, topic, publishers):
  3.     """
  4.     Callback from master of current publisher list for specified topic.
  5.     @param caller_id: ROS caller id
  6.     @type  caller_id: str
  7.     @param topic str: topic name
  8.     @type  topic: str
  9.     @param publishers: list of current publishers for topic in the form of XMLRPC URIs
  10.     @type  publishers: [str]
  11.     @return: [code, status, ignore]
  12.     @rtype: [int, str, int]
  13.     """
  14.     if self.reg_man:
  15.         for uri in publishers:
  16.             self.reg_man.publisher_update(topic, publishers)
  17.     return 1, "", 0

2、hasParam

request报文body

  1. <?xml version='1.0'?>
  2. <methodResponse>
  3.     <params>
  4.         <param>
  5.             <value>
  6.                 <array>
  7.                     <data>
  8.                         <value>
  9.                             <int>1</int>
  10.                         </value>
  11.                         <value>
  12.                             <string>/use_sim_time</string>
  13.                         </value>
  14.                         <value>
  15.                             <boolean>0</boolean>
  16.                         </value>
  17.                     </data>
  18.                 </array>
  19.             </value>
  20.         </param>
  21.     </params>
  22. </methodResponse>

response报文body

  1. <?xml version="1.0"?>
  2. <methodCall>
  3.     <methodName>requestTopic</methodName>
  4.     <params>
  5.         <param>
  6.             <value>/rosout</value>
  7.         </param>
  8.         <param>
  9.             <value>/rosout</value>
  10.         </param>
  11.         <param>
  12.             <value>
  13.                 <array>
  14.                     <data>
  15.                         <value>
  16.                             <array>
  17.                                 <data>
  18.                                     <value>TCPROS</value>
  19.                                 </data>
  20.                             </array>
  21.                         </value>
  22.                     </data>
  23.                 </array>
  24.             </value>
  25.         </param>
  26.     </params>
  27. </methodCall>

先说结论：

调用 hasParam 接口，检查参数服务器是否有参数 /test_sub/use_sim_time

返回值是 [1, /use_sim_time, 0]，表示没有

再看代码：

hasParam

有了上面的分析经验，我们可以很轻松地的出结论，这是在调用 hasParam 接口

我们可以很轻松地找到这个方法的代码，在rosmaster模块的master_api.py文件中

  1. @apivalidate(False, (non_empty_str('key'),))
  2. def hasParam(self, caller_id, key):
  3.     """
  4.     Check if parameter is stored on server. 
  5.     @param caller_id str: ROS caller id
  6.     @type  caller_id: str
  7.     @param key: parameter to check
  8.     @type  key: str
  9.     @return: [code, statusMessage, hasParam]
  10.     @rtype: [int, str, bool]
  11.     """
  12.     key = resolve_name(key, caller_id)
  13.     if self.param_server.has_param(key):
  14.         return 1, key, True
  15.     else:
  16.         return 1, key, False

根据协议

  1. caller_id 传参是 /test_sub
  2. key 传参是 /use_sim_time

根据注释和代码，我们可以确认，这个就口就是在检查，参数服务器是否有参数 /test_sub/use_sim_time

resolve_name 接口接收两个参数，根据调用：

  1. name是/use_sim_time
  2. namespace_是/test_sub

所以，才能确认上面的全局参数 /test_sub/use_sim_time

hasParam 接口的返回值有三个

  1. code，整型，这里无论有没有，都返回1，可以忽略
  2. key，这里就是 /use_sim_time
  3. hasParam，表示是否有这个参数，True/False

根据 response 报文，这里应该返回 [1, /use_sim_time, 0]，表示没有这个参数

use_sim_time

想要理解为什么要调用这个接口，就要理解 use_sim_time 参数的作用

use_sim_time是一个重要的参数，它默认值为false，可以配合Rosbag使用，是一个很重要的离线调试工具

我们都知道，ROS 中的时间有两种：

  1. ROS::Time()
  2. ROS::WallTime()

ROS::Time()和ROS::WallTime()

表示ROS网络中的时间，如果当时在非仿真环境里运行，那它就是当前的时间。但是假设去回放当时的情况，那就需要把当时的时间录下来

以控制为例，很多的数据处理需要知道当时某一个时刻发生了什么。Wall Time可以理解为墙上时间，墙上挂着的时间没有人改变的了，永远在往前走；ROS Time可以被人为修改，你可以暂停它，可以加速，可以减速，但是Wall Time不可以。

在开启一个Node之前，当把use_sim_time设置为true时，这个节点会从clock Topic获得时间。所以操作这个clock的发布者，可以实现一个让Node中得到ROS Time暂停、加速、减速的效果。同时下面这些方面都是跟Node透明的，所以非常适合离线的调试方式。当把ROSbag记下来以后重新play出来时，加两个横杠，--clock，它就会发布出这个消息

3、registerSubscriber

先看报文：

request报文body

  1. <?xml version="1.0"?>
  2. <methodCall>
  3.     <methodName>registerSubscriber</methodName>
  4.     <params>
  5.         <param>
  6.             <value>/test_sub</value>
  7.         </param>
  8.         <param>
  9.             <value>/ros_message</value>
  10.         </param>
  11.         <param>
  12.             <value>my_package/MessageDefine</value>
  13.         </param>
  14.         <param>
  15.             <value>http://192.168.1.150:43597</value>
  16.         </param>
  17.     </params>
  18. </methodCall>

response报文body

  1. <?xml version='1.0'?>
  2. <methodResponse>
  3.     <params>
  4.         <param>
  5.             <value>
  6.                 <array>
  7.                     <data>
  8.                         <value>
  9.                             <int>1</int>
  10.                         </value>
  11.                         <value>
  12.                             <string>Subscribed to [/ros_message]</string>
  13.                         </value>
  14.                         <value>
  15.                             <array>
  16.                                 <data>
  17.                                     <value>
  18.                                         <string>http://sherlock:41689/</string>
  19.                                     </value>
  20.                                 </data>
  21.                             </array>
  22.                         </value>
  23.                     </data>
  24.                 </array>
  25.             </value>
  26.         </param>
  27.     </params>
  28. </methodResponse>

registerSubscriber

registerSubscriber 代码在rosmaster包中的master_api.py文件中，如下：

  1. @apivalidate([], ( is_topic('topic'), valid_type_name('topic_type'), is_api('caller_api')))
  2. def registerSubscriber(self, caller_id, topic, topic_type, caller_api):
  3.     """
  4.     Subscribe the caller to the specified topic. In addition to receiving
  5.     a list of current publishers, the subscriber will also receive notifications
  6.     of new publishers via the publisherUpdate API.        
  7.     @param caller_id: ROS caller id
  8.     @type  caller_id: str
  9.     @param topic str: Fully-qualified name of topic to subscribe to. 
  10.     @param topic_type: Datatype for topic. Must be a package-resource name, i.e. the .msg name.
  11.     @type  topic_type: str
  12.     @param caller_api: XML-RPC URI of caller node for new publisher notifications
  13.     @type  caller_api: str
  14.     @return: (code, message, publishers). Publishers is a list of XMLRPC API URIs
  15.        for nodes currently publishing the specified topic.
  16.     @rtype: (int, str, [str])
  17.     """
  18.     #NOTE: subscribers do not get to set topic type
  19.     try:
  20.         self.ps_lock.acquire()
  21.         self.reg_manager.register_subscriber(topic, caller_id, caller_api)
  23.         # ROS 1.1: subscriber can now set type if it is not already set
  24.         #  - don't let '*' type squash valid typing
  25.         if not topic in self.topics_types and topic_type != rosgraph.names.ANYTYPE:
  26.             self.topics_types[topic] = topic_type
  28.         mloginfo("+SUB [%s] %s %s",topic, caller_id, caller_api)
  29.         pub_uris = self.publishers.get_apis(topic)
  30.     finally:
  31.         self.ps_lock.release()
  32.     return 1, "Subscribed to [%s]"%topic, pub_uris

根据协议往来，我们可以看到调用过程

  1. registerSubscriber("/test_sub", "/ros_message", "my_package/MessageDefine", "http://192.168.1.150:43597")

入参有4个：

  1. 订阅节点名：/test_sub
  2. 需要订阅的topic 名称：/ros_message
  3. topic的数据类型：my_package/MessageDefine
  4. 订阅节点自己的uri，即发布者通知时的发送目标

返回值有3个：

  1. code，这里固定是1

  2. message，这里是 Subscribed to [/ros_message]

  3. publisher 的订阅URI 列表，因为这里只有一个publisher，所以只有一个 http://sherlock:41689/，首先，这可能是主机里面某个几点的uri，需要从机去订阅

代码说明：

和第一条，注册publisher相反，这里是注册subscriber

有几个关键代码

register_subscriber 代码，位于rosmaster包中的registerations.py文件中：

  1. def register_subscriber(self, topic, caller_id, caller_api):
  2.     """
  3.     Register topic subscriber
  4.     @return: None
  5.     """
  6.     self._register(self.subscribers, topic, caller_id, caller_api)

_register 代码，调用 _register_node_api 接口更新节点信息，如果之前有该节点的注册信息，则先删除：

  1. def _register(self, r, key, caller_id, caller_api, service_api=None):
  2.     # update node information
  3.     node_ref, changed = self._register_node_api(caller_id, caller_api)
  4.     node_ref.add(r.type, key)
  5.     # update pub/sub/service indicies
  6.     if changed:
  7.         self.publishers.unregister_all(caller_id)
  8.         self.subscribers.unregister_all(caller_id)
  9.         self.services.unregister_all(caller_id)
  10.         self.param_subscribers.unregister_all(caller_id)
  11.     r.register(key, caller_id, caller_api, service_api)

_register_node_api 代码：

  1. def _register_node_api(self, caller_id, caller_api):
  2.     """
  3.     @param caller_id: caller_id of provider
  4.     @type  caller_id: str
  5.     @param caller_api: caller_api of provider
  6.     @type  caller_api: str
  7.     @return: (registration_information, changed_registration). changed_registration is true if 
  8.     caller_api is differet than the one registered with caller_id
  9.     @rtype: (NodeRef, bool)
  10.     """
  11.     node_ref = self.nodes.get(caller_id, None)
  13.     bumped_api = None
  14.     if node_ref is not None:
  15.         if node_ref.api == caller_api:
  16.             return node_ref, False
  17.         else:
  18.             bumped_api = node_ref.api
  19.             self.thread_pool.queue_task(bumped_api, shutdown_node_task,
  20.                                         (bumped_api, caller_id, "new node registered with same name"))
  22.     node_ref = NodeRef(caller_id, caller_api)
  23.     self.nodes[caller_id] = node_ref
  24.     return (node_ref, bumped_api != None)

shutdown_node_task 代码，如果订阅节点退出了，则需要通知：

  1. def shutdown_node_task(api, caller_id, reason):
  2.     """
  3.     Method to shutdown another ROS node. Generally invoked within a
  4.     separate thread as this is used to cleanup hung nodes.
  5.     
  6.     @param api: XML-RPC API of node to shutdown
  7.     @type  api: str
  8.     @param caller_id: name of node being shutdown
  9.     @type  caller_id: str
  10.     @param reason: human-readable reason why node is being shutdown
  11.     @type  reason: str
  12.     """
  13.     try:
  14.         xmlrpcapi(api).shutdown('/master', "[{}] Reason: {}".format(caller_id, reason))
  15.     except:
  16.         pass #expected in many common cases
  17.     remove_server_proxy(api)

4、requestTopic

request报文body，如下，我们可以看到，xmlrpc发送的host是sherlock:41689，是上一步骤，收到的publisher的uri，如果有多个publisher，要request多次

  1. POST /RPC2 HTTP/1.1
  2. Host: sherlock:41689
  3. User-Agent: Go-http-client/1.1
  4. Content-Length: 307
  5. Content-Type: text/xml
  6. Accept-Encoding: gzip
  8. <?xml version="1.0"?>
  9. <methodCall>
  10.     <methodName>requestTopic</methodName>
  11.     <params>
  12.         <param>
  13.             <value>/test_sub</value>
  14.         </param>
  15.         <param>
  16.             <value>/ros_message</value>
  17.         </param>
  18.         <param>
  19.             <value>
  20.                 <array>
  21.                     <data>
  22.                         <value>
  23.                             <array>
  24.                                 <data>
  25.                                     <value>TCPROS</value>
  26.                                 </data>
  27.                             </array>
  28.                         </value>
  29.                     </data>
  30.                 </array>
  31.             </value>
  32.         </param>
  33.     </params>
  34. </methodCall>

response报文

  1. <?xml version="1.0"?>
  2. <methodResponse>
  3.     <params>
  4.         <param>
  5.             <value>
  6.                 <array>
  7.                     <data>
  8.                         <value>
  9.                             <i4>1</i4>
  10.                         </value>
  11.                         <value></value>
  12.                         <value>
  13.                             <array>
  14.                                 <data>
  15.                                     <value>TCPROS</value>
  16.                                     <value>sherlock</value>
  17.                                     <value>
  18.                                         <i4>33173</i4>
  19.                                     </value>
  20.                                 </data>
  21.                             </array>
  22.                         </value>
  23.                     </data>
  24.                 </array>
  25.             </value>
  26.         </param>
  27.     </params>
  28. </methodResponse>

根据协议，调用过程如下：

  1. requestTopic("/test_sub", "/ros_message", ["TCPROS"])

告诉publisher，subscriber准备好了，可以发数据了

requestTopic

  1. _remap_table['requestTopic'] = [0] # remap topic 
  2. @apivalidate([], (is_topic('topic'), non_empty('protocols')))
  3. def requestTopic(self, caller_id, topic, protocols):
  4.     """
  5.     Publisher node API method called by a subscriber node.
  7.     Request that source allocate a channel for communication. Subscriber provides
  8.     a list of desired protocols for communication. Publisher returns the
  9.     selected protocol along with any additional params required for
  10.     establishing connection. For example, for a TCP/IP-based connection,
  11.     the source node may return a port number of TCP/IP server. 
  12.     @param caller_id str: ROS caller id    
  13.     @type  caller_id: str
  14.     @param topic: topic name
  15.     @type  topic: str
  16.     @param protocols: list of desired
  17.      protocols for communication in order of preference. Each
  18.      protocol is a list of the form [ProtocolName,
  19.      ProtocolParam1, ProtocolParam2...N]
  20.     @type  protocols: [[str, XmlRpcLegalValue*]]
  21.     @return: [code, msg, protocolParams]. protocolParams may be an
  22.     empty list if there are no compatible protocols.
  23.     @rtype: [int, str, [str, XmlRpcLegalValue*]]
  24.     """
  25.     if not get_topic_manager().has_publication(topic):
  26.         return -1, "Not a publisher of [%s]"%topic, []
  27.     for protocol in protocols: #simple for now: select first implementation 
  28.         protocol_id = protocol[0]
  29.         for h in self.protocol_handlers:
  30.             if h.supports(protocol_id):
  31.                 _logger.debug("requestTopic[%s]: choosing protocol %s", topic, protocol_id)
  32.                 return h.init_publisher(topic, protocol)
  33.     return 0, "no supported protocol implementations", []

init_publisher 代码

  1. def init_publisher(self, resolved_name, protocol):
  2.     """
  3.     Initialize this node to receive an inbound TCP connection,
  4.     i.e. startup a TCP server if one is not already running.
  5.     
  6.     @param resolved_name: topic name
  7.     @type  resolved__name: str
  8.     
  9.     @param protocol: negotiated protocol
  10.       parameters. protocol[0] must be the string 'TCPROS'
  11.     @type  protocol: [str, value*]
  12.     @return: (code, msg, [TCPROS, addr, port])
  13.     @rtype: (int, str, list)
  14.     """
  15.     if protocol[0] != TCPROS:
  16.         return 0, "Internal error: protocol does not match TCPROS: %s"%protocol, []
  17.     start_tcpros_server()
  18.     addr, port = get_tcpros_server_address()
  19.     return 1, "ready on %s:%s"%(addr, port), [TCPROS, addr, port]

publisher 检查，是否支持指定协议，如果不支持，则返回1，否则返回0

返回值的第二个参数有三个值，分别是协议类型、ip地址 和 端口

5、unregisterSubscriber

request报文body

  1. <?xml version="1.0"?>
  2. <methodCall>
  3.     <methodName>unregisterSubscriber</methodName>
  4.     <params>
  5.         <param>
  6.             <value>/test_sub</value>
  7.         </param>
  8.         <param>
  9.             <value>/ros_message</value>
  10.         </param>
  11.         <param>
  12.             <value>http://192.168.1.150:43597</value>
  13.         </param>
  14.     </params>
  15. </methodCall>

response报文body

  1. <?xml version='1.0'?>
  2. <methodResponse>
  3.     <params>
  4.         <param>
  5.             <value>
  6.                 <array>
  7.                     <data>
  8.                         <value>
  9.                             <int>1</int>
  10.                         </value>
  11.                         <value>
  12.                             <string>Unregistered [/test_sub] as provider of [/ros_message]</string>
  13.                         </value>
  14.                         <value>
  15.                             <int>1</int>
  16.                         </value>
  17.                     </data>
  18.                 </array>
  19.             </value>
  20.         </param>
  21.     </params>
  22. </methodResponse>

unregisterSubscriber

  1. @apivalidate(0, (is_topic('topic'), is_api('caller_api')))
  2. def unregisterSubscriber(self, caller_id, topic, caller_api):
  3.     """
  4.     Unregister the caller as a subscriber of the topic.
  5.     @param caller_id: ROS caller id
  6.     @type  caller_id: str
  7.     @param topic: Fully-qualified name of topic to unregister.
  8.     @type  topic: str
  9.     @param caller_api: API URI of service to unregister. Unregistration will only occur if current
  10.        registration matches.    
  11.     @type  caller_api: str
  12.     @return: (code, statusMessage, numUnsubscribed). 
  13.       If numUnsubscribed is zero it means that the caller was not registered as a subscriber.
  14.       The call still succeeds as the intended final state is reached.
  15.     @rtype: (int, str, int)
  16.     """
  17.     try:
  18.         self.ps_lock.acquire()
  19.         retval = self.reg_manager.unregister_subscriber(topic, caller_id, caller_api)
  20.         mloginfo("-SUB [%s] %s %s",topic, caller_id, caller_api)
  21.         return retval
  22.     finally:
  23.         self.ps_lock.release()

取消订阅，固定返回1

6、TCP数据私有协议

首先保证主从机的数据类型一致，包括字段的顺序，实际ROS框架内是通过md5检测，保证数据类型一致的

数据传输前提：

  1. 数据类型一致
  2. 字段名一致
  3. 字段顺序一致

数据传输模式：小端hex

数据包结构

数据域长度 数据域
4 byte n byte

数据域长度固定4byte，长度不包括自身

数据域

数据域根据字段类型解析，ros 通信的内置数据类型有：

原始类型 字节数
bool 1
int8 1
uint8 1
int16 2
uint16 2
int32 4
uint32 4
int64 8
uint64 8
float32 4
float64 8
string n(n > 4)
time 8
duration 8
数组 n(n > 4)

其中，除 string、time、duration 和 数组 类型外的其余类型，直接根据字节数读取即可

string

字符串类型，也可认为是字符数组（则可以和数组类型复用），因为是不定长度，所以需要知道字符串的长度，ROS中使用uint32类型表示长度/数组元素数量，即4byte

所以，如果出现字符串类型，则数据域为：

字符串长度 字符
4 byte n byte

数组

数组类型，因为是不定长度，所以需要知道数组的元素数量，和string同理，ROS 中使用uint32类型表示数组的元素数量，再结合数组元素的类型，即可得到总长度

所以，出现数组类型，则数据域为：

数组元素数量 数组数据
4 byte n byte

如果数组类型是 int32，则数组数据占 4 * n byte，其余类型以此类推

time

ROS 中把 time 单独提取作为基本数据类型，对应 ROS 中的 ros::Time 类，因为我们可以认为是嵌套类型

ros::Time 有两个字段：

  1. sec： uint32
  2. nsec： uint32

所以，time 类型在数据域占8byte，如果出现 time 类型，则数据域为：

sec nsec
4 byte 4 byte

duration

duration 类型和 time 相同，在 ROS 中对应 ros::Duration 类，可以认为是嵌套类型

ros::Duration 有两个字段：

  1. sec： uint32
  2. nsec： uint32

所以，duration 类型在数据域中占8byte，如果出现 duration 类型，则数据域为：

sec nsec
4 byte 4 byte

嵌套类型

嵌套类型可以认为是数据域的组合，如果发现字段类型不是内置数据类型，则可认为是嵌套类型，嵌套类型按照类型的字段，递归处理即可

协议分析示例

示例1：

.msg 文件为：

  1. int8 shutdown_time
  2. string text

主机发出数据为：

  1. shutdown_time = 123
  2. text = abc

从机收到数据为：

  1. 08 00 00 00 7b 03 00 00 00 61 62 63

分析如下：

  1. 包头4 byte表示数据与长度

    08 00 00 00，表示数据域长度为8，即后续数据总长度为8

  2. 字段1为shutdown_time，类型是int8，1byte

    7b转10进制，为123

  3. 字段2为text，类型是字符串 (4+n)byte

    4byte 长度：03 00 00 00，表示字符串长度为3，后面3byte 为字符串内容：61 62 63，ASCII转换为：abc）

示例2：

.msg 文件为：

  1. Header header
  2. int8 shutdown_time
  3. int32 shutdown_time2
  4. string text
  5. float32 num
  6. string text2
  7. int8[] data
  8. int16[] data2

Header的数据类型为：

  1. uint32 seq
  2. time stamp
  3. string frame_id

主机发出数据为：

  1. //header一般由ROS系统自己处理，这里写出来是为了方便观察
  2. header.seq = 29;
  3. header.time.sec = 0;
  4. header.time.nsec = 0;
  5. header.frame_id = "";
  6. shutdown_time = 123;
  7. shutdown_time2 = 987654;
  8. text2 = "lmn";
  9. text = "abc";
  10. num = 23.4;
  11. data = [1, 2, 4, 89];
  12. data2 = [11, 22, 908]

从机收到的数据为：

  1. 39 00 00 00 1d 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 7b 06 12 0f 00 03 00 00 00 61 62 63 33 33 bb 41 03 00 00 00 6c 6d 6e 04 00 00 00 01 02 04 59 03 00 00 00 0b 00 16 00 8c 03

分析如下：

  1. 包头4byte表示数据域长度

    0x39 00 00 00，10进制57，表明后续数据域长度57byte

  2. 字段1，Header 类型，可以认为是嵌套类型，Header字段如下：

    1. 字段1，seq，uint32，4byte，数据为，0x1d 00 00 00，十进制29；
    2. 字段2，time类型，可以认为是嵌套类型，字段如下：
      1. 字段1，sec，uint32，4byte，数据为：0x00 00 00 00，十进制0；
      2. 字段2，nsec，uint32，4byte，数据为：0x00 00 00 00，十进制0；
    3. 字段3，frame_id，字符串类型，4byte 表示长度，00 00 00 00，表示长度为0，字符串为空

  3. 字段2，shutdown_time，int8，1byte，数据为：0x7b，十进制123；

  4. 字段3，shutdown_time2，int32，4byte，数据为：0x06 12 0f 00，十进制：987654；

  5. 字段4，text，字符串：

    1. 4byte 长度，数据为：0x03 00 00 00 ，表示字符产长度为3；
    2. 字符串内容，数据为：0x61 62 63 ，ASCII对应：abc；

  6. 字段5，num，flota32，4byte，数据为：33 33 bb 41，十进制：23.4；

  7. 字段6：text2，字符串：

    1. 4byte长度，数据为：0x03 00 00 00，表示字符串长度为3；
    2. 字符产内容，数据为：0x6c 6d 6e，ASCII对应lmn；

  8. 字段7，data，int8数组：

    1. 4byte表示数组元素数量，数据为：0x04 00 00 00，表示有4个int8元素：
    2. 数组内容：[0x01, 0x02, 0x04, 0x59,]，表示：[1,2,4,89]；

  9. 字段8，data2，int16数组：

    1. 4byte表示长度，数据为：0x03 00 00 00，表示有3个int16数据；
    2. 数组内容：[0x0b00, 0x1600, 0x8c03]，表示：[11, 22, 908]

7、小结

宗上，整体的从机订阅时序图如下：

image-20221119152442437

原文链接：https://www.cnblogs.com/sherlock-lin/p/16906333.html

 友情链接：直通硅谷 　点职佳 　北美留学生论坛

C++热门文章

  • C/C++编程笔记：编写不会在C ++中编译的C程序，我教你呀！
  • 洛谷 P2085 最小函数值
  • 使用C++界面框架ImGUI开发一个简单程序
  • C++ Json解析CJsonObject的详细使用
  • C++编译错误杂记
  • 二分法（二）：采用二分法解决“最小化最大值问题”
  • C++实现json字符串与map的转换
  • C++借助curses库实现俄罗斯方块
  • 长乐培训Day5
  • 【OpenFOAM】——OpenFOAM入门算例学习

    • C++推荐文章

  • #8. 「模板」树链剖分
  • 预设型 DP
  • Standard Template Libary or C++ Standard Library
  • 一种优秀的虚拟机内存架构 - AQ
  • 如何让 MGR 不从 Primary 节点克隆数据？
  • TLScanary：Pwn中的利器
  • c++临时对象导致的生命周期问题
  • C++(Qt)-GIS开发-简易瓦片地图下载器
  • 逆向通达信 x 逆向微信 x 逆向Qt
  • C++面向对象

    • 本站QQ群：前端 618073944 | Java 606181507 | Python 626812652 | C/C++ 612253063 | 微信 634508462 | 苹果 692586424 | C#/.net 182808419 | PHP 305140648 | 运维 608723728

    W3xue 的所有内容仅供测试，对任何法律问题及风险不承担任何责任。通过使用本站内容随之而来的风险与本站无关。
    关于我们  |  意见建议  |  捐助我们  |  报错有奖  |  广告合作、友情链接（目前9元/月）请联系QQ：27243702 沸活量
    皖ICP备17017327号-2　皖公网安备34020702000426号