ROS学习之TF变换的时间戳

参考

1.TF坐标广播时的时间戳

1.1.时间戳的作用

先明确什么是TF的坐标变换：实际上就是子坐标系在父坐标系中表示的位姿，也可以看成是子坐标系到父坐标系的坐标变换关系。

通过使用TF广播，每次只需要发布相邻两个坐标系之间的坐标关系。然后通过TF订阅者得到这些相邻的坐标变换关系，TF就能计算出任意两个坐标系之间的坐标变换关系。

但是存在一个问题：由于TF建立的坐标系一般都是动坐标系，也就是坐标系之间的变换关系是随着时间变化的。并且这个坐标关系是人为设置或者传感器检测的数据，在程序中调用TF广播发布的。

这就出现了时间对齐的问题：比如一个机械手臂有大臂和小臂，大臂绕着肩关节转，小臂绕着肘关节转。在大臂和小臂上分别有两个IMU可以检测姿态，比如A检测大臂相对肩膀的姿态，B检测小臂相对大臂的姿态。通过这两个数据计算，最后就能得到小臂相对肩膀的姿态。但是两个IMU的数据一直在发送，如果想知道0.5S时刻小臂对肩膀的姿态，那么就需要使用0.5S时刻A的数据和0.5S时刻B的数据相乘得到，而不能使用其他时刻的数据相乘，因为需要时间对齐。坐标点在不同坐标系下表示的时候也是同理。所以TF中就有时间戳的概念。

1.2.TF坐标广播中的时间戳

以ROS中的tf2（原来是tf，后来官方改进了成为tf2，并且建议使用tf2）坐标变换为例，如下是进行TF广播的程序，改程序是订阅turtlesim的乌龟位姿，然后根据乌龟的位姿发布乌龟坐标系相对世界坐标系的tf变换。

// 1.包含头文件
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

void doPose(const turtlesim::Pose::ConstPtr& pose){
    //  5-1.创建 TF 广播器
    static tf2_ros::TransformBroadcaster broadcaster;
    //  5-2.创建 广播的数据(通过 pose 设置)
    geometry_msgs::TransformStamped tfs;
    //  |----头设置
    tfs.header.frame_id = "world";
    tfs.header.stamp = ros::Time::now();

    //  |----坐标系 ID
    tfs.child_frame_id = "turtle1";

    //  |----坐标系相对信息设置
    tfs.transform.translation.x = pose->x;
    tfs.transform.translation.y = pose->y;
    tfs.transform.translation.z = 0.0; // 二维实现，pose 中没有z，z 是 0
    //  |--------- 四元数设置
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,pose->theta);
    tfs.transform.rotation.x = qtn.getX();
    tfs.transform.rotation.y = qtn.getY();
    tfs.transform.rotation.z = qtn.getZ();
    tfs.transform.rotation.w = qtn.getW();


    //  5-3.广播器发布数据
    broadcaster.sendTransform(tfs);
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"dynamic_tf_pub");
    // 3.创建 ROS 句柄
    ros::NodeHandle nh;
    // 4.创建订阅对象
    ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose",1000,doPose);
    //     5.回调函数处理订阅到的数据(实现TF广播)
    //        
    // 6.spin
    ros::spin();
    return 0;
}

程序中比较重要的是下面三句，即指定发布的tf广播的父坐标系、子坐标系和时间戳。这里的时间戳使用的是ros::Time::now();，也就是当前调用这条命令的时候的时间，比如得到的结果是150.3S这个时刻，意思为这个tf变换是在150.3S时刻的坐标变换。

    //  |----头设置
    tfs.header.frame_id = "world";
    tfs.header.stamp = ros::Time::now();

    //  |----坐标系 ID
    tfs.child_frame_id = "turtle1";

2.坐标点的时间戳

2.1.坐标点进行坐标变换时的时间戳

仍旧以tf2为例，监听tf广播，并进行坐标变换的程序如下。注意这里和tf有点不同，这里的坐标变换是使用buffer缓存监听到的tf广播，然后调用buffer.transform函数，传入要变换的坐标点和父坐标系，就完成了坐标变换，即 point_base = buffer.transform(point_laser,"world");。

