ROS2 中 TF变换发布与订阅：实现 base_link 和 test_link 实时可视显示

欢迎大家来到IT世界,在知识的湖畔探索吧!

欢迎大家来到IT世界,在知识的湖畔探索吧!

视频讲解

ROS2 中 TF 变换发布与订阅：实现 base_link 和 test_link 实时可视化显示_哔哩哔哩_bilibili

安装环境依赖

sudo apt-get install ros-humble-tf2-ros ros-humble-tf2-geometry-msgs ros-humble-tf-transformations

欢迎大家来到IT世界,在知识的湖畔探索吧!

创建一个包名为tf_test_pkg的包

欢迎大家来到IT世界,在知识的湖畔探索吧!ros2 pkg create --build-type ament_python tf_test_pkg --dependencies rclpy tf2_ros geometry_msgs

依赖为tf2_ros，geometry_msgs

在tf_test_pkg/tf_test_pkg中创建tf_publisher.py

import rclpy from rclpy.node import Node from geometry_msgs.msg import TransformStamped import tf2_ros import math class TFPublisher(Node): def __init__(self): super().__init__('tf_publisher') self.tf_broadcaster = tf2_ros.TransformBroadcaster(self) self.timer = self.create_timer(0.1, self.broadcast_tf) self.pitch = 0.0 def broadcast_tf(self): t = TransformStamped() t.header.stamp = self.get_clock().now().to_msg() t.header.frame_id = '/base_link' t.child_frame_id = '/test_link' # 设置平移 t.transform.translation.x = 1.0 t.transform.translation.y = 0.0 t.transform.translation.z = 0.0 # 增加pitch角度 self.pitch += 0.2 if self.pitch > 2 * math.pi: self.pitch -= 2 * math.pi # 设置旋转（仅pitch变化） from tf_transformations import quaternion_from_euler q = quaternion_from_euler(0, self.pitch, 0) t.transform.rotation.x = q[0] t.transform.rotation.y = q[1] t.transform.rotation.z = q[2] t.transform.rotation.w = q[3] # 发布TF变换 self.tf_broadcaster.sendTransform(t) def main(args=None): rclpy.init(args=args) tf_publisher = TFPublisher() rclpy.spin(tf_publisher) tf_publisher.destroy_node() rclpy.shutdown() if __name__ == '__main__': main()

同样的，创建tf_subscriber.py

欢迎大家来到IT世界,在知识的湖畔探索吧!import rclpy from rclpy.node import Node import tf2_ros from tf2_ros.buffer import Buffer from tf2_ros.transform_listener import TransformListener class TFSubscriber(Node): def __init__(self): super().__init__('tf_subscriber') self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) self.timer = self.create_timer(1.0, self.lookup_transform) def lookup_transform(self): try: trans = self.tf_buffer.lookup_transform( 'base_link', 'test_link', rclpy.time.Time()) self.get_logger().info(f"Translation: {trans.transform.translation}") self.get_logger().info(f"Rotation: {trans.transform.rotation}") except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): self.get_logger().warn("Could not find transform between /base_link and /test_link") def main(args=None): rclpy.init(args=args) tf_subscriber = TFSubscriber() rclpy.spin(tf_subscriber) tf_subscriber.destroy_node() rclpy.shutdown() if __name__ == '__main__': main()

修改setup.py，增加内容如下：

entry_points={ 'console_scripts': [ 'tf_publisher = tf_test_pkg.tf_publisher:main', 'tf_subscriber = tf_test_pkg.tf_subscriber:main', ], },

编译运行

欢迎大家来到IT世界,在知识的湖畔探索吧!colcon build --packages-select tf_test_pkg

source install/setup.bash ros2 run tf_test_pkg tf_publisher ros2 run tf_test_pkg tf_subscriber

rviz2配置

设置固定坐标系

启动 Rviz 后，在 Rviz 窗口左侧的 “Displays” 面板中，找到 “Global Options” 部分，点击 “Fixed Frame” 右侧的下拉框，选择 /base_link 作为固定坐标系。这一步很关键，因为 Rviz 会以固定坐标系为参考来显示其他坐标系的位置和姿态。

添加 TF 显示插件

在 Rviz 窗口左侧 “Displays” 面板的上方，点击 “Add” 按钮，在弹出的 “Add Display” 对话框中，选择 “TF” 选项，然后点击 “OK”。此时，“Displays” 面板中会新增一个 “TF” 条目。

调整 TF 显示设置

在 “Displays” 面板中展开 “TF” 条目

• Marker Scale：用于调整坐标系箭头的大小。你可以根据实际情况增大或减小该值，使显示效果更清晰。
• Show Axes：勾选该选项会显示每个坐标系的坐标轴（X 轴为红色，Y 轴为绿色，Z 轴为蓝色），方便直观地了解坐标系的方向。
• Show Names：勾选此选项会在每个坐标系旁边显示其名称（如 /base_link 和 /test_link），便于识别不同的坐标系。