4. 加载机器人模型到MoveIt：使用robot_state_publisher发布TF，验证模型在Rviz中的显示

说实话，每次我搭建一个新的机械臂仿真环境，最期待的就是这一步——看着机器人模型在Rviz里“活过来”。

你可能会问：不就是加载个模型吗？但这里面的门道，尤其是TF坐标树的正确发布，直接决定了后面运动学能不能算、规划能不能跑。今天我们就来亲手把机器人模型加载进MoveIt，用robot_state_publisher把TF树发布出来，然后在Rviz里亲眼验证。

4.1 准备工作：一个完整的URDF

要发布TF，首先得有一个“说清楚”坐标关系的URDF。我个人习惯在URDF里把每个link的坐标系原点、方向都定义清楚，尤其是<joint>的origin标签——它决定了父子frame之间的偏移。

举个例子，一个简单的两连杆臂：

<robot name="simple_arm">
  <link name="base_link">
    <visual>
      <geometry><box size="0.2 0.2 0.1"/></geometry>
    </visual>
  </link>
  <joint name="joint1" type="revolute">
    <parent link="base_link"/>
    <child link="link1"/>
    <origin xyz="0 0 0.15" rpy="0 0 0"/>
  </joint>
  <link name="link1">
    <visual>
      <geometry><cylinder length="0.3" radius="0.05"/></geometry>
    </visual>
  </link>
</robot>

注意看origin xyz="0 0 0.15"，这意味着link1的坐标系原点在base_link的Z正方向0.15米处。robot_state_publisher就是靠这些信息计算TF的。

4.2 启动robot_state_publisher

在ROS2里，最直接的方式就是写一个launch文件。我早期经常手敲命令，后来发现还是launch文件省心，尤其是要传参数的时候。

# 加载机器人模型并启动 state publisher
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            name='robot_state_publisher',
            parameters=[{'robot_description': open('simple_arm.urdf').read()}]
        ),
        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2'
        )
    ])

这里有个小坑：robot_description参数必须是字符串，而且URDF里不能有中文注释（我之前被这个坑过，Rviz里模型死活不出来）。

💡 我的习惯： 把URDF文件放在urdf/目录下，launch文件里用open()读取。如果你用xacro，记得先转换成纯URDF，或者用xacro命令预处理。

4.3 验证TF树：在Rviz里看什么？

启动后，打开Rviz。你可能会看到一片灰，别慌。按以下步骤操作：

添加RobotModel显示 —— 在Displays面板点“Add”，选“RobotModel”。在Description Topic里选/robot_description（默认就有）。
添加TF显示 —— 同样Add，选“TF”。你会看到所有frame的坐标轴。
设置Fixed Frame —— 在Global Options里，把Fixed Frame设为base_link（或者你URDF里的根link）。

如果一切正常，你会看到机器人的几何模型，并且每个link上都飘着红绿蓝坐标轴。我记得第一次看到自己画的URDF在Rviz里动起来，那种感觉真的很爽。

            ✅ 验证要点：
            TF树是否完整？每个link都有对应的frame吗？
坐标轴方向是否符合预期？比如基座的Z轴朝上。
有没有悬空的link？通常是因为joint的origin写错了。

        

4.4 常见翻车现场 & 我的避坑指南

我曾经在项目里遇到一个特别诡异的问题：模型加载出来了，但所有link都堆在原点。查了半天，发现是URDF里<joint>的type写成了fixed，但origin的xyz却全是0。说白了，就是坐标偏移没写进去。

另一个经典错误：robot_state_publisher启动时提示“No transform between ...”。这通常是因为URDF里某个joint的parent link名字拼错了，或者大小写不一致。ROS2的TF对名称是大小写敏感的。

症状	可能原因	解决办法
模型不显示	robot_description参数未正确加载	检查launch文件，打印参数内容
TF缺失某个frame	joint的child link名称不匹配	核对URDF中的link名称
坐标轴方向奇怪	rpy设置错误	用右手定则重新计算
模型闪烁/抖动	多个节点同时发布相同TF	检查是否有重复的state publisher

⚠️ 特别注意： 如果你同时启动了joint_state_publisher_gui，记得它也会发布关节状态，但不会发布TF。TF完全由robot_state_publisher负责。两者不要搞混。

4.5 用命令行快速诊断

有时候Rviz里看不清楚，我习惯打开终端用tf2_monitor或者tf2_echo来检查。比如：

# 查看所有frame
ros2 run tf2_tools view_frames

# 查看两个frame之间的变换
ros2 run tf2_ros tf2_echo base_link link1

如果view_frames生成一个pdf，里面只有孤零零一个frame，那说明URDF的joint连接有问题。嗯，这招我用了无数次，比瞪眼找bug快多了。

好了，到这一步，你的机器人模型应该已经在Rviz里稳稳地显示出来了，TF树也完整发布。但今天先到这里，把基础打牢。