【一看就会】【nav2源码解析系列】【第六篇】--【基础算法实现类】--【规划类】--nav2_costmap_2d模块

文章目录

• 前言
• 一、nav2_costmap_2d综述
• 二、逻辑流程
• • controller模块和DWB模块中的调用
  • • nav2_controller
    • nav2_dwb_controller
    • • DWBLocalPlanner::coreScoringAlgorithm
      • DWBLocalPlanner::scoreTrajectory
      • BaseObstacleCritic::scoreTrajectory
      • BaseObstacleCritic::scorePose
    • costmap中被调用的两个函数
    • worldToMap
    • getCost
• 三、costmap生成逻辑
• • 一、costmap 是什么
  • 二、输入输出
  • • 输入（全部是话题）
    • 输出（话题和服务）
    • 对外提供的进程内函数调用接口（被 controller/planner/DWB 使用）
  • 三、阶段一：初始化（configure，一次性）
  • 四、阶段二：持续更新（激活后，周期性）
  • 五、各 layer 具体怎么"生成"内容
  • 六、产出

---

前言

前面那五篇，把从全局规划–控制输出，这个流程讲清楚了。
但是在机器人行驶中，还有一个很重要的，就是绕障。
本文进行nav2中的绕障部分以及其中的关键模块nav2_costmap_2d讲解。

---

一、nav2_costmap_2d综述

这个模块有两种作为两种角色：
作为库：被 controller_server、planner_server 等在同一进程内创建成对象
作为独立节点：也有独立可执行文件（costmap_2d_node），可单独跑

这个模块核心就是生成代价地图，然后供别的模块使用。

二、逻辑流程

在讲costmap生成之前，我们先讲清楚绕障的逻辑流程。

需要注意的是，controller可以选择不同的控制插件，本文只讲DWB插件。

controller模块和DWB模块中的调用

nav2_controller

controller模块中只是判断了一下代价地图是否就绪：while (!costmap_ros_->isCurrent())

nav2_dwb_controller

核心绕障在DWB中。
就像前文讲述的，controller调用dwb中的computeVelocityCommands函数。
而computeVelocityCommands函数中，会做路径剪裁转换，初始化评分器，还有核心绕障循环coreScoringAlgorithm，生成不同轨迹，然后打分选出得分最高的一条轨迹，使用其作为当前控制。

DWBLocalPlanner::coreScoringAlgorithm

这个核心逻辑可以见我另外一篇讲DWA的博客，在此就不细讲了。

这个函数中，调用了给生成轨迹打分函数 scoreTrajectory

源码注释：

