ROS中时间戳的读取和同步

---

一. 读取话题时间戳

1. 添加头文件

#include "ros/time.h"

2. 定义变量

ros::Time time1;

3. 将时间戳保存到变量中

先通过话题查看格式：

header: 
  seq: 278
  stamp: 
    secs: 173
    nsecs: 427000000
  frame_id: "camera_link"

接着对变量赋值：

void Control::getImuData(const sensor_msgs::ImuConstPtr &msg)
{
	time1 = msg->header.stamp;
}

4. 时间戳转换为浮点型

double time2;
time2 = time1.toSec();

5. 浮点型转换为时间戳

msg.header.stamp = time2.fromSec();

二. 解决tf监听时的同步

1. 读取话题中的时间戳

void Control::getImuData(const sensor_msgs::ImuConstPtr &msg)
{
	time1 = msg->header.stamp;
}

2. 监听该时间戳对应的tf变换

• 预定义

tf::TransformListener tf_listener;
tf::StampedTransform transform;

(1) 获得指定时间戳的tf变换:

tf_listener.lookupTransform("map", "base_link", time1, transform);

(2) 获得缓冲区中最新的tf变换:

tf_listener.lookupTransform("map", "base_link", ros::Time(0), transform);

• 对比上面(1)和(2)中的结果：

[ INFO] [1617799700.275293869, 111.644000000]: (time1, TimeTF) is (111.12, 111.63)

差了0.5 (111.63-111.12 )秒，对于快速运动的飞机，结果影响还是比较大的。

(3) 获取当前时刻的tf变换:

tf_listener.lookupTransform("map", "base_link", ros::Time::now(), transform);

• 直接执行这行代码会报错：

[ERROR] [1617796735.781526284, 34.990000000]: Lookup would require extrapolation into the future.  Requested time 34.990000000 but the latest data is at time 34.984000000, when looking up transform from frame [base_link] to frame [map]

解决方法可参考
https://zhuanlan.zhihu.com/p/165150773
https://blog.csdn.net/qq_43481884/article/details/102799048