第 13 课 TF 坐标系广播与监听的编程 实现

1.机器人的坐标变换

在进行编程前，先需要了解机器人的坐标变换。这里以运行海龟案例来进行解说，步骤如下：

1. 输入指令“sudo apt-get install ros-melodic-turtle-tf”，安装功能包。
   
   其中“melodic”对应的是ROS版本。
2. 输入指令“roslaunch turtle_tf turtle_tf_demo.launch”，启动launch文件。
   
3. 输入指令“rosrun turtlesim turtle_teleop_key”，运行海龟键盘控制节点。
   
   
4. 输入指令“rosrun tf view_frames”，可视化框架。
   
   在主目录下可以找到“frames.pdf”文件，如下图：
   
   打开“frames.pdf”文件，可以看到当前系统TF坐标之间的位置关系，如下图：
   

2.创建功能包

下面，就开始创建功能包，步骤如下：

1. 输入指令“cd catkin_ws/src/”，回车，进入工作空间。
   
2. 输入指令“catkin_create_pkg tf_hiwonder rospy std_msgs”，回车，创建功能包。
   

3.编程方法

3.1 编写广播和监听程序

1. 打开Linux命令行终端。
2. 输入进入功能包所在目录的指令“roscd tf_hiwonder”，并按下回车。
   
3. 输入指令“mkdir scripts”，并按下回车，新建用于存放Python脚本的文件夹“scripts”。
   
4. 输入指令“cd scripts/”，并按下回车，进入用于存放Python脚本的文件夹“scripts”。
   
5. 输入指令“vi turtle_tf_broadcaster.py”编辑程序，复制下面程序。如需修改，再按下“i”即可修改。修改完成，按下“Esc”，输入“：wq”保存并退出。
   

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程监听turtlesim节点发布的乌龟位姿，并将其转换为tf转换消息广播出去

# 导入tf模块，这是ROS中用于处理转换的库
import tf
# 导入rospy，这是Python编写ROS节点的主要库
import rospy
# 导入turtlesim包的Pose消息类型
import turtlesim.msg

# 定义处理乌龟位姿消息的函数
def handle_turtle_pose(msg, turtlename):
    # 创建一个TransformBroadcaster对象，用于发送转换
    br = tf.TransformBroadcaster()
    # 发送转换，包括：
    # - 位置：根据乌龟的x、y坐标和固定的z坐标（0，因为turtlesim是2D的）
    # - 方向：将乌龟的角度转换成四元数，因为转换需要使用四元数来表示旋转
    # - 时间戳：当前的ROS时间
    # - 子坐标系：乌龟的名称
    # - 父坐标系：世界坐标系（通常称为"world"）
    br.sendTransform((msg.x, msg.y, 0),
                     tf.transformations.quaternion_from_euler(0, 0, msg.theta),
                     rospy.Time.now(),
                     turtlename,
                     "world")

# 程序的主入口
if __name__ == '__main__':
    # 初始化ROS节点，名为'turtle_tf_broadcaster'
    rospy.init_node('turtle_tf_broadcaster')
    # 获取私有命名空间下的'turtle'参数，这将确定我们关注的是哪个乌龟
    turtlename = rospy.get_param('~turtle')
    # 订阅乌龟的位姿话题，这个话题名称是通过拼接乌龟的名字动态确定的
    rospy.Subscriber('/%s/pose' % turtlename,
        turtlesim.msg.Pose,
        handle_turtle_pose,
        turtlename)
    # ROS的spin循环，这将保持你的程序不会退出直到节点被明确关闭
    rospy.spin()

6) 输入指令“vi turtle_tf_listener.py”编辑程序，复制下面程序。如需修改，再按下“i”即可修改。修改完成，按下“Esc”，输入“：wq”保存并退出。

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程监听turtle1的tf变换，并使turtle2向turtle1移动

# 导入所需要的模块
import roslib
import rospy
import math
import tf  # 用于处理ROS中的坐标变换
import geometry_msgs.msg  # 导入消息类型，用于发布速度
import turtlesim.srv  # 导入服务类型，用于创建新的乌龟

# 程序的主入口
if __name__ == '__main__':
    # 初始化ROS节点，名为'turtle_tf_listener'
    rospy.init_node('turtle_tf_listener')
    # 创建一个TransformListener对象，用于接收坐标变换
    listener = tf.TransformListener()
    # 等待spawn服务变得可用
    rospy.wait_for_service('spawn')
    # 创建一个服务客户端，用于调用spawn服务以生成新的乌龟
    spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
    # 调用spawn服务，在(4, 2)坐标处生成新的乌龟，角度为0，名为'turtle2'
    spawner(4, 2, 0, 'turtle2')
    
    # 创建一个发布者，用于发布turtle2的速度
    turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist, queue_size=1)
    
    # 设置循环的频率为10Hz
    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():  # 当ROS没有关闭时循环执行
        try:
            # 查询turtle2到turtle1的最新变换，返回平移和旋转信息
            (trans, rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
        # 如果在查询变换时发生异常，则继续下一次循环
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue
    
        # 根据turtle2和turtle1的相对位置计算出需要向turtle1移动的速度
        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)  # 发布速度消息，使turtle2向turtle1移动
    
        rate.sleep()  # 等待一段时间直到达到10Hz的循环频率

6) 输入指令“chmod +x turtle_tf_broadcaster.py”和“chmod +x turtle_tf_listener.py”，并按下回车，赋予文件可执行权限。


7) 输入指令“cd …”和“mkdir launch”，新建用于存放launch脚本的文件夹“launch”。


8) 输入指令“cd launch/”，并按下回车，进入用于存放Python脚本的文件夹“launch”。

9) 输入指令“vi start_tf_demo_py.launch”编辑程序，复制下面程序。如需修改，再按下“i”即可修改。修改完成，按下“Esc”，输入“：wq”保存并退出。

<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="tf_hiwonder" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
	 <param name="turtle" type="string" value="turtle1" />
	</node>
	<node name="turtle2_tf_broadcaster" pkg="tf_hiwonder" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
	 <param name="turtle" type="string" value="turtle2" /> 
	</node>
	
	 <node pkg="tf_hiwonder" type="turtle_tf_listener.py" name="listener" />
	 
</launch>

3.2 运行程序

1. 输入指令“source ./devel/setup.bash”，并按下回车，设置工作环境。
   
2. 输入指令“roslaunch tf_hiwonder start_tf_demo_py.launch”，并按下回车，运行launch程序。
   
   一只海龟自动行走到另一只海龟位置，显示如下图：
   
3. 如需终止程序，可按下快捷键“Ctrl+C”。