【ROS】让你的回调函数并行起来

【前言】

众所周知，ROS中的所有回调函数，都由 ros::spin() 这个家伙来统一管理和唤醒。这里说的是所有通过ROS方式创建出来的回调函数，比如ros::Subscriber、ros::Timer等等的回调函数。

【举例】

我们先来看一个示例节点：

#include <ros/ros.h>
#include <cstdlib>
#include <time.h>
#include <iostream>void test_1()
{usleep(200 * 1000);ROS_INFO("test 1");
}void test_2()
{ROS_INFO("test 2");
}int main(int argc, char** argv)
{ros::init(argc, argv, "test_node");ros::NodeHandle nh;ros::Timer timer_1 = nh.createTimer(ros::Duration(0.1), [&](const ros::TimerEvent &){test_1();});ros::Timer timer_2 = nh.createTimer(ros::Duration(0.05), [&](const ros::TimerEvent &){test_2();});ros::spin();return 1;
}

你猜 timer_2 定时器的回调函数 test_2() 的实际运行频率是多少？会是我们设置的0.05秒一次吗？

很遗憾，test_2() 的实际运行频率是 0.2秒一次！

聪明的你肯定已经察觉出端倪，test_1() 这个回调中阻塞了200毫秒，并且它也影响了 test_2() 的调用。

没错，ros::spin() 是通过单线程的方式，管理所有回调函数的！！！

【出路】

那如果我们的节点中，有些回调函数确实需要执行一些复杂操作，导致回调执行被阻塞一段时间，我们又该怎么办呢？

此时，我们应该把 spin() 替换成 ros::AsyncSpinner ，也就是异步的多线程回调执行器！同样是上面的代码，我们做一下修改：

#include <ros/ros.h>
#include <cstdlib>
#include <iostream>
#include <time.h>void test_1()
{usleep(200 * 1000);ROS_INFO("test 1");
}void test_2()
{ROS_INFO("test 2");
}int main(int argc, char** argv)
{ros::init(argc, argv, "test_node");ros::NodeHandle nh;ros::Timer timer_1 = nh.createTimer(ros::Duration(0.1), [&](const ros::TimerEvent &){test_1();});ros::Timer timer_2 = nh.createTimer(ros::Duration(0.05), [&](const ros::TimerEvent &){test_2();});ros::AsyncSpinner spinner(4);           //非阻塞式的spinner, 可以使用start和stop进行启停spinner.start();                        //启动线程ros::waitForShutdown();return 1;
}

再次运行这个节点，你会发现，test_2() 的执行频率变成了我们期望的 20Hz了。

注意：上面代码中的 spinner(4) 代表开启4个线程循环，你可以根据你的运行环境，设置影响的线程数量。