// 包含DWB消息类型中轨迹评分的定义dwb_msgs::msg::TrajectoryScore// DWB局部规划器的核心评分算法函数// 参数：// pose: 机器人当前的位置和朝向（二维位姿）// velocity: 机器人当前的速度（线速度和角速度）// results: 指向局部规划评估结果消息的共享指针，用于存储所有轨迹的评分详情DWBLocalPlanner::coreScoringAlgorithm(constgeometry_msgs::msg::Pose2D&pose,constnav_2d_msgs::msg::Twist2D velocity,std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation>&results){// 声明用于存储采样速度的变量nav_2d_msgs::msg::Twist2D twist;// 声明用于存储生成的轨迹dwb_msgs::msg::Trajectory2D traj;// 声明存储最优和最差评分的变量dwb_msgs::msg::TrajectoryScore best,worst;// 初始化最优总分为-1（表示未找到合法轨迹）best.total=-1;// 初始化最差总分为-1（表示未找到合法轨迹）worst.total=-1;// 创建非法轨迹跟踪器，用于记录非法轨迹的统计信息IllegalTrajectoryTracker tracker;// 通知轨迹生成器开始新一轮迭代，传入当前速度作为参考traj_generator_->startNewIteration(velocity);// 当还有更多速度采样值时循环while(traj_generator_->hasMoreTwists()){// 获取下一个待评估的速度采样值twist=traj_generator_->nextTwist();// 基于当前位姿、当前速度和采样速度生成对应的轨迹traj=traj_generator_->generateTrajectory(pose,velocity,twist);try{// 尝试对生成的轨迹进行评分，传入当前最优总分作为参考（用于提前终止优化）dwb_msgs::msg::TrajectoryScore score=scoreTrajectory(traj,best.total);// 将当前轨迹标记为合法轨迹，记录到跟踪器中tracker.addLegalTrajectory();// 如果results指针非空，则将本次评分结果存入results中if(results){results->twists.push_back(score);}// 如果当前最优总分为负（即尚未找到合法轨迹）或当前评分优于最优评分，则更新最优评分if(best.total<0||score.total<best.total){best=score;// 如果results非空，记录最优轨迹在列表中的索引if(results){results->best_index=results->twists.size()-1;}}// 如果当前最差总分为负或当前评分差于最差评分，则更新最差评分if(worst.total<0||score.total>worst.total){worst=score;// 如果results非空，记录最差轨迹在列表中的索引if(results){results->worst_index=results->twists.size()-1;}}}catch(constdwb_core::IllegalTrajectoryException&e){// 捕获非法轨迹异常（例如轨迹碰撞、超出速度限制等）// 如果results非空，仍然记录该轨迹的信息if(results){// 创建一个失败的评分对象dwb_msgs::msg::TrajectoryScore failed_score;// 保存生成的轨迹（即便非法，也记录其路径点）failed_score.traj=traj;// 创建一个批判器评分条目，记录是哪个批判器判定该轨迹非法dwb_msgs::msg::CriticScore cs;cs.name=e.getCriticName();// 获取抛出异常的批判器名称cs.raw_score=-1.0;// 原始分数设为-1表示无效failed_score.scores.push_back(cs);failed_score.total=-1.0;// 总分设为-1表示非法// 将该失败轨迹评分存入results列表results->twists.push_back(failed_score);}// 将非法轨迹信息记录到跟踪器中tracker.addIllegalTrajectory(e);}}// 循环结束后，如果最优总分仍为负，说明没有找到任何合法轨迹if(best.total<0){// 如果开启了调试轨迹详细信息，则输出错误日志if(debug_trajectory_details_){// 输出跟踪器中汇总的错误信息RCLCPP_ERROR(rclcpp::get_logger("DWBLocalPlanner"),"%s",tracker.getMessage().c_str());// 遍历并输出各类非法轨迹的百分比统计（按批判器名称和非法原因分类）for(autoconst&x:tracker.getPercentages()){RCLCPP_ERROR(rclcpp::get_logger("DWBLocalPlanner"),"%.2f: %10s/%s",x.second,x.first.first.c_str(),x.first.second.c_str());}}// 抛出无合法轨迹的异常，通知上层调用者规划失败throwNoLegalTrajectoriesException(tracker);}// 返回评分最优的轨迹returnbest;}

DWBLocalPlanner::scoreTrajectory

这个只是个调用的壳子，核心是调用了critic->scoreTrajectory。也就是BaseObstacleCritic::scoreTrajectory函数。

源码注释：

// 对单条轨迹进行综合评分，返回包含各项批判器评分及总分的评分结果dwb_msgs::msg::TrajectoryScoreDWBLocalPlanner::scoreTrajectory(constdwb_msgs::msg::Trajectory2D&traj,// 待评分的轨迹double best_score)// 当前已知的最优总分，用于提前终止优化{// 创建评分结果对象dwb_msgs::msg::TrajectoryScore score;// 将待评分的轨迹存入结果中，便于后续调试或记录score.traj=traj;// 遍历所有已注册的轨迹批判器（TrajectoryCritic）// 每个批判器负责评估轨迹的某个方面（如安全性、平滑性、与全局路径的偏差等）for(TrajectoryCritic::Ptr&critic:critics_){// 创建单个批判器的评分条目dwb_msgs::msg::CriticScore cs;// 记录批判器的名称（用于调试和日志）cs.name=critic->getName();// 记录批判器的权重缩放因子（scale），用于调整该批判器在总评分中的影响力cs.scale=critic->getScale();// 如果缩放因子为0，表示该批判器被禁用，无需计算其评分// 直接将空评分条目加入列表并跳过本次循环if(cs.scale==0.0){score.scores.push_back(cs);continue;}// 调用批判器对当前轨迹进行评分，得到原始评分值double critic_score=critic->scoreTrajectory(traj);// 将原始评分存入评分条目cs.raw_score=critic_score;// 将评分条目加入结果列表score.scores.push_back(cs);// 将原始评分乘以权重后累加到总分中// 注意：通常情况下轨迹评分越低表示越好（成本函数），所以这里total是累加成本score.total+=critic_score*cs.scale;// 如果启用了“短路轨迹评估”优化，并且当前已经存在一个正的最优分数（best_score > 0），// 且当前累计总分已经超过了最优分数，则说明当前轨迹不可能优于已知最优轨迹。// 因为后续批判器只会继续增加正值（成本），总分只会越来越高（更差），// 因此可以提前终止评估，避免不必要的计算开销。if(short_circuit_trajectory_evaluation_&&best_score>0&&score.total>best_score){// 跳出循环，不再评估后续的批判器break;}}// 返回评分结果（包含轨迹、各批判器评分和总分）returnscore;}

