如何修改imu_filter_madgwick将RPY从imu_sensor帧转至base_link帧

针对无磁力计IMU的场景，要让imu/data和imu/rpy/filtered消息以base_link帧发布，按以下步骤操作：

• 确认参数配置生效
  除了代码中声明参数，需确保参数实际被正确赋值。推荐在参数文件（如params.yaml）中明确配置：

  imu_filter_madgwick:
    ros__parameters:
      fixed_frame: "base_link"
      reverse_tf: true
      publish_tf: false  # 无需发布TF时关闭，避免帧关系冲突
  

  启动节点时加载该参数文件，确保fixed_frame和reverse_tf参数生效。

• 修改输出消息的header.frame_id
  找到节点中发布imu/data和imu/rpy/filtered的代码段，直接设置消息头的帧ID：

  1. 对于imu/data（sensor_msgs/msg/Imu类型）：

     sensor_msgs::msg::Imu filtered_imu;
     // 填充角速度、线加速度、姿态四元数等数据...
     filtered_imu.header.frame_id = "base_link";  // 替换原有帧ID
     filtered_imu_publisher_->publish(filtered_imu);
     

  2. 对于imu/rpy/filtered（通常为sensor_msgs/msg/Vector3Stamped类型）：

     sensor_msgs::msg::Vector3Stamped filtered_rpy;
     // 填充Roll、Pitch、Yaw数据...
     filtered_rpy.header.frame_id = "base_link";  // 设置为base_link
     rpy_publisher_->publish(filtered_rpy);
     

• 验证结果
  编译修改后的包：

  colcon build --packages-select imu_filter_madgwick
  

  启动节点后，通过以下命令检查消息帧ID：

  ros2 topic echo /imu/data --field header.frame_id
  ros2 topic echo /imu/rpy/filtered --field header.frame_id
  

  输出应为base_link，同时确认RPY数据符合预期。

备注：无磁力计情况下，Yaw角会随时间漂移，这是纯IMU融合的固有特性，无法避免。

内容的提问来源于stack exchange，提问作者Abbas