ROS tf 变换中的时间戳问题

[weijiz]

我们从ros wiki 上tf变换的第一个例子说起

#!/usr/bin/env python  
import roslib
roslib.load_manifest('learning_tf')
import rospy

import tf
import turtlesim.msg

def handle_turtle_pose(msg, turtlename):
    br = tf.TransformBroadcaster()
    br.sendTransform((msg.x, msg.y, 0),
                     tf.transformations.quaternion_from_euler(0, 0, msg.theta),
                     rospy.Time.now(),
                     turtlename,
                     "world")

if __name__ == '__main__':
    rospy.init_node('turtle_tf_broadcaster')
    turtlename = rospy.get_param('~turtle')
    rospy.Subscriber('/%s/pose' % turtlename,
                     turtlesim.msg.Pose,
                     handle_turtle_pose,
                     turtlename)
    rospy.spin()

上面的代码是根据小海龟的位置发布一个从小海龟到 world 的 tf 变换。理想情况下这样是没有问题的，但是实际使用中，这样写就会产生问题。问题的根源就在这个tf的时间戳上

br.sendTransform((msg.x, msg.y, 0),
                     tf.transformations.quaternion_from_euler(0, 0, msg.theta),
                     rospy.Time.now(),
                     turtlename,
                     "world")

这里的程序tf时间戳用的是当前时间，也就是处理这个位置信息时的时间。但是如果我们的机器人性能比较弱，可能处理这个位置信息的时间和位置信息实际产生的时间差挺多。这样当其他数据需要坐标系转换的时候，就会采用错误的tf数据从而产生误差。我们以具体的数字为例
比如我们现在在1s的时刻得到小海龟的位置是(0，0), 小海龟在以1m/s的速度沿X轴直线运动。我们的小海龟性能很弱。消息处理的时间和实际位置消息产生的时间差了1s。我们现在通过超声波测量到了小海龟前面1m处有一个障碍物。我们想知道这个障碍物在world坐标系下的坐标。我们首先找到当前时刻附近的tf变换，但是这个tf变换实际上是根据1s之前小海龟的位置计算的，当时小海龟的位置是(-1,0)。所以根据这个tf变换我们计算出这个障碍物的位置坐标是(0,0)。如果用这个数据去做路径规划，肯定出现的是错误的结果。

当然上面是一个夸张的例子，但是实际确实会因此引入误差。那么正确的做法是什么呢？
以消息产生的时间戳作为tf变换的时间戳。因为坐标变换是根据消息计算出来的，所以其有效的时刻就应该是消息有效的时刻。所以上面的例子应该写为

#!/usr/bin/env python  
import roslib
roslib.load_manifest('learning_tf')
import rospy

import tf
import turtlesim.msg

def handle_turtle_pose(msg, turtlename):
    br = tf.TransformBroadcaster()
    br.sendTransform((msg.x, msg.y, 0),
                     tf.transformations.quaternion_from_euler(0, 0, msg.theta),
                     msg.header.timestamp, # 由于这里订阅的是Pose并不是PoseStamped所以没有时间戳。这里只作为示例
                     turtlename,
                     "world")

if __name__ == '__main__':
    rospy.init_node('turtle_tf_broadcaster')
    turtlename = rospy.get_param('~turtle')
    rospy.Subscriber('/%s/pose' % turtlename,
                     turtlesim.msg.Pose,
                     handle_turtle_pose,
                     turtlename)
    rospy.spin()