BaseObstacleCritic::scoreTrajectory

这个也是个壳子，实际上是每个点调用的是scorePose函数。然后计算出每个点的分值，再加起来。

// 基类障碍物批判器的轨迹评分函数// 该函数遍历轨迹中的所有位姿点，累加每个位姿点的障碍物代价double BaseObstacleCritic::scoreTrajectory(constdwb_msgs::msg::Trajectory2D&traj){// 初始化轨迹总分为0double score=0.0;// 遍历轨迹中的所有位姿点（从起点到终点）for(unsigned int i=0;i<traj.poses.size();++i){// 调用scorePose函数计算当前位姿点的障碍物代价（距离障碍物越近，代价越高）double pose_score=scorePose(traj.poses[i]);// 优化的/无分支版本的条件累加逻辑：// 如果sum_scores_为true，则累加各点分数（score = score + pose_score）// 如果sum_scores_为false，则只保留当前点分数（score = 0 + pose_score）//// 解释：// - sum_scores_是一个布尔标志，控制评分方式是"累加"还是"取最大值"// - 当sum_scores_为true时：static_cast<double>(true) = 1.0，score = 1.0 * score + pose_score，即累加// - 当sum_scores_为false时：static_cast<double>(false) = 0.0，score = 0.0 * score + pose_score，即用当前点覆盖之前的值//// 这种写法避免了if分支判断，提高了执行效率（尤其在遍历大量位姿点时）// 等价于：// if (sum_scores_) {// score += pose_score; // 累加模式：轨迹总分为各点位姿代价之和// } else {// score = pose_score; // 取最后一点模式：轨迹总分等于最后一点的代价// }score=static_cast<double>(sum_scores_)*score+pose_score;}// 返回计算得到的轨迹障碍物评分returnscore;}

BaseObstacleCritic::scorePose

这个是评估每个点的分值。
到了这一步，终于到了调用nav2_costmap_2d模块中的函数：
将世界坐标系下的位姿（x, y）转换为代价地图中的栅格坐标：costmap_->worldToMap
从代价地图中获取该栅格点的代价值（0~255，值越高表示越危险）：costmap_->getCost

// 基类障碍物批判器的位姿评分函数// 该函数评估单个位姿点处的障碍物代价，如果该位姿非法则抛出异常double BaseObstacleCritic::scorePose(constgeometry_msgs::msg::Pose2D&pose){// 声明用于存储地图栅格坐标的变量unsigned int cell_x,cell_y;// 将世界坐标系下的位姿（x, y）转换为代价地图中的栅格坐标// 如果转换失败（即位姿点落在地图外部），则抛出非法轨迹异常if(!costmap_->worldToMap(pose.x,pose.y,cell_x,cell_y)){throwdwb_core::IllegalTrajectoryException(name_,"Trajectory Goes Off Grid.");}// 从代价地图中获取该栅格点的代价值（0~255，值越高表示越危险）unsigned char cost=costmap_->getCost(cell_x,cell_y);// 检查该栅格点的代价值是否合法（即是否为可通行区域）// 如果代价值表示障碍物（如被标记为LETHAL_OBSTACLE等），则抛出非法轨迹异常if(!isValidCost(cost)){throwdwb_core::IllegalTrajectoryException(name_,"Trajectory Hits Obstacle.");}// 返回该位姿点的代价值作为评分（代价值越低表示越安全）returncost;}

