6. 传感器数据管线

本文描述 Autonomy 感知子系统中传感器数据的采集、同步、坐标变换与分发机制。

6.1 传感器类型与消息

传感器	消息类型	命名空间	典型频率
2D 激光雷达	`LaserScan`	`sensor_msgs`	10–40 Hz
3D 激光/深度相机	`PointCloud2`	`sensor_msgs`	10–20 Hz
RGB 相机	`Image`	`sensor_msgs`	15–30 Hz
深度图	`Image` (encoding=16UC1/32FC1)	`sensor_msgs`	15–30 Hz
IMU	`Imu`	`sensor_msgs`	100–200 Hz
里程计	`Odometry`	`planning_msgs`	20–50 Hz

消息定义位于 autonomy/commsgs/sensor_msgs.hpp。

6.2 数据流总览

┌─────────────┐     ┌──────────────┐     ┌─────────────────┐
│ Driver/Sim  │────→│ Sensor Buffer │────→│ Transform (TF)  │
│ (硬件/仿真)  │     │ (时间戳队列)   │     │ frame 对齐       │
└─────────────┘     └──────────────┘     └────────┬────────┘
                                                   │
                    ┌──────────────────────────────┼──────────────────┐
                    ▼                              ▼                  ▼
            ObstacleLayer                   PerceptionServer      Localization
            (costmap)                       (检测/聚类)            (定位)

6.3 驱动层配置

config/driver/driver.lua 定义传感器 topic 与转发目标：

-- 规划示例
laser = {
    topic = "/scan",
    frame_id = "laser_link",
    rate = 10.0,
}

front_camera = {
    topic = "/camera/front/image_raw",
    frame_id = "camera_front_link",
    forward_targets = {"perception"},  -- 预留
}

6.4 观测缓冲（ObservationBuffer）

map/costmap_2d/observation_buffer.hpp 实现传感器观测的时间缓冲：

功能	说明
入队	`bufferObservation(obs)` 按时间戳入队
出队	`getObservations(observations)` 获取有效窗口内观测
过期清理	超过 `observation_keep_time` 的观测自动丢弃

用途：costmap 更新时聚合多帧激光观测，提高鲁棒性。

6.5 坐标变换

所有感知算法需将传感器数据变换到统一参考系（通常为 map 或 odom）：

// 查询 TF
geometry_msgs::TransformStamped tf;
transform::Buffer::Instance()->lookupTransform(
  target_frame, source_frame, time, &tf);

// 点变换
p_world = R * p_sensor + t;

数学：见 03_math.md §3.1。

6.6 时间同步策略

策略	适用场景	说明
最近邻	低频融合	取时间戳最接近的传感器帧
插值	高频 IMU + 低频相机	对位姿做 SLERP 插值
消息过滤器	ROS 风格	`message_filters::ApproximateTime` 近似同步

PerceptionServer 实现后，建议在 @frequency 循环中：

收集各传感器最新帧（或同步帧组）
统一变换到 global_frame
分发给各 PerceptionInterface 插件

6.7 激光数据管线（当前可用）

LaserScan (/scan)
    │
    ▼
ObstacleLayer::laserScanCallback()
    │
    ├─ TF: laser_link → global_frame
    ├─ 射线追踪标记自由/障碍栅格
    └─ costmap 更新 → Planning / Control

关键参数（costmap.lua）：

参数	含义
`obstacle_range`	标记障碍的最大距离 (m)
`raytrace_range`	射线清除自由空间的最大距离 (m)
`max_obstacle_height`	障碍高度上限 (m)

6.8 相机数据管线（规划）

Image (/camera/...)
    │
    ▼
PerceptionServer::OnImage()
    │
    ├─ 图像解码 (RGB/BGR)
    ├─ 预处理 (resize, normalize)  ← common/network
    ├─ ONNX 推理
    ├─ 后处理 (decode + NMS)
    └─ Detection2DArray 发布

6.9 点云数据管线（规划）

PointCloud2
    │
    ▼
PerceptionServer::OnPointCloud()
    │
    ├─ TF 变换到 map 系
    ├─ 体素下采样
    ├─ 地面分割 (RANSAC)
    ├─ 欧氏聚类
    └─ ObstacleArray / 写入 VoxelLayer

VoxelLayer（map/costmap_2d/layers/voxel_layer.*）支持 3D 点云体素化后投影到 2D costmap。

6.10 与仿真器的对接

仿真方式	传感器来源	配置
`autonomy_nav_test`	无真实传感器，静态地图	costmap 静态层
Gazebo（`autonomy_gazebo`）	`/scan`, `/camera`, `/odom`	`autonomy_ros` bridge
Stage（规划）	`cave.world` 中 hokuyolaser + camera	`simulation.lua`

详见 12 Simulation。