tf2. Положение в пространстве

Для выполнения полезной работы либо сам робот, либо отдельные его компоненты выполняют перемещение в пространстве. Обычно мы можем ввести локальные системы координат, связанные с компонентами робота и внешним миром и образующие "дерево". Если для каждой локальной системы координат мы укажем родительскую, а также определим положение и ориентацию в заданный момент времени, то получим описание положения всей системы в пространстве.

tf2 предназначен для работы с трансформациями. Этот инструмент, решает следующие задачи:

• обмен данными о трансформациях в системе;
• хранение истории трансформаций;
• определение преобразований между двумя системами координат.

 

На самом деле использование tf2 не является чем-то обязательным. Долгое время я прекрасно обходился без этого модуля, описывая все преобразования координат самостоятельно. Однако, если вы хотите использовать другие стандартрые инструменты ROS или планируете реализовать что-то полезное для других разработчиков, стоит включить tf2 в свой арсенал.

Данный модуль содержит как библиотеки для создания кода, так и инструменты командной строки. Начнём с последних. Визуализировать структуру связей в системе позволяет view_frames: 

ros2 run tf2_tools view_frames

Какое-то время он "слушает" сообщения, после чего генерирует файл frames.pdf с графом связей и дополнительной информацией о частоте сообщений, размере буфера и других параметрах. 

Команда tf2_echo позволяет отображать данные о трансформации в реальном времени:

ros2 run tf2_ros tf2_echo фрейм_1 фрейм_2 

отображает положение фрейма_1 относительно фрейма_2. 

Мониторинг преобразований между двумя системами координат выполняет tf2_monitor:

ros2 run tf2_ros tf2_monitor фрейм_1 фрейм_2 

Программа определяет величину задержек, которые могут быть связаны с асинхронным характером данных или качеством передачи. 

Иногда бывает необходимо опубликовать преобразования, которые не меняются с течением времени (например, положение и ориентация сенсоров). Чтобы не писать код, можно воспользоваться одной из следующих команд:

ros2 run tf2_ros static_transform_publisher x y z yaw pitch roll фрейм потомок 

или 

ros2 run tf2_ros static_transform_publisher x y z qx qy qz qw фрейм потомок

В первом случае ориентация задана через углы Эйлера, во втором - с помощью кватерниона. 

Часто робот бывает описан с помощью URDF или Xacro файла, в этом случае удобно использовать robot_state_publisher. Его вызов из launch-файла может быть выполнен следующим образом 

robot_state_publisher позаботится о статических трансформациях, однако, если робот содержит суставы, которые изменяют состояние со временем, необходимо также добавить joint_state_publisher, либо самостоятельно публиковать необходимые трансформации, как будет показано дальше.

В коде tf2 может быть использован либо для публикации преобразований между смежными системами координат, либо для их прослушивания и вычисления преобразований между произвольными (не смежными) фреймами. Для обмена данными используются сообщения типа geometry_msg/msg/TransformStamped.  

Публикация может быть организована следующим образом. 

Структура кода проста, в конструкторе мы создаём объект для широковещательной публикации данных, а когда вызывается метод handle_pose, заполняем информацию о положении и ориентации frame2 относительно frame1 в текущий момент времени и транслируем полученное сообщение.

При реализации подписчика необходимо создать буфер, в котором сообщения будут храниться (буфер кольцевой, время хранения в нём ограничено). 

 

В данном примере предполагается, что обработка преобразований происходит по таймеру. Функция lookup_transform вычисляет положение и ориентацию frame2 относительно frame1 в текущий момент (аргумент rclpy.time.Time()).

Если в момент вызова нет информации обо всех промежуточных преобразованиях между frame1 и frame2, функция вызовет исключение. Если данная проблема связана с задержками, её можно решить с помощью параметра timeout, который задаёт время ожидания. Функция будет выглядеть следующим образом.

 

Вместо rclpy.time.Time() можно задавать время в прошлом (в виде self.get_clock().now() - rclpy.time.Duration(seconds=..)). Для более сложных запросов можно использовать lookup_transform_full, который позволяет находить преобразования между фреймами, разделёнными как в пространстве, так и во времени.