TF 坐标变换(已整理)

文章目录

2. Python实现

动态坐标变换

2. Python实现

坐标msg消息

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 坐标A相对于坐标B的欧拉角(三个角)
float64 x 四元数，旋转
float64 y
float64 z
float64 w

geometry_msgs/PointStamped
传输某个坐标系内坐标点的信息
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Point point
float64 x
float64 y
float64 z

静态坐标变换

静态坐标变换，是指两个坐标系之间的相对位置是固定的。
问题背景：
现有一机器人模型，核心构成包含主体与雷达，各对应一坐标系，坐标系的原点分别位于主体与雷达的物理中心，已知雷达原点相对于主体原点位移关系如下: x 0.2 y0.0 z0.5。当前雷达检测到一障碍物，在雷达坐标系中障碍物的坐标为 (2.0 3.0 5.0),请问，该障碍物相对于主体的坐标是多少？

建文件
tf01_static
roscpp rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs

查看
rostopic list
rostopic echo /tf_static

图形化设置
rviz

transforms:
header:
seq: 0
stamp:
secs: 1658132852
nsecs: 825164513
frame_id: “base_link”
child_frame_id: “laser”
transform:
translation:
x: 0.2
y: 0.0
z: 0.5
rotation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0

1. C++实现

发布方 demo01_static_pub.cpp

#include"ros/ros.h"
 #include"tf2_ros/static_transform_broadcaster.h"
 #include"geometry_msgs/TransformStamped.h"
 #include"tf2/LinearMath/Quaternion.h"/*需求： 发布两个坐标系的相对关系流程：1. 包含头文件2. 设置编码 节点初始化 NodeHandle3. 创建发布对象4. 组织被发布的消息5. 发布数据6. spin()
 */int main(int argc, char *argv[])
 {// 2. 设置编码 节点初始化 NodeHandlesetlocale(LC_ALL,"");ros::init(argc,argv,"static_pub");ros::NodeHandle nh;// 3. 创建发布对象 静态坐标转换广播器，对应头文件2tf2_ros::StaticTransformBroadcaster pub;// 4. 组织被发布的消息，对应头文件3geometry_msgs::TransformStamped tfs;// 创建坐标系信息tfs.header.stamp = ros::Time::now();tfs.header.frame_id = "base_link";// 相对坐标系关系中被参考的那个，基坐标系tfs.child_frame_id = "laser";//雷达坐标系tfs.transform.translation.x = 0.2;//雷达x偏移值tfs.transform.translation.y = 0.0;//雷达y偏移值tfs.transform.translation.z = 0.5;//雷达z偏移值// 4.1 需要根据欧拉角转换 对应头文件4tf2::Quaternion qtn; // 创建四元数对象// 4.2 向该对象设置欧拉角，这个对象可以将欧拉角转换成四元数qtn.setRPY(0,0,0);// 设置偏航值，俯仰值，翻滚值，单位是弧度tfs.transform.rotation.x = qtn.getX();tfs.transform.rotation.y = qtn.getY();tfs.transform.rotation.z = qtn.getZ();tfs.transform.rotation.w = qtn.getW();// 5. 发布数据pub.sendTransform(tfs);// 6. spin()ros::spin();return 0;
 }
 

订阅方 demo02_static_sub.cpp

