7. 传感器与地图消息

本文详述 sensor_msgs、map_msgs 及 PointCloud2 工具链。

7.1 sensor_msgs

头文件：autonomy/commsgs/sensor_msgs.hpp

7.1.1 激光雷达

LaserScan

字段	类型	说明
`header`	`std_msgs::Header`	时间戳 + `frame_id`（通常为 `laser`）
`angle_min`	`float`	起始角度 (rad)
`angle_max`	`float`	结束角度 (rad)
`angle_increment`	`float`	角度步进 (rad)
`time_increment`	`float`	测量间隔 (s)
`scan_time`	`float`	扫描周期 (s)
`range_min` / `range_max`	`float`	有效量程 (m)
`ranges`	`vector<float>`	距离读数
`intensities`	`vector<float>`	反射强度

commsgs::sensor_msgs::LaserScan scan;
scan.header.frame_id = "laser";
scan.angle_min = -M_PI;
scan.angle_max = M_PI;
scan.angle_increment = M_PI / 180.0;
scan.range_min = 0.1f;
scan.range_max = 30.0f;
scan.ranges.resize(360, std::numeric_limits<float>::infinity());

7.1.2 IMU

Imu

字段组	字段	说明
方向	`orientation` (Quaternion)	融合后姿态
角速度	`angular_velocity` (Vector3)	rad/s
线加速度	`linear_acceleration` (Vector3)	m/s²
协方差	`orientation_covariance[9]`	3×3 行主序
协方差	`angular_velocity_covariance[9]`	3×3
协方差	`linear_acceleration_covariance[9]`	3×3

协方差 -1 表示未知
orientation 全零表示不提供方向估计

7.1.3 点云

PointCloud2

字段	类型	说明
`header`	`std_msgs::Header`	时间戳 + 坐标系
`height`	`uint32`	行数（无序点云为 1）
`width`	`uint32`	列数（总点数 = height × width）
`fields`	`vector<PointField>`	字段描述
`is_bigendian`	`bool`	字节序
`point_step`	`uint32`	单点字节数
`row_step`	`uint32`	单行字节数
`data`	`vector<uint8>`	二进制点云数据
`is_dense`	`bool`	是否无 NaN/Inf

PointField

字段	类型	说明
`name`	`string`	字段名（`"x"`, `"y"`, `"z"`, `"rgb"`, `"intensity"`）
`offset`	`uint32`	在点内的字节偏移
`datatype`	`uint8`	数据类型枚举
`count`	`uint32`	元素数量

datatype 枚举：

值	常量	C++ 类型
1	`INT8`	`int8_t`
2	`UINT8`	`uint8_t`
3	`INT16`	`int16_t`
4	`UINT16`	`uint16_t`
5	`INT32`	`int32_t`
6	`UINT32`	`uint32_t`
7	`FLOAT32`	`float`
8	`FLOAT64`	`double`

PointCloud（旧格式，含 ChannelFloat32）已标记 deprecated，推荐使用 PointCloud2。

7.1.4 相机

类型	关键字段	用途
`Image`	`height, width, encoding, step, data`	原始图像
`CompressedImage`	`format, data`	压缩图像（JPEG/PNG）
`CameraInfo`	`k[9], d, r[9], p[12], distortion_model`	相机标定
`RegionOfInterest`	`x_offset, y_offset, height, width`	ROI

CameraInfo 矩阵：

K = [fx  0 cx]     内参（3×3 行主序，9 元素）
    [ 0 fy cy]
    [ 0  0  1]

P = [fx'  0  cx' Tx]   投影矩阵（3×4 行主序，12 元素）
    [ 0  fy' cy' Ty]
    [ 0   0   1   0]

7.1.5 其他传感器

类型	用途
`Range`	超声波/红外测距
`Joy`	手柄输入
`Illuminance`	光照度
`BatteryState`	电池状态（proto 别名）

7.1.6 转换完整度

类型	ToProto	FromProto
`LaserScan`	⏳ 部分	⏳ 部分
`Imu`	⏳ 部分	⏳ 部分
`PointCloud2`	⏳ 部分	⏳ 部分
`Image`	⏳ 部分	⏳ 部分
`CameraInfo`	⏳ 部分	⏳ 部分

7.2 PointCloud2 工具链

7.2.1 PointCloud2Modifier

头文件：autonomy/commsgs/point_cloud2_iterator.hpp

便捷设置字段与大小：

commsgs::sensor_msgs::PointCloud2Modifier modifier(cloud);
modifier.setPointCloud2FieldsByString(4, "x", "y", "z", "intensity");
modifier.resize(num_points);

方法	说明
`setPointCloud2Fields(n, ...)`	按名称/类型/数量设置字段
`setPointCloud2FieldsByString(n, ...)`	按字符串简写（`"xyz"`, `"rgb"`）
`resize(n)`	设置点数并分配 data
`reserve(n)`	预分配
`clear()`	清空

