ros通信与回调函数多线程应用

一、ros topic通信流程

参考资料:
一个ros节点至少有几个线程(1058): https://zhuanlan.zhihu.com/p/680188065
鱼香rostopic底层流程(1005~1008): https://zhuanlan.zhihu.com/p/656716718
王方浩-ROS发布订阅实现(二): https://zhuanlan.zhihu.com/p/439208597

基础的topic通信流程涉及到三个线程:
①发布者节点调用publish()函数
②pollset线程中处理基于socket的tcp通信和基于文件标识符的事件处理
③订阅者节点spin()中处理回调函数队列
publish() 函数只是把要发送的消息放到了消息队列中,实际上并没有真的执行tcp通信,这跟普通的基于socket的通信是类似的。这个消息队列的大小由定义发布者时的参数定义,这个队列建立的意义实际上是为了tcp通信建立的,tcp通信需要时间(通信时间和等待系统调度的时间),与订阅者的回调函数队列是不同的。除了将数据放入队列,publish还触发了一个信号poll_signal_,这个信号在pollset中被轮讯监控,一旦出发就调用槽函数processPublishQueues.
pollset线程中的槽函数processPublishQueues,就会根据subscrib_link列表依次实现tcp通讯,subscrib_link是一个所有不同tcp连接的线程池。这个列表是节点在调用advertise时注册进去的。topic通信是异步的,所以实质上一个topic如果有两个发布者和三个订阅者,就要建立六条独立的tcp连接。pollset线程除了处理发布的线程,也把接受的消息放进了订阅者的队列中
订阅者正常情况下是一个单线程的程序spin()中是一个循环,不断的监控回调函数队列,有数据时就依次执行。如果用spinonce()的话,就是每次调用时把队列中的数据全部执行一次.
以上就是一个基本流程,具体细节可能不是很清楚,但基本上知道ros自己有单独的线程在处理tcp通信,而节点中的代码只是在处理队列信息就可以了

二、ros 多线程通信

参考资料:
动手学ROS（7）：精讲多线程之MultiThreadedSpinner: https://zhuanlan.zhihu.com/p/375418691

代码主要都来自上面的链接
ros的多线程主要体现在回调函数队列的处理上

1、设置两个发布者

这两个发布者都以1hz的频率往不同的topic发送消息
发布者A

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>

int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "talker_A");
    ros::NodeHandle n;

    // 创建两个发布者对象，分别发布到"A/message"和"B/message"主题
    ros::Publisher pub_a = n.advertise<std_msgs::String>("A/message", 100);

    // 设置发布频率为1kHz
    ros::Rate loop_rate(1);

    int count = 0;
    while (ros::ok())
    {
        // 创建消息对象
        std_msgs::String msg_a;

        // 填充消息内容
        std::stringstream ss_a;
        ss_a << "Hello from A " << count;
        msg_a.data = ss_a.str();


        // 打印发布的信息
        ROS_INFO("%s", msg_a.data.c_str());

        // 发布消息
        pub_a.publish(msg_a);
        // 增加计数器
        count++;

        // 休眠一段时间以达到设定的发布频率
        loop_rate.sleep();
    }

    return 0;
}

发布者B

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>

int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "talker_B");
    ros::NodeHandle n;

    // 创建两个发布者对象，分别发布到"A/message"和"B/message"主题
    ros::Publisher pub_b = n.advertise<std_msgs::String>("B/message", 1000);

    // 设置发布频率为1kHz
    ros::Rate loop_rate(1);

    int count = 0;
    while (ros::ok())
    {
        // 创建消息对象
        std_msgs::String msg_b;

        // 填充消息内容
        std::stringstream ss_b;
        ss_b << "Hello from B " << count;
        msg_b.data = ss_b.str();

        // 发布消息
        pub_b.publish(msg_b);

        // 打印发布的信息
        ROS_INFO("%s", msg_b.data.c_str());

        // 增加计数器
        count++;

        // 休眠一段时间以达到设定的发布频率
        loop_rate.sleep();
    }

    return 0;
}

