引言
這是之前看到一位大佬做的集群通信中間件,突發奇想,自己也來做一個,實現更多的功能、更清楚的架構和性能更加高效的ROS多機通信的功能包
鏈接:https://blog.csdn.net/benchuspx/article/details/128576723
Multibotnet
Multibotnet 是一個專為多機器人系統設計的 ROS 包,利用 ZeroMQ 技術實現高效的分布式通信。簡單來說,它能讓多臺機器人或電腦之間輕松共享 ROS 話題和服務,哪怕它們不在同一個網絡環境下也能協作工作。無論是機器人團隊協同任務,還是跨設備的數據共享,Multibotnet 都能派上用場!
- 項目地址:github 倉庫——Multibotnet
主要功能
話題共享
- 發送話題:把本地的 ROS 話題通過網絡發出去,其他機器人就能收到。
- 接收話題:從網絡上抓取其他機器人發來的話題,融入本地 ROS 系統。
服務管理
- 提供服務:讓你的機器人通過網絡為別人提供 ROS 服務,比如遠程開關控制。
- 請求服務:調用其他機器人提供的服務,實現跨設備的功能交互。
支持的消息類型
支持常見的 ROS 消息類型,包括但不限于:
- sensor_msgs/Imu(IMU 數據)
- geometry_msgs/Twist(速度指令)
- std_msgs/String(字符串)
- nav_msgs/Odometry(里程計)
- sensor_msgs/LaserScan(激光雷達)
- sensor_msgs/Image(圖像)
- geometry_msgs/Pose(位姿)
- geometry_msgs/Point(點坐標)
- std_msgs/Float32(浮點數)
- std_msgs/Int32(整數)
- geometry_msgs/PoseStamped(帶時間戳的位姿)
- sensor_msgs/PointCloud2(點云)
- geometry_msgs/Vector3(三維向量)
- 自定義類型(稍作修改就能支持,超靈活!)
支持的服務類型
- std_srvs/SetBool(布爾開關服務)
- nav_msgs/GetPlan(路徑規劃服務)
- 自定義服務(同樣支持擴展)
項目優勢
-
簡單配置,一鍵搞定
通過一個 YAML 文件就能設置話題、服務、IP 和端口,想改啥改啥,完全不用碰代碼。 -
通信超快,效率爆表
用 ZeroMQ 技術,支持多對多通信,哪怕是大規模機器人集群也能hold住。 -
頻率可控,不怕卡頓
發送話題時可以限制頻率,避免網絡堵塞,帶寬利用率剛剛好。 -
擴展方便,隨心所欲
想加新的消息或服務類型?改幾行代碼就行,完美適配你的項目需求。 -
跨平臺無壓力
不管是機器人還是普通電腦,只要有 ROS 環境,就能跑起來,分布式系統so easy!
效果部分展示
安裝步驟
準備工作
需要先裝好以下依賴:
- ZeroMQ:網絡通信核心
- yaml-cpp:解析配置文件用
- ROS:確保你的 ROS 環境已經配置好
安裝命令
-
安裝依賴:
sudo apt-get install libzmq3-dev libyaml-cpp-dev
-
克隆項目到你的 catkin 工作空間:
cd ~/catkin_ws/src git clone https://github.com/nanwanuser/multibotnet.git
-
編譯項目:
cd ~/catkin_ws catkin_make
使用方法
-
配置一下
打開 config/default.yaml 文件,填入你想要共享的話題和服務信息,比如 IP 地址、端口號等。 -
啟動程序
一行命令搞定:roslaunch multibotnet multibotnet.launch
配置說明
config/default.yaml 是你的“控制中心”,里面有這些關鍵項:
- IP:給 IP 地址起個別名,比如 self: ‘*’ 表示本機所有 IP。
- send_topics:設置要發出去的話題(話題名、類型、頻率、地址、端口)。
- recv_topics:設置要接收的話題(話題名、類型、地址、端口)。
- provide_services:定義你要提供的服務(服務名、類型、地址、端口)。
- request_services:定義你要調用的遠程服務(服務名、類型、地址、端口)。
具體格式可以參考默認文件,照著改就行!
如何擴展自定義類型
想用自己的消息或服務類型?很簡單,按以下步驟操作:
添加自定義消息類型
-
引入頭文件
在 include/multibotnet/ros_sub_pub.hpp 中加一行:#include <your_package/YourMessage.h>
-
映射類型
在 getMsgType 函數中添加:if (type == "your_package/YourMessage") return "your_package::YourMessage";
-
發送邏輯
在 src/zmq_manager.cpp 的 sendTopic 函數中加一段:else if (message_type == "your_package/YourMessage") {sub = nh.subscribe<your_package::YourMessage>(topic, 1, [this, ¤t_socket, index, topic](const your_package::YourMessage::ConstPtr& msg) {if (send_freq_control(index)) {auto buffer = serializeMsg(*msg);zmq::message_t zmq_msg(buffer.size());memcpy(zmq_msg.data(), buffer.data(), buffer.size());if (!current_socket.send(zmq_msg, zmq::send_flags::none)) {ROS_ERROR("Failed to send message on topic %s", topic.c_str());}}}); }
-
接收邏輯
在 src/zmq_manager.cpp 的 recvTopic 函數中,首先為新消息類型創建發布者:else if (message_type == "your_package/YourMessage") {pub = nh.advertise<your_package::YourMessage>(topic, 1); }
然后,在接收線程中反序列化并發布:
else if (message_type == "your_package/YourMessage") {your_package::YourMessage msg = deserializeMsg<your_package::YourMessage>(static_cast<uint8_t*>(zmq_msg.data()), zmq_msg.size());pub.publish(msg); }
-
更新依賴
在 package.xml 中添加:<depend>your_package</depend>
-
重新編譯
catkin_make
添加自定義服務類型
-
引入頭文件
在 include/multibotnet/ros_sub_pub.hpp 中添加對你的服務類型頭文件的引用,以便編譯器識別該類型:#include <your_package/YourService.h>
-
在 ServiceManager 中支持新服務類型
在 src/service_manager.cpp 的 createHandler 函數中,為你的服務類型添加一個條件分支,創建對應的 SpecificServiceHandler:else if (service_type == "your_package/YourService") {return std::make_shared<SpecificServiceHandler<your_package::YourService>>(service_name); }
這步是核心,ServiceManager 通過 createHandler 為提供的服務創建處理程序,綁定到 REP 套接字。
SpecificServiceHandler 會自動調用本地 ROS 服務并處理序列化/反序列化。 -
更新模板實例化(可選)
如果你需要在代碼中通過 callService 調用該服務,需要在 src/service_manager.cpp 文件末尾添加模板實例化:template bool ServiceManager::callService<your_package/YourService>(const std::string&, your_package/YourService::Request&, your_package/YourService::Response&);
如果你只提供服務(provide_services),這步可以跳過;但如果涉及請求服務(request_services),則必須添加。
-
更新依賴
在 package.xml 中添加對你服務包的依賴,確保項目能找到服務定義:<depend>your_package</depend>
-
重新編譯
在工作空間根目錄下運行以下命令以應用更改:catkin_make
-
配置 YAML 文件
在 config/default.yaml 中添加你的服務配置,例如:提供服務:
provide_services: - service_name: /your_serviceservice_type: your_package/YourServicebind_address: selfport: 5560
請求服務:
request_services: - service_name: /remote_your_serviceservice_type: your_package/YourServiceconnect_address: robot1port: 5560
應用示例
場景描述
- Robot1(IP: 192.168.1.101):發送 /imu 話題,提供 /set_bool 服務。
- Robot2(IP: 192.168.1.102):接收 /imu 話題(顯示為 /imu_recv),調用 /set_bool 服務。
Robot1 配置
IP:self: '*'robot2: 192.168.1.102send_topics:
- topic: /imumessage_type: sensor_msgs/Imumax_frequency: 50bind_address: selfport: 3001provide_services:
- service_name: /set_boolservice_type: std_srvs/SetBoolbind_address: selfport: 5555
Robot2 配置
IP:self: '*'robot1: 192.168.1.101recv_topics:
- topic: /imu_recvmessage_type: sensor_msgs/Imuconnect_address: robot1port: 3001request_services:
- service_name: /set_boolservice_type: std_srvs/SetBoolconnect_address: robot1port: 5555
啟動兩臺機器上的 Multibotnet 后,Robot2 就能收到 Robot1 的 IMU 數據,并遠程控制它的開關服務。
總結
Multibotnet 是一個簡單又強大的工具,能讓多機器人系統高效協作。無論是話題共享還是服務調用,它都能通過靈活的配置和高性能通信滿足你的需求。快來試試吧,讓你的機器人團隊更聰明、更協同!