< >
Home » ROS与Matlab入门教程 » ROS与Matlab语言入门教程-访问ROS中的tf变换树

ROS与Matlab语言入门教程-访问ROS中的tf变换树

ROS中的“tf”系统保持对多坐标系的跟踪并维护坐标系之间的关系于树型结构中。“tf”被分发，所以在ROS网络中的有关所有坐标系的信息每个节点都可以获得。MATLAB允许用户获得该变换树。本例程将让用户熟悉如何获取坐标系信息、获取坐标系之间的变换关系和在两个坐标系之间点、向量和其它实体的变换。
预备知识：1.2开始使用ROS，1.3连接到ROS网络。

启动

初始化ROS系统：

rosinit
Initializing ROS master on http://bat5136glnxa64.mathworks.com:11311/.
Initializing global node /matlab_global_node_8332 with NodeURI http://bat5136glnxa64:50092/

使用“exampleHelperROSStartTfPublisher”发布若干变换，为本例建立真实的环境这些变换表示安装在机器人上的相机。在这个变换树中，总共有三个坐标系。

  • 机器人基准坐标（robot_base）
  • 相机的安装位置（mounting_point）
  • 相机的光学中心（camera_center）

发布两个变换：从机器人基准坐标到相机安装位置的变换和从相机的安装位置到相机的光学中心的变换。
exampleHelperROSStartTfPublisher
三个坐标系的可视化如图10.1所示。

请输入图片描述

此处，每个坐标系的x，y和z坐标轴分别用红色、绿色和蓝色表示。坐标系之间的父子关系由棕色箭头表示，由子坐标系指向父坐标系。
使用“rostf”函数创建一个新的变换树对象，用户可使用该对象获得所有可用的变换并应用于其他实体。

tftree = rostf
tftree =
TransformationTree with properties:

AvailableFrames: {0x1 cell}
 LastUpdateTime: [0x1 Time]

一旦变换树的对象创建，它便开始接受“tf”变换并缓存，用户可以将“tftree”保持在工作空间中，让它继续接收数据。
停顿一下，确保所有的变换都已经接收：
pause(1);
用户可以使用“AvailableFrames”属性查看所有可用坐标系的名称：

tftree.AvailableFrames
ans =

'camera_center'
'mounting_point'
'robot_base'

这将会显示三个坐标系，描述相机中心、相机安装位置和机器人之间的变换关系。

接收变换

用户可以确认可用变换，任何变换都由ROS消息“geometry_msgs/TransformStamped”描述，包含平移和旋转两个部分。
使用“getTransform”函数，获取描述相机安装位置和相机光学中心关系的信息：

mountToCamera = getTransform(tftree, 'mounting_point', 'camera_center');
mountToCameraTranslation = mountToCamera.Transform.Translation
quat = mountToCamera.Transform.Rotation
mountToCameraRotationAngles = rad2deg(quat2eul([quat.W quat.X quat.Y quat.Z]))
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 =
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 =

   0    90     0

相对于相机安装位置，相机中心在z轴方向的0.5米处，绕y轴旋转90度。
再次使用“getTransform”函数，查看机器人基准坐标到相机安装位置的相对关系。

baseToMount = getTransform(tftree, 'robot_base', 'mounting_point');
baseToMountTranslation = baseToMount.Transform.Translation
baseToMountRotation = baseToMount.Transform.Rotation
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 =
ROS Quaternion message with properties:

MessageType: 'geometry_msgs/Quaternion'
      X: 0
      Y: 0
      Z: 0
      W: 1

相机安装位置在机器人基准坐标的x轴的1米处。

应用变换

假设有一个在“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]

变换的点“tfpt”有如下的3维坐标：

tfpt.Point
ans =
ROS Point message with properties:

MessageType: 'geometry_msgs/Point'
      X: 1.2000
      Y: 1.5000
      Z: -2.5000

除了“PointStamped”消息，“transform ”函数还允许用户转换其它类型的实体，例如：姿态“”，四元数“”，和点云“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]

该变换能够将“camera_center”坐标系的坐标转换到“robot_base”坐标系下的坐标。

robotToCamera.Transform.Translation
robotToCamera.Transform.Rotation
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
ans =
ROS Quaternion message with properties:

MessageType: 'geometry_msgs/Quaternion'
      X: 0
      Y: 0.7071
      Z: 0
      W: 0.7071

发送变换

用户还可以发布一个新的变换到ROS网络。
假设用户有一个简单的变换描述“wheel”坐标系到“robot_base”坐标系的偏移。轮子安装在y轴的-0.2m，z轴的-0.3米处，在y轴上有30°的旋转。
创建一个相应的“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”被描述成四元数，用户可以调用“axang2quat”函数，实现绕y轴选择30°到四元数的转换。

tfStampedMsg.Transform.Translation.X = 0;
tfStampedMsg.Transform.Translation.Y = -0.2;
tfStampedMsg.Transform.Translation.Z = -0.3;
quatrot = axang2quat([0 1 0 deg2rad(30)])
tfStampedMsg.Transform.Rotation.W = quatrot(1);
tfStampedMsg.Transform.Rotation.X = quatrot(2);
tfStampedMsg.Transform.Rotation.Y = quatrot(3);
tfStampedMsg.Transform.Rotation.Z = quatrot(4);
quatrot =

0.9659         0    0.2588         0

使用“rostime”函数获取当前系统时间，并作为变换的时间戳，让接收者知道该变换可用的时间点。

tfStampedMsg.Header.Stamp = rostime('now');

使用“sendTransform”函数发布该变换：

sendTransform(tftree, tfStampedMsg)

现在，新的坐标系“wheel”也在可用坐标系的列表中：

tftree.AvailableFrames
ans =

'camera_center'
'mounting_point'
'robot_base'
'wheel'

最终，四个坐标系的可视化如图10.2所示：

请输入图片描述

图10.2
用户可以看到“wheel”坐标系具有刚才发布的变换的平移和旋转。

停止示例发布器和关闭ROS网络

停止例程的变换发布器：

exampleHelperROSStopTfPublisher

停止ROS主控节点并删除全局节点：

rosshutdown
Shutting down global node /matlab_global_node_8332 with NodeURI http://bat5136glnxa64:50092/
Shutting down ROS master on http://bat5136glnxa64.mathworks.com:11311/.

纠错,疑问,交流: 请进入讨论区或 点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: ros与matlab语言入门教程