2、单spin线程

在这里插入图片描述

 #include <thread>
 #include "ros/ros.h"
 #include "std_msgs/String.h"
 void CallbackA(const std_msgs::String::ConstPtr &msg)
 {
     std::this_thread::sleep_for(std::chrono::seconds(2));
     ROS_INFO(" I heard: [%s]", msg->data.c_str());
 }
 void CallbackB(const std_msgs::String::ConstPtr &msg)
 {
     ROS_INFO(" I heard: [%s]", msg->data.c_str());
 }
 int main(int argc, char **argv)
 {
     ros::init(argc, argv, "listener");
     ros::NodeHandle n;
     ros::Subscriber sub_b = n.subscribe("B/message", 1000, CallbackB);

     ros::Subscriber sub_a = n.subscribe("A/message", 1000, CallbackA);
     ros::spin();

     return 0;
 }

(1) 测试1:先启动订阅者,然后启动两个发布者

由于是手动启动,A节点早,所以最开始有多个A
可以看到B被A托慢了,也变成两秒一次了

[ INFO] [1740131084.794161868]:  I heard: [Hello from A 1]
[ INFO] [1740131086.796284945]:  I heard: [Hello from A 2]
[ INFO] [1740131088.796593596]:  I heard: [Hello from A 3]
[ INFO] [1740131088.796690551]:  I heard: [Hello from B 1]
[ INFO] [1740131090.796933583]:  I heard: [Hello from A 4]
[ INFO] [1740131090.797038170]:  I heard: [Hello from B 2]
[ INFO] [1740131092.797252138]:  I heard: [Hello from A 5]
[ INFO] [1740131092.797336626]:  I heard: [Hello from B 3]
[ INFO] [1740131094.797485092]:  I heard: [Hello from A 6]
[ INFO] [1740131094.797575846]:  I heard: [Hello from B 4]
[ INFO] [1740131096.797819265]:  I heard: [Hello from A 7]
[ INFO] [1740131096.797901295]:  I heard: [Hello from B 5]
[ INFO] [1740131098.798104208]:  I heard: [Hello from A 8]
[ INFO] [1740131098.798182412]:  I heard: [Hello from B 6]
[ INFO] [1740131100.798384053]:  I heard: [Hello from A 9]
[ INFO] [1740131100.798458471]:  I heard: [Hello from B 7]

3、多spin线程非并发

在这里插入图片描述

 #include <thread>
 #include "ros/ros.h"
 #include "std_msgs/String.h"
 void CallbackA(const std_msgs::String::ConstPtr &msg)
 {
     std::this_thread::sleep_for(std::chrono::seconds(2));
     ROS_INFO(" I heard: [%s]", msg->data.c_str());
 }
 void CallbackB(const std_msgs::String::ConstPtr &msg)
 {
     ROS_INFO(" I heard: [%s]", msg->data.c_str());
 }
 int main(int argc, char **argv)
 {
     ros::init(argc, argv, "listener");
     ros::NodeHandle n;
     ros::Subscriber sub_a = n.subscribe("A/message", 10, CallbackA);
     ros::Subscriber sub_b = n.subscribe("B/message", 10, CallbackB);

     ros::MultiThreadedSpinner spinner(2);
     spinner.spin();

     return 0;
 }

(2) 测试2:先启动订阅者,然后启动两个发布者

注意这里修改了订阅者队列的长度
可以看到B不再被托慢了,但是在持续一段时间后A会出现丢包,因为A处理的慢,所以队列被A的数据沾满了
这两个线程会区分两个函数,也就是非并发,即不会两个线程都去处理同一个类型的回调函数,所以B一直没有丢包,就是因为一直有一个线程在处理B

