文章目录
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核心使用一个线程。
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示例的等效用法是:
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线程。
//一个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。
//这是执行结果，可以看到主线程
[ 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队列的回调。这样确保每个订阅回调相互独立不影响。下面的代码展示如何进行上述操作：
//为每个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的回调处理。
[ 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， 多线程处理回调示例
#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;
}
扫一扫
1807
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
