ROS C++ : 使用ros::AsyncSpinner，实现多线程处理ROS消息

牛魔王的小怪兽2024-10-05 18:09

文章目录

1、原理说明

ROS提供了2中方式支持多个线程调用回调函数：使用ros::MultiThreadedSpinner和ros::AsyncSpinner.

  • ros::MultiThreadedSpinner------相当于while(true)大循环，启动指定数量的Spinner线程并发执行Callback队列中的可用回调。可指定Callback队列。
  • ros::AsyncSpinner------异步启停，开启指定数量的Spinner线程并发执行Callback队列中的可用回调。可指定Callback队列。

1.1、ros::MultiThreadedSpinner

ros::MultiThreadSpinner是一个阻塞式的spin，类似于ros::spin()。但可以在它的构造函数中指定线程数，如果没有指定(或设置为0)，它将为每个CPU核心使用一个线程。

cpp 复制代码 
ros::MultiThreadedSpinner spinner(4); // Use 4 threads
spinner.spin(); // spin() will not return until the node has been shutdown

1.2、ros::AsyncSpinner

一个更有用的线程微调器是AsyncSpinner。它不是一个阻塞的spin()调用，而是调用start()和stop()，并且会在它被销毁时自动停止。AsyncSpinner与上面MultiThreadedSpinner示例的等效用法是:

cpp 复制代码 
ros::AsyncSpinner spinner(4); // Use 4 threads
spinner.start();
ros::waitForShutdown(); // Wait for this node to be shutdown, whether through Ctrl-C, ros::shutdown(), or similar.

请注意，ros::waitForShutdown()函数不会自己旋转，因此上面的示例总共会旋转4个线程。

1.3、多线程原理

1.3.1、 消息发布

ros::Publisher将要发布的消息加入到Publisher队列中，再由专门的Publisher线程发布出去。注意这其中并不涉及Callback队列：

1.3.2、 消息订阅

ROS默认有维护一个全局回调队列（名为：Global Callback Queue），将已可用的回调插入Callback队列中。再通过Spinner线程获取并执行当前可用的回调。

定时器启动后会生成一个Timer线程，根据定时器的参数，当定时器超时后将定时器的回调函数加入Callback队列中。然后再由用户调用的Spinner线程（ros::spin）从Callback队列中依次取出当前已可用的回调并执行。

2、ros::AsyncSpinner 示例1

下面这段代码展示如何使用ros::AsyncSpinner启用多个Spinner线程。

cpp 复制代码 
//一个topic多个线程来执行的代码
#include "ros/ros.h"
#include "boost/thread.hpp"
#include "my_msg/weather_pub.h"

//回调函数，注意参数是const类型的boost::shared_ptr指针
void weatherCallback(const my_msg::weather_pubConstPtr& msg)
{
  ROS_INFO_STREAM("Thread["<< boost::this_thread::get_id()
    <<"],after looping 24 hours weather:"<< msg->weather.c_str());
}

int main(int argc, char **argv){
  ros::init(argc, argv, "multi_subscriber");
  ros::NodeHandle n;
  /*通知ROS master，本node要订阅名为"Weather"的话题（topic），
  并指定回调函数weatherCallback*/
  ros::Subscriber sub = n.subscribe("Weather", 48, weatherCallback);

  ROS_INFO_STREAM("Thread["<< boost::this_thread::get_id()
    <<"]This is main thread.");

  //声明spinner对象，参数2表示并发线程数，默认处理全局Callback队列
  ros::AsyncSpinner spinner(2);
  //启动两个spinner线程并发执行可用回调  
  spinner.start();
  ros::waitForShutdown();
}

从执行结果中可以看到，进程中包括三个线程：主线程、Spinner线程1、Spinner线程2。

cpp 复制代码 
//这是执行结果，可以看到主线程
[ INFO] [1637131602.089381910]: Thread[7f9a1ad24780]This is main thread.
[ INFO] [1637131602.375058712]: Thread[7f9a11bb6700],after looping 24 hours weather:Sunny 679
[ INFO] [1637131602.488504089]: Thread[7f9a11bb6700],after looping 24 hours weather:Sunny 680
[ INFO] [1637131602.688845441]: Thread[7f9a123b7700],after looping 24 hours weather:Sunny 681
[ INFO] [1637131602.888828136]: Thread[7f9a123b7700],after looping 24 hours weather:Sunny 682

下图展示了相关的线程和队列处理过程 ：

3、ros::AsyncSpinner 示例2

实际项目中一个节点往往要订阅多个topic，在使用默认全局Callback队列时，如果某些topic发布频率高回调处理又耗时的话，容易影响其他topic消息的处理。下图中TopicB的消息巨多可能影响TopicA的处理。

这种情况下，ROS提供了机制，可以为每个ros::Subscriber指定Callback队列，再分别指定Spinner线程仅处理指定Callback队列的回调。这样确保每个订阅回调相互独立不影响。下面的代码展示如何进行上述操作：

