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

stan-coder

已于 2024-07-31 16:41:54 修改

阅读量94

点赞数 1

分类专栏： ROS 文章标签：机器人自动驾驶人工智能

于 2024-07-31 16:41:30 首次发布

本文链接：https://blog.csdn.net/weixin_39538031/article/details/140825305

版权

ROS 专栏收录该内容

15 篇文章 3 订阅

订阅专栏

【前言】

众所周知，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个线程循环，你可以根据你的运行环境，设置影响的线程数量。