产品服务 Github 技术交流 激光雷达 ROS教程 深度学习 机器视觉

ROS tf 变换中的时间戳问题


  • administrators

    我们从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()