ROS学习笔记—TF2监听器和广播器(Python)_ros2 python tf监听指定消息-优快云博客

根据官网教程的建议，直接学习tf2。

一、tf2

机器人系统通常有许多随时间变化的3D坐标系，例如世界坐标系、基础坐标系、夹持器坐标系、头部坐标系等。 tf2 随时间跟踪所有这些坐标系，并允许您提出以下问题：

5 秒前，头部坐标系相对于世界坐标系在哪里？
夹具中的物体相对于我的基础坐标系的姿势是什么？
全局坐标系中基础坐标系的当前位姿是什么？

tf2 可以在分布式系统中运行。这意味着系统中任何计算机上的所有 ROS 组件都可以使用有关机器人坐标系的所有信息。tf2 可以与包含所有转换信息的中央服务器一起运行，或者您可以让分布式系统中的每个组件构建自己的转换信息数据库。

二、编写tf2广播器

1、在工作空间下创建名为 learning_tf2 的功能包

$ catkin_create_pkg learning_tf2 tf2 tf2_ros roscpp rospy turtlesim

2、功能包中创建节点文件夹(nodes)

 $ roscd learning_tf2

 $ mkdir nodes

使用vim或其他编辑器创建文件nodes/static_turtle_tf2_broadcaster.py

#!/usr/bin/env python  
import rospy

# Because of transformations
import tf_conversions

import tf2_ros
import geometry_msgs.msg
import turtlesim.msg


def handle_turtle_pose(msg, turtlename):
    br = tf2_ros.TransformBroadcaster()
    t = geometry_msgs.msg.TransformStamped()

    t.header.stamp = rospy.Time.now()
    t.header.frame_id = "world"
    t.child_frame_id = turtlename
    #turtle姿势消息的handler函数广播这个turtle的平移和旋转，
    #并将其作为从坐标系“world”到坐标系“turtleX”的转换发布。

    t.transform.translation.x = msg.x
    t.transform.translation.y = msg.y
    t.transform.translation.z = 0.0
    q = tf_conversions.transformations.quaternion_from_euler(0, 0, msg.theta)
    t.transform.rotation.x = q[0]
    t.transform.rotation.y = q[1]
    t.transform.rotation.z = q[2]
    t.transform.rotation.w = q[3]
    #在这里，将3D海龟姿势的信息复制到3D变换中。

    br.sendTransform(t)
    #这才是真正的工作。发送带有transformbroadcasting的转换只需要传入转换本身。

if __name__ == '__main__':
    rospy.init_node('tf2_turtle_broadcaster')
    turtlename = rospy.get_param('~turtle')   
    #该节点接受单个参数“turtle”，用于指定turtle名称，例如。“turtle1”或“turtle2”。

    rospy.Subscriber('/%s/pose' % turtlename,          
                     turtlesim.msg.Pose,
                     handle_turtle_pose,
                     turtlename)
    #该节点订阅主题“turtleX/pose”，并对每个传入消息运行函数handle_turtle_pose。
    
    rospy.spin()

三、编写tf2监听器

1、在功能包learning_tf2 下创建文件nodes/static_turtle_tf2_broadcaster.py

#!/usr/bin/env python  
import rospy

import math
import tf2_ros
#tf2_ros包提供了一个tf2_ros的实现。TransformListener以帮助简化接收转换的任务。
import geometry_msgs.msg
import turtlesim.srv

if __name__ == '__main__':
    rospy.init_node('tf2_turtle_listener')

    tfBuffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(tfBuffer)
    #在这里，我们创建一个tf2_ros。TransformListener对象。
    #一旦创建了侦听器，它就开始通过网络接收tf2转换，并将它们缓冲长达10秒。

    rospy.wait_for_service('spawn')
    spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
    turtle_name = rospy.get_param('turtle', 'turtle2')
    spawner(4, 2, 0, turtle_name)

    turtle_vel = rospy.Publisher('%s/cmd_vel' % turtle_name, geometry_msgs.msg.Twist, queue_size=1)

    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        try:
            trans = tfBuffer.lookup_transform(turtle_name, 'turtle1', rospy.Time())
            #查询特定转换的侦听器（想要这个帧的变换，到这个坐标系这个坐标系，转变的时间）
        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
            rate.sleep()
            continue

        msg = geometry_msgs.msg.Twist()

        msg.angular.z = 4 * math.atan2(trans.transform.translation.y, trans.transform.translation.x)
        msg.linear.x = 0.5 * math.sqrt(trans.transform.translation.x ** 2 + trans.transform.translation.y ** 2)

        turtle_vel.publish(msg)

        rate.sleep()

四、运行

启动文件（Launch File）是ROS中一种同时启动多个节点的途径，它还可以自动启动ROS Master节点管理器，并且可以实现每个节点的各种配置，为多个节点的操作提供很大便利。

编写launch文件start_demo.launch

  <launch>
      <!-- 海龟仿真器 -->
      <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
      
      <!-- 键盘控制 -->
      <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>

    <!-- 两只海龟的tf2广播 -->
    <node name="turtle1_tf2_broadcaster" pkg="learning_tf2" type="turtle_tf2_broadcaster.py" respawn="false" output="screen" >
      <param name="turtle" type="string" value="turtle1" />
    </node>
    <node name="turtle2_tf2_broadcaster" pkg="learning_tf2" type="turtle_tf2_broadcaster.py" respawn="false" output="screen" >
      <param name="turtle" type="string" value="turtle2" /> 
    </node>

    <!-- 监听tf2广播，并且控制turtle2移动 -->
    <node pkg="learning_tf2" type="turtle_tf2_listener.py" name="listener" output="screen"/>

  </launch>

$ roslaunch learning_tf2 start_demo.launch

便可看到两只乌龟，并且通过键盘控制turtle1移动，turtle2也跟随移动。

使用 tf_echo 工具在 TF 树中查找乌龟坐标系之间的变换关系:

$ rosrun tf tf_echo turtle1 turtle2

也可以通过rviz的图形界面更加形象地看到这三者之间的坐标关系:

$ rosrun rviz rviz -d `rospack find turtle_tf2`/rviz/turtle_rviz.rviz