#include"ros/ros.h"
 #include"tf2_ros/transform_listener.h"
 #include"tf2_ros/buffer.h"
 #include"geometry_msgs/PointStamped.h"
 #include"tf2_geometry_msgs/tf2_geometry_msgs.h"/*订阅方: 订阅发布的坐标系相对关系，传入一个坐标系，调用tf实现转换流程:1. 包含头文件2. 编码 初始化 NodeHandle(必须)3. 创建订阅对象: ---> 订阅坐标系相对关系4. 组织一个坐标点数据5. 转换算法，需要调用tf内置实现6. 最后输出
 */int main(int argc, char *argv[])
 {// 2. 编码 初始化 NodeHandle(必须)setlocale(LC_ALL,"");ros::init(argc,argv,"static_sub");ros::NodeHandle nh;// 3. 创建订阅对象: ---> 订阅坐标系相对关系 头文件23// 3.1 创建一个buffer缓存tf2_ros::Buffer buffer;// 3.2 再创建监听对象(监听对象可以将订阅的数据存入buffer)tf2_ros::TransformListener listener(buffer);// 4. 组织一个坐标点数据 头文件4geometry_msgs::PointStamped ps;ps.header.stamp = ros::Time::now();ps.header.frame_id = "laser";// 坐标系 障碍物位置相对于雷达坐标系而言ps.point.x = 2.0; // 障碍物相对于雷达坐标系的位置ps.point.y = 3.0;ps.point.z = 5.0;// 添加休眠 防止订阅方没有订阅到发布方消息ros::Duration(2).sleep();// 5. 转换算法，需要调用tf内置实现ros::Rate rate(1);while(ros::ok()){// 核心代码 ---- 将 ps 转换成相对于 base_link的坐标点 头文件5geometry_msgs::PointStamped ps_out; /*调用buffer(数据存在缓存buffer里面)的转换函数 transform参数1: 被转换的坐标点参数2: 目标坐标系返回值: 输出的坐标点注意1: 调用时必须包含头文件 tf2_geometry_msgs/tf2_geometry_msgs.h注意2: 运行时存在的问题，抛出一个异常 base_link不存在原因:订阅数据是一个耗时操作，可能调用transform转换函数时，坐标系相对关系还没有订阅到，因此出现异常解决:方案1: 在调用转换函数时，执行休眠方案2: 进行异常处理*/ps_out = buffer.transform(ps,"base_link"); // 目标点信息，基参考系，输出转换后的坐标// 6. 最后输出 ROS_INFO("转换后的坐标值:(%.2f,%.2f,%.2f),参考的坐标系:%s",ps_out.point.x,ps_out.point.y,ps_out.point.z,ps.header.frame_id.c_str());rate.sleep();ros::spinOnce();}return 0;
 }
 

2. Python实现

发布方 demo01_static_pub_p.py

