Запуск

Инициализируйте систему 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 для трансляции нескольких преобразований. Преобразования представляют камеру, которая установлена на роботе.

В этом дереве преобразований заданы три системы координат:

Публикуются два преобразования:

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 координатная система координат имеет перемещение и поворот, которые вы задали при отправке преобразования.

Остановите издатель примера и завершите работу сети ROS

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

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.