ROS中的一种多线程编程方法

版权声明：本文为博主原创文章，遵循 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();
	}
}