//1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h" //注意: 调用 transform 必须包含该头文件

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"dynamic_tf_sub");
    ros::NodeHandle nh;
    // 3.创建 TF 订阅节点
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);

    ros::Rate r(1);
    while (ros::ok())
    {
    // 4.生成一个坐标点(相对于子级坐标系)
        geometry_msgs::PointStamped point_laser;
        point_laser.header.frame_id = "turtle1";
        point_laser.header.stamp = ros::Time();
        point_laser.point.x = 1;
        point_laser.point.y = 1;
        point_laser.point.z = 0;
    // 5.转换坐标点(相对于父级坐标系)
        //新建一个坐标点，用于接收转换结果  
        //--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------
        try
        {
            geometry_msgs::PointStamped point_base;
            point_base = buffer.transform(point_laser,"world");
            ROS_INFO("坐标点相对于 world 的坐标为:(%.2f,%.2f,%.2f)",point_base.point.x,point_base.point.y,point_base.point.z);

        }
        catch(const std::exception& e)
        {
            // std::cerr << e.what() << '\n';
            ROS_INFO("程序异常:%s",e.what());
        }


        r.sleep();  
        ros::spinOnce();
    }


    return 0;
}

1.2.1.坐标点的时间戳的含义

程序中比较重要的是下面两句，即指定了当前这个坐标点是相对于"turtle1"这个坐标系的。

point_laser.header.frame_id = "turtle1";
point_laser.header.stamp = ros::Time();

那么point_laser.header.stamp是什么意思呢？是当前坐标点的时间戳吗？

不是！记住坐标点是没有时间戳的，这里指的是turtle1这个坐标系的时间戳！或者说是tf广播中的时间戳。因为前面说了，tf广播的是两个坐标系之间的坐标变换关系，也就是子坐标系在父坐标系下的位姿表示。

所以这里对坐标点指定时间戳，就相当于说明了这个坐标点是哪一时刻下的turtle1坐标系中的坐标。因为同样的一个点，在不同时刻下的turtle1坐标系中，其坐标表示可能是不一样的（因为坐标系在动）。

1.2.2.坐标点的时间戳的选择

1.2.2.1.选择tf广播中的时间戳

这种选择肯定是最精确的，比如tf广播中发布的是150.3S这个时刻的坐标变换，那么这里坐标点我也设置成150.3S时刻，这样就做到了时空的同步。

但是问题是我们这里是tf广播的订阅者，并且tf广播中发布的是实时的坐标变换，也就是这个150.3S实际运行的时候到底是多少你是不知道的，并且它一直在变，所以根本不可能写死在程序中。

1.2.2.2.选择当前时刻ros::Time()::now()

很显然这就存在前面说的时间对齐的问题（但是猜测这个问题并不严重，后面解释），并且很重要的问题是根本无法进行坐标变换。

因为当tf广播的时候，使用的是它广播那一时刻的时间，比如150.3S。那么当他广播之后，肯定要经过一段时间才能被订阅者接收到，也就是广播和订阅之间存在时间差，假设这个时间差是0.5S。也就是说，当订阅者这边指定的时间戳是time::now()的时候，必须至少再等0.5S才能得到发来的这个时刻的tf广播，或者说除非订阅者那边发布的时间戳比真实时刻超前了至少0.5S。所以在time::now这个时刻就请求坐标变换，会报找不到坐标变换的错误，如下所示，也就是提示现在请求的时间是过去0.018ms，但是广播中最新的数据是过去7.681ms，也就是还没有过去0.018ms这个时刻的数据，即要求的tf变换是之后的，目前还没有。

[ERROR] [1287871653.885277559]: 
You requested a transform that is 0.018 miliseconds in the past, but the most recent transform in the tf buffer is 7.681 miliseconds old.                                                 
When trying to transform between /turtle1 and /turtle2. 

那么上面为什么说时间对齐这个问题不是很严重呢？

个人猜测：

• 不可能完全对齐时间
  很显然，不可能做到严格的时间对齐，比如坐标点指定的时刻和tf广播中的时刻一模一样，他们可能会有微小差距。

• 看官方的一个解决办法（这个程序是tf1的）

请求现在时刻的坐标变换

listener.lookupTransform("/turtle2", "/turtle1", ros::Time::now(), transform);

报错找不到当前时刻的tf广播：

