3 · 传感器驱动与数据读取

写ROS节点驱动激光雷达、IMU、相机，再把数据流可视化出来——这是SLAM流水线的“第一公里”。我见过太多同学在算法上花大把时间，结果一跑数据发现时间戳是乱的，或者点云畸变到没法看。说白了，驱动写不好，后面全是白搭。

3.1 激光雷达驱动：从串口到点云

我早期做机器人时，用的还是单线雷达，串口数据一包一包读，校验和算错一个位，整个点云就飞了。现在ROS里大部分激光雷达都有现成驱动包，比如 rplidar_ros、urg_node。但核心逻辑你得懂：

• 串口/网络配置：波特率、设备权限（chmod 666 /dev/ttyUSB0）
• 数据帧解析：起始位、角度、距离、强度
• 发布话题：sensor_msgs/LaserScan 或 PointCloud2

<launch>
  <node name="lidar_driver" pkg="rplidar_ros" type="rplidarNode" output="screen">
    <param name="serial_port" value="/dev/ttyUSB0"/>
    <param name="frame_id" value="laser_frame"/>
    <param name="angle_compensate" value="true"/>
  </node>
</launch>

3.2 IMU驱动：加速度+角速度的原始味道

IMU数据看着简单，但坑特别多。我遇到过最离谱的一次——IMU的坐标系和机器人坐标系差了90度，结果融合出来的姿态直接翻车。驱动节点里至少要做这几件事：

1. 读取原始寄存器或串口数据
2. 转换到ROS坐标系（ENU或NED，务必和你的其他传感器统一）
3. 填充协方差矩阵（很多人直接填0，后面优化会出问题）

# Python 示例：发布IMU消息
import rospy
from sensor_msgs.msg import Imu

pub = rospy.Publisher('/imu/data_raw', Imu, queue_size=10)
imu_msg = Imu()
imu_msg.header.frame_id = 'imu_link'
imu_msg.linear_acceleration.x = ax
imu_msg.angular_velocity.z = wz
# 别忘了时间戳
imu_msg.header.stamp = rospy.Time.now()
pub.publish(imu_msg)

3.3 相机驱动：图像与相机信息

相机驱动相对成熟，usb_cam、uvc_camera 或者 realsense2_camera 都能直接出图。但有个细节——相机内参必须通过 CameraInfo 话题发布，否则后面的图像矫正、V-SLAM全得崩。

话题	消息类型	说明
/camera/image_raw	sensor_msgs/Image	原始图像数据
/camera/camera_info	sensor_msgs/CameraInfo	内参矩阵、畸变系数

你想想看，如果内参给错了，后面特征点投影到3D时就会歪掉。我一般会在驱动里加一个camera_info_url参数，从YAML文件加载内参，这样标定一次，到处使用。

3.4 可视化：让数据“说话”

驱动写完了，怎么知道数据对不对？别只靠打印。用 rviz 看激光点云和图像，用 rqt_image_view 看实时画面。我习惯在rviz里同时显示TF树和传感器数据，一眼就能看出frame_id有没有配错。

# 启动可视化（终端）
rosrun rviz rviz -d my_slam.rviz
# 或者直接看图像
rosrun rqt_image_view rqt_image_view /camera/image_raw

3.5 驱动节点的时间同步陷阱

三个传感器各自发布话题，但时间戳可能来自不同时钟源。激光雷达用电脑时间，IMU用板载时钟，相机用硬件触发——这就乱了。我建议在驱动节点里统一使用 ros::Time::now() 打时间戳，或者用 message_filters 做近似同步。后面第4章会细讲，但驱动阶段就要有这个意识。