7.2.2 PointCloud2Iterator

按字段名遍历点云：

commsgs::sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
commsgs::sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
commsgs::sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");

for (size_t i = 0; i < cloud.width; ++i, ++iter_x, ++iter_y, ++iter_z) {
    *iter_x = points[i].x;
    *iter_y = points[i].y;
    *iter_z = points[i].z;
}

只读版本：PointCloud2ConstIterator<T>。

使用方：autonomy/map/costmap_2d/layers/obstacle_layer.cpp、voxel_layer.cpp。

7.2.3 point_field_conversion

头文件：autonomy/commsgs/point_field_conversion.hpp

编译期类型映射：

// enum → C++ 类型
using T = commsgs::sensor_msgs::pointFieldTypeAsType<
    commsgs::sensor_msgs::PointField::FLOAT32>::type;  // float

// C++ 类型 → enum
constexpr auto dt = commsgs::sensor_msgs::typeAsPointFieldType<float>::value;

// 从 buffer 读取
float val = commsgs::sensor_msgs::readPointCloud2BufferValue<float>(data_ptr, datatype);

7.3 map_msgs

头文件：autonomy/commsgs/map_msgs.hpp

7.3.1 静态栅格地图

OccupancyGrid

字段	类型	说明
`header`	`std_msgs::Header`	`frame_id` 通常为 `"map"`
`info`	`MapMetaData`	分辨率、尺寸、原点
`data`	`vector<int16>`	行主序栅格值

data 值语义：

值	含义
`-1`	未知
`0`	自由
`1–99`	概率占据
`100`	完全占据

MapMetaData

字段	类型	说明
`map_load_time`	`builtin_interfaces::Time`	地图加载时间
`resolution`	`float`	m/cell
`width` / `height`	`uint32`	栅格尺寸 (cells)
`origin`	`geometry_msgs::Pose`	左下角 (0,0) 的世界坐标

OccupancyGridUpdate

增量更新：x, y, width, height 定义矩形区域 + vector<int8> data。

7.3.2 代价地图（Nav2 风格）

类型	说明
`Costmap`	完整代价地图（`header` + `CostmapMetaData` + `data`）
`CostmapUpdate`	增量更新
`CostmapMetaData`	分辨率、尺寸、原点、layer 名
`CostmapFilterInfo`	速度限制过滤器元数据
`GetCostmapRequest/Response`	获取代价地图服务
`LoadMapRequest/Response`	加载地图服务
`SaveMapRequest/Response`	保存地图服务

Costmap data 值语义：

值	含义
`0`	自由
`1–252`	代价递增
`253`	内切障碍
`254`	致命障碍
`255`	未知

7.3.3 GridMap（2.5D 多层栅格）

类型	说明
`GridMapInfo`	分辨率、尺寸、中心位姿
`GridMap`	`layers`（层名）+ `data`（行主序 float 数组，按层排列）
`basic_layers`	决定有效性的基础层

典型层名："elevation", "traversability", "cost"。

7.3.4 3D 地图

类型	说明
`Octomap`	八叉树二进制序列化
`OctomapWithPose`	带位姿的八叉树
`VoxelGrid`	体素栅格

7.3.5 辅助类型

类型	说明
`GridCells`	2D 栅格单元点集

7.3.6 转换完整度

类型	ToProto	FromProto
`OccupancyGrid`	⏳ 部分	⏳ 部分
`Costmap`	⏳ 部分	⏳ 部分
`GridMap`	⏳ 部分	⏳ 部分
`MapMetaData`	✅ 大部分	✅ 大部分

7.4 数据流：传感器 → 代价地图

LaserScan / PointCloud2
        │
        ▼
  obstacle_layer（PointCloud2Iterator 解析 xyz）
        │
        ▼
  Costmap2D（内部 uint8 数组）
        │
        ▼
  map_msgs::Costmap（发布/可视化）

7.5 stereo_msgs

头文件：autonomy/commsgs/stereo_msgs.hpp

类型	说明
`DisparityImage`	`sensor_msgs::Image` + 视差范围/增量/基线等参数

用于双目立体视觉深度估计。

7.6 使用建议

场景	推荐消息	注意事项
2D 导航地图	`OccupancyGrid`	`data` 行主序，origin 为左下角
实时避障	`Costmap` / `CostmapUpdate`	253/254 为障碍
3D 地形	`GridMap`	多层 float，注意 `basic_layers`
激光输入	`LaserScan`	检查 `ranges.size()` 与角度一致性
3D 障碍	`PointCloud2`	使用 Modifier + Iterator 构造
相机标定	`CameraInfo`	K 全零表示未标定