关于ROS消息缓存区的存取理解

qq_35189442

已于 2022-09-02 16:46:24 修改

阅读量1k

点赞数 7

于 2022-09-01 11:30:16 首次发布

本文链接：https://blog.csdn.net/qq_35189442/article/details/126627782

版权

众所周知，ros中发布和订阅都有消息缓存区的概念。

ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter1", 10);
ros::Subscriber sub = n.subscribe("chatter2", 10, chatterCallback1);

上面代码语句中的10表示发布和订阅的消息缓存区大小为10个msg。

void SubscriptionQueue::push(const SubscriptionCallbackHelperPtr& helper, const MessageDeserializerPtr& deserializer,
                                 bool has_tracked_object, const VoidConstWPtr& tracked_object, bool nonconst_need_copy,
                                 ros::Time receipt_time, bool* was_full)
{
  boost::mutex::scoped_lock lock(queue_mutex_);

  if (was_full)
  {
    *was_full = false;
  }

  if(fullNoLock())
  {
    queue_.pop_front();
    --queue_size_;

    if (!full_)
    {
      ROS_DEBUG("Incoming queue was full for topic \"%s\". Discarded oldest message (current queue size [%d])", topic_.c_str(), (int)queue_.size());
    }

    full_ = true;

    if (was_full)
    {
      *was_full = true;
    }
  }
  else
  {
    full_ = false;
  }

  Item i;
  i.helper = helper;
  i.deserializer = deserializer;
  i.has_tracked_object = has_tracked_object;
  i.tracked_object = tracked_object;
  i.nonconst_need_copy = nonconst_need_copy;
  i.receipt_time = receipt_time;
  queue_.push_back(i);
  ++queue_size_;
}

上面代码为官方melodic版本关于队列推入的功能函数，当判断队列满则执行pop_front从队首丢弃一个msg，之后在队尾push_back推入一个新的msg。因此可以看出，ros的发布订阅用到的是先入先出的队列结构。

为了更好的说明该问题，我们通过程序来测试下。

发布：

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <ros/console.h>
#include <sstream>

int main(int argc, char **argv)
{
  ros::init(argc, argv, "talker");
  ros::NodeHandle n;
  ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter1", 10);
  ros::Publisher chatter_pub2 = n.advertise<std_msgs::String>("chatter2", 10);
  printf("cesh222i-------------\n");
  ros::Rate loop_rate(1);/*每秒发一次数据*/
  int count = 0;
  while (ros::ok())
  {
    std_msgs::String msg;
    std::stringstream ss;
    std::stringstream ss2;
    ss << "hello world chatter1: " << count;
    ss2 << "hello world chatter2: " << count;
    msg.data = ss.str();
    chatter_pub.publish(msg);
    msg.data = ss2.str();
    chatter_pub2.publish(msg);
    ros::spinOnce();
    loop_rate.sleep();
    ++count;
  }
}

一秒发送一次msg。

#include "std_msgs/String.h"
#include <ros/ros.h>
#include <pthread.h>
using namespace std;

int ccount=0;
ros::ServiceClient liftAutoClient;

void chatterCallback1(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("I heard: [%s]", msg->data.c_str());
  ccount++;
  if(ccount%2 == 0)
    sleep(5);
}

void chatterCallback2(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("I heard: [%s]", msg->data.c_str());
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "listener");
  ros::NodeHandle n;
  pthread_t idpccount;
  //pthread_create(&idpccount, NULL, pPrint, NULL);
  ros::Subscriber sub = n.subscribe("chatter2", 10, chatterCallback1);
  ros::Subscriber sub2 = n.subscribe("chatter1", 10, chatterCallback2);
  sleep(5);//启动后等待5秒
  printf("after 5 s\n");
  while(ros::ok()){
    ros::spinOnce();//执行一次回调，你认为是连续5个消息，实际因为每次处理时间为5秒，后面几次的消息被挤出了缓冲区，读到的不是最开始的数据。
    printf("hello\n");
  }
  return 0;
}

启动后先等待5s，之后进入循环执行spinOnce()，并在每执行一次spinOnce之后输出一个hello，在callback1中进行求余判断，进而在每接收两次数据之后sleep 5s（注意订阅回调中的ccount和发布msg中的count不一样，因此打印不一定是偶数才开始sleep 5s）

先运行talker一段时间后再运行listener。得到以下输出结果：

after 5 s

[ INFO] [1661936454.313555402]: I heard: [hello world chatter1: 2381]

[ INFO] [1661936454.317567002]: I heard: [hello world chatter2: 2381]

[ INFO] [1661936454.317992592]: I heard: [hello world chatter1: 2382]

[ INFO] [1661936454.318182426]: I heard: [hello world chatter2: 2382]

[ INFO] [1661936459.318573388]: I heard: [hello world chatter1: 2383]

[ INFO] [1661936459.318782039]: I heard: [hello world chatter2: 2383]

[ INFO] [1661936459.318927423]: I heard: [hello world chatter1: 2384]

