Трансформирующая поза с помощью tf listener не работает в Rviz - PullRequest
Новая
       64

Трансформирующая поза с помощью tf listener не работает в Rviz

0 голосов
/ 27 января 2019

Я установил некоторые фиксированные значения положения и ориентации (позы) для head_plate_frame PR2 в Rviz, пытаясь преобразовать позу из head_plate_frame во фрейм base_link и сохранить значения в массивах позиции и кватернионов.Но вместо того, чтобы попробовать блок, выполняется блок кроме, кто-нибудь может помочь?В чем здесь ошибка?

Вот мой код: 

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

1 Ответ

0 голосов
/ 29 января 2019

Вы должны сначала проверить, что преобразование действительно допустимо. Посмотрите на дерево 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, и он будет просто пытаться искать преобразование до ваш тайм-аут Здесь является хорошим примером этого.

...