[ERROR] [1287871653.885277559]: 
You requested a transform that is 0.018 miliseconds in the past, but the most recent transform in the tf buffer is 7.681 miliseconds old.                                                 
When trying to transform between /turtle1 and /turtle2. 

解决：等待一段时间，知道我要的 “当前时刻” 的广播到达，也就是这个时间戳的时刻变成past的时刻。

注意： 这里 当前时刻 加了引号，程序中也可以看出来，是先获取了ros::Time::now()存到now变量中，然后获取这个等待这个now的时刻到达，然后再请求这个now的tf变换。

也就是说，我等待过了now这个时刻之后最新的一次坐标变换，等到了之后就使用这个时刻的坐标变换。

比如：now这个时刻是180.5S，tf那边每0.2S发一次坐标变换消息，延迟时间是0.4S。

• 发布方180.2S发布的消息：180.6S的时候才能到接收方，但是消息中的时间戳仍然是180.2S，显然相对180.5S，180.2S是过去时刻的坐标变换，不满足要求；继续等
• 发布方180.4S发布的消息：180.8S的时候才能到接收方，但是消息中的时间戳仍然是180.4S，显然相对180.5S，180.4S是过去时刻的坐标变换，不满足要求；继续等
• 发布方180.6S发布的消息：181.0S的时候才能到接收方，但是消息中的时间戳仍然是180.6S，显然相对180.5S，180.6S已经是满足要求的最新时刻的tf广播了，所以满足要求，然后使用这个tf关系进行坐标变换就可以了。

ros::Time now = ros::Time::now();
listener.waitForTransform("/turtle2", "/turtle1",now, ros::Duration(3.0));
listener.lookupTransform("/turtle2", "/turtle1",now, transform);

所以说，并不要求时间戳完全对齐，而是会根据要求的时间戳，在tf广播中找一个在这个时间戳之后且离这个时间戳最近的坐标变换使用。

• 检测之后好像不太对
  使用程序检测，tf以1HZ的频率发布消息，接收方以3HZ的频率进行坐标变换，并且每次使用的点的时间戳都是当前时间的前2S，如果按照上面的推论，这3HZ里面，使用的不论是时间戳前面的还是后面的坐标变换，结果应该都是tf发布的1HZ的消息中的tf消息。但是实际进行坐标变换的时候发现好像对于这种不是tf广播发布的时间戳的坐标变换请求，tf会进行计算，类似插值，找到一个近似请求的时间戳的坐标变换。

测试程序如下所示：

/***************** tf发布文件 **********/
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

void doPose(const turtlesim::Pose::ConstPtr& pose){
    //  5-1.创建 TF 广播器
    static tf2_ros::TransformBroadcaster broadcaster;
    //  5-2.创建 广播的数据(通过 pose 设置)
    geometry_msgs::TransformStamped tfs;
    //  |----头设置
    tfs.header.frame_id = "world";
    tfs.header.stamp = ros::Time::now();

    //  |----坐标系 ID
    tfs.child_frame_id = "turtle";

    //  |----坐标系相对信息设置
    tfs.transform.translation.x = pose->x;
    tfs.transform.translation.y = pose->y;
    tfs.transform.translation.z = 0.0; // 二维实现，pose 中没有z，z 是 0
    //  |--------- 四元数设置
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,pose->theta);
    tfs.transform.rotation.x = qtn.getX();
    tfs.transform.rotation.y = qtn.getY();
    tfs.transform.rotation.z = qtn.getZ();
    tfs.transform.rotation.w = qtn.getW();


    //  5-3.广播器发布数据
    broadcaster.sendTransform(tfs);
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"tf_pub");
    // 3.创建 ROS 句柄
    ros::NodeHandle nh;

    tf2_ros::TransformBroadcaster broadcaster;
    //  5-2.创建 广播的数据(通过 pose 设置)
    geometry_msgs::TransformStamped tfs;
    tfs.header.frame_id = "world";
    tfs.header.stamp = ros::Time::now();   // 如果把时间写在循环外面，那么每次发的时间都是一样的

    //  |----坐标系 ID
    tfs.child_frame_id = "turtle1";
    tfs.transform.rotation.w = 1.0;  

    ros::Rate rate(1);
    double x = 0.0;

    while(ros::ok())
    {
        //  |----坐标系相对信息设置
        tfs.transform.translation.x = x;
        ROS_INFO("Time Send: %d， x = %.2f", tfs.header.stamp.sec, x);
        
        tfs.header.stamp = ros::Time::now();  // 这样的时间才是正确的时间
        //  5-3.广播器发布数据
        broadcaster.sendTransform(tfs);
        x += 1.0;       
        rate.sleep();
        ros::spinOnce();
    }
    
    return 0;
}

