ROS 多个传感器 publish 同一个Topic

Posted 2020-11-30

梳理一下概念

ROS Node 之间进行通信所利用的最重要的机制就是消息传递，在 ROS 中，消息有组织的（其实就是定义 Msg 格式）放到 Topic 里进行传递

Publisher

1. 生成信息，通过ROS Topic与其它Node进行通信。
2. 通常用于处理原始的传感器信息，如相机、编码器等。

Subscriber

1. 接收信息，通过ROS Topic接收来自其它Node的信息，并通过回调函数处理
2. 通常用于监测系统状态，如当机器人关节到达限位位置时触发运动中断

Topic 通信过程为：

1. Publisher 节点和 Subscriber 节点分别在 Master 进行注册
2. Publisher 发布 Topic
3. Subscriber 在 Master 指挥下订阅 Topic，从而建立起 Pub-Sub 之间的通信

   注意：消息是直接从发布节点传递到订阅节点，并不经过 Master,只是从 Master 获取到 Topic 信息

下图是ROS Node和ROS Topic概念的图形化表示，我们可以看到两个Node（圆形）通过Topic（长方形）实现通信

Topic通信的特点为：

1. Topic通信是多对多的异步通信方式:

Topic Publisher调用publish()方法发布消息，发送完立即返回，不用等待反馈；Subscriber通过回调函数的方式来处理消息。

对于同一Topic，系统中可以同时存在多个Publisher和多个Subscriber；

另外，Publisher并不知道哪个节点会接收消息，而Subscriber也并不知道接收的消息来自哪个节点，节点之间是松耦合的，这也是ROS最关键的设计特性之一。

2. 对于实时性、周期性的消息，使用topic传输是最佳的选择

3. Topic通信方式充分体现了分布式系统通信的好处：扩展性好，软件复用率高

Topic同时收发

相比于单纯的Topic多收或多发，同时收发会复杂一些。首先，根据前面知识知道Topic接收是通过NodeHandle的成员函数subscribe()和自定义的回调函数实现的，同时回调函数有严格的定义规定：参数只能有一个且必须以const修饰、参数类型为xxxConstPtr、参数为引用传递、函数没有返回值。这就意味着单纯的回调函数几乎无法同外界做任何直接的数据交换，数据只能在它内部处理，除了保存到文件以外，其它没有办法输出数据。

解决这个问题的核心就是数据或变量在不同函数之间的共享问题。在 C++、Python 中对于这种情况有两种办法：一种是采用全局变量，二是类

这里直接介绍第二种方法代码如下：

在每个 callback 里都调用 check_wall_sonar_distance() 函数检查 wall_sonar_distance 变量是否满足条件，满足后调用 publisher 发送数据

#!/usr/bin/env python
# -*- coding: UTF-8 -*-
"""
超声波墙检
"""
import rospy
from sensor_msgs.msg import Range
from ak_ros_pkg.msg import Wall_sonar_msg

class Processer:
def init(self):

实例化订阅多个 Topic

    self.sub1 = rospy.Subscriber("/sensor/sonar_left", Range, self.callback1)
    self.sub2 = rospy.Subscriber("/sensor/sonar_right", Range, self.callback2)

    self.pub1 = rospy.Publisher(‘ankobot_wall_sonar‘, Wall_sonar_msg, queue_size=10)
    # self.pub2 = rospy.Publisher(‘ankobot_wall_tof‘, Wall_tof_all_msg, queue_size=10)

    self.wall_sonar_distance = {}

def callback1(self, data):
    """
    左超声波
    """
    distance = int(data.range * 1000)

    self.wall_sonar_distance[‘l_sonar‘] = distance
    self.check_wall_sonar_distance()

def callback2(self, data):
    """
    右超声波
    """
    distance = int(data.range * 1000)

    self.wall_sonar_distance[‘r_sonar‘] = distance
    self.check_wall_sonar_distance()

def check_wall_sonar_distance(self):
    """
    检查 wall_sonar_distance 字典是否满足两个值的条件
    """
    if len(self.wall_sonar_distance) == 2:
        wall_sonar_single = Wall_sonar_msg()
        # 单位 m
        wall_sonar_single.l_sonar = self.wall_sonar_distance[‘l_sonar‘]
        wall_sonar_single.r_sonar = self.wall_sonar_distance[‘r_sonar‘]
        # 预留
        # wall_sonar_single.c_sonar = self.wall_sonar_distance[‘c_sonar‘]
        # wall_sonar_single.d_sonar = self.wall_sonar_distance[‘d_sonar‘]
        self.pub1.publish(wall_sonar_single)

if name == ‘main‘:
rospy.init_node("wall_sonar")

p = Processer()

rospy.spin()

##### 更多示例请参考 
[C++、Python 版本的 topic 多收、多发、多收多发](https://github.com/creazy412/ROS-Multi-Topic-Demo)
---

参考：<br/>
- [单节点的多Topic同时收发](http://zhaoxuhui.top/blog/2019/10/20/ros-note-7.html)
- [Topic 概念梳理](https://tr-ros-tutorial.readthedocs.io/zh_CN/latest/_source/basics/1.4_ROS_Topic.html)