ROS 多個傳感器 publish 同一個Topic

梳理一下概念

ROS Node 之間進行通訊所利用的最重要的機制就是消息傳遞,在 ROS 中,消息有組織的(其實就是定義 Msg 格式)放到 Topic 裏進行傳遞html

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 信息node

下圖是ROS Node和ROS Topic概念的圖形化表示,咱們能夠看到兩個Node(圓形)經過Topic(長方形)實現通訊
ROS 多個傳感器 publish 同一個Topic
python

Topic通訊的特色爲:

1. Topic通訊是多對多的異步通訊方式:

Topic Publisher調用publish()方法發佈消息,發送完當即返回,不用等待反饋;Subscriber經過回調函數的方式來處理消息。git

對於同一Topic,系統中能夠同時存在多個Publisher和多個Subscriber;github

另外,Publisher並不知道哪一個節點會接收消息,而Subscriber也並不知道接收的消息來自哪一個節點,節點之間是鬆耦合的,這也是ROS最關鍵的設計特性之一。異步

2. 對於實時性、週期性的消息,使用topic傳輸是最佳的選擇
3. Topic通訊方式充分體現了分佈式系統通訊的好處:擴展性好,軟件複用率高

Topic同時收發

相比於單純的Topic多收或多發,同時收發會複雜一些。首先,根據前面知識知道Topic接收是經過NodeHandle的成員函數subscribe()和自定義的回調函數實現的,同時回調函數有嚴格的定義規定:參數只能有一個且必須以const修飾、參數類型爲xxxConstPtr、參數爲引用傳遞、函數沒有返回值。這就意味着單純的回調函數幾乎沒法同外界作任何直接的數據交換,數據只能在它內部處理,除了保存到文件之外,其它沒有辦法輸出數據。分佈式

解決這個問題的核心就是數據或變量在不一樣函數之間的共享問題。在 C++、Python 中對於這種狀況有兩種辦法:一種是採用全局變量,二是類ide

這裏直接介紹第二種方法代碼以下:函數

在每一個 callback 裏都調用 check_wall_sonar_distance() 函數檢查 wall_sonar_distance 變量是否知足條件,知足後調用 publisher 發送數據ui

#!/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)
相關文章
相關標籤/搜索