机器人 / 系统课时
将 PointCloud2 映射到障碍层
将 PointCloud2 映射到障碍层
深度相机给出的是带坐标和时间戳的三维点云,不能直接当作二维导航障碍。本课用 Nav2 的 ObstacleLayer 接收 PointCloud2,经 TF 变换、按高度过滤,再将有效点投影到局部代价图,最终在 RViz 2 中检查障碍是否随观测更新。
学习目标
- 解释点云坐标变换、高度筛选与二维网格标记的先后关系。
- 配置 Nav2 局部障碍层订阅深度点云并设置高度、距离和清除参数。
- 用话题、TF 和 RViz 2 排查点云存在但障碍图不更新的问题。
确定点云的坐标与高度基准
假设深度相机在距地面 0.35 m 处发布 /camera/depth/points,消息类型为 sensor_msgs/msg/PointCloud2,header.frame_id 为 camera_depth_optical_frame。相机光学坐标系通常以 Z 轴指向镜头前方,因此不能拿点云原始 z 值当离地高度。
局部代价图设置 global_frame: odom,Nav2 通过带时间戳的 TF 链 odom、base_link、camera_depth_optical_frame 将点变换到 odom,再检查变换后的 z 值并把 x、y 映射到二维网格。以下高度阈值假设平地且 odom 的 z≈地面高度;坡道或底盘起伏明显时,应先做相对地面高度估计,不能照搬固定窗口。
以 0.10~1.20 m 为障碍高度窗口:地板点 z=0.02 m 被忽略;椅腿上的点 z=0.45 m 被标记;离地 1.50 m 的横梁被忽略。低于 0.10 m 的真实小障碍也会漏检,阈值必须结合机器人最低离地间隙实测。

配置局部 ObstacleLayer
下面是 nav2_params.yaml 中 local_costmap 的参数段(片段示例):应合并到已有的 Nav2 参数文件,其他导航节点的配置仍须保留。前提是相机持续发布上述点云、相机外参 TF 正确,且 odom 到 base_link 的变换已由里程计提供。
local_costmap:
local_costmap:
ros__parameters:
global_frame: odom
robot_base_frame: base_link
update_frequency: 5.0
publish_frequency: 2.0
rolling_window: true
width: 4.0
height: 4.0
resolution: 0.05
robot_radius: 0.25
plugins: ["obstacle_layer", "inflation_layer"]
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: true
max_obstacle_height: 1.20
observation_sources: depth_cloud
depth_cloud:
topic: /camera/depth/points
data_type: "PointCloud2"
min_obstacle_height: 0.10
max_obstacle_height: 1.20
obstacle_min_range: 0.20
obstacle_max_range: 2.50
raytrace_min_range: 0.0
raytrace_max_range: 3.0
marking: true
clearing: true
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
inflation_radius: 0.35
cost_scaling_factor: 3.04.0 m × 4.0 m、0.05 m 分辨率对应 80×80 个单元,窗口随机器人移动。obstacle_max_range 限制标记距离,raytrace_max_range 限制沿有效观测射线清除旧障碍的距离;二者不是同一个开关。marking 负责写入占据单元,clearing 利用有效射线清除可见空闲区域。没有深度返回的区域不能直接判定为空闲。
验证障碍是否写入代价图
使用现有启动方式加载修改后的参数并激活 Nav2 节点。在相机和 Nav2 都运行后执行:
ros2 topic type /camera/depth/points
ros2 param get /local_costmap/local_costmap obstacle_layer.depth_cloud.max_obstacle_height预期输出:
sensor_msgs/msg/PointCloud2
Double value is: 1.2如果节点名被命名空间重映射,第二条命令需要改用实际节点全名。随后执行 ros2 run tf2_ros tf2_echo odom camera_depth_optical_frame,预期持续看到平移和旋转数值;若一直等待变换或报告外推错误,先核对 header.frame_id、TF 链及消息时间戳。时间戳不一致时,还应检查各节点的 use_sim_time 是否统一。
打开 RViz 2,将 Global Options → Fixed Frame 设为 odom,通过 Add → By topic 添加 /camera/depth/points 的 PointCloud2 显示和 /local_costmap/costmap 的 Map 显示。把纸箱放在前方 1 m、距地约 0.4 m 的观测区内:应看到纸箱点云对应位置出现障碍单元及周围膨胀代价;移开后,在有效清除观测覆盖下障碍应消退。
根据现象调整阈值与清除
- 有点云、无障碍:先看
PointCloud2的frame_id和 TF 是否连通,再检查坐标变换后的实际离地高度是否落在0.10~1.20 m,以及距离是否在0.20~2.50 m。 - 地面大面积误标:核对相机外参、机器人姿态与
odom的高度基准;仅在确认地面高度分布后,逐步提高min_obstacle_height,例如从0.10调到0.15 m,同时重新测试低矮障碍。 - 障碍移走仍残留:确认
clearing: true、有效射线覆盖原障碍区域,必要时检查点云丢帧和时间同步。无效深度点不会自动清空旧障碍,不能靠无限增大清除距离解决。 - 多个高度物体互相遮挡:ObstacleLayer 是三维点经高度筛选后的二维投影,不保留每个网格内的多层高度结构;需要精细处理悬空物体或垂直占据时,再考虑 VoxelLayer 或点云预处理。
验收时不要只看点云显示正常,还要确认同一障碍在局部代价图中出现、离开后合理消退,并且地面和机器人可通过的上方空间没有被错误标记。