cpp 复制代码 
//为每个subscriber指定队列
#include "ros/ros.h"
#include "boost/thread.hpp"
#include "my_msg/weather_pub.h"
#include "my_msg/air_quality_pub.h"
#include <ros/callback_queue.h>

//回调函数，注意参数是const类型的boost::shared_ptr指针
void weatherCallback(const my_msg::weather_pubConstPtr& msg)
{
  ROS_INFO_STREAM("Thread["<< boost::this_thread::get_id()
    <<"],before loop 24 hours weather:"<< msg->weather.c_str()); 
  //死循环
  while(true){}

  ROS_INFO_STREAM("Thread["<< boost::this_thread::get_id()
    <<"],24 hours weather:"<< msg->weather.c_str()); 
}

void weatherCallback_A(const my_msg::weather_pubConstPtr& msg)
{
  ROS_INFO_STREAM("Thread["<< boost::this_thread::get_id()
    <<"],A 24 hours weather:"<< msg->weather.c_str()); 
}

//回调函数，注意参数是const类型的boost::shared_ptr指针
void airQualityCallback(const my_msg::air_quality_pubConstPtr& msg)
{
  ROS_INFO_STREAM("Thread["<< boost::this_thread::get_id()
    <<"],24 hours air quality:"<< msg->air_quality_index); 
}

int main(int argc, char **argv){
  ros::init(argc, argv, "multi_subscriber");
  ros::NodeHandle n;
  /*通知ROS master，本node要订阅名为"Weather"的话题（topic），
  并指定回调函数weatherCallback*/
  ros::Subscriber sub = n.subscribe("Weather", 48, weatherCallback);
  ros::Subscriber sub_a = n.subscribe("WeatherA", 48, weatherCallback_A);

  //需要单独声明一个ros::NodeHandle
  ros::NodeHandle n_1;
  //为这个ros::Nodehandle指定单独的Callback队列
  ros::CallbackQueue my_queue;
  n_1.setCallbackQueue(&my_queue);
  /*通知ROS master，本node要订阅名为"AirQuality"的话题（topic），
  并指定回调函数airQualityCallback*/
  ros::Subscriber air_sub = n_1.subscribe("AirQuality", 48, airQualityCallback);
  
  ROS_INFO_STREAM("Thread["<< boost::this_thread::get_id()<<"]This is main thread.");
  
  //启动两个线程处理全局Callback队列 
  ros::AsyncSpinner spinner(2);
  spinner.start();

  //启动一个线程处理AirQuality单独的队列
  ros::AsyncSpinner spinner_1(1, &my_queue);
  spinner_1.start();
  
  ros::waitForShutdown(); 
}

从执行结果中可以看到，进程中包括四个线程：主线程、全局队列Spinner线程1、全局队列Spinner线程2，以及本地队列Spinner线程3。尽管Spinner线程1被回调函数中的死循环卡住，但并不影响其他topic的回调处理。

bash 复制代码 
[ INFO] [1637132247.535142399]: Thread[7f73e4384780]This is main thread.
[ INFO] [1637132247.743935399]: Thread[7f73d77fe700],A 24 hours weather:Sunny 3926
[ INFO] [1637132247.744032493]: Thread[7f73d6ffd700],before loop 24 hours weather:Sunny 3906
[ INFO] [1637132247.744203496]: Thread[7f73d67fc700],24 hours air quality:4034
[ INFO] [1637132247.888403207]: Thread[7f73d77fe700],A 24 hours weather:Sunny 3927
[ INFO] [1637132247.888433359]: Thread[7f73d67fc700],24 hours air quality:4035
[ INFO] [1637132248.088418911]: Thread[7f73d67fc700],24 hours air quality:4036
[ INFO] [1637132248.088461907]: Thread[7f73d77fe700],A 24 hours weather:Sunny 3928
[ INFO] [1637132248.288417795]: Thread[7f73d67fc700],24 hours air quality:4037
[ INFO] [1637132248.288448289]: Thread[7f73d77fe700],A 24 hours weather:Sunny 3929

下图展示了相关的线程和队列处理过程：

4、使用 ros::AsyncSpinner， 多线程处理回调示例

cpp 复制代码 
#include "geometry_msgs/Twist.h"
#include "nav_msgs/Odometry.h"
#include "ros/callback_queue.h"
#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/LaserScan.h"
#include "std_msgs/UInt16MultiArray.h"
#include <thread>

class RobotDevice
{
public:
    RobotDevice()
    {
        pub_actuator_ = nh_.advertise<geometry_msgs::Twist>(rostopic_set_actuator_.c_str(), 1);
        thread_ = new std::thread(&RobotDevice::Run, this);
    }

    ~RobotDevice()
    {
        ros::shutdown();
        if (thread_)
        {
            thread_->join();
            delete thread_;
            thread_ = nullptr;
        }
    }

