Запуск

Инициализируйте систему 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'    }

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

Преобразования приема

Теперь, когда преобразования доступны, их можно проверить. Любое преобразование описывается АФК 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 и 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
    {'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.