#! /usr/bin/env python"""发布方: 发布两个坐标系的相对关系(车辆底盘 --- base_link 和 雷达 --- laser)流程:1. 导包2. 初始化节点3. 创建发布对象4. 组织被发布的数据5. 发布数据6. spin()
 """import rospy
 import tf2_ros
 import tf
 from geometry_msgs.msg import TransformStampedif __name__ == "__main__":# 2. 初始化节点rospy.init_node("start_pub_p")# 3. 创建发布对象 头文件2pub = tf2_ros.StaticTransformBroadcaster()# 4. 组织被发布的数据 头文件4tfs = TransformStamped()# 4.1 headertfs.header.stamp = rospy.Time.now()tfs.header.frame_id = "base_link"# 4.2 child frametfs.child_frame_id = "laser"# 4.3 相对关系(偏移 与 四元数)tfs.transform.translation.x = 0.2tfs.transform.translation.y = 0.0tfs.transform.translation.z = 0.5# 4.4 先从欧拉角转换成四元数 头文件3qtn = tf.transformations.quaternion_from_euler(0,0,0)# 4.5 再设置四元数tfs.transform.rotation.x = qtn[0]tfs.transform.rotation.y = qtn[1]tfs.transform.rotation.z = qtn[2]tfs.transform.rotation.w = qtn[3]# 5. 发布数据pub.sendTransform(tfs)# 6. spin()rospy.spin()
 

订阅方 demo02_static_sub_p.py

#! /usr/bin/env python"""订阅方: 订阅坐标变换消息，传入被转换的坐标，调用转换算法流程:1. 导包2. 初始化3. 创建订阅对象4. 组织被转换的坐标点5. 转换逻辑实现，调用tf封装的算法6. 输出结果
 """import rospy
 import tf2_ros
 from tf2_geometry_msgs import tf2_geometry_msgsif __name__ == "__main__":# 2. 初始化rospy.init_node("static_sub_p")# 3. 创建订阅对象# 3.1 创建一个缓存对象 头文件2buffer = tf2_ros.Buffer()# 3.2 创建订阅对象(将缓存传入)sub = tf2_ros.TransformListener(buffer)# 4. 组织被转换的坐标点 头文件3ps = tf2_geometry_msgs.PointStamped()ps.header.stamp = rospy.Time.now()ps.header.frame_id = "laser"ps.point.x = 2.0ps.point.y = 3.0ps.point.z = 5.0# 5. 转换逻辑实现，调用tf封装的算法rate = rospy.Rate(1)while not rospy.is_shutdown():# 使用try防止try:# 转换实现"""参数1: 被转换的坐标点参数2: 目标坐标系返回值: 转换后的坐标点PS:问题: 抛出异常 base_link 不存在原因: 转换函数调用时，可能还没有订阅到坐标系的相对关系解决: try 捕获异常，并处理"""ps_out = buffer.transform(ps,"base_link")# 6. 输出结果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 e:rospy.logwarn("错误提示:%s",e) rate.sleep()
 

当坐标系之间的相对位置固定时，那么所需参数也是固定的: 父系坐标名称、子级坐标系名称、x偏移量、y偏移量、z偏移量、x 翻滚角度、y俯仰角度、z偏航角度，实现逻辑相同，参数不同，那么 ROS 系统就已经封装好了专门的节点，摄像头相对于基坐标系位置
rosrun tf2_ros static_transform_publisher 0.1 0.0 0.3 0 0 0 /base_link /camera

rviz
依次是偏航角，俯仰角，翻滚角

偏航：Yaw
俯仰：Pitch
翻滚：Roll
X轴：红色(翻滚角)正值 顺时针转动；负值 逆时针转动
Y轴：绿色(俯仰角)
Z轴：蓝色(偏航角)

动态坐标变换

话题：/turtle1/pose
消息类型：turtlesim/Pose
消息格式：

float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity

获取话题: rostopic list

rosrun turtlesim turtlesim_node
rostopic list

/rosout
/rosout_agg
/turtle1/cmd_vel
/turtle1/color_sensor
/turtle1/pose 位姿发布使用的话题

获取消息类型：rostopic info /turtle1/pose

turtle1/pose
Type: turtlesim/Pose 消息类型
Publishers: * /turtlesim (http://rosnoetic-VirtualBox:37261/)
Subscribers: None

获取消息格式: rosmsg info turtlesim/Pose

float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity

1. C++实现

发布方 demo01_dynamic_pub.cpp

#include"ros/ros.h"
 #include"turtlesim/Pose.h"
 // 创建发布对象
 #include"tf2_ros/transform_broadcaster.h"
 // 组织发布数据
 #include"geometry_msgs/TransformStamped.h"
 // 坐标系四元数
 #include"tf2/LinearMath/Quaternion.h"
 /*发布方: 需要订阅乌龟的位姿信息, 转换成相对于窗体的坐标关系，并发布准  备: 话题： /turtle1/pose消息： turtlesim/Pose流程:1. 包含头文件2. 设置编码，初始化， NodeHandle3. 创建订阅对象，订阅/turtle1/pose4. 回调函数处理订阅信息: 将位姿信息转换成坐标相对关系并发布(关注)5. spin()*/// 给传进回调函数的数据定义范型 头文件2
 void doPose(const turtlesim::Pose::ConstPtr& pose){// 获取位姿信息，转换成坐标系相对关系(核心)，并发布// a. 创建发布对象 头文件3static tf2_ros::TransformBroadcaster pub;// b. 组织被发布的数据 头文件4geometry_msgs::TransformStamped ts;ts.header.frame_id = "world"; // 相对于世界坐标系ts.header.stamp = ros::Time::now();ts.child_frame_id = "turtle1";// 坐标系偏移量ts.transform.translation.x = pose->x;ts.transform.translation.y = pose->y;ts.transform.translation.z = 0; // z一直是零（2D）// 坐标系四元数 头文件5/*位姿信息中没有四元数，但是有偏航角度，又已知乌龟时2D，没有翻滚和俯仰角，所以得出乌龟的欧拉角: 0 0 theta*/tf2::Quaternion qtn;qtn.setRPY(0,0,pose->theta);ts.transform.rotation.x = qtn.getX();ts.transform.rotation.y = qtn.getY();ts.transform.rotation.z = qtn.getZ();ts.transform.rotation.w = qtn.getW();// c. 发布pub.sendTransform(ts);}int main(int argc, char *argv[])
 {// 2. 设置编码，初始化， NodeHandlesetlocale(LC_ALL,"");ros::init(argc,argv,"dynamic_pub");ros::NodeHandle nh;// 3. 创建订阅对象，订阅/turtle1/poseros::Subscriber sub = nh.subscribe("/turtle1/pose",100,doPose);// 4. 回调函数处理订阅信息: 将位姿信息转换成坐标相对关系并发布(关注)// 5. spin()ros::spin();return 0;
 }
 

订阅方 demo02_dynamic_sub.cpp

roscore
rosrun turtlesim turtlesim_node
rosrun tf02_dynamic demo01_dynamic_pub
rosrun tf02_dynamic demo02_dynamic_sub

#include"ros/ros.h"
 #include"tf2_ros/transform_listener.h"
 #include"tf2_ros/buffer.h"
 #include"geometry_msgs/PointStamped.h"
 #include"tf2_geometry_msgs/tf2_geometry_msgs.h"/*订阅方: 订阅发布的坐标系相对关系，传入一个坐标系，调用tf实现转换流程:1. 包含头文件2. 编码 初始化 NodeHandle(必须)3. 创建订阅对象: ---> 订阅坐标系相对关系4. 组织一个坐标点数据5. 转换算法，需要调用tf内置实现6. 最后输出
 */int main(int argc, char *argv[])
 {// 2. 编码 初始化 NodeHandle(必须)setlocale(LC_ALL,"");ros::init(argc,argv,"dynamic_sub");ros::NodeHandle nh;// 3. 创建订阅对象: ---> 订阅坐标系相对关系 头文件23// 3.1 创建一个buffer缓存tf2_ros::Buffer buffer;// 3.2 再创建监听对象(监听对象可以将订阅的数据存入buffer)tf2_ros::TransformListener listener(buffer);// 4. 组织一个坐标点数据 头文件4geometry_msgs::PointStamped ps;// 参考的坐标系ps.header.frame_id = "turtle1";// 坐标系 物体位置相对于乌龟坐标系而言// 时间戳ps.header.stamp = ros::Time(0.0);ps.point.x = 2.0; // 物体相对于乌龟坐标系的位置ps.point.y = 3.0;ps.point.z = 5.0;// 添加休眠 防止订阅方没有订阅到发布方消息(这个方法不建议用，最好使用try方法// ros::Duration(2).sleep();// 5. 转换算法，需要调用tf内置实现ros::Rate rate(1);while(ros::ok()){// 核心代码 ---- 将 ps 转换成相对于 base_link的坐标点 头文件5geometry_msgs::PointStamped ps_out; /*调用buffer(数据存在缓存buffer里面)的转换函数 transform参数1: 被转换的坐标点参数2: 目标坐标系返回值: 输出的坐标点注意1: 调用时必须包含头文件 tf2_geometry_msgs/tf2_geometry_msgs.h注意2: 运行时存在的问题，抛出一个异常 base_link不存在原因:订阅数据是一个耗时操作，可能调用transform转换函数时，坐标系相对关系还没有订阅到，因此出现异常解决:方案1: 在调用转换函数时，执行休眠方案2: 进行异常处理*/try{ps_out = buffer.transform(ps,"world"); // 目标点信息，基参考系，输出转换后的坐标 头文件5r// 6. 最后输出 ROS_INFO("转换后的坐标值:(%.2f,%.2f,%.2f),参考的坐标系:%s",ps_out.point.x,ps_out.point.y,ps_out.point.z,ps.header.frame_id.c_str());}catch(const std::exception& e){ROS_INFO("异常消息:%s",e.what());}rate.sleep();ros::spinOnce();}return 0;
 }
 

2. Python实现

发布方 demo01_dynamic_sub_p.py

roscore
rosrun turtlesim turtlesim_node
cd demo02_ws/
source ./devel/setup.bash
rosrun tf02_dynamic demo01_dynamic_pub_p.py

/rosout
/rosout_agg
/tf
/turtle1/cmd_vel
/turtle1/color_sensor
/turtle1/pose

rostopic echo /tf 查看具体信息

ransforms:
header:
seq: 0
stamp:
secs: 1658287751
nsecs: 917598485
frame_id: “world”
child_frame_id: “turtle1”
transform:
translation:
x: 5.544444561004639
y: 5.544444561004639
z: 0.0
rotation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0

#! /usr/bin/env python"""发布方: 需要订阅乌龟的位姿信息, 转换成相对于窗体的坐标关系，并发布准  备: 话题： /turtle1/pose消息： turtlesim/Pose流程:1. 包含头文件2. 初始化ROS节点3. 创建订阅对象，订阅/turtle1/pose4. 回调函数处理订阅信息: 将位姿信息转换成坐标相对关系并发布(关注)5. spin()
 """import rospy
 from turtlesim.msg import Pose
 import tf2_ros
 from geometry_msgs.msg import TransformStamped
 import tf
 def doPose(pose):# 1. 创建发布坐标系相对关系的对象 头文件3pub = tf2_ros.TransformBroadcaster()# 2. 将 pose 转换成 坐标系相对关系消息 头文件4ts = TransformStamped()ts.header.frame_id = "world"ts.header.stamp = rospy.Time.now()ts.child_frame_id = "turtle1"# 2.1 子级坐标系相对于父级坐标系的偏移量ts.transform.translation.x = pose.xts.transform.translation.y = pose.yts.transform.translation.z = 0# 2.2 四元数 头文件5# 从欧拉角转换四元数"""乌龟是2D，不存在 x 上的翻滚和 y 上的俯仰，只有 z 上的偏航0 0 pose.theta"""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]# 3. 发布pub.sendTransform(ts)if __name__ == "__main__":# 2. 初始化ROS节点rospy.init_node("dynamic_pub_p")# 3. 创建订阅对象 头文件2sub = rospy.Subscriber("/turtle1/pose",Pose,doPose,queue_size = 100)# /tur是话题名称# 4. 回调函数处理订阅信息# 5. spin()rospy.spin()
 

订阅方 demo02_dynamic_sub_p.py

roscore
cd demo02_ws/
rosrun turtlesim turtlesim_node
source ./devel/setup.bash
rosrun tf02_dynamic demo01_dynamic_pub_p.py
rosrun tf02_dynamic demo02_dynamic_sub_p.py
控制乌龟运动
rosrun turtlesim turtle_teleop_key

#! /usr/bin/env python"""订阅方: 订阅坐标变换消息，传入被转换的坐标，调用转换算法流程:1. 导包2. 初始化3. 创建订阅对象4. 组织被转换的坐标点5. 转换逻辑实现，调用tf封装的算法6. 输出结果
 """import rospy
 import tf2_ros
 from tf2_geometry_msgs import tf2_geometry_msgsif __name__ == "__main__":# 2. 初始化rospy.init_node("dynamic_sub_p")# 3. 创建订阅对象# 3.1 创建一个缓存对象 头文件2buffer = tf2_ros.Buffer()# 3.2 创建订阅对象(将缓存传入)sub = tf2_ros.TransformListener(buffer)# 4. 组织被转换的坐标点 头文件3ps = tf2_geometry_msgs.PointStamped()# 时间戳--0ps.header.stamp = rospy.Time()# 坐标系ps.header.frame_id = "turtle1"ps.point.x = 2.0ps.point.y = 3.0ps.point.z = 5.0# 5. 转换逻辑实现，调用tf封装的算法rate = rospy.Rate(1)while not rospy.is_shutdown():# 使用try防止try:# 转换实现"""参数1: 被转换的坐标点参数2: 目标坐标系返回值: 转换后的坐标点PS:问题: 抛出异常 base_link 不存在原因: 转换函数调用时，可能还没有订阅到坐标系的相对关系解决: try 捕获异常，并处理"""ps_out = buffer.transform(ps,"world")# 6. 输出结果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 e:rospy.logwarn("错误提示:%s",e) rate.sleep()