文章目录
- 第 13 课 TF 坐标系广播与监听的编程 实现
-
- 1.机器人的坐标变换
- 2.创建功能包
- 3.编程方法
-
- 3.1 编写广播和监听程序
- 3.2 运行程序
第 13 课 TF 坐标系广播与监听的编程 实现
1.机器人的坐标变换
在进行编程前,先需要了解机器人的坐标变换。这里以运行海龟案例来进行解说,步骤如下:
- 输入指令“sudo apt-get install ros-melodic-turtle-tf”,安装功能包。
其中“melodic”对应的是ROS版本。 - 输入指令“roslaunch turtle_tf turtle_tf_demo.launch”,启动launch文件。
- 输入指令“rosrun turtlesim turtle_teleop_key”,运行海龟键盘控制节点。
- 输入指令“rosrun tf view_frames”,可视化框架。
在主目录下可以找到“frames.pdf”文件,如下图:
打开“frames.pdf”文件,可以看到当前系统TF坐标之间的位置关系,如下图:
2.创建功能包
下面,就开始创建功能包,步骤如下:
- 输入指令“cd catkin_ws/src/”,回车,进入工作空间。
- 输入指令“catkin_create_pkg tf_hiwonder rospy std_msgs”,回车,创建功能包。
3.编程方法
3.1 编写广播和监听程序
- 打开Linux命令行终端。
- 输入进入功能包所在目录的指令“roscd tf_hiwonder”,并按下回车。
- 输入指令“mkdir scripts”,并按下回车,新建用于存放Python脚本的文件夹“scripts”。
- 输入指令“cd scripts/”,并按下回车,进入用于存放Python脚本的文件夹“scripts”。
- 输入指令“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 运行程序
- 输入指令“source ./devel/setup.bash”,并按下回车,设置工作环境。
- 输入指令“roslaunch tf_hiwonder start_tf_demo_py.launch”,并按下回车,运行launch程序。
一只海龟自动行走到另一只海龟位置,显示如下图:
- 如需终止程序,可按下快捷键“Ctrl+C”。