[ INFO] [1740131454.639828056]:  I heard: [Hello from B 1]
[ INFO] [1740131454.653981192]:  I heard: [Hello from A 1]
[ INFO] [1740131455.639724860]:  I heard: [Hello from B 2]
[ INFO] [1740131456.639754199]:  I heard: [Hello from B 3]
[ INFO] [1740131456.654096513]:  I heard: [Hello from A 2]
[ INFO] [1740131457.639836399]:  I heard: [Hello from B 4]
[ INFO] [1740131458.639711465]:  I heard: [Hello from B 5]
[ INFO] [1740131458.654176314]:  I heard: [Hello from A 3]
[ INFO] [1740131459.639715444]:  I heard: [Hello from B 6]
[ INFO] [1740131460.639766550]:  I heard: [Hello from B 7]
[ INFO] [1740131460.654252110]:  I heard: [Hello from A 4]
[ INFO] [1740131461.639824670]:  I heard: [Hello from B 8]
[ INFO] [1740131462.639892430]:  I heard: [Hello from B 9]
[ INFO] [1740131462.654330989]:  I heard: [Hello from A 5]
[ INFO] [1740131463.640043684]:  I heard: [Hello from B 10]
[ INFO] [1740131464.639945528]:  I heard: [Hello from B 11]
[ INFO] [1740131464.654449080]:  I heard: [Hello from A 6]
[ INFO] [1740131465.640064511]:  I heard: [Hello from B 12]
[ INFO] [1740131466.639987938]:  I heard: [Hello from B 13]
[ INFO] [1740131466.654532060]:  I heard: [Hello from A 7]
[ INFO] [1740131467.639809394]:  I heard: [Hello from B 14]
[ INFO] [1740131468.639899303]:  I heard: [Hello from B 15]
[ INFO] [1740131468.654613126]:  I heard: [Hello from A 8]
[ INFO] [1740131469.639891146]:  I heard: [Hello from B 16]
[ INFO] [1740131470.639872857]:  I heard: [Hello from B 17]
[ INFO] [1740131470.654735428]:  I heard: [Hello from A 9]
[ INFO] [1740131471.639807625]:  I heard: [Hello from B 18]
[ INFO] [1740131472.639937912]:  I heard: [Hello from B 19]
[ INFO] [1740131472.654906283]:  I heard: [Hello from A 10]
[ INFO] [1740131473.639993581]:  I heard: [Hello from B 20]
[ INFO] [1740131474.639798626]:  I heard: [Hello from B 21]
[ INFO] [1740131474.655083576]:  I heard: [Hello from A 12]
[ INFO] [1740131475.639838516]:  I heard: [Hello from B 22]
[ INFO] [1740131476.639747335]:  I heard: [Hello from B 23]
[ INFO] [1740131476.655254436]:  I heard: [Hello from A 14]
[ INFO] [1740131477.639884596]:  I heard: [Hello from B 24]
[ INFO] [1740131478.639752969]:  I heard: [Hello from B 25]
[ INFO] [1740131478.655428197]:  I heard: [Hello from A 16]
[ INFO] [1740131479.639717125]:  I heard: [Hello from B 26]
[ INFO] [1740131480.639765754]:  I heard: [Hello from B 27]
[ INFO] [1740131480.655604308]:  I heard: [Hello from A 18]
[ INFO] [1740131481.639775598]:  I heard: [Hello from B 28]
[ INFO] [1740131482.639767451]:  I heard: [Hello from B 29]
[ INFO] [1740131482.655720449]:  I heard: [Hello from A 20]
[ INFO] [1740131483.639719414]:  I heard: [Hello from B 30]
[ INFO] [1740131484.639791912]:  I heard: [Hello from B 31]
[ INFO] [1740131484.655802413]:  I heard: [Hello from A 22]
[ INFO] [1740131485.639867103]:  I heard: [Hello from B 32]
[ INFO] [1740131486.639875775]:  I heard: [Hello from B 33]
[ INFO] [1740131486.655971307]:  I heard: [Hello from A 24]
[ INFO] [1740131487.639853388]:  I heard: [Hello from B 34]
[ INFO] [1740131488.639927926]:  I heard: [Hello from B 35]
[ INFO] [1740131488.656069911]:  I heard: [Hello from A 26]
[ INFO] [1740131489.639944256]:  I heard: [Hello from B 36]
[ INFO] [1740131490.639953991]:  I heard: [Hello from B 37]
[ INFO] [1740131490.656157923]:  I heard: [Hello from A 28]
[ INFO] [1740131491.639920782]:  I heard: [Hello from B 38]
[ INFO] [1740131492.639786618]:  I heard: [Hello from B 39]
[ INFO] [1740131492.656243807]:  I heard: [Hello from A 30]
[ INFO] [1740131493.639929171]:  I heard: [Hello from B 40]
[ INFO] [1740131494.639995655]:  I heard: [Hello from B 41]
[ INFO] [1740131494.656327222]:  I heard: [Hello from A 32]
[ INFO] [1740131495.639892878]:  I heard: [Hello from B 42]
[ INFO] [1740131496.639876764]:  I heard: [Hello from B 43]
[ INFO] [1740131496.656411572]:  I heard: [Hello from A 34]
[ INFO] [1740131497.639974678]:  I heard: [Hello from B 44]
[ INFO] [1740131498.639951815]:  I heard: [Hello from B 45]
[ INFO] [1740131498.656500217]:  I heard: [Hello from A 36]
[ INFO] [1740131499.639837483]:  I heard: [Hello from B 46]

