0. 简介

我们上面讲了最基础的通信机制以及在Matlab中如何使用这些通信，下面我们这一讲来主要介绍ROS当中最常用的TF坐标系在Matlab中的使用。tf是分布式的，因此所有的坐标帧信息对ROS网络中的每个节点都是可用的。所以这一节就是带领读者熟悉该怎么在Matlab中使用ROS的TF坐标系

1. TF概念

要为这个示例创建一个真实的环境，可以使用exampleHelperROSStartTfPublisher来传播几个转换。这里通过转换来表示安装在机器人上的摄像机的位姿。

在这个变换树中定义了三个坐标坐标系:

  1. 机器人基本框架(robot_base)
  2. 相机的安装点(mounting_point)
  3. 相机的光学中心(camera_center)

同时会发布两个转换:

  1. 从机器人底座到相机安装点的转换
  2. 从安装点到相机中心的转换
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’ }

这里显示三个坐标框架，描述相机、其安装点和机器人之间的关系。

2. 获取TF坐标系

现在TF转换是可用的，同时可以检查它们。任何转换都是由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

这里表示相对于安装点，摄像机中心位于沿z轴0.5米的位置，并围绕y轴旋转90度。

3. 发布TF坐标系

假设现在在camera_center坐标框架中定义了一个3D点，并且希望计算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

如果您想存储一个转换，您可以使用上面描述的getTransform函数检索它。

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

然后就可以使用rostime检索当前系统时间，并使用该时间戳对转换进行时间戳。这让接收者知道这个转换在哪个时间点是有效的。我们仍然用上面的示意

pt = rosmessage('geometry_msgs/PointStamped');
pt.ChildFrameId = 'wheel';
pt.Header.FrameId = 'camera_center';
pt .Transform.Translation.X = 0;
pt .Transform.Translation.Y = -0.2;
pt .Transform.Translation.Z = -0.3;
quatrot = axang2quat([0 1 0 deg2rad(30)])
pt .Transform.Rotation.W = quatrot(1);
pt .Transform.Rotation.X = quatrot(2);
pt .Transform.Rotation.Y = quatrot(3);
pt .Transform.Rotation.Z = quatrot(4);
pt.Header.Stamp = rostime('now');

然后使用sendTransform函数来广播这个转换

sendTransform(tftree, tfStampedMsg)
tftree.AvailableFrames

ans = 4x1 cell
{‘camera_center’ }
{‘mounting_point’}
{‘robot_base’ }
{‘wheel’ }

4. 参考链接

https://ww2.mathworks.cn/help/ros/ug/access-the-tf-transformation-tree-in-ros.html

https://www.ncnynl.com/archives/201909/3296.html