ROS - tf（二）-阿里云开发者社区

ROS - tf（一）+https://developer.aliyun.com/article/1585192

2.1.2 代码解释

现在，让我们来看看将乌龟姿势发布到 tf 的相关代码。

turtlename = rospy.get_param('~turtle')

该节点只接受一个参数 "turtle"，即指定一个乌龟名称，如 "turtle1 "或 "turtle2"。

rospy.Subscriber('/%s/pose' % turtlename,
                     turtlesim.msg.Pose,
                     handle_turtle_pose,
                     turtlename)

节点订阅主题 "turtleX/pose"，并对每条传入的信息运行函数 handle_turtle_pose。

br = tf.TransformBroadcaster()
    br.sendTransform((msg.x, msg.y, 0),
                     tf.transformations.quaternion_from_euler(0, 0, msg.theta),
                     rospy.Time.now(),
                     turtlename,
                     "world")

乌龟姿势信息的处理函数会广播乌龟的平移和旋转，并将其发布为从坐标系 "world "到坐标系 "turtleX "的变换。

2.2 运行广播器

现在为该演示创建一个启动文件。使用文本编辑器新建一个名为 launch/start_demo.launch 的文件，并添加以下几行：

<launch>
    <!-- Turtlesim Node-->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
    <node name="turtle1_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
      <param name="turtle" type="string" value="turtle1" />
    </node>
    <node name="turtle2_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
      <param name="turtle" type="string" value="turtle2" /> 
    </node>
  </launch>

现在你可以开始制作自己的海龟广播器演示了：

roslaunch learning_tf start_demo.launch

你应该看到只有一只乌龟的乌龟模拟。

2.3 检查结果

现在，使用 tf_echo 工具来检查乌龟姿势是否真正广播到了 tf：

rosrun tf tf_echo /world /turtle1

这会显示第一只乌龟的姿势。使用方向键绕着海龟行驶（确保终端窗口处于活动状态，而不是模拟器窗口）。如果运行 tf_echo 命令在世界和乌龟 2 之间进行变换，应该看不到变换，因为第二个乌龟还没有出现。不过，一旦我们在下一个教程中添加了第二只乌龟，乌龟 2 的姿势就会广播给 tf。

三、编写一个 tf 监听器（Python）

3.1 如何创建 tf 监听器

首先创建源文件。转到我们在上一教程中创建的软件包：

roscd learning_tf

3.1.1 代码

启动你最喜欢的编辑器，将以下代码粘贴到名为 nodes/turtle_tf_listener.py 的新文件中。

