Доступ к tf Дереву Преобразования в ROS

tf система в ROS отслеживает несколько координат, структурирует и поддерживает отношения между ними в древовидной структуре. tf распределяется, так, чтобы вся координатная информация о системе координат была доступна для каждого узла в сети ROS. MATLAB® позволяет вам получить доступ к этому дереву преобразования. Этот пример ознакомляет вас с доступом к доступным координатным системам координат, получая преобразования между ними, и преобразуйте точки, векторы и другие сущности между любыми двумя координатными системами координат.

Предпосылки: Запуск с ROS, соединитесь с сетью ROS

Запуск

Инициализируйте систему ROS.

rosinit
Initializing ROS master on http://bat6306glnxa64:44263/.
Initializing global node /matlab_global_node_62039 with NodeURI http://bat6306glnxa64:38445/

Чтобы создать реалистическую среду для этого примера, используйте 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

Если объект создается, он начинает получать tf преобразования и буферизует их внутренне. Сохраните tftree переменная в рабочей области так, чтобы это продолжило получать данные.

Сделайте паузу для немного, чтобы убедиться, что все преобразования получены.

pause(1);

Вы видите имена всех доступных координатных систем координат путем доступа к AvailableFrames свойство.

tftree.AvailableFrames
ans = 3x1 cell array
    {'camera_center' }
    {'mounting_point'}
    {'robot_base'    }

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

Получите преобразования

Теперь, когда преобразования доступны, можно смотреть их. Любое преобразование описано geometry_msgs/TransformStamped ROS обменивайтесь сообщениями и имеет поступательный и вращательный компонент.

Получите преобразование, которое описывает отношение между точкой монтирования и центром камеры. Используйте 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]
    ChildFrameId: 'camera_center'
       Transform: [1x1 Transform]

  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 и Rotation. Заполните значения, которые упоминаются выше.

Rotation описан как кватернион. Чтобы преобразовать 30 вращений степени вокруг оси Y к кватерниону, можно использовать axang2quat функция. Ось 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 array
    {'camera_center' }
    {'mounting_point'}
    {'robot_base'    }
    {'wheel'         }

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

Вы видите что wheel координатная система координат имеет перевод и вращение, которое вы задали в отправке преобразования.

Остановите издателя в качестве примера и закрытие сеть ROS

Остановите издателя преобразования в качестве примера.

exampleHelperROSStopTfPublisher

Закройте ведущее устройство ROS и удалите глобальный узел.

rosshutdown
Shutting down global node /matlab_global_node_62039 with NodeURI http://bat6306glnxa64:38445/
Shutting down ROS master on http://bat6306glnxa64:44263/.
Для просмотра документации необходимо авторизоваться на сайте