costmap中被调用的两个函数

worldToMap

这个就没啥解释的，就是一个坐标系转换，把世界坐标系转到代价地图中。

getCost

这个也没啥解释的，获取costmap中的对应点的分值。

三、costmap生成逻辑

上文梳理清楚了绕障的整个逻辑，接下来我们梳理在nav2_costmap_2d模块中，costmap的生成逻辑。

一、costmap 是什么

一张二维栅格矩阵，每个格子存一个代价值（0~255），值代表该位置的障碍程度。生成过程分两阶段：一次性初始化（configure）+ 后台持续更新（激活后的循环线程）。
代价值定义（cost_values.hpp:42-46）：
0 = 自由空间（FREE_SPACE）
1~252 = 离障碍物越近代价越高（膨胀梯度）
253 = 内切障碍（INSCRIBED_INFLATED_OBSTACLE，footprint 必撞）
254 = 致命障碍（LETHAL_OBSTACLE，栅格被障碍占据）
255 = 未知区域（NO_INFORMATION）

二、输入输出

又是最关键的核心部分来了：

输入（全部是话题）

订阅话题 /scan（话题名可在参数 obstacle_layer.scan.topic 里改），消息类型 sensor_msgs/LaserScan，由 obstacle_layer 订阅。
订阅话题 <源名>（话题名在参数 obstacle_layer.<源名>.topic 里改，默认等于源名，如配源名 pointcloud 则订阅 /pointcloud），消息类型 sensor_msgs/PointCloud2，由 obstacle_layer/voxel_layer 订阅。
订阅话题 /map（话题名可在参数 static_layer.map_topic 里改），消息类型 nav_msgs/OccupancyGrid，由 static_layer 订阅。
订阅话题 /map_updates（static_layer 自动订阅 map_topic 加后缀 _updates），消息类型 map_msgs/OccupancyGridUpdate。
订阅话题 <topics 参数指定的列表>（参数 range_sensor_layer.topics），消息类型 sensor_msgs/Range，由 range_sensor_layer 订阅。
订阅话题 <filter_info_topic 参数指定>（参数 keepout_layer.filter_info_topic），消息类型 nav2_msgs/CostmapFilterInfo，由 keepout_filter 订阅。
订阅话题 <CostmapFilterInfo 消息里 filter_mask_topic 字段指定>，消息类型 nav_msgs/OccupancyGrid，由 keepout_filter 订阅。
订阅话题 <filter_info_topic 参数指定>（参数 speed_filter.filter_info_topic），消息类型 nav2_msgs/CostmapFilterInfo，由 speed_filter 订阅。
订阅话题 <CostmapFilterInfo 消息里 filter_mask_topic 字段指定>，消息类型 nav_msgs/OccupancyGrid，由 speed_filter 订阅。
订阅话题 /local_costmap/footprint，消息类型 geometry_msgs/Polygon，由 Costmap2DROS 直接订阅，用于动态设置机器人足迹。

输出（话题和服务）

发布话题（节点下的相对名）：
发布话题 /local_costmap/costmap，消息类型 nav_msgs/OccupancyGrid，内容为标准代价图（0-100 代价值）。
发布话题 /local_costmap/costmap_raw，消息类型 nav2_msgs/Costmap，内容为原始代价图（带 layer 元数据）。
发布话题 /local_costmap/costmap_updates，消息类型 map_msgs/OccupancyGridUpdate，内容为代价图增量更新。
发布话题 /local_costmap/published_footprint，消息类型 geometry_msgs/PolygonStamped，内容为机器人足迹。

提供服务：
提供服务 /local_costmap/get_costmap，消息类型 nav2_msgs/srv/GetCostmap，供外部主动查询整张代价图。
提供服务 /local_costmap/clear_around_local_costmap，清空机器人周围区域代价。
提供服务 /local_costmap/clear_except_local_costmap，清空除指定区域外代价。
提供服务 /local_costmap/clear_entirely_local_costmap，清空整张代价图。

对外提供的进程内函数调用接口（被 controller/planner/DWB 使用）

因为这个模块被使用时，并不是通过接收发布的话题，而是直接使用函数调用的模式。

