版权声明:本文为博主原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。
本文链接: https://blog.csdn.net/Zed_Of_Zoe/article/details/116706974
目的
ROS主线程
ROS本身占用计算资源较多, 且多传感器的时间戳不同步.
对于流程较长且循环执行的算法, 如果将所有处理任务都放在一个ROS节点的主线程中,
每次 ros::spinOnce();
执行一次算法流程, 主线程中的传感器回调函数也只执行一次, 可能导致高频传感器数据丢失.
多线程异步处理
本文受LeGO-LOAM
源码中mapOptmization.cpp
中闭环图优化的启发, 介绍一种在单个ROS节点内使用多线程的编程方法.
核心思想是:
(1) 在ROS主线程中仅处理传感器回调函数和一些耗时低的数据预处理, 并存放在队列或堆栈中;
(2) 将耗时高的算法流程分解为多个相对独立的子流程, 每个子流程放在一个新线程中.
这样, 将传感器数据采集和处理流程分开, 保证了ROS主线程的循环频率, 减少数据丢失.
需要注意的是, 最好使得每个子流程的处理时间相近, 若子流程之间有相互逻辑关系, 需要加入一些其他的控制标识量.
下面的示例代码中, func1()
func2()
func3()
分别为子流程
代码
实现一个流程类
class WorkFlow()
{
public:
ros::NodeHandle nh;
// 变量
int a, b, c;
// 处理函数
void func1() {a++};
void func2() {b++};
void func3() {c++};
// 线程回调函数
void func1Thread()
{
ros::Rate rate(10);
while(ros::ok())
{
func1();
rate.sleep();
}
}
void func2Thread()
{
ros::Rate rate(10);
while(ros::ok())
{
func2();
rate.sleep();
}
}
void func3Thread()
{
ros::Rate rate(10);
while(ros::ok())
{
func3();
rate.sleep();
}
}
WorkFlow():nh("~")
{
a = b = c = 0;
}
/*
其他处理函数
以及传感器回调函数
*/
};
主函数
int main(int argc, char** argv)
{
ros::init(argc, argv, "msg_buildIntensityMap");
WorkFlow EX;
std::thread Thread1(&WorkFlow::func1Thread, &EX);
std::thread Thread2(&WorkFlow::func2Thread, &EX);
std::thread Thread3(&WorkFlow::func3Thread, &EX);
ros::Rate rate(20);
while (ros::ok())
{
ros::spinOnce();
rate.sleep();
}
Thread1.join();
Thread2.join();
Thread3.join();
return 0;
}
解释
主循环
下面主函数中的循环, 可用于处理耗时低的任务, 例如传感器数据回调函数
while (ros::ok())
{
ros::spinOnce();
rate.sleep();
}
std::Thread线程
下面线程函数中的循环, 可用于处理耗时较高的任务, 例如地图建模/点云分割/运动规划等
以Thread1
为例
Thread1
线程执行WorkFlow
类对象EX
的func1Thread()
函数
std::thread Thread1(&WorkFlow::func1Thread, &EX);
Thread1.join();
func1Thread()
函数循环执行func1()
函数, func1()
函数通常可用于处理耗时较高的任务
// 处理函数
void func1() {a++};
// 线程回调函数
void func1Thread()
{
ros::Rate rate(10);
while(ros::ok())
{
func1();
rate.sleep();
}
}