3、传感器仿真与数据读取：激光雷达仿真、相机仿真、IMU仿真、使用ros2bag录制与回放传感器数据
各位同学，欢迎来到第三章。这一章我们聊聊传感器仿真。
说实话，做自动驾驶最头疼的是什么？不是算法写不出来，而是你写完了算法，没有传感器数据去验证。真车上路跑一圈？成本太高，风险太大。所以，仿真就成了我们的救命稻草。
我个人习惯，在写任何感知算法之前，先把传感器仿真环境搭好。这样调试起来，心里踏实。
3.1 激光雷达仿真：从点云到感知
激光雷达，说白了就是车的“眼睛”。它通过发射激光束，测量周围物体的距离，生成点云数据。
在ROS2里，我们常用gazebo配合velodyne插件来做激光雷达仿真。我记得第一次搭这个环境时，折腾了一整天，最后发现是模型文件里少了个参数。
核心思路：在Gazebo中加载激光雷达模型，通过插件发布/scan话题，节点订阅后解析点云。
先看一个典型的激光雷达URDF模型配置：
<link name="laser_link">
<visual>
<geometry>
<cylinder length="0.05" radius="0.05"/>
</geometry>
</visual>
</link>
<joint name="laser_joint" type="fixed">
<parent link="base_link"/>
<child link="laser_link"/>
<origin xyz="0.3 0 0.2" rpy="0 0 0"/>
</joint>
<gazebo reference="laser_link">
<sensor type="ray" name="laser_sensor">
<pose>0 0 0 0 0 0</pose>
<visualize>true</visualize>
<update_rate>10</update_rate>
<ray>
<scan>
<horizontal samples="360" resolution="1" min_angle="-3.14159" max_angle="3.14159"/>
</scan>
<range min="0.1" max="30.0" resolution="0.01"/>
</ray>
</sensor>
</gazebo>
这里要注意几个关键参数：
- samples：水平采样点数，360就是每度一个点。我建议至少设360，少了点云太稀疏。
- update_rate：更新频率，10Hz是常规设置。想模拟高线束雷达，可以提到20Hz。
- range：探测距离，我一般设30米。太远了计算量大，太近了感知范围不够。
小技巧：启动Gazebo时加上--verbose参数，能看到传感器插件的加载日志。出问题时，这个很有用。
启动仿真后，用ros2 topic echo /scan就能看到数据流。但注意，原始数据是sensor_msgs/LaserScan格式，里面只有距离和角度。想做更高级的处理，得转成点云。
我习惯用laser_geometry包里的LaserScanToPointCloud2节点来做转换。命令很简单：
ros2 run laser_geometry laser_scan_to_point_cloud2
它会订阅/scan，发布/cloud。这样你就能在Rviz2里看到漂亮的三维点云了。
3.2 相机仿真：图像数据的获取与处理
相机仿真，比激光雷达要复杂一点。因为图像数据量大，而且涉及到畸变、曝光等参数。
在Gazebo里，相机模型也是通过插件实现的。我遇到过最坑的事，是仿真图像一直黑屏，查了半天发现是camera_info话题没发布，导致图像编码不对。
来看一个标准的相机URDF配置：
<gazebo reference="camera_link">
<sensor type="camera" name="camera_sensor">
<update_rate>30</update_rate>
<camera>
<horizontal_fov>1.047</horizontal_fov>
<image>
<width>640</width>
<height>480</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.1</near>
<far>100</far>
</clip>
</camera>
</sensor>
</gazebo>
几个要点：
- horizontal_fov：水平视场角，1.047弧度约60度。想模拟广角相机，可以设到1.57（90度）。
- format：图像格式，R8G8B8是24位真彩色。如果只做灰度处理，用L8更省带宽。
- update_rate：30Hz是常规帧率。但注意，帧率越高，CPU负载越大。我一般先设15Hz调试，稳定后再提上去。
注意：相机仿真很吃资源。如果你电脑配置一般，建议把图像分辨率降到320x240。否则Gazebo可能卡成PPT。
启动后，用ros2 topic echo /camera/image_raw能看到图像数据吗？不行，那是二进制流。想看图像，得用ros2 run rqt_image_view rqt_image_view。或者直接在Rviz2里添加Image显示。
我个人习惯，在写图像处理节点前，先用image_tools包里的showimage工具快速验证一下：
ros2 run image_tools showimage --ros-args -r image:=/camera/image_raw
这样能确保图像话题确实在发布，而且格式正确。
3.3 IMU仿真：惯性测量单元的数据模拟
IMU，全称惯性测量单元。它提供加速度和角速度数据，是定位和姿态估计的关键。
说实话，IMU仿真比前两个简单。因为数据量小，而且Gazebo自带的插件就很成熟。
配置如下：
<gazebo reference="imu_link">
<sensor type="imu" name="imu_sensor">
<pose>0 0 0 0 0 0</pose>
<update_rate>100</update_rate>
<imu>
<noise>
<type>gaussian</type>
<rate>
<mean>0.0</mean>
<stddev>0.01</stddev>
</rate>
<accel>
<mean>0.0</mean>
<stddev>0.01</stddev>
</accel>
</noise>
</imu>
</sensor>
</gazebo>
这里我特别想强调噪声参数。很多新手喜欢把噪声设成0，觉得这样数据干净。但你想，真实IMU哪有那么完美？
我曾经在项目里用零噪声的仿真数据训练了一个滤波器，结果一上真车，滤波器直接发散。从那以后，我仿真时一定会加高斯噪声，标准差至少设0.01。
IMU数据的话题是/imu/data，格式是sensor_msgs/Imu。里面包含：
- orientation：四元数表示的姿态
- angular_velocity：角速度（rad/s）
- linear_acceleration：线加速度（m/s²）
调试技巧：把IMU数据可视化出来。在Rviz2里添加Axes显示，选择/imu/data话题，你就能看到小车姿态的实时变化。很直观。
3.4 使用ros2bag录制与回放传感器数据
好了，传感器都跑起来了。但每次调试都要启动Gazebo，太慢了。怎么办？用ros2bag把数据录下来，以后直接回放。
我个人觉得，ros2bag是ROS2里最实用的工具之一。它就像黑匣子，把所有的传感器数据都记录下来。
录制命令很简单：
ros2 bag record /scan /camera/image_raw /imu/data
这条命令会录制三个话题。如果你想录所有话题，用-a参数：
ros2 bag record -a
但注意，-a会录下所有话题，包括tf、clock这些。数据量会很大。我一般只录需要的几个话题。
录制完成后，会在当前目录生成一个rosbag2_xxx文件夹。里面是数据库文件，不是以前的.bag文件了。ROS2改用SQLite3存储，好处是查询更快，坏处是不能直接用老工具打开。
回放命令：
ros2 bag play rosbag2_xxx
回放时，默认按录制时的速度播放。你可以用-r参数控制速度：
-r 0.5：半速播放，适合仔细分析-r 2.0：两倍速，适合快速验证
避坑指南：我曾经回放数据时，发现图像数据全是黑的。查了半天，原来是回放时/clock话题没同步。解决办法：回放时加上--clock参数，让系统时间跟着bag走。
完整的回放命令：
ros2 bag play rosbag2_xxx --clock -r 1.0
还有一个实用功能：查看bag信息。用ros2 bag info命令：
ros2 bag info rosbag2_xxx
它会显示：
- 录制时长
- 话题列表及消息数量
- 总数据量大小
这个功能我经常用。比如调试时发现某个话题数据量异常少，就能快速定位问题。
3.5 实战：搭建一个完整的传感器仿真系统
说了这么多，我们来动手搭一个完整的系统。目标：启动Gazebo，加载一辆带激光雷达、相机和IMU的小车，然后用ros2bag录制所有数据。
步骤很简单：
- 启动Gazebo世界：
gazebo --verbose worlds/empty.world - 生成小车模型：用
ros2 run gazebo_ros spawn_entity.py -file car.urdf - 验证话题：
ros2 topic list，确认有/scan、/camera/image_raw、/imu/data - 录制数据：
ros2 bag record /scan /camera/image_raw /imu/data -o my_sensor_data - 停止录制：按Ctrl+C
- 回放验证：
ros2 bag play my_sensor_data --clock
核心要点：仿真不是目的，目的是拿到可靠的数据去验证算法。所以，每次录制前，先确认传感器数据是正常的。我习惯在Rviz2里同时看点云、图像和IMU姿态，三路数据都正常了，才开始录制。
嗯，这一章的内容就到这里。传感器仿真这块，说白了就是“先模拟，再实战”。你想想看，如果仿真环境里都跑不通，真车上就更别想了。
下一章，我们会基于这些传感器数据，开始写感知算法。到时候，这些录好的bag文件就是我们的“黄金数据集”。
记住：好的数据，是算法成功的一半。