ROS 学习记录（五）——TF 坐标变换
文章目录
创建一个包 catkin_create_pkg tf_demo geometry_msgs roscpp rospy std_msgs tf2 tf2_geometry_msgs tf2_ros
坐标消息
坐标消息有两种常用类型，geometry_msgs/TransformStamped 和 geometry_msgs/PoseStamped。
1$ rosmsg info geometry_msgs/TransformStamped
2std_msgs/Header header
3 uint32 seq
4 time stamp
5 string frame_id
6string child_frame_id
7geometry_msgs/Transform transform
8 geometry_msgs/Vector3 translation
9 float64 x
10 float64 y
11 float64 z
12 geometry_msgs/Quaternion rotation
13 float64 x
14 float64 y
15 float64 z
16 float64 w
translation 中的三个值表示位移，rotation 中的四个值表示四元数，可以用来表示旋转。
1$ rosmsg info geometry_msgs/PointStamped
2std_msgs/Header header
3 uint32 seq
4 time stamp
5 string frame_id
6geometry_msgs/Point point
7 float64 x
8 float64 y
9 float64 z
TransformStamped 用于表示两个坐标系之间的相对关系，而 PointStamped 用于表示某个坐标系中的某个点的位置。两者组合使用，即可表示某个坐标系中的某个点在另一个坐标系中的位置。
静态坐标转换
静态坐标转中，两个坐标系的相对位置不发生改变，只需要知道某点在 A 坐标系中的位置，以及 A 坐标系到 B 坐标系的相对位置，就可以计算出 B 坐标系中的点的位置。
C++ 实现静态坐标转换
广播方代码：
1#include "ros/ros.h"
2#include "tf2/LinearMath/Quaternion.h"
3#include "tf2_ros/static_transform_broadcaster.h"
4
5int main(int argc, char **argv) {
6 setlocale(LC_ALL, "");
7 ros::init(argc, argv, "static_broadcaster_tf");
8
9 tf2_ros::StaticTransformBroadcaster broadcaster; // 创建静态坐标转换广播器
10 geometry_msgs::TransformStamped ts; // 创建坐标信息
11
12 // 设置头信息
13 ts.header.stamp = ros::Time::now(); // 设置时间辍
14 ts.header.frame_id = "base_link"; // 设置发送者的坐标系名称
15
16 ts.child_frame_id = "laser"; // 设置子级坐标系，一个 TransformStamped
17 // 对象只能有一个子级坐标系
18
19 // 设置子级坐标系相对父坐标系的偏移量
20 ts.transform.translation.x = 1;
21 ts.transform.translation.y = 2;
22 ts.transform.translation.z = 3;
23
24 tf2::Quaternion qtn; // 创建四元数对象
25 qtn.setRPY(0, 0, 0); // 设置 Roll、Pitch、Yaw 角度
26
27 ts.transform.rotation.x = qtn.getX();
28 ts.transform.rotation.y = qtn.getY();
29 ts.transform.rotation.z = qtn.getZ();
30 ts.transform.rotation.w = qtn.getW();
31
32 broadcaster.sendTransform(ts); // 广播坐标信息
33 ros::spin();
34 return 0;
35}
接收方代码：
1#include "ros/ros.h"
2#include "tf2_geometry_msgs/tf2_geometry_msgs.h" // 调用 transform 必须包含该头文件
3#include "tf2_ros/transform_listener.h"
4
5int main(int argc, char **argv) {
6 setlocale(LC_ALL, "");
7 ros::init(argc, argv, "static_listener_tf");
8 ros::NodeHandle nh;
9 ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,
10 ros::console::levels::Debug); // 设置日志输出级别为 Debug
11
12 // 创建 TF 订阅对象并监听
13 tf2_ros::Buffer buffer;
14 tf2_ros::TransformListener listener(buffer);
15
16 ros::Rate rate(1);
17 while (ros::ok()) // 只要 ROS Master 存活
18 {
19 // 创建子级坐标系中的座标点
20 geometry_msgs::PointStamped point_laser;
21 point_laser.header.frame_id = "laser";
22 point_laser.header.stamp = ros::Time::now();
23 point_laser.point.x = 1.1;
24 point_laser.point.y = 2.2;
25 point_laser.point.z = 3.3;
26
27 // 用 try-catch 防止接收延时导致坐标转换失败
28 try {
29 // 创建新的座标点，用于接收转换后的坐标
30 geometry_msgs::PointStamped point_base = buffer.transform(point_laser, "base_link");
31 ROS_DEBUG("\nTransformed position:\nx=%.2f\ny=%.2f\nCoordinate:\n%s\n",
32 point_base.point.x,
33 point_base.point.y,
34 point_base.header.frame_id.c_str());
35 } catch (const tf2::TransformException &ex) {
36 ROS_WARN("Failed to transform.");
37 }
38
39 rate.sleep();
40 ros::spinOnce();
41 }
42
43 return 0;
44}
Python 实现静态坐标转换
广播方代码：
1import rospy
2import tf
3import tf2_ros
4from geometry_msgs.msg import TransformStamped
5
6
7def main():
8 rospy.init_node("static_broadcaster_tf")
9
10 broadcaster = tf2_ros.StaticTransformBroadcaster() # 创建静态坐标转换广播器
11 ts = TransformStamped() # 创建坐标信息
12
13 ts.header.stamp = rospy.Time.now() # 设置时间辍
14 ts.header.frame_id = "base_link" # 设置发送者的坐标系名称
15
16 ts.child_frame_id = "laser" # 设置子级坐标系
17
18 # 设置子级坐标系相对父坐标系的偏移量
19 ts.transform.translation.x = 1
20 ts.transform.translation.y = 2
21 ts.transform.translation.z = 3
22
23 qtn = tf.transformations.quaternion_from_euler(
24 0, 0, 0
25 ) # 创建四元数对象，并分别设置 RPY 角度
26
27 ts.transform.rotation.x = qtn[0]
28 ts.transform.rotation.y = qtn[1]
29 ts.transform.rotation.z = qtn[2]
30 ts.transform.rotation.w = qtn[3]
31
32 broadcaster.sendTransform(ts) # 广播器发送坐标
33 rospy.spin()
34
35
36if __name__ == "__main__":
37 main()
接收方代码：
1import logging
2
3import rospy
4import tf2_ros
5from tf2_geometry_msgs import PointStamped # 使用 tf2 内置的 PointStamped
6
7
8def main():
9 rospy.init_node("static_listener_tf")
10 logging.getLogger("rosout").setLevel(logging.DEBUG) # 修改日志级别
11
12 # 创建 TF 订阅对象并且监听
13 buffer = tf2_ros.Buffer()
14 listener = tf2_ros.TransformListener(buffer)
15
16 rate = rospy.Rate(1)
17 while not rospy.is_shutdown():
18
19 # 创建子级坐标系中的座标点
20 point_laser = PointStamped()
21 point_laser.header.frame_id = "laser"
22 point_laser.header.stamp = rospy.Time.now()
23 point_laser.point.x = 1.1
24 point_laser.point.y = 2.2
25 point_laser.point.z = 3.3
26
27 try:
28 point_base = buffer.transform(point_laser, "base_link")
29 rospy.logdebug(
30 "\nTransformed position:\nx=%.2f\ny=%.2f\nCoordinate:\n%s\n",
31 point_base.point.x,
32 point_base.point.y,
33 point_base.header.frame_id,
34 )
35 except tf2_ros.TransformException:
36 rospy.logwarn("Failed to transform.")
37 rate.sleep()
38
39
40if __name__ == "__main__":
41 main()
动态坐标转换
动态坐标转换中，两个坐标系之间的相对位置会发生改变，需要实时计算。动态坐标转换的实质是坐标变换的积分，即将坐标转换的过程分解为多个离散的小步骤，并将每个步骤的结果线性组合，得到最终的转换结果。在动态坐标转换中，我们使用 turtlesim_node 作为测试对象，编写一个随机移动节点 random_track 用于模拟动态坐标，并且使用 rviz 对坐标转换结果进行可视化。最终效果如下：
C++ 实现动态坐标转换
发送方代码：
1#include "geometry_msgs/TransformStamped.h"
2#include "ros/ros.h"
3#include "tf2/LinearMath/Quaternion.h"
4#include "tf2_ros/transform_broadcaster.h"
5#include "turtlesim/Pose.h"
6
7/*
8 回调函数监听 turtlesim_node 的 Pose 消息，然后通过 Pose
9 中的参数创建子级坐标系
10*/
11void callback(const turtlesim::Pose::ConstPtr &pose) {
12 static tf2_ros::TransformBroadcaster
13 broadcaster; // 声明为 static 以确保程序生命周期中只会创建一个 TransformBroadcaster 对象
14
15 // 创建 TransformStamped 对象并配置
16 geometry_msgs::TransformStamped ts;
17 ts.header.frame_id = "world";
18 ts.header.stamp = ros::Time::now();
19 ts.child_frame_id = "turtle1";
20
21 ts.transform.translation.x = pose->x;
22 ts.transform.translation.y = pose->y;
23 ts.transform.translation.z = 0.0; // 平面上 z 为 0
24
25 tf2::Quaternion qtn;
26 qtn.setRPY(0.0, 0.0, pose->theta);
27 ts.transform.rotation.x = qtn.getX();
28 ts.transform.rotation.y = qtn.getY();
29 ts.transform.rotation.z = qtn.getZ();
30 ts.transform.rotation.w = qtn.getW();
31
32 broadcaster.sendTransform(ts);
33}
34
35int main(int argc, char **argv) {
36 setlocale(LC_ALL, "");
37 ros::init(argc, argv, "dynamic_broadcaster_tf");
38 ros::NodeHandle nh;
39 ros::Subscriber subscriber = nh.subscribe<turtlesim::Pose>("/turtle1/pose", 100, callback);
40 ros::spin();
41 return 0;
42}
接收方代码：
1#include "geometry_msgs/PointStamped.h"
2#include "ros/ros.h"
3#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
4#include "tf2_ros/buffer.h"
5#include "tf2_ros/transform_listener.h"
6
7int main(int argc, char **argv) {
8 setlocale(LC_ALL, "");
9 ros::init(argc, argv, "dynamic_listener_tf");
10 ros::NodeHandle nh;
11 ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug);
12
13 // 创建 TF 订阅对象并且监听
14 tf2_ros::Buffer buffer;
15 tf2_ros::TransformListener listener(buffer);
16
17 ros::Rate rate(1);
18 while (ros::ok()) {
19 // 生成子级坐标系的座标点并配置
20 geometry_msgs::PointStamped point_laser;
21 point_laser.header.frame_id = "turtle1";
22 point_laser.header.stamp = ros::Time();
23 point_laser.point.x = 1;
24 point_laser.point.y = 3;
25 point_laser.point.z = 0;
26
27 // 座标点转换
28 try {
29 geometry_msgs::PointStamped point_base;
30 point_base = buffer.transform(point_laser, "world");
31 ROS_DEBUG("\nTransformed position:\nx=%.2f\ny=%.2f\nCoordinate:\n%s\n",
32 point_base.point.x,
33 point_base.point.y,
34 point_base.header.frame_id.c_str());
35 } catch (const tf2::TransformException &ex) {
36 ROS_WARN("Failed to transform.");
37 }
38 rate.sleep();
39 }
40 return 0;
41}
随机移动代码：
1#include "geometry_msgs/Twist.h"
2#include "ros/ros.h"
3
4#include <random>
5
6double generate_random() {
7 std::random_device rand;
8 std::mt19937 gen(rand());
9 std::uniform_real_distribution<> dis(-1.3, 1.3);
10 return dis(gen);
11}
12
13int main(int argc, char **argv) {
14 setlocale(LC_ALL, "");
15 ros::init(argc, argv, "random_track");
16 ros::NodeHandle n;
17
18 ros::Publisher publisher = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 1000);
19 geometry_msgs::Twist twist;
20
21 ros::Rate rate(1);
22 while (ros::ok()) {
23 twist.linear.x = generate_random();
24 twist.angular.z = generate_random();
25 publisher.publish(twist);
26 ros::spinOnce();
27 rate.sleep();
28 }
29 return 0;
30}
此时运行上述三个节点和 turtlesim_node 节点，可以看到乌龟移动以及实时相对位置输出，但是如果运行 rviz 节点则无法看到子级坐标运动，这是因为 rviz 中需要手动监听 /tf 话题才能显示坐标转换结果。具体方法如下：
随后将当前配置文件保存至 config/rviz/turtle.rviz，以后每次启动时直接加载配置文件，不需要手动设置。
修改 launch 文件：
1<launch>
2 <node pkg="tf_demo" type="dynamic_broadcaster_tf_node" name="dynamic_broadcaster_tf_node"
3 output="screen" />
4 <node pkg="tf_demo" type="dynamic_listener_tf_node" name="dynamic_listener_tf_node"
5 output="screen" />
6 <node pkg="tf_demo" type="random_track" name="random_track" output="screen" />
7 <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node" required="true" />
8 <node pkg="rviz" type="rviz" name="rviz" args="-d $(find tf_demo)/config/rviz/turtle.rviz" />
9</launch>
经实测，rviz 节点可以正常传参，而 rqt_rviz 节点无法正常传参。
Python 实现动态坐标转换
发送方代码：
1import rospy
2import tf
3import tf2_ros
4import tf.transformations
5from geometry_msgs.msg import TransformStamped
6from turtlesim.msg import Pose
7
8
9def callback(pose):
10
11 # 创建 TF 广播器和 TransformStamped 对象
12 broadcaster = tf2_ros.TransformBroadcaster()
13 ts = TransformStamped()
14 ts.header.frame_id = "world"
15 ts.header.stamp = rospy.Time.now()
16 ts.child_frame_id = "turtle1"
17
18 ts.transform.translation.x = pose.x
19 ts.transform.translation.y = pose.y
20 ts.transform.translation.z = 0.0
21
22 qtn = tf.transformations.quaternion_from_euler(0, 0, pose.theta)
23 ts.transform.rotation.x = qtn[0]
24 ts.transform.rotation.y = qtn[1]
25 ts.transform.rotation.z = qtn[2]
26 ts.transform.rotation.w = qtn[3]
27
28 broadcaster.sendTransform(ts)
29
30
31def main():
32 rospy.init_node("dynamic_broadcaster_tf")
33 subscriber = rospy.Subscriber("/turtle1/pose", Pose, callback)
34 rospy.spin()
35
36
37if __name__ == "__main__":
38 main()
接收方代码：
1import logging
2
3import rospy
4import tf2_ros
5from tf2_geometry_msgs import PointStamped
6
7
8def main():
9 rospy.init_node("dynamic_listener_tf")
10 logging.getLogger("rosout").setLevel(logging.DEBUG)
11
12 # 创建 TF 订阅对象
13 buffer = tf2_ros.Buffer()
14 listener = tf2_ros.TransformListener(buffer)
15
16 rate = rospy.Rate(1)
17 while not rospy.is_shutdown():
18 point_laser = PointStamped()
19 point_laser.header.frame_id = "turtle1"
20 point_laser.header.stamp = rospy.Time.now()
21 point_laser.point.x = 1
22 point_laser.point.y = 3
23 point_laser.point.z = 0.0
24
25 try:
26 point_base = buffer.transform(point_laser, "world", rospy.Duration(1))
27 rospy.logdebug(
28 "\nTransformed position:\nx=%.2f\ny=%.2f\n\Coordinate: %s\n",
29 point_base.point.x,
30 point_base.point.y,
31 point_base.header.frame_id,
32 )
33 except:
34 rospy.logwarn("Failed to transform.")
35 rate.sleep()
36
37
38if __name__ == "__main__":
39 main()
随机移动代码：
1from random import uniform
2
3import rospy
4from geometry_msgs.msg import Twist
5
6
7def main():
8 rospy.init_node("random_track")
9 publisher = rospy.Publisher("/turtle1/cmd_vel", Twist, queue_size=1000)
10
11 twist = Twist()
12
13 rate = rospy.Rate(1)
14 while not rospy.is_shutdown():
15 twist.linear.x = uniform(-1.3, 1.3)
16 twist.angular.z = uniform(-1.3, 1.3)
17 publisher.publish(twist)
18 rate.sleep()
19
20
21if __name__ == "__main__":
22 main()
launch 文件：
1<launch>
2 <node pkg="tf_demo" type="dynamic_broadcaster_tf.py" name="dynamic_broadcaster_tf"
3 output="screen" />
4 <node pkg="tf_demo" type="dynamic_listener_tf.py" name="dynamic_listener_tf"
5 output="screen" />
6 <node pkg="tf_demo" type="random_track.py" name="random_track" output="screen" />
7 <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node" required="true" />
8 <node pkg="rviz" type="rviz" name="rviz" args="-d $(find tf_demo)/config/rviz/turtle.rviz" />
9</launch>
多坐标系变换
方便起见，直接用 launch 文件启动两个子坐标系节点：
1<launch>
2 <node pkg="tf2_ros" type="static_transform_publisher" name="son1"
3 args="0.1 0.1 0.3 0.1 0.2 0.3 /world /son1" output="screen" />
4 <node pkg="tf2_ros" type="static_transform_publisher" name="son2"
5 args="0.1 0.3 0.3 0.3 0.2 0.1 /world /son2" output="screen" />
6</launch>
这两个节点分别创建了 son1 和 son2 两个坐标系，其中 son1 与 world 坐标系相对位置为 (0.1, 0.1, 0.3) 且姿态为 (0.1, 0.2, 0.3)，son2 同理。本段中，我们能够计算出 son1 与 son2 之间的相对位置，以及 son1 中的点到 son2 坐标系的转换。
C++ 实现多坐标系变换
1#include "geometry_msgs/PointStamped.h"
2#include "geometry_msgs/TransformStamped.h"
3#include "ros/ros.h"
4#include "tf2_geometry_msgs/tf2_geometry_msgs.h" // 这个头文件不能少，否则过不了编译
5#include "tf2_ros/transform_listener.h"
6
7int main(int argc, char **argv) {
8 setlocale(LC_ALL, "");
9 ros::init(argc, argv, "sub_frames_cpp");
10 ros::NodeHandle nh;
11
12 // 创建 TF 订阅对象
13 tf2_ros::Buffer buffer;
14 tf2_ros::TransformListener listener(buffer);
15
16 ros::Duration(1.3).sleep();
17
18 // 解析 son1 相对于 son2 的坐标
19 geometry_msgs::TransformStamped ts = buffer.lookupTransform("son2", "son1", ros::Time(0));
20 ROS_INFO("----------");
21 ROS_INFO("Parent frame id: %s", ts.header.frame_id.c_str());
22 ROS_INFO("Children frame id: %s", ts.child_frame_id.c_str());
23 ROS_INFO("Relative location: x=%.2f, y=%.2f, z=%.2f",
24 ts.transform.translation.x,
25 ts.transform.translation.y,
26 ts.transform.translation.z);
27 ROS_INFO("Relative angle: w=%.2f, x=%.2f, y=%.2f, z=%.2f",
28 ts.transform.rotation.w,
29 ts.transform.rotation.x,
30 ts.transform.rotation.y,
31 ts.transform.rotation.z);
32
33 // 子级坐标系座标点解析
34 geometry_msgs::PointStamped ps;
35 ps.header.frame_id = "son1";
36 ps.header.stamp = ros::Time::now();
37 ps.point.x = 1.1;
38 ps.point.y = 2.2;
39 ps.point.z = 3.3;
40
41 geometry_msgs::PointStamped ps_son2;
42 ps_son2 = buffer.transform(ps, "son2", ros::Duration(0));
43 ROS_INFO("The coordinates of the point in frame son2: x=%.2f, y=%.2f, z=%.2f",
44 ps_son2.point.x,
45 ps_son2.point.x,
46 ps_son2.point.x);
47 ROS_INFO("----------");
48
49 return 0;
50}
Python 实现多坐标系变换
1import rospy
2import tf2_ros
3from geometry_msgs.msg import TransformStamped
4from tf2_geometry_msgs import PointStamped
5
6
7def main():
8 rospy.init_node("sub_frames_py")
9
10 # 创建 TF 订阅对象
11 buffer = tf2_ros.Buffer()
12 listener = tf2_ros.TransformListener(buffer)
13
14 rospy.sleep(3.3)
15
16 # 解析 son1 相对于 son2 的坐标
17 ts = buffer.lookup_transform("son2", "son1", rospy.Time(0))
18 rospy.loginfo("----------")
19 rospy.loginfo("Parent frame id: %s", ts.header.frame_id)
20 rospy.loginfo("Children frame id: %s", ts.child_frame_id)
21 rospy.loginfo(
22 "Relative location: x=%.2f, y=%.2f, z=%.2f",
23 ts.transform.translation.x,
24 ts.transform.translation.y,
25 ts.transform.translation.z,
26 )
27 rospy.loginfo(
28 "Relative angle: w=%.2f, x=%.2f, y=%.2f, z=%.2f",
29 ts.transform.rotation.w,
30 ts.transform.rotation.x,
31 ts.transform.rotation.y,
32 ts.transform.rotation.z,
33 )
34
35 # 在 son1 中创建一个座标点
36 ps = PointStamped()
37 ps.header.frame_id = "son1"
38 ps.header.stamp = rospy.Time.now()
39 ps.point.x = 1.1
40 ps.point.y = 2.2
41 ps.point.z = 3.3
42
43 # 计算点在 son2 中的坐标
44 ps_son2 = buffer.transform(ps, "son2", rospy.Duration(0))
45 rospy.loginfo(
46 "The coordinates of the point in frame son2: x=%.2f, y=%.2f, z=%.2f",
47 ps_son2.point.x,
48 ps_son2.point.x,
49 ps_son2.point.x,
50 )
51 rospy.loginfo("----------")
52
53
54if __name__ == "__main__":
55 main()
对应的 launch 文件：
1<launch>
2 <node pkg="tf2_ros" type="static_transform_publisher" name="son1"
3 args="0.1 0.1 0.3 0.1 0.2 0.3 /world /son1" output="screen" />
4 <node pkg="tf2_ros" type="static_transform_publisher" name="son2"
5 args="0.1 0.3 0.3 0.3 0.2 0.1 /world /son2" output="screen" />
6 <node pkg="tf_demo" type="multi_frame" name="multi_frame_cpp" output="screen" />
7 <node pkg="tf_demo" type="multi_frame.py" name="multi_frame_py" output="screen" />
8</launch>
输出结果：
1----------
2Parent frame id: son2
3Children frame id: son1
4Relative location: x=-0.06, y=-0.19, z=0.01
5Relative angle: w=0.99, x=0.12, y=-0.02, z=-0.10
6The coordinates of the point in frame son2: x=1.23, y=1.23, z=1.23
7----------