Halo，这里是Ppeua。平时主要更新C++，数据结构算法…感兴趣就关注我吧！你定不会失望。
0.ROS中的坐标转换消息包
在日常生活中，特别是对于机器人来说，各个目标系中的坐标转换是很关键的，通过右手系来标注坐标。
ROS中提供了坐标转换的软件包 Transform Frame TF的作用是ROS中实现不同坐标点/向量的转换。
不过TF在若干个版本前已经弃用，现在使用的是全新的版本：TF2，其有几个相关的功能包：
1.tf2_geometry_msgs:可以将ROS消息转换成tf2消息。
2.tf2: 封装了坐标变换的常用消息。
3.tf2_ros:为tf2提供了roscpp和rospy绑定，封装了坐标变换常用的API。
在坐标系转换中，在geometry下有两个重要的消息类型：TransformStamped、PointStamped,前者用于坐标系间的转换，后者用于点之间的坐标转换，这对我们之后的使用很重要。先来了解下这两种消息类型中的内容。
0.1 geometry_msgs/TransformStamped
该消息类型表示坐标系之间的关系
在终端中输入
rosmsg info geometry_msgs/TransformStamped
查看该消息类型的具体信息：
std_msgs/Header header # 头信息 uint32 seq ## 序列号 time stamp ## 时间戳 string frame_id ## 坐标 string child_frame_id # 子坐标 geometry_msgs/Transform transform #坐标信息 geometry_msgs/Vector3 translation ##偏移量 float64 x float64 y float64 z geometry_msgs/Quaternion rotation #四元数(欧拉角) float64 x float64 y float64 z float64 w
可以看出 PointStamped消息是由:
std_msgs/Header,string,geometry_msgs/Transform封装在一起，组成的新消息类型。
其中Transform又是由geometry_msgs/Vector3,geometry_msgs/Quaternion进行封装的。
0.2 geometry_msgs/PointStamped
该消息类型表示坐标点之间的转换
在终端中输入
rosmsg info geometry_msgs/PointStamped
可以查看该消息中的具体信息
std_msgs/Header header #头信息 uint32 seq ##序列号 time stamp ##时间戳 string frame_id ##坐标系 geometry_msgs/Point point #点坐标 float64 x float64 y float64 z
可以看出 PointStamped消息是由:
std_msgs/Header与geometry_msgs/Point封装在一起，组成的新消息类型。
1.静态坐标转换
现有一机器人模型，核心构成包含主体与雷达，各对应一坐标系，坐标系的原点分别位于主体与雷达的物理中心，已知雷达原点相对于主体原点位移关系如下: x 0.2 y0.0 z0.5。当前雷达检测到一障碍物，在雷达坐标系中障碍物的坐标为 (2.0 3.0 5.0),请问，该障碍物相对于主体的坐标是多少？
组织下我们发布方的整体逻辑:
- 导入所需功能包
- 初始化ros节点
- 创建静态坐标广播器
- 编写静态坐标信息
- 发送消息
- spin()
这里是接收方的逻辑：
- 导入所需要的功能包
- 初始化ros节点
- 创建TF订阅对象
- 创建lase的坐标点
- 坐标转换
- spin（）
1.1导入所需功能包
在这个案例中，需要:rospy,std_msgs 这两个标准件
还需要：
tf2:封装了坐标变换的常用消息。
tf2_ros: 为tf2提供了roscpp和rospy绑定，封装了坐标变换常用的API。
tf2_geometry_msgs:可以将ROS消息转换成tf2消息。
1.2发布方实现
""" 导入功能包 """ import tf2_ros from geometry_msgs.msg import TransformStamped import tf import rospy """ 初始化节点信息 创建发布对象 组织发布数据 发布数据 spin() """ #初始化ros节点 rospy.init_node("static_pub") #创建静态发布对象 pub=tf2_ros.StaticTransformBroadcaster() #组织消息类型 ts=TransformStamped() ts.header.seq=123 ts.header.stamp=rospy.Time.now() ts.child_frame_id="laser" ts.header.frame_id="frame_id" ts.transform.translation.x=0.2 ts.transform.translation.y=0 ts.transform.translation.z=0.5 """ 将欧拉角放到四元数中进行转换 用到了tf中的transformation.quaternion_from_euler """ qtn=tf.transformations.quaternion_from_euler(0,0,0) ts.transform.rotation.x=qtn[0] ts.transform.rotation.y=qtn[1] ts.transform.rotation.z=qtn[2] ts.transform.rotation.w=qtn[3] #发布消息 pub.sendTransform(tf) rospy.spin()
1.3 订阅方实现
""" 导入功能包 """ import rospy from tf2_geometry_msgs import tf2_geometry_msgs import tf2_ros #初始化节点 rospy.init_node("static_sub") #创建缓存对象 buffer=tf2_ros.Buffer() """ 调用tf2_ros.Buffer()创建一个buffer用来存储坐标消息 """ tf2_ros.TransformListener(buffer) """ 监听tf坐标变换,将值存入buffer中 """ """ 创建点坐标信息 """ ps=tf2_geometry_msgs.PointStamped() ps.header.stamp=rospy.Time.now() ps.header.frame_id="laser" ps.point.x=2.0 ps.point.y=3.0 ps.point.z=5.0 rate=rospy.Rate(10) while not rospy.is_shutdown(): try: """ 调用buffer.transform 将点坐标与原始坐标进行转换 """ ps_out=buffer.transform(ps,"frame_id") rospy.loginfo("转换后的坐标：（%.2f,%.2f,%.2f）,参考坐标系：%s", ps_out.point.x, ps_out.point.y, ps_out.point.z, ps_out.header.frame_id) except Exception as ee: rospy.logwarn("错误提示%s",ee) rate.sleep()
1.4 tf2_ros实现静态坐标转换
由于静态坐标转换中的整体逻辑大致相同,所以tf2_ros提供了一个功能包来直接实现坐标转换,不需要每次都使用编写代码
rosrun tf2_ros static_transform_publisher x偏移量 y偏移量 z偏移量 z偏航角度 y俯仰角度 x翻滚角度 父级坐标系 子级坐标系
2.动态坐标转换
在现实生活中，我们面对的不仅有点对点的坐标转换，还动态的坐标转换。
我们以乌龟为例来实现一下动态坐标转换
先来组织下发布方的逻辑
1.导包 rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs turtlesim
2.初始化ros节点
3.订阅 /turtle1/pose 话题消息
4.回调函数
1.创建TF广播器
2.组织广播数据
3.广播器发布数据
5.spin
接收方的逻辑
6.导包
7.初始化ros节点
8.创建TF对象
9.处理订阅数据
2.1发布方实现
import rospy from turtlesim.msg import Pose import tf2_ros from geometry_msgs.msg import TransformStamped import tf """ 订阅乌龟的位姿信息 """ def doPose(pose): #创建动态坐标发布对象 pub=tf2_ros.TransformBroadcaster() #组织点坐标消息类型 ts=TransformStamped() ts.header.frame_id="world" ts.child_frame_id="turtle1" ts.header.stamp=rospy.Time.now() #坐标系相对于子集坐标系 ts.transform.translation.x=pose.x ts.transform.translation.y=pose.y ts.transform.translation.z=0 #四元数转换 qtn=tf.transformations.quaternion_from_euler(0,0,pose.theta) ts.transform.rotation.x=qtn[0] ts.transform.rotation.y=qtn[1] ts.transform.rotation.z=qtn[2] ts.transform.rotation.w=qtn[3] pub.sendTransform(ts) #初始化ROS节点 rospy.init_node("tf02_pub") #订阅消息位姿信息，创建回调函数 sub=rospy.Subscriber("/turtle1/pose",Pose,doPose,queue_size=100) rospy.spin()
2.2订阅方逻辑
import rospy import tf2_ros # 不要使用 geometry_msgs,需要使用 tf2 内置的消息类型 from tf2_geometry_msgs import PointStamped # from geometry_msgs.msg import PointStamped if __name__ == "__main__": # 2.初始化 ROS 节点 rospy.init_node("static_sub_tf_p") # 3.创建 TF 订阅对象 buffer = tf2_ros.Buffer() # 监听坐标变换存入buffer中 tf2_ros.TransformListener(buffer) rate = rospy.Rate(1) while not rospy.is_shutdown(): # 4.创建坐标点信息 # 仅需提供目标坐标系 point_source = PointStamped() point_source.header.frame_id = "turtle1" point_source.header.stamp = rospy.Time.now() try: # 5.调研订阅对象的 API 将 4 中的点坐标转换成相对于 world 的坐标 point_target = buffer.transform(point_source,"world",rospy.Duration(1)) rospy.loginfo("转换结果:x = %.2f, y = %.2f, z = %.2f", point_target.point.x, point_target.point.y, point_target.point.z) except Exception as e: rospy.logerr("异常:%s",e) # 6.spin rate.sleep()
2.3实现效果
首先启动turtlesim的键盘控制节点与GUI
rosrun turtlesim turtlesim_node rosrun turtlesim turtle_teleop_key
接着启动发布方与接收方 之后就可以在屏幕上看到转换后的坐标系
rosrun tf02_dynamic demo01_tf02_pub.py rosrun tf02_dynamic demo01_tf02_sub.py
3.0多坐标转换
将多个坐标先相对于世界坐标系进行转换,然后在调用api将转换后的数据进行相互转换
3.1发布方实现
直接调用静态坐标转换的ros包,写成launch文件
<launch> <node pkg="tf2_ros" type="static_transform_publisher" name="son1" args="0.2 0.8 0.3 0 0 0 /world /son1" output="screen" /> <node pkg="tf2_ros" type="static_transform_publisher" name="son2" args="0.5 0 0 0 0 0 /world /son2" output="screen"/> </launch>
3.2订阅方实现
订阅方逻辑实现
1.导入包 rospy std_msgs tf2_ros geometry_msgs(TransformStamped 坐标系转换) tf2_geomerty_msgs(PointStamped 坐标点转换)
2.初始化ros节点
3.创建TF订阅对象,实现两个坐标系之间相互转换
import rospy from tf2_geometry_msgs import tf2_geometry_msgs import tf2_ros from geometry_msgs.msg import TransformStamped rospy.init_node("static_sub") #创建缓存对象 buffer=tf2_ros.Buffer() sub=tf2_ros.TransformListener(buffer) rate=rospy.Rate(10) while not rospy.is_shutdown(): try: """ 计算son1相对于son2的坐标关系 lookup_transform(父级坐标系，子级坐标系，取坐标的时间，时间间隔) """ ts=buffer.lookup_transform("son2","son1",rospy.Time(0)) rospy.loginfo("父级坐标系：%s，子级坐标系：%s,%.2f,%.2f,%.2f", ts.header.frame_id, ts.child_frame_id, ts.transform.translation.x, ts.transform.translation.y, ts.transform.translation.z ) except Exception as ee: pass rospy.logwarn("错误提示%s",ee) rate.sleep()
3.3 view_frames查看当前坐标系
运行以上节点后，在任意工作目录下输入
rosrun tf2_tools view_frames.py
会在当前目录下生成一个可以坐标关系的pdf，可以利用此工具查看坐标关系
请添加图片描述
4.0 tf坐标变换实操
我们先来创建turtle，运行turtlesim这个节点
rosrun turtlesim turtlesim_node
通过rosservice的/spawn服务来多生成一只turtle来完成我们的多坐标转换,生成一只名为H的乌龟
rosservice call /spawn "x: 0.0 y: 0.0 theta: 0.0 name: ''"
若返回输入的名字，此时就能在屏幕上看到刚刚生成的那只乌龟
准备工作都做完了，现在开始创建坐标系
4.1乌龟位姿信息发布
先来理清整个跟随的逻辑：
1.在坐标系中发布两只乌龟的信息
2.将第二只乌龟的位姿信息相对第一只乌龟作转换
3.控制cmd发布速度信息
import rospy import sys from turtlesim.msg import Pose import tf2_ros from geometry_msgs.msg import TransformStamped import tf def doPose(pose): pub=tf2_ros.TransformBroadcaster() ts=TransformStamped() ts.header.frame_id="world" ts.header.stamp=rospy.Time.now() ts.child_frame_id=turtle_name ts.transform.translation.x=pose.x ts.transform.translation.y=pose.y qtn=tf.transformations.quaternion_from_euler(0,0,pose.theta) ts.transform.rotation.x=qtn[0] ts.transform.rotation.y=qtn[1] ts.transform.rotation.z=qtn[2] ts.transform.rotation.w=qtn[3] pub.sendTransform(ts) rospy.init_node("dynamic_pub",anonymous=True) if len(sys.argv)>=2: turtle_name=sys.argv[1] sub=rospy.Subscriber(turtle_name+"/pose",Pose,doPose,queue_size=10) rospy.spin() else: print(sys.argv[1]) rospy.loginfo("请输入坐标名称") sys.exit()
这份代码出现过很多次了，这里就不过多赘述。注意：sys.argv的第一个参数为文件名 之后的为传入参数
4.2 控制乌龟进行跟随运动
总体逻辑：
1.计算两个乌龟之间的相对坐标
2.控制乌龟的线速度与角速度
3.发布
import rospy from tf2_geometry_msgs import tf2_geometry_msgs import tf2_ros from geometry_msgs.msg import TransformStamped,Twist import math import sys """ 创建订阅对象 组织被转换的坐标点 转换逻辑实现调用tf封装的算法 输出结果 """ rospy.init_node("static_sub") #创建缓存对象 buffer=tf2_ros.Buffer() sub=tf2_ros.TransformListener(buffer) pub=rospy.Publisher("/H/cmd_vel",Twist,queue_size=10) rate=rospy.Rate(10) while not rospy.is_shutdown(): try: """ 计算son1相对于son2的坐标关系 直接监听整个坐标系，不需要订阅话题 """ ts=buffer.lookup_transform("H","turtle1",rospy.Time(0)) rospy.loginfo("父级坐标系：%s，子级坐标系：%s,%.2f,%.2f,%.2f", ts.header.frame_id, ts.child_frame_id, ts.transform.translation.x, ts.transform.translation.y, ts.transform.translation.z ) twist=Twist() twist.linear.x=0.5*math.sqrt(math.pow(ts.transform.translation.x,2)+math.pow(ts.transform.translation.y,2)) twist.angular.z=4*math.atan2(ts.transform.translation.y,ts.transform.translation.x) pub.publish(twist) except Exception as ee: pass rospy.logwarn("错误提示%s",ee) rate.sleep()
4.3查看当前坐标关系
rosrun tf2_tools view_frames.py
