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;
}
相关推荐
小飞猪Jay25 分钟前
面试速通宝典——10
linux·服务器·c++·面试
程序猿阿伟1 小时前
《C++高效图形用户界面（GUI）开发：探索与实践》
开发语言·c++
阿客不是客1 小时前
深入计算机语言之C++：C到C++的过度
c++
LN-ZMOI1 小时前
c++学习笔记1
c++·笔记·学习
no_play_no_games1 小时前
「3.3」虫洞 Wormholes
数据结构·c++·算法·图论
￴ㅤ￴￴ㅤ9527超级帅1 小时前
LeetCode hot100---数组及矩阵专题(C++语言)
c++·leetcode·矩阵
五味香1 小时前
C++学习，信号处理
android·c语言·开发语言·c++·学习·算法·信号处理
TANGLONG2222 小时前
【C语言】数据在内存中的存储(万字解析)
java·c语言·c++·python·考研·面试·蓝桥杯
summ1ts2 小时前
组合数求法汇总
c++·数学·算法·离散数学·组合数学
热门推荐
01【经验分享】Ubuntu22.04安装微信(linux官方版)02安卓系列机型永久去除data分区加密 详细步骤解析032024年高教社杯数学建模国赛C题超详细解题思路分析04组基轨迹建模 GBTM的介绍与实现（Stata 或 R）05RAG 实践- Ollama+RagFlow 部署本地知识库06【2024数模国赛赛题思路公开】国赛B题思路丨附可运行代码丨无偿自提07分享几个惊艳的 Three.js 个人站点08苍穹外卖面试总结09【2024高教社杯全国大学生数学建模竞赛】B题 生产过程中的决策问题——解题思路 代码 论文102024CCPC网络预选赛 I. 找行李 【DP】