3. 传感器驱动接口适配：LiDAR、Camera、Radar、IMU、GPS驱动如何对接Autoware的Sensor Msgs
各位同行，今天我们来聊聊传感器驱动适配这个绕不开的话题。说实话，我见过太多项目在传感器对接这一步卡壳了——驱动写好了，数据也出来了，但Autoware就是不吃。说白了，问题往往出在消息格式上。
Autoware对传感器数据有自己的一套规矩，也就是sensor_msgs。你的驱动必须按这个规矩来，否则系统不认。我个人习惯先把这几种传感器的消息类型搞清楚，再动手写代码。
3.1 LiDAR驱动适配
LiDAR是Autoware的"眼睛"，用得最多的是sensor_msgs/PointCloud2。嗯，这里要注意，不是所有点云格式Autoware都认。
核心要求：点云必须包含x、y、z、intensity四个字段，时间戳要精确到纳秒。
我在项目中遇到过一台国产16线LiDAR，驱动默认输出的是自定义格式。当时我花了一整天排查，最后发现是point_step和row_step算错了。你想想看，一个字段偏移量不对，整个点云就全歪了。
// 正确的PointCloud2填充示例
sensor_msgs::PointCloud2 cloud;
cloud.header.frame_id = "velodyne";
cloud.header.stamp = ros::Time::now();
cloud.height = 1; // 无序点云
cloud.width = point_count;
cloud.is_dense = false;
cloud.is_bigendian = false;
// 定义字段
cloud.fields.resize(4);
cloud.fields[0].name = "x";
cloud.fields[0].offset = 0;
cloud.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
cloud.fields[0].count = 1;
cloud.fields[1].name = "y";
cloud.fields[1].offset = 4;
cloud.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
cloud.fields[1].count = 1;
cloud.fields[2].name = "z";
cloud.fields[2].offset = 8;
cloud.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
cloud.fields[2].count = 1;
cloud.fields[3].name = "intensity";
cloud.fields[3].offset = 12;
cloud.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
cloud.fields[3].count = 1;
cloud.point_step = 16; // 4个float，每个4字节
cloud.row_step = cloud.point_step * cloud.width;
避坑指南：我曾经因为frame_id写成了"velodyne16"而不是"velodyne"，导致tf树找不到坐标系，定位模块直接罢工。frame_id必须和你的tf配置完全一致。
3.2 Camera驱动适配
Camera的消息类型是sensor_msgs/Image，有时候还需要sensor_msgs/CompressedImage。我个人建议用原始图像，因为压缩图像会丢失一些细节，影响目标检测精度。
图像数据的关键在于编码格式。Autoware默认支持bgr8和rgb8，但很多USB摄像头输出的是yuyv或mjpeg。这时候你得做格式转换。
// 使用cv_bridge进行格式转换
cv_bridge::CvImage cv_image;
cv_image.encoding = "bgr8";
cv_image.image = cv::imdecode(cv::Mat(1, data.size(), CV_8UC1, data.data()), cv::IMREAD_COLOR);
cv_image.toImageMsg(); // 输出sensor_msgs/Image
小技巧：如果摄像头帧率是30fps，但Autoware处理不过来，可以在驱动里加一个节流阀。我一般用ros::Rate(10)限制到10fps，既保证实时性，又不给CPU太大压力。
3.3 Radar驱动适配
Radar的数据比较特殊，Autoware用sensor_msgs/PointCloud2来承载，但每个点除了位置信息，还要带velocity和amplitude。说白了，就是把Radar的检测目标当成特殊的点云来处理。
| 字段名 | 数据类型 | 说明 |
|---|---|---|
| x, y, z | float32 | 目标位置（米） |
| velocity | float32 | 径向速度（米/秒） |
| amplitude | float32 | 回波强度（dB） |
| id | uint16 | 目标跟踪ID |
我在适配某款77GHz毫米波雷达时，发现它的数据包里有track_id字段。一开始我没在意，结果Autoware的融合模块一直报错。后来才明白，Radar的目标跟踪需要ID来维持时间一致性。
3.4 IMU驱动适配
IMU的消息类型是sensor_msgs/Imu。这个相对简单，但有个坑——坐标系。Autoware默认IMU的坐标系是ENU（东-北-天），但很多IMU模块输出的是NED（北-东-地）。
我曾经踩过的坑：用了一款国产IMU，输出的是NED坐标系，没做转换直接喂给Autoware。结果车辆定位在转弯时直接漂移了2米。后来加了坐标变换才解决。
// IMU数据填充示例
sensor_msgs::Imu imu_msg;
imu_msg.header.stamp = ros::Time::now();
imu_msg.header.frame_id = "imu_link";
// 线性加速度（m/s^2）
imu_msg.linear_acceleration.x = accel_x;
imu_msg.linear_acceleration.y = accel_y;
imu_msg.linear_acceleration.z = accel_z;
// 角速度（rad/s）
imu_msg.angular_velocity.x = gyro_x;
imu_msg.angular_velocity.y = gyro_y;
imu_msg.angular_velocity.z = gyro_z;
// 方向（四元数）
imu_msg.orientation.x = qx;
imu_msg.orientation.y = qy;
imu_msg.orientation.z = qz;
imu_msg.orientation.w = qw;
3.5 GPS驱动适配
GPS对应的是sensor_msgs/NavSatFix。这个接口很直观，就是经纬度和海拔。但Autoware真正需要的是nav_msgs/Odometry，因为定位模块要做坐标转换。
我的做法是：驱动输出NavSatFix，然后在Autoware内部用robot_localization包转成Odometry。这样既保持了原始数据，又方便后续融合。
// NavSatFix消息填充
sensor_msgs::NavSatFix gps_msg;
gps_msg.header.stamp = ros::Time::now();
gps_msg.header.frame_id = "gps_link";
gps_msg.latitude = 39.9042; // 纬度
gps_msg.longitude = 116.4074; // 经度
gps_msg.altitude = 50.0; // 海拔（米）
gps_msg.status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
gps_msg.position_covariance[0] = 0.01; // 水平精度
建议：GPS的position_covariance矩阵别随便填。我见过有人全填0，结果卡尔曼滤波器直接发散。至少填个对角线值，代表你的GPS精度。
3.6 传感器驱动适配流程图
下面这张图是我自己总结的适配流程，帮你理清思路：
3.7 总结与避坑清单
好了，我把这五种传感器的适配要点捋了一遍。最后给你一份我自己的避坑清单：
- LiDAR：PointCloud2的
point_step和row_step必须精确计算，多一个字节都不行。 - Camera：编码格式一定要转成
bgr8或rgb8，别偷懒用压缩图像。 - Radar：别忘了加
velocity和id字段，否则融合模块不认。 - IMU：坐标系转换是重灾区，ENU和NED搞反了定位就废了。
- GPS：
position_covariance别填零，至少给个对角线值。
记住一句话：传感器驱动适配，七分在消息格式，三分在坐标系。这两样搞定了，Autoware就能乖乖吃你的数据。
可可精品资料库