getCostmap() 函数，返回 Costmap2D 指针，可继续调用 getCost(x,y)、worldToMap()、setCost() 直接读写栅格代价。
getRobotPose() 函数，返回机器人当前位姿。
getBaseFrameID() 函数，返回机器人基座坐标系名（如 base_link）。
getGlobalFrameID() 函数，返回全局坐标系名（如 odom/map）。
getTfBuffer() 函数，返回共享的 tf 缓冲指针。
isCurrent() 函数，查询代价图是否已更新到最新。
updateMap() 函数，手动触发一次代价图刷新。
configure()、activate()、deactivate()、cleanup() 函数，控制代价图生命周期。
getLayeredCostmap() 函数，返回分层代价图对象，可操作单个 layer。

三、阶段一：初始化（configure，一次性）

入口是 Costmap2DROS::on_configure（costmap_2d_ros.cpp:106），controller_server 在 configure 生命周期时调用它。
按顺序做：
读参数 getParameters()：global_frame、robot_base_frame、resolution、map_width/map_height、plugins 列表、rolling_window 等。
创建栅格容器 LayeredCostmap，按 分辨率×宽×高 用 resizeMap 分配一块空栅格矩阵（costmap_2d_ros.cpp:121）。
创建 tf 对象：tf_buffer + tf_listener（后面定位机器人和变换传感器数据用）。
加载 layer 插件（pluginlib，plugin_loader_.createSharedInstance，costmap_2d_ros.cpp:141）：按你 yaml 里 plugins 参数逐个实例化并 initialize。每个 layer 在自己 initialize 里创建传感器订阅。
加载 costmap filter（如 keepout/speed filter，如果有配置）。
此时栅格还是"空白"的，只搭好了骨架和传感器通道，还没有内容。

四、阶段二：持续更新（激活后，周期性）

controller_server activate 时调用 costmap_ros_->activate()，Costmap2DROS::on_activate 启动一个后台线程 map_update_thread_（costmap_2d_ros.cpp:295），这个线程在进程存活期间永不停止，每个 update_frequency（如 5Hz）周期执行一次：
mapUpdateLoop（costmap_2d_ros.cpp:469）
│
▼ 每周期
updateMap()（costmap_2d_ros.cpp:515）
│
├─ ① 更新滚动窗口原点（机器人在图中央）
├─ ② 对每个 layer 调 updateBounds() ← 读传感器/地图，算出要更新的区域
├─ ③ 对每个 layer 调 updateCosts() ← 把障碍写进栅格 + 膨胀
└─ ④ 发布 costmap 话题

五、各 layer 具体怎么"生成"内容

static_layer（静态地图）：
initialize 时订阅你配置的 map_topic（如 /map）。
每周期把收到的 OccupancyGrid 整块拷贝进自己的栅格，占据格子标 LETHAL，空闲标 FREE。

obstacle_layer（传感器障碍，绕障的主力）：
initialize 时按 observation_sources 为每个源创建订阅，接收 LaserScan/PointCloud2，存进 ObservationBuffer（内部会把数据 tf 变换到全局系）。
updateBounds 阶段（obstacle_layer.cpp:180）：从缓冲取观测 → 对"清除观测"做 raytraceFreespace（沿激光路径把栅格标成 FREE，清除已消失的障碍）→ 算出更新区域。
updateCosts 阶段（obstacle_layer.cpp:255）：把"标记观测"中的障碍点栅格标成 LETHAL_OBSTACLE。

inflation_layer（膨胀）：
在 obstacle/static 写出的障碍基础上，把每个障碍向外扩散，生成 LETHAL → INSCRIBED(253) → 递减到 1 的代价梯度（inflation_layer.cpp），让 DWB 的评分能"感知到远离障碍更安全"。
filters（keepout/speed filter）：在最终图上叠加禁行/限速区域。

六、产出

生成结果是一张"最新的 Costmap2D 栅格"，三条出路：
进程内被 DWB 读：costmap_ros_->getCostmap() 返回指针，getCost(x,y) 查任意栅格代价值——这是绕障用的主通道。
发布话题：/local_costmap/costmap、/costmap_raw、/costmap_updates（给 rviz 可视化或跨进程模块）。
提供服务：/local_costmap/get_costmap（外部主动查询整张图）。

---