4、多spin线程并发

在这里插入图片描述

 #include <thread>
 #include "ros/ros.h"
 #include "std_msgs/String.h"

 void ChatterCallback(const std_msgs::String::ConstPtr& msg) {
   ROS_INFO(" I heard: [%s]", msg->data.c_str());
   std::this_thread::sleep_for(std::chrono::seconds(2));
 }
 int main(int argc, char **argv) {
   ros::init(argc, argv, "listener");
   
   ros::NodeHandle n;
   ros::SubscribeOptions ops;
   ops.init<std_msgs::String>("A/message", 1, ChatterCallback);
   ops.allow_concurrent_callbacks = true;
   ros::Subscriber sub1 = n.subscribe(ops);
   ros::MultiThreadedSpinner spinner(2);
   
   spinner.spin();
   return 0;
 }

(3) 测试3:先启动订阅者,然后启动A发布者

注意这里修改了订阅者队列的长度
A不再丢包,因为两个线程可以同时处理同一个类型的回调,注意这里只是同一个类型,不是同一个
注意这里由于涉及到两个线程操作同一个类型的回调,如果回调里有共享资源,需要有锁
上面非并发的实际上如果有共享资源的话也需要

[ INFO] [1740131990.803107129]:  I heard: [Hello from A 1]
[ INFO] [1740131991.802900084]:  I heard: [Hello from A 2]
[ INFO] [1740131992.804808048]:  I heard: [Hello from A 3]
[ INFO] [1740131993.803073730]:  I heard: [Hello from A 4]
[ INFO] [1740131994.804944148]:  I heard: [Hello from A 5]
[ INFO] [1740131995.803246682]:  I heard: [Hello from A 6]
[ INFO] [1740131996.805189157]:  I heard: [Hello from A 7]
[ INFO] [1740131997.803411020]:  I heard: [Hello from A 8]
[ INFO] [1740131998.805365004]:  I heard: [Hello from A 9]
[ INFO] [1740131999.803570627]:  I heard: [Hello from A 10]
[ INFO] [1740132000.805542830]:  I heard: [Hello from A 11]
[ INFO] [1740132001.803732381]:  I heard: [Hello from A 12]
[ INFO] [1740132002.805717474]:  I heard: [Hello from A 13]
[ INFO] [1740132003.803927256]:  I heard: [Hello from A 14]
[ INFO] [1740132004.805870949]:  I heard: [Hello from A 15]
[ INFO] [1740132005.804098520]:  I heard: [Hello from A 16]
[ INFO] [1740132006.806118693]:  I heard: [Hello from A 17]
[ INFO] [1740132007.804269056]:  I heard: [Hello from A 18]
[ INFO] [1740132008.806373739]:  I heard: [Hello from A 19]
[ INFO] [1740132009.804453823]:  I heard: [Hello from A 20]
[ INFO] [1740132010.806567750]:  I heard: [Hello from A 21]
[ INFO] [1740132011.804627770]:  I heard: [Hello from A 22]
[ INFO] [1740132012.806739975]:  I heard: [Hello from A 23]
[ INFO] [1740132013.804808557]:  I heard: [Hello from A 24]

