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

第 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)# 设置循环的频率为10Hzrate = 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”。