关于ROS话题订阅发布和录制可视化的一些笔记

话题的发送与接收处理

发布缓冲区和接收缓冲区的作用：

发布缓冲区：

• 先入先出
• 在数据传输出现问题，保证不丢失发送数据。
  • eg: 如无线组网时，出现偶然传输失败，那么数据可以留在缓冲区内，等待下一次传输，在单机上一般作用不大。

接收缓冲区：

• 先入先出

• 当回调函数来不及处理最新数据时，保证该数据不丢失。

  • eg: 某一个低频回调执行时间长，导致高频数据的回调函数过了很久才得到执行，如果没有缓冲区，那么这段时间的高频数据都会丢失，而有缓冲区则可以将数据暂存，下次回调函数多次执行将其清空。

代码例子：

/**
 * 该例程将订阅/person_info话题，自定义消息类型learning_topic::Person
 */

#include <ros/ros.h>
#include "learning_topic/Person.h"

// 接收到订阅的消息后，会进入消息回调函数
void personInfoCallback(const learning_topic::Person::ConstPtr &msg)
{
    // 将接收到的消息打印出来
    ROS_INFO("Subcribe Person Info: name:%s  age:%d  sex:%d",
             msg->name.c_str(), msg->age, msg->sex);
}

int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "person_subscriber");

    // 创建节点句柄
    ros::NodeHandle n;

    // 创建一个Subscriber，订阅名为/person_info的topic，注册回调函数personInfoCallback
    ros::Subscriber person_info_sub1 = n.subscribe("/person_info/1", 1, personInfoCallback); 
    ros::Subscriber person_info_sub2 = n.subscribe("/person_info/2", 3, personInfoCallback);
    ros::Subscriber person_info_sub3 = n.subscribe("/person_info/3", 3, personInfoCallback);
    // 经过实验测试，每一次spin调用，都会把缓冲区的数据全部处理！也就是回调函数会快速被调用好多次
    //这位老铁和我做了类似的实验来验证：https://blog.csdn.net/cp_csdn_id/article/details/108026151
    // create Publishers and subscribers.
    // it takes time of register of publishers and subscribers. so, add a delay for 0.5s .
    // 如果没有下面这行代码，那么开始spin时，可能还没完成pub sub的注册，这会导致丢掉一些数据。
    ros::Duration(0.5).sleep();

    ros::Rate loop_rate(10);

    // while (ros::ok())
    // {
    //     ros::spinOnce();
    // 会按造注册顺序，依次查询各个消息队列（/person_info/1，/person_info/2，/person_info/3 这里我们有三个队列）
    // 有无消息，如果有，那就一次将队列中所有数据都处理了,没有就继续往下执行。（ ros::spin()也是一样的 ）。
    // 慕课那个图完全是误导人！！！
    // https://sychaichangkun.gitbooks.io/ros-tutorial-icourse163/content/chapter6/6.3.html
    //
    //     loop_rate.sleep();
    // }

    // 循环等待回调函数
    ros::spin();

    return 0;
}

---

rosbag录制问题

在rosbag record录制rosbag时，每个话题消息除了它自己原本的时间戳（header timestamps，ROS官网是这么称呼的）外，rosbag会给每个消息再打上录制时的时间戳（message timestamps，ROS官网是这么称呼的）。按我们的期望来说，如果数据即时地发布了出去，那么我们希望rosbag添加的时间戳应该是和消息自己的时间戳是一样的，至少相差很小。但是，这两个东西并不一样，尤其是高频数据。如果只是绝对时间不一样那其实也问题不大。但是！ rosbag录制的结果连相对时间间隔都不一致。这主要是ROS1的通讯机制决定的，其默认采用了tcp通讯，导致接收数据不能保证实时性。

一个很详细的回答：https://answers.ros.org/question/318536/understanding-rosbag-timestamps/

rosbag的这个bug就导致了一些很烦人的事情：

• rosbag在rqt打开时，其时间戳显示的时rosbag的时间戳。这和实际并不符合，我们想从这里看时间戳同步的情况是不准确的！

  • 在下图，我们可以看到vins_estimator/imu_propagate和imu对不上，而且差的很多。
    

• rosbag在播放的时后，其播放时序取决于rosbag的时间戳。这就导致播放时的msg发布速率与实际不完全一致。

  • 那么如果我们用录制的bag来做参数标定或者算法验证，这怎么行呢？还好有话题自己的时间戳，我们只要留下足够的缓冲区，然后和时间相关的运算都用话题自己的时间戳就行了。注意，用rosbag的数据，保险起见，话题订阅的缓存一定要有且足够大。不然出现如下情况，我们就会漏掉很多消息。在下面这个例子中，如果没有订阅消息的缓冲，那么回调函数只能处理0、1和9的数据，中间的部分大概率会被丢掉

    eg:(时间戳单位为s)

    时间戳归属	时间戳	…	…
    rosbag	0	1	2	2	2	2	2	2	2	9
    msg	0	1	2	3	4	5	6	7	8	9

官方自己也知道这个问题，为此给出了如下的补救措施：

---

Python

Dependencies

rosbag python package uses Cryptodomex and gnupg packages. They can be installed via pip using:

$ pip3 install pycryptodomex python-gnupg

Rewrite bag with header timestamps

To replace message timestamps in a bag with header timestamps:

来源：http://wiki.ros.org/rosbag/Cookbook 这里也有一些更详细的例子：https://blog.csdn.net/weixin_42840360/article/details/119844648

---

除了以上措施，还有一个解决办法：

• 采用rosbag record --tcpnodelay 就可以录制一个时间间隔基本一致的rosbag，效果如下：
  不过这个时间戳和真实的还是有区别。下面是我用rosbag record --tcpnodelay录制的t265 imu数据的分析处理结果。ros_dt是录制的时间戳的各帧时间间隔/ms，IMU_dt是消息自己各帧的时间间隔/ms，ros_imu_dt是同一帧数据的header.stamp和ros录制的时间戳的差别/ms，也就是延时时间。我们可以看到其中的差异。这里需要注意，外部传感器驱动发布topic的延迟挺高的，这个目前我还不太清楚原因。一般情况下，tcpnodelay模式下，自己写的收发代码测试，其延迟是小于1ms的。
  在实际的应用程序中，为了保证数据传输的即时，一般也是采用tcpnodelay模式：

ros::Subscriber imu_sub = n.subscribe<sensor_msgs::Imu>("/mavros/imu/data", 10, imuCallback, ros::VoidConstPtr(),ros::TransportHints().tcpNoDelay());

一些更详细的内容：https://zhuanlan.zhihu.com/p/552346221

话题数据可视化

目前我们常使用plotjuggler对一些数据进行分析。plotjuggler在采集话题实时播放时，其时间轴是plotjuggler采集到数据时的时间，和消息自己本身的时间无关。如果是在读取bag文件来显示的话，它默认选取bag录制时产生的时间戳，也可以选择以消息自己的时间戳来绘制if present,use the timestamp in the field[header.stamp]。

值得注意的是plotjuggler实时采集时也不能保证其接收时间戳和实际的发布消息时间一致，差距大概在0.5ms左右，可以认为其自动采用了tcpnodelay模式。这意味着在实时采集时，高频高精度的数据分析也是不能完全保证的，因为一点点的时间偏移都会导致结果与实际不符。不过对大部分情况而言，还是基本够用了。