使用 tf listener 转换姿势在 Rviz 中不起作用

Question

我在 Rviz 中为 PR2 的 head_plate_frame 设置了一些固定的位置和方向（姿势）值，尝试将姿势从 head_plate_frame 转换为 base_link 帧并将值保存在位置和四元数数组中。但是不是去尝试块，而是正在执行except块，任何人都可以帮忙吗？这里有什么错误？

这是我的代码：

pose1 = PoseStamped()
pose1.header.stamp = rospy.Time.now()
pose1.header.frame_id = "head_plate_frame"
pose1.pose.position.x = 0.7
pose1.pose.position.y = -0.05
pose1.pose.position.z = 1.1
pose1.pose.orientation.x = 0.3
pose1.pose.orientation.y = 0.3
pose1.pose.orientation.z = 0.2
pose1.pose.orientation.w = 0.2

listener = tf.TransformListener()

try:
    position, quaternion = listener.lookupTransform("/base_link",  "/head_plate_frame", rospy.Time())

except (tf.LookupException, tf.ConnectivityException,   tf.ExtrapolationException):
    print("Not working")
    pass

Answer 1

您应该首先检查转换是否确实有效。用下面的命令查看TF树，可以看到树中其实有两个链接之间的路径：

rosrun rqt_tf_tree rqt_tf_tree

还有另一个实用程序可以执行相同的操作并将输出到 PDF 文件：

rosrun tf view_frames

一旦您确定您的树看起来符合预期，您就可以继续我的下一个建议。一般来说，我发现这些 TF 查找永远不会非常可靠。当你的节点第一次启动时，你经常会碰到那个 except 块，因为在收到足够的 TF 消息之前最初可能会有一个 LookupException，或者队列中可能有旧数据，你会得到一个 ExtrapolationException。即使您的 TF 树中的所有内容都是共生的，这些也可能发生。一种更稳健的方法是不断尝试查找，直到获得所需内容：

position = None
quaternion = None
while position is None and quaternion is None:
    try:
        position, quaternion = listener.lookupTransform("/base_link",  "/head_plate_frame", rospy.Time())
    except (tf.LookupException, tf.ConnectivityException,   tf.ExtrapolationException):
        print("Not working")
        pass

如果您这样做，我可能还建议添加超时检查，以便它只会在这个 while 循环中运行直到某个时间限制，这样您就不会等待永恒。他们在tf2 中使它更易于使用，有一个内置超时的查找调用，因此您可以取消 while 循环，它只会继续尝试查找转换，直到您的超时被命中. Here 就是一个很好的例子。