[ INFO] [1661936459.319022500]: I heard: [hello world chatter2: 2384]

[ INFO] [1661936464.319386333]: I heard: [hello world chatter1: 2386]

[ INFO] [1661936464.319718159]: I heard: [hello world chatter2: 2386]

hello

[ INFO] [1661936464.319911673]: I heard: [hello world chatter1: 2387]

[ INFO] [1661936464.320015838]: I heard: [hello world chatter2: 2387]

[ INFO] [1661936469.320353471]: I heard: [hello world chatter1: 2391]

[ INFO] [1661936469.320531880]: I heard: [hello world chatter2: 2391]

[ INFO] [1661936469.320675120]: I heard: [hello world chatter1: 2392]

[ INFO] [1661936469.320806134]: I heard: [hello world chatter2: 2392]

[ INFO] [1661936474.321440911]: I heard: [hello world chatter1: 2396]

[ INFO] [1661936474.321712350]: I heard: [hello world chatter2: 2396]

[ INFO] [1661936474.321888903]: I heard: [hello world chatter1: 2397]

[ INFO] [1661936474.321993164]: I heard: [hello world chatter2: 2397]

[ INFO] [1661936479.322353376]: I heard: [hello world chatter1: 2401]

[ INFO] [1661936479.322638671]: I heard: [hello world chatter2: 2401]

[ INFO] [1661936479.322918461]: I heard: [hello world chatter1: 2402]

[ INFO] [1661936479.323041988]: I heard: [hello world chatter2: 2402]

[ INFO] [1661936484.323447971]: I heard: [hello world chatter1: 2406]

[ INFO] [1661936484.323769812]: I heard: [hello world chatter2: 2406]

[ INFO] [1661936484.324056258]: I heard: [hello world chatter1: 2407]

[ INFO] [1661936484.324335313]: I heard: [hello world chatter2: 2407]

hello

[ INFO] [1661936489.324932003]: I heard: [hello world chatter1: 2411]

[ INFO] [1661936489.325281622]: I heard: [hello world chatter2: 2411]

[ INFO] [1661936489.325947544]: I heard: [hello world chatter1: 2412]

[ INFO] [1661936489.326064862]: I heard: [hello world chatter2: 2412]

【输出分析】

第一次由于延迟5s，缓存区接收了5次发布msg。

第一次发布接收：2381 2382 2383 2384 2385

订阅缓存区：2381 2382 2383 2384 2385 输出2381、2382

after 5 s

[ INFO] [1661936454.313555402]: I heard: [hello world chatter1: 2381]

[ INFO] [1661936454.317567002]: I heard: [hello world chatter2: 2381]

[ INFO] [1661936454.317992592]: I heard: [hello world chatter1: 2382]

[ INFO] [1661936454.318182426]: I heard: [hello world chatter2: 2382]

第二次是由于回调里的延迟5s，再次接收5次发布的msg，由于第一次输出了2个msg，缓冲区总共还剩8个msg。

第二次发布接收：2386 2387 2388 2389 2390

订阅缓存区：2383 2384 2385 2386 2387 2388 2389 2390 输出2383、2384

[ INFO] [1661936459.318573388]: I heard: [hello world chatter1: 2383]

[ INFO] [1661936459.318782039]: I heard: [hello world chatter2: 2383]

[ INFO] [1661936459.318927423]: I heard: [hello world chatter1: 2384]

[ INFO] [1661936459.319022500]: I heard: [hello world chatter2: 2384]

第三次延迟又接收5个msg，除去上回输出的2个msg，还剩11个msg。由于缓存区空间只有10个msg，前1个msg=2385被丢弃，输出了2386和2387，由于输出了第一次spinOnce调用时缓存区里的msg5个，中间还输出了hello。

第三次发布接收：2391 2392 2393 2394 2395

订阅缓存区：2386 2387 2388 2389 2390 2391 2392 2393 2394 2395 输出2386 hello 2387

[ INFO] [1661936464.319386333]: I heard: [hello world chatter1: 2386]

[ INFO] [1661936464.319718159]: I heard: [hello world chatter2: 2386]

hello

[ INFO] [1661936464.319911673]: I heard: [hello world chatter1: 2387]

[ INFO] [1661936464.320015838]: I heard: [hello world chatter2: 2387]

第四次发布接收：2396 2397 2398 2399 2400

订阅缓存区：2391 2392 2393 2394 2395 2396 2397 2398 2399 2400 输出2391 2392

......

【总结】

1、spinOnce不是指执行一次callback，而是执行当前缓存区中msg的数量。

2、callback取的是缓存区队列中最早接收的数据。

3、callback缓存区是先进先出的队列格式。

参考资料：

(57条消息) ros::spin() 或 ros::spinOnce(),ros消息队列理解_路人么过客的博客-CSDN博客_ros::spinonce

ros_comm/subscription_queue.cpp at melodic-devel · ros/ros_comm · GitHub