ROS订阅消息时缓冲区的读取

最新推荐文章于 2023-10-30 16:30:39 发布

cp_csdn_id

最新推荐文章于 2023-10-30 16:30:39 发布

阅读量2.2k

点赞数 9

分类专栏： ROS

本文链接：https://blog.csdn.net/cp_csdn_id/article/details/108026151

版权

ROS 专栏收录该内容

2 篇文章 0 订阅

订阅专栏

这篇blog主要是通过测试解释ROS在调用callback函数时如何读取缓冲区数据。问题源自于读知乎文章——从零开始做自动驾驶定位(三): 软件框架 ¹中的缓冲区机制部分产生的疑问。

问题描述

知乎文章中对ROS缓冲区机制的部分描述如下：
在这里插入图片描述

因为ROS一直在入门(从未踏入过)，所以当时对“把缓冲区中的数据读完”产生了疑惑。回调函数每次只读取缓冲区中最旧的数据，而回调函数“返回”的指针也无法操作读取缓冲区中的其它数据，那如何实现把缓冲区的数据读完的？

这个问题的大背景是GPS消息频率较高，而激光点云消息频率则相对较低，如果激光点云的回调函数处理时间过长，GPS消息可能会丢帧。为了保证不丢帧，需要把GPS的subscriber中的缓冲区大小设得大一点。但如果按照“回调函数每次只读取缓冲区中最旧的数据”思考会发现这并不能解决问题（当时仍思维定视地认为回调函数依旧定周期执行）。

测试

测试用的数据是含IMU (500Hz)和PointCloud (10Hz)消息的bag。下面是测试代码的最终版。

#include <ros/ros.h>
#include <iostream>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Imu.h>
#include <sys/times.h>

struct timeval start;

void imuHandle(const sensor_msgs::ImuConstPtr _imuMsg)
{
    gettimeofday(&start, NULL);
    std::cout << start.tv_sec * 1000000 + start.tv_usec << std::endl;
    std::cout << "imu message!" << "    ";
    //std::cout << _imuMsg->header.stamp.toNSec() << std::endl;
}

void laserPointHandle(const sensor_msgs::PointCloud2ConstPtr _laserPointMsg)
{
    std::cout << "laser message!" << "    ";
    std::cout << _laserPointMsg->header.stamp.toNSec() << std::endl;
    for(int i=0; i<20000; i++)
    {
        for(int j=0; j<1000; j++)
        {
            
        }
    }
    std::cout << "out" << std::endl;

}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "bufferTest");
    ros::NodeHandle nh;
    ros::Subscriber subImu = nh.subscribe<sensor_msgs::Imu>("/imu_raw", 100, imuHandle);
    ros::Subscriber subLaserPoint = nh.subscribe<sensor_msgs::PointCloud2>("/points_raw", 1, laserPointHandle);
    ros::spin();
    return 0;
}

当把imu的subscriber中的缓冲区大小设为1的时候，测试发现当laser的回调函数运行时，imu的回调函数是不会将其中断的。相邻两次laser回调函数之间，imu回调函数的执行次数低于50，这意味着imu丢消息了。
把缓冲取的大小改大之后，会发现imu的回调函数仍然不会中断laser的回调函数，但相邻两次laser回调函数之间，imu回调函数的执行次数恢复到50左右了。之后在imu的回调函数中添加了系统时间显示，发现imu回调函数50次执行中，靠前的调用相邻两次之间的时间间隔非常短，之后的则恢复500Hz。
下图是imu的callback函数的回调时间显示，单位是微秒。我们可以看出靠近前面的相邻两次回调之间的时间间隔很短(这和回调函数的运行时间有关)，大概几微秒，而靠后的相邻两次回调之间的时间间隔则是几千微秒。
imu callback函数的回调时间打印(微秒)