python - 使用 tf 监听器转换姿势在 Rviz 中不起作用

Question

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

这是我的代码:

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 block ，因为最初可能会出现 LookupException，直到收到足够的 TF 消息，或者队列中可能有旧数据，您将得到 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就是一个很好的例子。