Question

我在Rviz中为PR2的head_plate_frame设置了一些固定的位置和方向（姿势）值，试图将姿势从head_plate_frame转换为base_link帧，并将这些值保存在位置和四元数数组中。但是除了执行try块以外，其他块都没有执行，有谁能帮忙？这里的错误是什么？

这是我的代码：

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查找从来都不是非常可靠的。当您的节点首次启动时，您经常会碰到该例外，因为最初可能会出现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

如果您这样做，我可能还建议您添加一个超时检查，以便它仅在循环到某个时间限制时才通过此检查，这样您就不必等待永恒。他们使它在tf2中更好用了，内置了一个带有超时的查找调用，因此您可以取消while循环，它将一直尝试查找转换直到超时被击中。 Here是一个很好的例子。

在tvi侦听器中转换姿势在Rviz中不起作用

1 个答案: