4. 传感器仿真与数据采集

做机器人导航，传感器就是眼睛和耳朵。没有真实的传感器数据，SLAM和Nav2都成了空中楼阁。这一章我们就在Gazebo里把激光雷达、IMU、里程计全部仿真出来，再搭一个能跑起来的测试环境。我个人习惯先把传感器“喂饱”，再调算法，这样后面少踩很多坑。

4.1 激光雷达仿真：从模型到点云

激光雷达是SLAM的主力传感器。Gazebo里用gazebo_plugins提供的RayPlugin就能模拟2D激光。我刚开始搞仿真时，总以为雷达模型越复杂越好，其实一个16线或单线就够用，关键是帧率和噪声参数要调得像真家伙。

<!-- 激光雷达插件示例 -->
<gazebo reference="laser_link">
  <sensor type="ray" name="head_laser">
    <pose>0 0 0.1 0 0 0</pose>
    <visualize>true</visualize>
    <update_rate>10</update_rate>
    <ray>
      <scan>
        <horizontal>
          <samples>360</samples>
          <resolution>1</resolution>
          <min_angle>-3.14159</min_angle>
          <max_angle>3.14159</max_angle>
        </horizontal>
      </scan>
      <range>
        <min>0.15</min>
        <max>12.0</max>
      </range>
      <noise>
        <type>gaussian</type>
        <mean>0.0</mean>
        <stddev>0.01</stddev>
      </noise>
    </ray>
  </sensor>
</gazebo>

你想想看，如果stddev设成0，仿真数据就太完美了，拿到真实机器人上反而容易崩。我一般加0.01~0.02m的高斯噪声，这样建图时能提前暴露定位抖动问题。

4.2 IMU仿真：角速度与加速度

IMU提供高频的角速度和线加速度，对里程计融合至关重要。Gazebo里用ImuPlugin就能生成6轴数据。嗯，这里要注意：IMU的坐标系定义和真实传感器一致，通常ENU或NED，我习惯用ENU，和ROS默认的REP105对齐。

<gazebo reference="imu_link">
  <sensor type="imu" name="imu_sensor">
    <pose>0 0 0 0 0 0</pose>
    <update_rate>200</update_rate>
    <imu>
      <noise>
        <type>gaussian</type>
        <rate>
          <mean>0.0</mean>
          <stddev>0.0002</stddev>
        </rate>
        <accel>
          <mean>0.0</mean>
          <stddev>0.017</stddev>
        </accel>
      </noise>
    </imu>
  </sensor>
</gazebo>

IMU的更新率我设200Hz，和真实工业级IMU接近。噪声参数参考了MPU9250的数据手册，这样仿真数据可以直接用来测试滤波算法。

4.3 里程计仿真：轮式编码器与差分驱动

里程计是导航的基础。Gazebo里用libgazebo_ros_diff_drive.so插件，直接根据轮速积分出odom。说白了，就是给机器人装一对虚拟编码器。

<gazebo>
  <plugin name="diff_drive" filename="libgazebo_ros_diff_drive.so">
    <ros>
      <namespace>/</namespace>
      <argument>odom</argument>
    </ros>
    <update_rate>50</update_rate>
    <left_joint>left_wheel_joint</left_joint>
    <right_joint>right_wheel_joint</right_joint>
    <wheel_separation>0.35</wheel_separation>
    <wheel_diameter>0.1</wheel_diameter>
    <max_wheel_torque>20</max_wheel_torque>
    <max_wheel_acceleration>1.0</max_wheel_acceleration>
    <odometry_frame>odom</odometry_frame>
    <robot_base_frame>base_footprint</robot_base_frame>
    <publish_odom>true</publish_odom>
    <publish_odom_tf>true</publish_odom_tf>
    <publish_wheel_tf>true</publish_wheel_tf>
  </plugin>
</gazebo>

里程计的话题是/odom，频率50Hz。我个人习惯同时发布TF变换，这样rviz里直接能看到odom→base_footprint的连线，调试起来非常直观。

4.4 在Gazebo中搭建测试环境

有了传感器，还得有个场地跑起来。我一般用gazebo_ros的spawn_entity把机器人放进去，再加载一个带墙壁和障碍物的世界。

下面是一个简单的世界文件片段，包含地面、墙壁和两个柱子：

<?xml version="1.0" ?>
<sdf version="1.6">
  <world name="test_world">
    <include>
      <uri>model://sun</uri>
    </include>
    <include>
      <uri>model://ground_plane</uri>
    </include>
    <!-- 墙壁 -->
    <model name="wall1">
      <static>true</static>
      <link name="link">
        <collision name="collision">
          <geometry><box><size>10 0.2 0.5</size></box></geometry>
        </collision>
        <visual name="visual">
          <geometry><box><size>10 0.2 0.5</size></box></geometry>
          <material><ambient>0.8 0.8 0.8 1</ambient></material>
        </visual>
      </link>
      <pose>0 5 0.25 0 0 0</pose>
    </model>
    <!-- 障碍物 -->
    <model name="obstacle1">
      <static>true</static>
      <link name="link">
        <collision name="collision">
          <geometry><cylinder><radius>0.3</radius><length>0.6</length></cylinder></geometry>
        </collision>
        <visual name="visual">
          <geometry><cylinder><radius>0.3</radius><length>0.6</length></cylinder></geometry>
        </visual>
      </link>
      <pose>2 2 0.3 0 0 0</pose>
    </model>
  </world>
</sdf>

启动后，用ros2 topic echo /scan就能看到激光数据，/imu/data输出IMU读数，/odom发布里程计。我个人习惯同时打开rviz2，添加LaserScan、Imu和Odometry显示，一眼就能看出传感器是否正常工作。

4.5 联合检查：传感器同步与时间戳

三个传感器都跑起来后，一定要检查时间戳是否对齐。Gazebo默认使用仿真时间，但如果你忘了设置use_sim_time:=true，ROS2会拿系统时间，导致TF和odom错乱。我曾经因为这个bug排查了整整一个下午……

传感器	话题名	典型频率	常见问题
激光雷达	/scan	10 Hz	帧率太低导致建图畸变
IMU	/imu/data	200 Hz	重力方向未对齐
里程计	/odom	50 Hz	轮距误差导致旋转漂移

启动时加上--params-file指定use_sim_time: true，或者在launch文件里设置。我个人习惯在launch里统一声明：

from launch.actions import SetParameter
SetParameter(name='use_sim_time', value=True)

这样所有节点都自动使用仿真时钟，数据采集天然同步。