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