    void PubCmdVelManual(const float data[8])
    {
        memset(&vel_msg_, 0, sizeof(vel_msg_));
        vel_msg_.linear.x = data[0];
        vel_msg_.angular.z = data[1];
        DBUG("SetCtrl linear.x %0.3f, angualr.z %0.3f", data[0], data[1]);
        pub_actuator_.publish(vel_msg_);
    }

private:
    geometry_msgs::Twist vel_msg_;
    ros::NodeHandle nh_;
    ros::Subscriber sub_imu_;
    ros::Subscriber sub_laser_;
    ros::Subscriber sub_actuator_;
    ros::Subscriber sub_sonar_;
    ros::Publisher pub_actuator_;
    ros::CallbackQueue queue_;
    std::thread *thread_ = nullptr;
    const uint32_t ranger_finder_num_ = 7;
    const std::string rostopic_get_imu_ = "/imu";
    const std::string rostopic_get_sonar_ = "/sonar";
    const std::string rostopic_get_laser_ = "/scan_head";
    const std::string rostopic_get_actuator_ = "/odom";
    const std::string rostopic_set_actuator_ = "/cmd_vel_manual";

    void Run()
    {
        nh_.setCallbackQueue(&queue_);
        sub_imu_ = nh_.subscribe(rostopic_get_imu_.c_str(), 10, &RobotDevice::ImuSubCallback, this);
        sub_actuator_ = nh_.subscribe(rostopic_get_actuator_.c_str(), 10, &RobotDevice::ActuatorSubCallback, this);
        sub_laser_ = nh_.subscribe(rostopic_get_laser_.c_str(), 10, &RobotDevice::LaserSubCallback, this);
        sub_sonar_ = nh_.subscribe(rostopic_get_sonar_.c_str(), 10, &RobotDevice::SonarSubCallback, this);
        ros::AsyncSpinner spinner(0, &queue_);
        spinner.start();
        ros::waitForShutdown();
    }

    void ImuSubCallback(const sensor_msgs::Imu::ConstPtr &msg)
    {
        // DBUG("%s %d", __FUNCTION__, __LINE__);
    }

    void ActuatorSubCallback(const nav_msgs::Odometry::ConstPtr &msg)
    {
        // DBUG("%s %d", __FUNCTION__, __LINE__);
    }

    void LaserSubCallback(const sensor_msgs::LaserScan::ConstPtr &msg)
    {
        // DBUG("%s %d", __FUNCTION__, __LINE__);
    }

    void SonarSubCallback(const std_msgs::UInt16MultiArray::ConstPtr &msg)
    {
        // DBUG("%s %d", __FUNCTION__, __LINE__);
    }
};

int main(int argc, char *argv[])
{
	ros::init(argc, argv, "robot_device");
	RobotDevice device;
	ros::Rate rate(10);
	while (ros::ok())
	{
		rate.sleep();
	}
	return 0;
}
相关推荐
MinBadGuy19 分钟前
【GeekBand】C++设计模式笔记13_Flyweight_享元模式
c++·设计模式
一子二木生三火36 分钟前
IO流（C++）
c语言·开发语言·数据结构·c++·青少年编程
谁在夜里看海.37 分钟前
【从零开始的算法学习日记✨优选算法篇✨】第二章：流动之窗，探索算法的优雅之道
c++·学习·算法
菠菠萝宝1 小时前
【YOLOv8】安卓端部署-1-项目介绍
android·java·c++·yolo·目标检测·目标跟踪·kotlin
初晴~1 小时前
【spring】参数校验Validation
java·c++·spring boot·后端·python·spring·validation
古月居GYH2 小时前
ROS一键安装脚本
人工智能·机器人·ros
daily_23332 小时前
数据结构——小小二叉树第二幕（二叉树链式结构的实现以及二叉树的遍历）超详细！！！
数据结构·c++
时光の尘2 小时前
C语言菜鸟入门·关键字·void的用法
c语言·开发语言·c++·算法·c#·c·关键字
小林熬夜学编程2 小时前
【Linux系统编程】第四十九弹---日志系统构建指南：从基础结构到时间处理与Log类实现
linux·运维·服务器·c语言·开发语言·c++
DeepAlchemy2 小时前
ROSSERIAL与Arduino IDE交叉开发（UBUNTU环境，包含ESP32、arduino nano）
c++·单片机·ros·rosserial
热门推荐
01(欧拉)openEuler系统添加网卡文件配置流程、(欧拉)openEuler系统手动配置ipv6地址流程、(欧拉)openEuler系统网络管理说明02PyTorch机器学习实现液态神经网络03【HarmonyOS】HUAWEI DevEco Studio 下载地址汇总04玄机平台应急响应—webshell查杀05Coze扣子平台完整体验和实践（附国内和国际版对比）06RAG 实践- Ollama+RagFlow 部署本地知识库07【目标跟踪】相机运动补偿08Unity中PICO实现 隔空取物 和 接触抓取物体09Ubuntu 20.04使用Livox mid 360 测试 FAST_LIO1004 - matlab m_map地学绘图工具基础函数 - 设置网格