SDF 里的 joint 连上了，为什么 ROS 的 TF 树里还是断的？
1.问题现象
在之前的 小节 中，我使用 XTDrone + Gazebo + PX4 搭建了室内无人机仿真环境，配置 Mid360 激光雷达和 IMU，并运行 FAST-LIO 进行激光雷达惯性里程计估计。在后续检查 TF 树时，发现了一个比较奇怪的问题：base_link 和 Mid360::link 之间没有建立 TF 连接。
但是，查看 ~/PX4_Firmware/Tools/sitl_gazebo/models/iris_mid360/iris_mid360.sdf 后，可以发现雷达模型已经通过固定关节连接到了无人机机身：
本文主要解决两个问题：
- 解释为什么 SDF 中已经定义了固定关节，ROS 的 TF 树仍然可能断开。
- 修改 Livox 仿真插件的 TF 广播逻辑，并通过静态 TF 建立正确的雷达坐标系连接。
2. 为什么 SDF 中有固定关节，TF 树却是断开的？
首先需要明确：Gazebo 中的物理连接关系，不会自动转换成 ROS 的 TF 变换关系。
SDF 中的
这里可以对比一下 URDF 与 SDF 的使用场景。在常见的 URDF 机器人模型中，通常会使用 robot_state_publisher 根据 URDF 中的 link、joint 及其关节状态发布 TF。对于固定关节，相应的坐标变换也可以由该节点发布。而当前使用的 PX4 SITL 仿真模型主要由 SDF 描述，不能因为 SDF 中定义了固定关节，就认为 ROS 中也已经存在相应的 TF 变换。所以，排查这类问题时，需要分别检查 Gazebo 模型连接是否正确，以及 ROS 中是否有节点广播了所需的坐标变换。
3. 排查现有 TF 树中每条边的来源
在修改代码之前，先梳理当前 TF 树中各个坐标系之间的关系，明确每条变换究竟由哪个节点发布。
根据当前仿真环境的配置，各条 TF 的来源如下。
| TF变换 | 广播来源 | 作用 |
|---|---|---|
| map → map_ned | MAVROS | 地图坐标系与 NED 坐标系之间的变换 |
| odom → odom_ned | MAVROS | 里程计坐标系与 NED 坐标系之间的变换 |
| base_link → base_link_frd | MAVROS | 机体坐标系与 FRD 坐标系之间的变换 |
| map → camera_init | 重定位节点 | 建立全局地图与 FAST-LIO 初始坐标系之间的关系 |
| camera_init → body | FAST-LIO | 发布 FAST-LIO 估计的机体位姿 |
| iris_0::Mid360::link → laser_mid360 | Livox 仿真插件 | 发布插件内部生成的雷达相关 TF |
这里需要注意，view_frames 中显示的 10000 Hz 和接近 0.000 sec 的平均延迟，通常对应静态 TF 的发布特征。但频率和延迟只能作为辅助判断，不能单凭这两个数值就确定某条 TF 的来源，最终仍需要结合广播节点和代码进行确认。
3.1 MAVROS 中的 base_link 为什么也是孤立的？
除了雷达坐标系的问题，还需要注意 base_link 本身的连接情况。当前配置中，base_link → base_link_frd 由 MAVROS 发布，但 TF 树中并没有因此自动出现 map → base_link。这是因为在当前使用的 px4_config.yaml 中，local_position/tf/send 配置为 false，MAVROS 不会通过该选项发布对应的本地位置 TF。因此，即使 MAVROS 已经发布了 base_link → base_link_frd，也不意味着 base_link 一定已经接入全局坐标系。
这里要区分两个问题：一是某个坐标系之间有没有 TF 连接，二是无人机的位姿估计结果是否准确。前者需要检查 TF 广播，后者则需要检查定位算法和坐标变换是否正确。
3.2 为什么 Gazebo 中的坐标系名称带有 ::？
另一个容易忽略的问题是坐标系名称。Gazebo 模型中的 link 名称可能是 base_link、Mid360::link 等，但插件发布到 ROS 中的帧名可能包含模型实例名称，例如：
iris_0::base_link
iris_0::Mid360::link
它们与 MAVROS 或 FAST-LIO 使用的 base_link、body 等帧名并不是同一个坐标系。TF 只根据帧名建立连接，不会因为两个坐标系在 Gazebo 中属于同一个模型，就自动将它们合并。因此，Gazebo 中的 iris_0::base_link 和 MAVROS 使用的 base_link 即使名称相似，也不能视为同一个 TF 帧。至此，问题已经比较明确：SDF 中的固定关节正常工作，但 ROS 端缺少我们需要的机体与雷达之间的静态 TF 连接。
4. Livox 仿真插件为什么会发布不符合预期的 TF？
继续检查 TF 树，发现插件还发布了一条：iris_0::Mid360::link → laser_mid360, 从帧名来看，这似乎是我们需要的雷达坐标系连接。但通过移动无人机，并执行以下命令进行检查：rosrun tf tf_echo iris_0::Mid360::link laser_mid360, 可以发现，得到的变换会随着无人机在场景中的移动而变化，并不符合预期的静态雷达外参关系。
正常情况下，如果 laser_mid360 表示雷达相对于机体的固定坐标系，那么机体与雷达之间的相对位姿应当保持不变。无人机在 Gazebo 世界中移动时，这个相对变换不应随之变化。问题出在 Livox 仿真插件的 TF 广播逻辑上。
打开以下源码文件：gedit ~/catkin_ws/src/livox_laser_simulation/src/livox_points_plugin.cpp。在原版 livox_laser_simulation 插件中，OnNewLaserScans() 会在处理激光数据时调用 SendRosTf()。相关逻辑会使用 parentEntity->WorldPose() 获取实体在 Gazebo 世界坐标系下的位姿，然后将其传入 TF 广播函数。这里的问题是：传入的是实体的世界位姿，但发布的 TF 却使用了父子坐标系的形式来描述它。 世界位姿与相对外参不是同一个概念。假设雷达在世界坐标系下的位姿为 (T_{world,lidar})，无人机机体在世界坐标系下的位姿为 (\(T_{world,base}\))，那么雷达相对于机体的真实外参应当为：
\(T_{world,base}^{-1}T_{world,lidar}\)
只有在正确计算相对变换后，才能将其作为 base_link 与雷达坐标系之间的固定 TF 发布。如果直接将世界位姿作为相对变换广播，那么无人机移动时，TF 中记录的所谓父子关系就会出现不符合预期的变化。
5. 修改 Livox 插件，关闭错误的 TF 广播
既然 FAST-LIO 本身不依赖这条插件发布的 TF 来获取雷达外参，那么可以先关闭插件中这部分 TF 广播，再由我们自己建立正确的静态 TF。
打开源码：gedit ~/catkin_ws/src/livox_laser_simulation/src/livox_points_plugin.cpp, 找到 OnNewLaserScans() 中调用 SendRosTf() 的位置，在这里禁用该调用，不再由插件发布这条有问题的 TF。
需要注意：修改时应当只跳过 TF 广播逻辑，不要在点云生成和发布之前直接退出整个 OnNewLaserScans() 函数，否则可能会连带停止点云数据的处理与发布。修改完成后，重新编译工作空间中的插件：
点击查看代码
cd ~/catkin_ws
catkin build
source devel/setup.bash
重新运行后，原先由插件发布的：iris_0::Mid360::link → laser_mid360 不应再由该插件广播。此时，laser_mid360 可能暂时没有父节点，这是预期现象。接下来需要在 FAST-LIO 的启动文件中补充正确的静态 TF。
6. 在 FAST-LIO 启动文件中补充静态 TF
打开当前使用的启动文件：gedit ~/catkin_ws/src/FAST_LIO/launch/mapping_mid360.launch:在 launch 文件中添加两条静态 TF，分别建立 body 与 base_link 之间的连接，以及 base_link 与 laser_mid360 之间的连接。
现在重新运行rosrun tf tf_echo base_link laser_mid360移动无人机应该固定为 Translation: [-0.005, 0.005, 0.097]；同时rosrun tf tf_echo body base_link应该固定为Translation: [0, 0, 0]，最后可以重新生成 TF 树：
点击查看代码
rosrun tf view_frames
evince frames.pdf
7. 补充
这里再补充一个容易混淆的问题：既然雷达 TF 有问题，之前 FAST-LIO 为什么仍然能够正常运行？原因在于，FAST-LIO 的雷达与 IMU 外参并不是通过查询这条 ROS TF 自动获得的，而是通过配置文件中的外参参数进行计算。因此，只要 FAST-LIO 配置文件中的雷达与 IMU 外参正确，即使 ROS 的 TF 树中缺少 base_link → laser_mid360，FAST-LIO 仍然可能正常完成点云处理和里程计估计。
不过，这并不意味着 TF 可以忽略。对于 RViz 中的原始点云显示，如果点云消息使用的坐标系无法通过 TF 变换到 RViz 的 Fixed Frame，就可能出现点云无法显示、报错或显示位置不正确等问题。如果后续需要将定位结果与 PX4 的 EKF2 融合，或者与其他 ROS 定位、导航模块进行数据交互，也必须明确各个坐标系的含义和变换关系。但是否需要补充某条 TF、是否需要将定位结果输入 EKF2，以及应该采用何种坐标变换，还需要根据具体的数据接口和融合架构确定，不能仅凭 TF 树连通就认定整个闭环已经正确。
浙公网安备 33010602011771号