ROS2——基础6（TF2机器人坐标系管理器、URDF建模）
本文介绍了ROS2中TF2坐标系管理和URDF机器人建模方法。TF2部分包括：1)TF2简介，用于管理机器人坐标系；2)命令行测试方法；3)静态TF广播实现；4)TF监听机制。URDF部分包含：1)URDF简介，用于机器人建模；2)语法说明，包括连杆(link)和关节(joint)的描述方法；3)创建简单机器人模型的实践步骤。文章提供了详细的代码示例和操作流程，涵盖了从坐标系管理到机器人建模的关键
·
目录
1 TF2机器人坐标系管理器
1.1 TF2简介
机器人系统中有很多坐标系，机器人中心点设为基坐标系base link、雷达所在位置为雷达坐标系laser link、机器人移动里程计累积位置的参考系称为里程计坐标系odom link、里程计有累积误差和飘逸又有绝对位置参考系/地图坐标系map link。TF2在ROS中用于管理机器人的坐标系。
1.2 TF命令行测试
sudp apt install ros-humble-turtle-tf2-py ros2 launch turtle_tf2_py turtle_tf2_demo.launch.py另启终端
ros2 run turtlesim turtle_teleop_key另启终端，查看turtle1和2两个坐标系之间的具体关系
ros2 run tf2_ros tf2_echo turtle2 turtle1
那么这个工程包是怎么构建的呢，详情如下。
1.3 静态TF广播
（1）创建static_tf_broadcaster.py
安装tf工具-创建tf_demo功能包并进入-编写程序
sudo apt install ros-humble-tf2 \ ros-humble-tf2-tools \ ros-humble-tf2-ros \ ros-humble-tf-transformations \ ros-humble-tf2-msgs \ ros-humble-tf2-geometry-msgs \ ros-humble-tf2-kdl \ ros-humble-tf2-eigen \ ros-humble-tf2-sensor-msgs cd ros2_ws/src ros2 pkg create tf_demo --build-type ament_python --dependencies rclpy cd tf_demo/tf_demo/ gedit static_tf_broadcaster.py#!/usr/bin/env python3 # -*- coding: utf-8 -*- # ROS2 Python 接口库 import rclpy # ROS2 节点类 from rclpy.node import Node # 坐标变换消息 from geometry_msgs.msg import TransformStamped # TF 坐标变换库 import tf_transformations # TF 静态坐标系广播器类 from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster class StaticTFBroadcaster(Node): def __init__(self, name): # ROS2 节点父类初始化 super().__init__(name) # 创建一个 TF 广播器对象 self.tf_broadcaster = StaticTransformBroadcaster(self) # 创建一个坐标变换的消息对象 static_transformStamped = TransformStamped() # 设置坐标变换消息的时间戳 static_transformStamped.header.stamp = self.get_clock().now().to_msg() # 设置一个坐标变换的源坐标系 static_transformStamped.header.frame_id = 'world' # 设置一个坐标变换的目标坐标系 static_transformStamped.child_frame_id = 'house' # 设置坐标变换中的 X、Y、Z 向的平移 static_transformStamped.transform.translation.x = 10.0 static_transformStamped.transform.translation.y = 5.0 static_transformStamped.transform.translation.z = 0.0 # 将欧拉角转换为四元数（roll, pitch, yaw） quat = tf_transformations.quaternion_from_euler(0.0, 0.0, 0.0) # 设置坐标变换中的 X、Y、Z 向的旋转（四元数） static_transformStamped.transform.rotation.x = quat[0] static_transformStamped.transform.rotation.y = quat[1] static_transformStamped.transform.rotation.z = quat[2] static_transformStamped.transform.rotation.w = quat[3] # 广播静态坐标变换，广播后两个坐标系的位置关系保持不变 self.tf_broadcaster.sendTransform(static_transformStamped) def main(args=None): # ROS2 Python 接口 初始化 rclpy.init(args=args) # 创建 ROS2 节点对 象并进行初始化 node = StaticTFBroadcaster("static_tf_broadcaster") # 循环等待 ROS2 退出 rclpy.spin(node) # 销毁节点对象 node.destroy_node() rclpy.shutdown()chmod +x static_tf_broadcaster.py（2）创建tf_listener.py
gedit tf_listener.py#!/usr/bin/env python3 # -*- coding: utf-8 -*- import rclpy from rclpy.node import Node from tf2_ros import TransformListener, Buffer class TFListener(Node): def __init__(self): super().__init__('tf_listener') # 创建 TF buffer 和 listener self.buffer = Buffer() self.listener = TransformListener(self.buffer, self) # 每 0.5s 查询一次 TF self.timer = self.create_timer(0.5, self.on_timer) def on_timer(self): try: # 查询 world -> home 的 TF trans = self.buffer.lookup_transform( 'world', # parent 'house', # child rclpy.time.Time() ) # 打印 TF 坐标 t = trans.transform.translation self.get_logger().info( f"home 相对 world 的位置 = ({t.x:.2f}, {t.y:.2f}, {t.z:.2f})" ) except Exception as e: self.get_logger().warn(f"TF 尚未准备好：{e}") def main(args=None): rclpy.init(args=args) node = TFListener() rclpy.spin(node) node.destroy_node() rclpy.shutdown()chmod +x tf_listener.py（3）setup.py设置
（4）编译运行
cd ~/ros2_ws colcon build source ./install/setup.bash ros2 run tf_demo static_tf_broadcaster另启终端
cd ros2_ws source ./install/setup.bash ros2 run tf_demo tf_listener
1.4 TF监听
（1）创建turtle_tf_broadcaster.py
cd ros2_ws/src/tf_demo/tf_demo/ gedit turtle_tf_broadcaster.py#!/usr/bin/env python3 # -*- coding: utf-8 -*- # ROS2 Python 接口库 import rclpy # ROS2 节点类 from rclpy.node import Node # 坐标变换消息 from geometry_msgs.msg import TransformStamped # TF 坐标变换库 import tf_transformations # TF 坐标变换广播器 from tf2_ros import TransformBroadcaster # turtlesim 小海龟位置消息 from turtlesim.msg import Pose class TurtleTFBroadcaster(Node): def __init__(self, name): # ROS2 节点父类初始化 super().__init__(name) # 创建一个海龟名称的参数 self.declare_parameter('turtlename', 'turtle') # 优先使用外 部设置的参数值，否则用默认值 self.turtlename = self.get_parameter( 'turtlename').get_parameter_value().string_value # 创建一个 T F 坐标变换的广播对象并初始化 self.tf_broadcaster = TransformBroadcaster(self) # 创建一个订 阅者，订阅海龟的位置消息 self.subscription = self.create_subscription( Pose, # 使用参数中获 取到的海龟名称 f'/{self.turtlename}/pose', self.turtle_pose_callback, 1) # 创建一个处理海龟位置消息的回调函数，将位置消息转变成坐标变换 def turtle_pose_callback(self, msg): # 创建 一个坐标变换的消息对象 transform = TransformStamped() # 设置坐标变换消息的时间戳 transform.header.stamp = self.get_clock().now().to_msg() # 设 置一个坐标变换的源坐标系 transform.header.frame_id = 'world' # 设 置一个坐标变换的目标坐标系 transform.child_frame_id = self.turtlename # 设 置坐标变换中的 X、Y、Z 向的平移 transform.transform.translation.x = msg.x transform.transform.translation.y = msg.y transform.transform.translation.z = 0.0 # 将欧拉角转换为四元数（roll, pitch, yaw） q = tf_transformations.quaternion_from_euler(0, 0, msg.theta) # 设 置坐标变换中的 X、Y、Z 向的旋转（四元数） transform.transform.rotation.x = q[0] transform.transform.rotation.y = q[1] transform.transform.rotation.z = q[2] transform.transform.rotation.w = q[3] # Send the transformation # 广播坐标变换，海 龟位置变化后，将及时更新坐标变换信息 self.tf_broadcaster.sendTransform(transform) def main(args=None): # ROS2 Python 接口 初始化 rclpy.init(args=args) # 创建 ROS2 节点对象并进行初始化 node = TurtleTFBroadcaster("turtle_tf_broadcaster") # 循环等待 ROS2 退出 rclpy.spin(node) # 销毁节点对象 node.destroy_node() # 关闭 ROS2 Python 接口 rclpy.shutdown()chmod +x turtle_tf_broadcaster.py（2）创建turtle_following.py
gedit turtle_following.py#!/usr/bin/env python3 # -*- coding: utf-8 -*- import math # ROS2 Python 接口库 import rclpy # ROS2 节点类 from rclpy.node import Node # TF 坐标变换库 import tf_transformations # TF 左边变换的异常类 from tf2_ros import TransformException # 存储坐标变换信息的缓冲类 from tf2_ros.buffer import Buffer # 监听坐标变换 的监听器类 from tf2_ros.transform_listener import TransformListener # ROS2 速度控制消息 from geometry_msgs.msg import Twist # 海龟生成的服务接口 from turtlesim.srv import Spawn class TurtleFollowing(Node): def __init__(self, name): # ROS2 节点父类初始化 super().__init__(name) # 创建 一个源坐标系名的参数 self.declare_parameter('source_frame', 'turtle1') # 优先 使用外部设置的参数值，否则用默认值 self.source_frame = self.get_parameter( # 创建保 存坐标变换信息的缓冲区 'source_frame').get_parameter_value().string_value self.tf_buffer = Buffer() # 创建坐标变换的监听器 self.tf_listener = TransformListener(self.tf_buffer, self) # 创建 一个请求产生海龟的客户端 self.spawner = self.create_client(Spawn, 'spawn') # 是否已经请求海龟生成服务的标志位 self.turtle_spawning_service_ready = False # 海龟是否产生成功的标志位 self.turtle_spawned = False # 创建跟随运动海龟的速度话题 self.publisher = self.create_publisher(Twist, 'turtle2/cmd_vel', 1) # 创建 一个固定周期的定时器，控制跟随海龟的运动 self.timer = self.create_timer(1.0, self.on_timer) def on_timer(self): # 源坐标系 from_frame_rel = self.source_frame # 目标坐标系 to_frame_rel = 'turtle2' # 如果已经请求海龟生成服务 if self.turtle_spawning_service_ready: # 如果跟随海龟已经生成 if self.turtle_spawned: try: # 获取 ROS 系统的当前时间 now = rclpy.time.Time() # 监听当前时刻源坐标系到目标坐标系的坐标变换 trans = self.tf_buffer.lookup_transform( to_frame_rel, from_frame_rel, now) # 如果坐标变换获取失败，进入异常报告 except TransformException as ex: self.get_logger().info( f'Could not transform {to_frame_rel} to {from_frame_rel}: {ex}') return # 创建速度控制消息 msg = Twist() # 根据海龟角度，计算角速度 scale_rotation_rate = 1.0 msg.angular.z = scale_rotation_rate * math.atan2( trans.transform.translation.y, trans.transform.translation.x) # 根据海龟距离，计算线速度 scale_forward_speed = 0.5 msg.linear.x = scale_forward_speed * math.sqrt( trans.transform.translation.x ** 2 + trans.transform.translation.y ** 2) # 发布速度指令，海龟跟随运动 self.publisher.publish(msg) # 如果跟随海龟没有生成 else: # 查看海龟是否生成 if self.result.done(): self.get_logger().info( f'Successfully spawned {self.result.result().name} ') self.turtle_spawned = True # 依然没有生成跟随海龟 else: self.get_logger().info('Spawn is not finished') # 如果没有请求海龟生成服务 else: # 如果海龟生成服务器已经准备就绪 if self.spawner.service_is_ready(): # 创建一 个请求的数据 request = Spawn.Request() # 设置请求数据的内容，包括海龟名、xy 位置、姿态 request.name = 'turtle2' request.x = float(4) request.y = float(2) request.theta = float(0) # 发送服务请求 self.result = self.spawner.call_async(request) # 设置标志位，表示已经发送请求 self.turtle_spawning_service_ready = True else: # 海龟生成服务器还没准备就绪的提示 self.get_logger().info('Service is not ready') def main(args=None): # ROS2 Python 接口初始化 rclpy.init(args=args) # 创建 ROS2节点对象并进行初始化 node = TurtleFollowing("turtle_following") # 循环等待 ROS2 退出 rclpy.spin(node) # 销毁节点对象 node.destroy_node() # 关闭 ROS2 Python 接口 rclpy.shutdown()chmod +x turtle_following.py（3）创建turtle_following.launch.py
cd .. mkdir launch cd launch/ gedit turtle_following.launch.pyfrom launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node def generate_launch_description(): return LaunchDescription([ Node( package='turtlesim', executable='turtlesim_node', name='sim' ), Node( package='tf_demo', executable='turtle_tf_broadcaster', name='broadcaster1', parameters=[ {'turtlename': 'turtle1'} ] ), DeclareLaunchArgument( 'target_frame', default_value='turtle1', description='Target frame name.' ), Node( package='tf_demo', executable='turtle_tf_broadcaster', name='broadcaster2', parameters=[ {'turtlename': 'turtle2'} ] ), Node( package='tf_demo', executable='turtle_following', name='listener', parameters=[ {'target_frame': LaunchConfiguration('target_frame')} ] ), ])chmod +x turtle_following.launch.py（4）setup.py设置
（5）编译运行
cd ~/ros2_ws colcon build source ./install/setup.bash ros2 launch tf_demo turtle_following.launch.py另启终端
ros2 run turtlesim turtle_teleop_key可以通过上下左右箭进行控制，另一只小乌龟跟随。
2 URDF机器人建模方法
2.1 URDF简介
统一机器人描述格式URDF，常用与ROS中机器人建模，不仅可以清晰描述机器人自身模型、还可以描述机器人外部环境，URDF模型文件是XML。
2.2 语法
2.2.1 连杆Link描述
用于描述机器人刚体部分外观（尺寸、颜色、形状）和物理属性（质量、惯性矩阵、碰撞参数等）。
语法 作用 <link name="XXX"> 自定义名称 <visual> 用于描述机器人外观 <geometry> 描述几何形状 <mesh> 调用三维软件中设计好的模型文件 <origin> 表述坐标系相对初始位置的偏移，xyz方向平移、rpy旋转 <collision> 描述碰撞参数
2.2.2 关节joint描述
关节类型 作用 continuous 旋转关节，可以绕单轴无限旋转 revolute 旋转关节，旋转角度有极限 prismatic 滑动关节，沿某一轴线移动的关节，有位置极限 fixed 固定关节 floating 浮动关节，允许 平移、旋转运动 planar 平面关节，允许在平面正交方向上平移或旋转 ROS中默认平移单位是m，旋转是弧度rad，线速度m/s、角速度rad/s。
2.3 创建机器人模型
cd ros2_ws/src ros2 pkg create urdf_demo --build-type ament_python --dependencies rclpy cd urdf_demo mkdir urdf cd urdf gedit simple_demo.urdf<?xml version="1.0"?> <robot name="materials"> <material name="blue"> <color rgba="0 0 0.8 1"/> </material> <material name="white"> <color rgba="1 1 1 1"/> </material> <link name="base_link"> <visual> <geometry> <cylinder length="0.6" radius="0.2"/> </geometry> <material name="blue"/> </visual> </link> <link name="right_leg"> <visual> <geometry> <box size="0.6 0.1 0.2"/> </geometry> <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/> <material name="white"/> </visual> </link> <joint name="base_to_right_leg" type="fixed"> <parent link="base_link"/> <child link="right_leg"/> <origin xyz="0 -0.22 0.25"/> </joint> <link name="left_leg"> <visual> <geometry> <box size="0.6 0.1 0.2"/> </geometry> <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/> <material name="white"/> </visual> </link> <joint name="base_to_left_leg" type="fixed"> <parent link="base_link"/> <child link="left_leg"/> <origin xyz="0 0.22 0.25"/> </joint> </robot>chmod +x simple_demo.urdf改代码创建了一个简易的柱状机器人模型，关于调用等详见Rviz中。
火山引擎开发者社区是火山引擎打造的AI技术生态平台，聚焦Agent与大模型开发，提供豆包系列模型（图像/视频/视觉）、智能分析与会话工具，并配套评测集、动手实验室及行业案例库。社区通过技术沙龙、挑战赛等活动促进开发者成长，新用户可领50万Tokens权益，助力构建智能应用。
更多推荐
3
0- 0
已为社区贡献25条内容
所有评论(0)