4、多回调函数队列

在这里插入图片描述

 #include <thread>
 #include <ros/callback_queue.h>
 #include "ros/ros.h"
 #include "std_msgs/String.h"
 void CallbackA(const std_msgs::String::ConstPtr& msg) {
   ROS_INFO(" I heard: [%s]", msg->data.c_str());
   std::this_thread::sleep_for(std::chrono::seconds(2));
 }
 void CallbackB(const std_msgs::String::ConstPtr& msg) {
   ROS_INFO(" I heard: [%s]", msg->data.c_str());
 }
 int main(int argc, char **argv) {
   ros::init(argc, argv, "listener");
   ros::NodeHandle n;
   ros::Subscriber sub_b = n.subscribe("B/message", 10, CallbackB);
   
   ros::NodeHandle n_a;
   ros::CallbackQueue callback_queue_a;
   n_a.setCallbackQueue(&callback_queue_a);
   ros::Subscriber sub_a = n_a.subscribe("A/message", 10, CallbackA);
   std::thread spinner_thread_a([&callback_queue_a]() {
     ros::SingleThreadedSpinner spinner_a;
     spinner_a.spin(&callback_queue_a);
   });
   ros::spin();
   spinner_thread_a.join();
   return 0;
 }

(3) 测试3:先启动订阅者,然后启动A,过一段时间开始出现丢包后启动B

这实际上就是用不同的句柄在自己设置的线程里设置发布者了
A持续一段时间后,应该填满了回调队列,如果用之前那种就没办法在启动B后立即获得B了,而这种则不受影响,因为有两个独立的回调函数队列

[ INFO] [1740132598.928969904]:  I heard: [Hello from A 1]
[ INFO] [1740132600.930244784]:  I heard: [Hello from A 2]
[ INFO] [1740132602.930408245]:  I heard: [Hello from A 3]
[ INFO] [1740132604.930572266]:  I heard: [Hello from A 4]
[ INFO] [1740132606.930738016]:  I heard: [Hello from A 5]
[ INFO] [1740132608.930896064]:  I heard: [Hello from A 6]
[ INFO] [1740132610.931070513]:  I heard: [Hello from A 7]
[ INFO] [1740132612.931232193]:  I heard: [Hello from A 8]
[ INFO] [1740132614.931404774]:  I heard: [Hello from A 9]
[ INFO] [1740132616.931579713]:  I heard: [Hello from A 10]
[ INFO] [1740132618.931739429]:  I heard: [Hello from A 12]
[ INFO] [1740132620.931897491]:  I heard: [Hello from A 14]
[ INFO] [1740132622.932063793]:  I heard: [Hello from A 16]
[ INFO] [1740132624.932231221]:  I heard: [Hello from A 18]
[ INFO] [1740132626.932386668]:  I heard: [Hello from A 20]
[ INFO] [1740132626.941445056]:  I heard: [Hello from B 1]
[ INFO] [1740132627.941439882]:  I heard: [Hello from B 2]
[ INFO] [1740132628.932554571]:  I heard: [Hello from A 22]
[ INFO] [1740132628.941478266]:  I heard: [Hello from B 3]
[ INFO] [1740132629.941387904]:  I heard: [Hello from B 4]
[ INFO] [1740132630.932677572]:  I heard: [Hello from A 24]
[ INFO] [1740132630.941351494]:  I heard: [Hello from B 5]
[ INFO] [1740132631.941298778]:  I heard: [Hello from B 6]
[ INFO] [1740132632.932823637]:  I heard: [Hello from A 26]
[ INFO] [1740132632.941288302]:  I heard: [Hello from B 7]