/***********  tf发布函数订阅文件 **********/
//1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h" //注意: 调用 transform 必须包含该头文件

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"dynamic_tf_sub");
    ros::NodeHandle nh;
    // 3.创建 TF 订阅节点
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);

    ros::Rate r(3);

    while (ros::ok())
    {
        // 4.生成一个坐标点(相对于子级坐标系)
        geometry_msgs::PointStamped point_laser;
        point_laser.header.frame_id = "turtle1";
        point_laser.header.stamp = ros::Time::now() - ros::Duration(2.0);  // 3s之前的时刻  

        // point_laser.header.stamp = ros::Time();   // 这是使用最近的一次变换，没有检测意义

        point_laser.point.x = 0;
        point_laser.point.y = 0;
        point_laser.point.z = 0;
        // 5.转换坐标点(相对于父级坐标系)
        //新建一个坐标点，用于接收转换结果  
        //--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------
        try
        {
            geometry_msgs::PointStamped point_base;
            point_base = buffer.transform(point_laser,"world");
            ROS_INFO("坐标点相对于 world 的坐标为:(%.2f,%.2f,%.2f)",point_base.point.x,point_base.point.y,point_base.point.z);

        }
        catch(const std::exception& e)
        {
            // std::cerr << e.what() << '\n';
            ROS_INFO("程序异常:%s",e.what());
            ros::Duration(1.0).sleep();
            continue;
        }


        r.sleep();  
        ros::spinOnce();
    }


    return 0;
}
/*************  发布方打印结果   ************/
[ INFO] [1633167094.481382269]: Time Send: 1633167093， x = 13.00
[ INFO] [1633167095.481386880]: Time Send: 1633167094， x = 14.00
[ INFO] [1633167096.481388511]: Time Send: 1633167095， x = 15.00
[ INFO] [1633167097.481382270]: Time Send: 1633167096， x = 16.00
/************** 订阅方打印结果 *************/
[ INFO] [1633167420.094095828]: 坐标点相对于 world 的坐标为:(13.60,0.00,0.00)
[ INFO] [1633167420.427425581]: 坐标点相对于 world 的坐标为:(13.93,0.00,0.00)
[ INFO] [1633167420.760707966]: 坐标点相对于 world 的坐标为:(14.26,0.00,0.00)
[ INFO] [1633167421.094112750]: 坐标点相对于 world 的坐标为:(14.60,0.00,0.00)
[ INFO] [1633167421.427433171]: 坐标点相对于 world 的坐标为:(14.93,0.00,0.00)
[ INFO] [1633167421.760747038]: 坐标点相对于 world 的坐标为:(15.26,0.00,0.00)
[ INFO] [1633167422.094118597]: 坐标点相对于 world 的坐标为:(15.60,0.00,0.00)

由上面的实验结果可以看到，发布方1HZ发布一次整数的tf变化，由于订阅方请求的变换频率是3HZ，导致坐标变换的结果是小数，而且有种插值的感觉。

1.2.2.3.选择TF广播中最近的时刻ros::Time()或者ros::Time(0)

使用这样的时间戳，那么在进行坐标变换的时候，会使用tf广播中最近一次发布的坐标变换，认为设置的点的时间戳和这个左边变换的时间戳一致。

这是最常用的做法，并且官方也建议这样使用，一般都能够满足需求。

1.2.3.其他消息中的时间戳

上面介绍的是坐标点，而且是带时间戳的坐标点。

在其他消息中，也有时间戳。比如要使用的Marker在rviz中绘制三维物体，如长方体、球体等等，在绘制这些物体的时候，需要指定他们的位姿，那必然要指定参考坐标系，那必然又要指定时间戳。

所以这里的处理方式和上面对坐标点的处理方式是一样的，一般情况下使用ros::Time()即可，也就是指定要绘制的物体的姿态是相对于TF广播中最近的一次坐标变换的。

问题：

上面的描述是否全部正确有待考证