Система tf в ROS отслеживает несколько систем координат и поддерживает отношения между ними в древовидной структуре. tf распределяется таким образом, чтобы вся информация о координатной системе координат была доступна каждому узлу в сети ROS. MATLAB ® позволяет вам получить доступ к этому дереву преобразований. Этот пример знакомит вас с доступом к доступным системами координат, извлечением преобразований между ними и точками преобразования, векторами и другими сущностями между любыми двумя системами координат .
Необходимые условия: Запуск с ROS, подключение к сети ROS
Инициализируйте систему ROS.
rosinit
Launching ROS Core... Done in 0.68156 seconds. Initializing ROS master on http://192.168.0.10:59363. Initializing global node /matlab_global_node_63470 with NodeURI http://bat6315glnxa64:38681/
Чтобы создать реалистичное окружение для этого примера, используйте exampleHelperROSStartTfPublisher
для трансляции нескольких преобразований. Преобразования представляют камеру, которая установлена на роботе.
В этом дереве преобразований заданы три системы координат:
базовая система координат робота (robot_base
)
точка установки камеры (mounting_point
)
оптический центр камеры (camera_center
)
Публикуются два преобразования:
преобразование от основы робота к точке монтажа камеры
преобразование от точки крепления к центру камеры
exampleHelperROSStartTfPublisher
Визуальное представление трех систем координат выглядит следующим образом.
Здесь оси x , y и z каждой системы координат представлены красными, зелеными и синими линиями соответственно. Связи между родительскими объектами и их потомками элемент между системами координат показаны через коричневую стрелу, указывающую от дочернего элемента к его родительской системе координат.
Создайте новый древовидный объект преобразования с rostf
функция. Можно использовать этот объект для доступа ко всем доступным преобразованиям и применения их к различным сущностям.
tftree = rostf
tftree = TransformationTree with properties: AvailableFrames: {0x1 cell} LastUpdateTime: [0x1 Time] BufferTime: 10 DataFormat: 'object'
Когда объект создан, он начинает получать преобразования tf и буферизует их внутри. Сохраните tftree
переменная в рабочей области, чтобы она продолжала получать данные.
Пауза для небольшого бита, чтобы убедиться, что все преобразования получены.
pause(2);
Имена всех доступных систем координат можно увидеть, обратившись к AvailableFrames
свойство.
tftree.AvailableFrames
ans = 3x1 cell
{'camera_center' }
{'mounting_point'}
{'robot_base' }
Это должно показать три системы координат, которые описывают отношение между камерой, ее точкой установки и роботом.
Теперь, когда преобразования доступны, их можно просмотреть. Любое преобразование описывается ROS geometry_msgs/TransformStamped
сообщение и имеет поступательный и вращательный компонент.
Найдите преобразование, которое описывает отношение между точкой установки и центром камеры. Используйте getTransform
функция для этого.
mountToCamera = getTransform(tftree, 'mounting_point', 'camera_center'); mountToCameraTranslation = mountToCamera.Transform.Translation
mountToCameraTranslation = ROS Vector3 message with properties: MessageType: 'geometry_msgs/Vector3' X: 0 Y: 0 Z: 0.5000 Use showdetails to show the contents of the message
quat = mountToCamera.Transform.Rotation
quat = ROS Quaternion message with properties: MessageType: 'geometry_msgs/Quaternion' X: 0 Y: 0.7071 Z: 0 W: 0.7071 Use showdetails to show the contents of the message
mountToCameraRotationAngles = rad2deg(quat2eul([quat.W quat.X quat.Y quat.Z]))
mountToCameraRotationAngles = 1×3
0 90 0
Относительно точки установки центр камеры расположен на 0,5 метра вдоль оси Z и повернут на 90 степени вокруг оси Y.
Чтобы просмотреть отношение между основой робота и точкой установки камеры, вызовите getTransform
снова.
baseToMount = getTransform(tftree, 'robot_base', 'mounting_point'); baseToMountTranslation = baseToMount.Transform.Translation
baseToMountTranslation = ROS Vector3 message with properties: MessageType: 'geometry_msgs/Vector3' X: 1 Y: 0 Z: 0 Use showdetails to show the contents of the message
baseToMountRotation = baseToMount.Transform.Rotation
baseToMountRotation = ROS Quaternion message with properties: MessageType: 'geometry_msgs/Quaternion' X: 0 Y: 0 Z: 0 W: 1 Use showdetails to show the contents of the message
Точка крепления расположена на 1 метре вдоль оси X основы робота.
Предположим, что теперь у вас есть 3D точка, которая задана в camera_center
координатная система координат, и вы хотите вычислить, какие координаты точки в robot_base
системы координат.
Используйте waitForTransform
функцию, чтобы дождаться преобразования между camera_center
и robot_base
системы координат становятся доступными. Этот вызов блокируется до преобразования, которое принимает данные от camera_center
на robot_base
действителен и доступен в дереве преобразования.
waitForTransform(tftree, 'robot_base', 'camera_center');
Теперь задайте точку в положении [3 1.5 0.2]
в координатной системе координат камеры. Впоследствии вы преобразуете эту точку в robot_base
координаты.
pt = rosmessage('geometry_msgs/PointStamped'); pt.Header.FrameId = 'camera_center'; pt.Point.X = 3; pt.Point.Y = 1.5; pt.Point.Z = 0.2;
Можно преобразовать координаты точки, вызвав transform
функция на древовидном объекте преобразования. Задайте, что такое целевая координатная система координат этого преобразования. В этом примере используйте robot_base
.
tfpt = transform(tftree, 'robot_base', pt)
tfpt = ROS PointStamped message with properties: MessageType: 'geometry_msgs/PointStamped' Header: [1x1 Header] Point: [1x1 Point] Use showdetails to show the contents of the message
Преобразованная точка tfpt
имеет следующие 3D координаты.
tfpt.Point
ans = ROS Point message with properties: MessageType: 'geometry_msgs/Point' X: 1.2000 Y: 1.5000 Z: -2.5000 Use showdetails to show the contents of the message
Кроме того PointStamped
сообщений, transform
функция позволяет вам преобразовывать другие сущности, такие как положения (geometry_msgs/PoseStamped
), кватернионы (geometry_msgs/QuaternionStamped
) и облака точек (sensor_msgs/PointCloud2
).
Если вы хотите хранить преобразование, можно извлечь его с getTransform
функция.
robotToCamera = getTransform(tftree, 'robot_base', 'camera_center')
robotToCamera = ROS TransformStamped message with properties: MessageType: 'geometry_msgs/TransformStamped' Header: [1x1 Header] Transform: [1x1 Transform] ChildFrameId: 'camera_center' Use showdetails to show the contents of the message
Это преобразование может использоваться, чтобы преобразовать координаты в camera_center
система координат в координаты в robot_base
система координат.
robotToCamera.Transform.Translation
ans = ROS Vector3 message with properties: MessageType: 'geometry_msgs/Vector3' X: 1 Y: 0 Z: 0.5000 Use showdetails to show the contents of the message
robotToCamera.Transform.Rotation
ans = ROS Quaternion message with properties: MessageType: 'geometry_msgs/Quaternion' X: 0 Y: 0.7071 Z: 0 W: 0.7071 Use showdetails to show the contents of the message
Можно также транслировать новое преобразование в сеть ROS.
Предположим, что у вас есть простое преобразование, которое описывает смещение wheel
координатная рамка относительно robot_base
координатная система координат. Колесо установлено на расстоянии -0,2 метра по оси Y и -0,3 по оси Z. Колесо имеет относительный поворот на 30 степени вокруг оси Y.
Создайте соответствующую geometry_msgs/TransformStamped
сообщение, которое описывает это преобразование. Исходная система координат, wheel
, помещается в ChildFrameId
свойство. Целевая система координат, robot_base
, добавляется к Header.FrameId
свойство.
tfStampedMsg = rosmessage('geometry_msgs/TransformStamped'); tfStampedMsg.ChildFrameId = 'wheel'; tfStampedMsg.Header.FrameId = 'robot_base';
Само преобразование описано в Transform
свойство. Он содержит Translation
и a Rotation
. Введите перечисленные выше значения.
The Rotation
описывается как кватернион. Чтобы преобразовать вращение на 30 степени вокруг оси Y в кватернион, можно использовать axang2quat
(Navigation Toolbox) функция. Ось Y описывается [0 1 0]
вектор и 30 степени могут быть преобразованы в радианы с deg2rad
функция.
tfStampedMsg.Transform.Translation.X = 0; tfStampedMsg.Transform.Translation.Y = -0.2; tfStampedMsg.Transform.Translation.Z = -0.3; quatrot = axang2quat([0 1 0 deg2rad(30)])
quatrot = 1×4
0.9659 0 0.2588 0
tfStampedMsg.Transform.Rotation.W = quatrot(1); tfStampedMsg.Transform.Rotation.X = quatrot(2); tfStampedMsg.Transform.Rotation.Y = quatrot(3); tfStampedMsg.Transform.Rotation.Z = quatrot(4);
Использовать rostime
чтобы получить текущее системное время и использовать его для временной метки преобразования. Это позволяет получателям знать, в какую точку времени это преобразование было допустимым.
tfStampedMsg.Header.Stamp = rostime('now');
Используйте sendTransform
функция для широковещательной передачи этого преобразования.
sendTransform(tftree, tfStampedMsg)
Новая wheel
Теперь система координат также находится в списке доступных систем координат.
tftree.AvailableFrames
ans = 4x1 cell
{'camera_center' }
{'mounting_point'}
{'robot_base' }
{'wheel' }
Окончательное визуальное представление всех четырех систем координат выглядит следующим образом.
Вы можете увидеть, что wheel
координатная система координат имеет перемещение и поворот, которые вы задали при отправке преобразования.
Остановите издатель преобразования примера.
exampleHelperROSStopTfPublisher
Завершите работу хозяина ROS и удалите глобальный узел.
rosshutdown
Shutting down global node /matlab_global_node_63470 with NodeURI http://bat6315glnxa64:38681/ Shutting down ROS master on http://192.168.0.10:59363.