#!/usr/bin/env python  
import roslib
roslib.load_manifest('learning_tf')
import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srv
if __name__ == '__main__':
    rospy.init_node('turtle_tf_listener')
    listener = tf.TransformListener()
    rospy.wait_for_service('spawn')
    spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
    spawner(4, 2, 0, 'turtle2')
    turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)
    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        try:
            (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue
        angular = 4 * math.atan2(trans[1], trans[0])
        linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
        cmd = geometry_msgs.msg.Twist()
        cmd.linear.x = linear
        cmd.angular.z = angular
        turtle_vel.publish(cmd)
        rate.sleep()

别忘了让节点可执行：

chmod +x nodes/turtle_tf_listener.py

3.1.2 代码解释

现在，让我们来看看将乌龟姿势发布到 tf 的相关代码。

import tf

tf 软件包提供了一个 tf.TransformListener 实现，可帮助用户更轻松地接收变换。

listener = tf.TransformListener()

在此，我们创建了一个 tf.TransformListener 对象。一旦创建了监听器，它就会开始通过线路接收 tf 变换，并将其缓冲长达 10 秒。

rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        try:
            (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue

在这里，真正的工作已经完成，我们通过 lookupTransform 查询监听器的特定变换。让我们来看看参数：

我们希望从"/turtle1 "坐标系 ...... 转换到"/turtle2 "坐标系。

... 转换到"/turtle2 "坐标系。

我们想要变换的时间。如果提供 rospy.Time(0)，我们将得到最新的可用变换。

此函数返回两个列表。第一个是子坐标系相对于父坐标系的 (x, y, z) 线性变换，第二个是从父坐标系方向旋转到子坐标系方向所需的 (x, y, z, w) 四元数。

所有这些都包裹在一个 try-except 块中，以捕捉可能出现的异常。

3.2 运行监听器

用文本编辑器打开名为 start_demo.launch 的启动文件，并添加以下几行：

<launch>
    ...
    <node pkg="learning_tf" type="turtle_tf_listener.py" 
          name="listener" />
  </launch>

首先，确保停止了上一教程中的启动文件（使用 ctrl-c）。现在就可以开始完整的海龟演示了：

roslaunch learning_tf start_demo.launch

你应该看到有两只乌龟的乌龟模拟。

3.3 查看结果

要查看结果，只需使用方向键绕过第一只海龟（确保终端窗口处于活动状态，而不是模拟器窗口），就会看到第二只海龟跟在第一只海龟后面！

当海龟模拟器启动时，你可能会看到

[ERROR] 1253915565.300572000: Frame id /turtle2 does not exist! When trying to transform between /turtle1 and /turtle2.
[ERROR] 1253915565.401172000: Frame id /turtle2 does not exist! When trying to transform between /turtle1 and /turtle2.

出现这种情况的原因是，我们的监听器试图在收到有关海龟 2 的信息之前计算变换，因为海龟 2 在海龟模拟中生成并开始广播 tf 坐标系需要一点时间。

四、添加一个坐标系（Python）

4.1 为什么要添加坐标系

对于许多任务来说，在局部坐标系内进行思考更为容易，例如，在激光扫描仪中心的坐标系内对激光扫描进行推理更为容易。此外，tf 还会处理所有引入的额外坐标系变换。

4.2 在何处添加坐标系

tf 构建了一个树形坐标系结构；它不允许在坐标系结构中形成闭环。这意味着一个坐标系只有一个父坐标系，但可以有多个子坐标系。目前，我们的 tf 树包含三个坐标系：world、turtle1 和 turtle2。这两只乌龟是 world 的子坐标系。如果我们想在 tf 中添加一个新的坐标系，现有的三个框架中必须有一个是父坐标系，而新的坐标系将成为子坐标系。

4.3 如何添加坐标系

在我们的乌龟示例中，我们将为第一只乌龟添加一个新坐标系。这个坐标系将成为第二只乌龟的 "胡萝卜"。

首先创建源文件。转到我们为前面的教程创建的软件包：

roscd learning_tf

4.3.1 代码

启动你最喜欢的编辑器，将以下代码粘贴到名为 nodes/fixed_tf_broadcaster.py 的新文件中。

#!/usr/bin/env python  
import roslib
roslib.load_manifest('learning_tf')
import rospy
import tf
if __name__ == '__main__':
    rospy.init_node('fixed_tf_broadcaster')
    br = tf.TransformBroadcaster()
    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        br.sendTransform((0.0, 2.0, 0.0),
                         (0.0, 0.0, 0.0, 1.0),
                         rospy.Time.now(),
                         "carrot1",
                         "turtle1")
        rate.sleep()

别忘了让节点可执行：

chmod +x nodes/fixed_tf_broadcaster.py

代码与 tf 广播员教程中的示例非常相似。只是这里的变换不会随时间而改变。

4.3.2 代码解释

让我们来看看这段代码中的关键行：

br.sendTransform((0.0, 2.0, 0.0),
                         (0.0, 0.0, 0.0, 1.0),
                         rospy.Time.now(),
                         "carrot1",
                         "turtle1")

在这里，我们创建一个新的变换，从父代 "turtle1 "变换到新的子代 "carrot1"。胡萝卜 1 "坐标系与 "乌龟 1 "坐标系相距 2 米。

4.4 运行坐标系广播器

编辑 start_demo.launch 启动文件。只需添加以下一行即可：

<launch>
    ...
    <node pkg="learning_tf" type="fixed_tf_broadcaster.py"
          name="broadcaster_fixed" />
  </launch>

首先，确保停止了上一教程中的启动文件（使用 Ctrl-c）。现在就可以开始海龟广播员演示了：

roslaunch learning_tf start_demo.launch

ROS - tf（三）+https://developer.aliyun.com/article/1585199

ROS - tf（二）