文章目录前言一、包结构二、输入输出核心输入输出三、代码逻辑流程第一层外层computeVelocityCommands第二层内层computeVelocityCommands第一步填 results 头部可选如果用户请求记录评估结果则填充消息头第二步准备全局路径prepareGlobalPlan第三步锁 costmap 互斥锁关键第四步准备打分器critics prepare第五步核心——采样打分选最优第六步构造返回值第七步debrief 有状态打分器第八步发布 解锁 返回总结前言接着上一篇讲上一篇控制模块中的computeVelocityCommands函数进行实际的计算。而这个函数是调用了nav2_dwb_controller模块中的内容。本篇就继续讲解这个模块。一、包结构这个功能包下面有七个子文件夹nav_2d_msgs 2D 基础消息Pose2D、Twist2D、Path2D 等dwb_msgs DWA 专有消息Trajectory2D、TrajectoryScore、CriticScore、localPlanEvaluationdwb_core 核心DWBLocalPlanner 两个插件接口TrajectoryGenerator、TrajectoryCritic DWBPublisherdwb_plugins 轨迹生成器实现StandardTrajectoryGenerator、LimitedAccelGenerator、速度迭代器 XYThetaIteratordwb_critics 打分器实现PathDist、GoalDist、BaseObstacle、ObstacleFootprint、Oscillation、PreferForward 等 11 个nav_2d_utils 工具函数坐标转换、TF 辅助、路径操作、odom 订阅costmap_queue 代价地图 BFS 传播队列给 MapGrid/障碍物打分用二、输入输出这个输入输出就没有什么话题订阅了。核心输入1.就是在nav2_controller模块中的函数调用computeVelocityCommands(pose, velocity, goal_checker)其中的输入参数一共三个当前位姿 当前速度 goal checker 指针2.setPlan(path)全局路径nav_msgs/Path → 内部存为 global_plan_3.local costmap通过 costmap_ros_ 访问含 TF buffer4.速度限制 setSpeedLimit() → 转发给轨迹生成器5.参数critics启用哪些打分器如 PathDist GoalDist BaseObstacle…trajectory_generator_name默认 dwb_plugins::StandardTrajectoryGeneratorprune_plan(true)/prune_distance(2.0)/forward_prune_distance(2.0)全局路径裁剪shorten_transformed_plan(true)是否只把机器人附近的路径段传给打分器transform_tolerance(0.1)、short_circuit_trajectory_evaluation(true)、debug_trajectory_details(false)输出返回值最优 TwistStamped 速度指令回到 controller_server → 发 /cmd_vel调试话题话题 类型 内容global_plan nav_msgs/Path 收到的全局路径含裁剪后的transformed_global_plan nav_msgs/Path 转到机器人坐标系的路径local_plan nav_msgs/Path 本轮选中的最优轨迹local_plan_evaluation dwb_msgs/LocalPlanEvaluation 所有采样轨迹的完整打分trajectory_markers MarkerArray 所有被评估轨迹的 3D 可视化cost_grid PointCloud2 代价网格可视化三、代码逻辑流程首先程序入口就是nav2_controller调用的computeVelocityCommands函数不过其进行了重载就成了里外两层函数我们就还是剥洋葱一样从最外层一直往里面理清楚逻辑。第一层外层computeVelocityCommands就如之前说的这个有内外两层computeVelocityCommands函数外层函数逻辑很简单就是消息格式转换调用内层computeVelocityCommands函数。内层computeVelocityCommands则是真正的DWB核心实现。外层源码/** * brief DWB局部规划器的速度指令计算接口 * * 该函数是DWB(Dynamic Window Based)局部规划器的外部接口负责将输入的3D姿态和速度 * 转换为2D格式调用核心计算函数并将结果转换回3D格式返回。 * * 这是适配层函数主要处理数据格式转换和评估结果发布。 * * param pose 机器人当前3D姿态包含位置和朝向 * param velocity 机器人当前3D速度线速度和角速度 * param goal_checker 目标检查器指针当前未使用用注释标记 * return geometry_msgs::msg::TwistStamped 计算得到的速度指令3D格式 */geometry_msgs::msg::TwistStampedDWBLocalPlanner::computeVelocityCommands(constgeometry_msgs::msg::PoseStampedpose,constgeometry_msgs::msg::Twistvelocity,nav2_core::GoalChecker*/*goal_checker*/)// 参数未使用用注释标记避免编译警告{// 1. 准备评估结果存储 // 创建一个共享指针用于存储局部规划评估结果// 这个结果包含了评分、轨迹采样等信息用于调试和分析std::shared_ptrdwb_msgs::msg::LocalPlanEvaluationresultsnullptr;// 检查发布器是否配置为记录评估数据if(pub_-shouldRecordEvaluation()){// 如果需要记录创建评估结果对象resultsstd::make_shareddwb_msgs::msg::LocalPlanEvaluation();}try{// 2. 调用核心计算函数 // 将3D姿态转换为2D姿态去除Z轴和朝向的Z分量// 将3D速度转换为2D速度只保留X轴线速度和Z轴角速度// 这种转换适用于在平坦地面上移动的机器人nav_2d_msgs::msg::Twist2DStamped cmd_vel2dcomputeVelocityCommands(nav_2d_utils::poseStampedToPose2D(pose),// 3D姿态 - 2D姿态 (x, y, theta)nav_2d_utils::twist3Dto2D(velocity),// 3D速度 - 2D速度 (vx, vtheta)results);// 传入评估结果指针用于记录// 3. 发布评估结果 // 将本次规划评估结果发布到调试主题// 这包括所有采样轨迹的评分、选择的最优轨迹等信息pub_-publishEvaluation(results);// 4. 转换并返回速度指令 geometry_msgs::msg::TwistStamped cmd_vel;// 将计算得到的2D速度指令转换回3D格式// 2D速度包含线速度(vx)和角速度(omega)转换为3D时// - linear.x vx (前进速度)// - angular.z omega (偏航角速度)// - 其他分量置零cmd_vel.twistnav_2d_utils::twist2Dto3D(cmd_vel2d.velocity);returncmd_vel;}catch(constnav2_core::PlannerExceptione){// 5. 异常处理 // 如果核心计算函数抛出规划器异常在重新抛出前// 先发布已记录的评估结果如果有的话// 这样即使规划失败调试信息也能被保留pub_-publishEvaluation(results);// 重新抛出异常让上层调用者处理throw;}}第二层内层computeVelocityCommands这个是DWB的核心实现一共可以分为八步第一步填 results 头部可选如果用户请求记录评估结果则填充消息头if(results){results-header.frame_idpose.header.frame_id;results-header.stampclock_-now();}results 是否分配取决于上层 shouldRecordEvaluation()——只有 publish_evaluation_ 或 publish_trajectories_ 开启时才分配。为空时全程跳过记录省内存。第二步准备全局路径prepareGlobalPlannav_2d_msgs::msg::Path2D transformed_plan;nav_2d_msgs::msg::Pose2DStamped goal_pose;prepareGlobalPlan(pose,transformed_plan,goal_pose);做两件事transformed_plan transformGlobalPlan(pose) // ① 全局路径 → 机器人坐标系 裁剪(同时可能发布 transformed_global_plan 话题)goal_pose 全局路径终点用 TF 转到全局坐标系 // ② 给打分器用的目标点而其中的transformGlobalPlan实现的功能如下全局路径空 → 抛 PlannerException把机器人位姿用 TF 变换到路径所在坐标系裁剪三刀prune_plan(默认 true)prune_distance_(2m) 外的点按路径累计距离截断已走过的点直接从 global_plan_ 里 erase 掉这是有状态行为每帧前进一点删一点shorten_transformed_plan(默认 true)只保留机器人前方 forward_prune_distance_(2m) 内的一段传给打分器——给 DWB 更少的自由发挥空间让它贴住全局路径走用 costmap 尺寸算 dist_threshold超出本地地图范围的点也丢弃裁剪后的点逐点 TF 变换到机器人坐标系返回 transformed_plan第三步锁 costmap 互斥锁关键nav2_costmap_2d::Costmap2D*costmapcostmap_ros_-getCostmap();std::unique_lock...lock(*(costmap-getMutex()));为什么锁打分器要读取/遍历代价地图算障碍物距离而 costmap 可能被其他线程传感器回调、清图并发更新。用 unique_lock 加锁保护整个采样打分过程避免读到一半地图被改。这个锁持有到函数末尾 lock.unlock() 才释放包括异常分支。第四步准备打分器critics preparefor(TrajectoryCritic::Ptrcritic:critics_){if(!critic-prepare(pose.pose,velocity,goal_pose.pose,transformed_plan)){RCLCPP_WARN(...A scoring function failed to prepare);}}每个 critic 一次性接收 (当前位姿, 当前速度, 终点, 裁剪后路径) 做本帧预计算把共享的中间结果缓存下来。典型例子MapGrid / BaseObstacle此刻把代价地图跑一遍 BFS/传播算出每个格子到最近障碍的距离场之后每条轨迹打分直接查表不用重复算PathDist此刻把裁剪后路径离散成格子存入哈希表失败只打 WARN 不终止比如地图刚好无效该打分器本轮相当于被跳过。第五步核心——采样打分选最优dwb_msgs::msg::TrajectoryScore bestcoreScoringAlgorithm(pose.pose,velocity,results);就是之前我讲DWA的动态窗口循环控制法traj_generator_-startNewIteration(velocity)按当前速度 加速度限制划出可达速度窗口遍历窗口内所有采样 (vx, vy, ω) → generateTrajectory 积分出轨迹每个 critic 打分加权求和 → 记录 best / worst全部非法 → 抛 NoLegalTrajectoriesException第六步构造返回值nav_2d_msgs::msg::Twist2DStamped cmd_vel;cmd_vel.header.stampclock_-now();cmd_vel.velocitybest.traj.velocity;这一步就是输出了输出计算出来的控制命令。注意header 只设 stamp不设 frame_id——速度本就定义在机器人系不需要坐标帧速度直接取 best.traj.velocity采样时的目标速度不是轨迹末端的实际速度第七步debrief 有状态打分器for(TrajectoryCritic::Ptrcritic:critics_){critic-debrief(cmd_vel.velocity);}prepare 是帧前预计算debrief 是帧后回执。对有状态打分器如 Oscillation 防振荡很重要它要记住本轮实际选出的速度下轮判断机器人是否在左右/前后摇摆。传入本轮 cmd_vel 让它更新内部状态。第八步发布 解锁 返回lock.unlock();// 释放 costmap 锁pub_-publishLocalPlan(pose.header,best.traj);// 发布最优轨迹(local_plan)pub_-publishCostGrid(costmap_ros_,critics_);// 发布代价网格returncmd_vel;这里的发布发布的调试信息返回的是控制命令。内层源码/** * brief DWB局部规划器的核心速度计算函数 * * 这是DWB局部规划器的核心实现负责 * 1. 准备全局路径并转换为局部坐标系 * 2. 使用代价地图进行碰撞检测 * 3. 调用所有轨迹评价器进行准备 * 4. 执行核心评分算法找到最优轨迹 * 5. 发布调试信息局部路径、代价地图等 * * param pose 机器人当前2D位姿位置和朝向 * param velocity 机器人当前2D速度线速度和角速度 * param results 共享指针用于存储评估结果可为空 * return nav_2d_msgs::msg::Twist2DStamped 最优速度指令 */nav_2d_msgs::msg::Twist2DStampedDWBLocalPlanner::computeVelocityCommands(constnav_2d_msgs::msg::Pose2DStampedpose,constnav_2d_msgs::msg::Twist2Dvelocity,std::shared_ptrdwb_msgs::msg::LocalPlanEvaluationresults){// 1. 初始化评估结果的头信息 // 如果用户请求记录评估结果则填充消息头if(results){results-header.frame_idpose.header.frame_id;// 设置坐标系IDresults-header.stampclock_-now();// 设置时间戳}// 2. 准备全局路径 nav_2d_msgs::msg::Path2D transformed_plan;// 转换后的全局路径局部坐标系nav_2d_msgs::msg::Pose2DStamped goal_pose;// 目标位姿// 将全局路径转换到机器人局部坐标系并提取目标位姿// 这一步会裁剪路径只保留机器人前方的部分并转换坐标prepareGlobalPlan(pose,transformed_plan,goal_pose);// 3. 锁定代价地图 // 获取代价地图对象包含障碍物信息nav2_costmap_2d::Costmap2D*costmapcostmap_ros_-getCostmap();// 使用互斥锁锁定代价地图防止其他线程如代价地图更新线程同时修改// unique_lock提供了更灵活的锁管理可以提前解锁std::unique_locknav2_costmap_2d::Costmap2D::mutex_tlock(*(costmap-getMutex()));// 4. 准备所有轨迹评价器 // 遍历所有已注册的轨迹评价器如路径跟踪、避障、速度限制等for(TrajectoryCritic::Ptrcritic:critics_){// 调用每个评价器的prepare函数进行初始化计算// 参数当前位姿、当前速度、目标位姿、局部路径if(!critic-prepare(pose.pose,velocity,goal_pose.pose,transformed_plan)){// 如果某个评价器准备失败发出警告但不中断流程RCLCPP_WARN(rclcpp::get_logger(DWBLocalPlanner),A scoring function failed to prepare);}}try{// 5. 执行核心评分算法 // 这是DWB算法的核心采样多个轨迹用所有评价器评分选择最优的// 返回的best包含最优轨迹及其评分dwb_msgs::msg::TrajectoryScore bestcoreScoringAlgorithm(pose.pose,velocity,results);// 6. 构造返回的速度指令 nav_2d_msgs::msg::Twist2DStamped cmd_vel;cmd_vel.header.stampclock_-now();// 设置时间戳cmd_vel.velocitybest.traj.velocity;// 从最优轨迹中提取速度指令// 7. 通知评价器进行后处理 // 将计算得到的速度指令反馈给各个评价器// 这允许评价器更新内部状态如累积误差、历史信息等for(TrajectoryCritic::Ptrcritic:critics_){critic-debrief(cmd_vel.velocity);}// 提前解锁代价地图在发布调试信息之前// 这样可以尽早释放锁提高并发性能lock.unlock();// 8. 发布调试信息 // 发布最优轨迹用于可视化导航规划过程pub_-publishLocalPlan(pose.header,best.traj);// 发布代价地图显示障碍物和代价值pub_-publishCostGrid(costmap_ros_,critics_);// 返回速度指令returncmd_vel;}catch(constdwb_core::NoLegalTrajectoriesExceptione){// 异常处理没有合法的轨迹 // 当所有采样的轨迹都被评价器判定为非法时触发// 这通常意味着机器人被完全困住// 创建一个空的速度指令所有速度分量为零nav_2d_msgs::msg::Twist2D empty_cmd;dwb_msgs::msg::Trajectory2D empty_traj;// 通知评价器进行后处理使用空速度for(TrajectoryCritic::Ptrcritic:critics_){critic-debrief(empty_cmd);}// 提前解锁代价地图lock.unlock();// 发布空的轨迹表示无法找到合法路径pub_-publishLocalPlan(pose.header,empty_traj);// 发布代价地图pub_-publishCostGrid(costmap_ros_,critics_);// 重新抛出异常让上层适配器处理throw;}}总结这个模块就讲到这里这个模块是nav2默认使用的控制算法模块还有四个别的控制包nav2_mppi_controller MPPI 采样优化算法nav2_regulated_pure_pursuit_controller 纯 跟踪算法nav2_rotation_shim_controller 旋转垫片算法nav2_graceful_controller 平滑控制律算法这四个包都和dwb是平级的不过nav2_controller中选择了dwb模块。其他四个控制包我先不讲了我后续会继续讲控制的上游先理清楚整个系统。