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

前言

前面说了nav2_planner，而其默认选择的路径生成算法模块是nav2_navfn_planner。
这个算法和控制中的DWA算法不同，DWA本身就是一种标准的基础算法。
而navfn则是属于ros自研的一种算法。
navfn的讲解就还是需要看我的另外一篇博客【一看就会】nav2中的核心导航算法：Navfn（Navigation Function）算法

有了另外一篇的理解，这个源码讲解就会相对简单。

本文从其源码出发，进行讲解。

一、输入 / 输出

输入

起点和终点都是在nav2_planner中的createPlan(start, goal)函数调用输入的。

代价函数costmap_是nav2_planner自建之后在configure时给对应的规划插件的。

输出

函数返回路径：nav_msgs/Path（一条从起点到终点的路径点序列）

二、源码逻辑

整个算法逻辑就不细讲了，这里主要讲源码逻辑。

createPlan：入口函数

这个函数可以分为三步：
1.更新检查规划器状态
2.处理起点和终点重叠的情况
3.执行路径规划：makePlan

其中最主要的就是调用了makePlan函数。

源码

/** * @brief NavFn规划器的路径生成函数 * * 该函数是NavFn（Naviation Function，导航函数）规划器的核心实现， * 使用基于势场的方法进行路径规划。 * * NavFn规划器的工作原理： * 1. 在代价地图上生成一个势场，目标点具有最低势能 * 2. 从起始点开始，沿着势能下降最快的方向到达目标点 * 3. 自动避开障碍物（障碍物区域势能极高） * * @param start 起始位姿（位置和朝向） * @param goal 目标位姿 * @return nav_msgs::msg::Path 生成的路径，如果失败则返回空路径 */nav_msgs::msg::Path NavfnPlanner::createPlan(constgeometry_msgs::msg::PoseStamped&start,constgeometry_msgs::msg::PoseStamped&goal){#ifdefBENCHMARK_TESTING// 性能测试模式：记录函数开始时间，用于计算规划耗时steady_clock::time_point a=steady_clock::now();#endif// ========== 1. 检查并更新规划器状态 ==========// 如果代价地图的大小发生了变化（如地图更新或扩展），// 需要重新初始化规划器的导航数组if(isPlannerOutOfDate()){// 设置规划器导航数组的大小为当前代价地图的尺寸planner_->setNavArr(costmap_->getSizeInCellsX(),// X方向单元格数量costmap_->getSizeInCellsY());// Y方向单元格数量}// 创建空路径对象nav_msgs::msg::Path path;// ========== 2. 处理特殊边界情况 ==========// 当起始点与目标点重合时（起点=终点）if(start.pose.position.x==goal.pose.position.x&&start.pose.position.y==goal.pose.position.y){// 2.1 将起始点从世界坐标转换为地图坐标（单元格坐标）unsigned int mx,my;costmap_->worldToMap(start.pose.position.x,start.pose.position.y,mx,my);// 2.2 检查该单元格是否为致命障碍物if(costmap_->getCost(mx,my)==nav2_costmap_2d::LETHAL_OBSTACLE){// 如果起点/目标点位于障碍物上，无法生成路径RCLCPP_WARN(logger_,"Failed to create a unique pose path because of obstacles");returnpath;// 返回空路径}// 2.3 创建单点路径（只有一个位姿点）path.header.stamp=clock_->now();// 设置时间戳path.header.frame_id=global_frame_;// 设置坐标系（通常是地图坐标系）geometry_msgs::msg::PoseStamped pose;pose.header=path.header;pose.pose.position.z=0.0;// Z轴位置设为0（2D规划）pose.pose=start.pose;// 使用起始点的位置// 2.4 处理方向（朝向）的优先级// 如果起始朝向和目标朝向不同，且不强制使用最终接近朝向if(start.pose.orientation!=goal.pose.orientation&&!use_final_approach_orientation_){// 通常使用目标朝向，因为最终到达时机器人应该面向目标方向pose.pose.orientation=goal.pose.orientation;}// 如果 use_final_approach_orientation_ 为true，则保持起始朝向// 这可以避免局部规划器在起点处产生不必要的旋转运动// 将位姿添加到路径中path.poses.push_back(pose);returnpath;// 返回单点路径}// ========== 3. 执行实际的路径规划 ==========// 调用底层规划算法生成从起点到目标点的路径// makePlan使用势场方法在代价地图上寻找最优路径// 参数：起始位姿、目标位姿、规划容忍度、输出路径if(!makePlan(start.pose,goal.pose,tolerance_,path)){// 如果规划失败，记录警告日志RCLCPP_WARN(logger_,"%s: failed to create plan with ""tolerance %.2f.",name_.c_str(),tolerance_);}#ifdefBENCHMARK_TESTING// 性能测试模式：计算规划耗时并输出steady_clock::time_point b=steady_clock::now();duration<double>time_span=duration_cast<duration<double>>(b-a);std::cout<<"It took "<<time_span.count()*1000<<std::endl;#endif// 返回生成的路径（可能为空）returnpath;}

makePlan：主流程函数

这个函数就是nav2_navfn_planner生成路径的主流程函数，也就是实现navfn的函数。

makePlan函数可以分为十步：

第 0 步：清空路径 + 打头

plan.poses.clear();plan.header.stamp=clock_->now();plan.header.frame_id=global_frame_;// map 系

这个没什么要说的。

第 1 步：起点世界坐标 → 栅格坐标

if(!worldToMap(wx,wy,mx,my)){..."robot's start position is off the global costmap"returnfalse;// 起点不在代价地图范围内 → 直接失败}

坐标转换+起点检查

第 2 步：清除起点格的障碍（容易被忽略但重要）

clearRobotCell(mx,my);

把起点所在格子的代价强制设为空地。原因：机器人的当前位置在代价地图上可能正好贴着障碍或站在障碍边缘（因为地图更新滞后）。如果不清，势场把起点当障碍，路径就出不来了。

第 3 步：锁地图 + 同步 NavFn 网格尺寸

std::unique_lock<...>lock(*(costmap_->getMutex()));planner_->setNavArr(costmap_->getSizeInCellsX(),costmap_->getSizeInCellsY());

锁 costmap：防止传感器线程并发改地图
setNavArr：地图尺寸变了就重建 NavFn 内部数组

第 4 步：把代价地图搬进 NavFn（核心准备）：相当于另一篇讲navfn算法的第一步

planner_->setCostmap(costmap_->getCharMap(),true,allow_unknown_);lock.unlock();

就是要把代价栅格地图做一下规整，能让后续算法看懂

第 5~6 步：起点/终点都转成栅格坐标

map_start[0]=mx;map_start[1]=my;// 起点栅格wx=goal.position.x;wy=goal.position.y;if(!worldToMap(wx,wy,mx,my)){..."goal off the global costmap"// 终点不在图内 → 失败returnfalse;}map_goal[0]=mx;map_goal[1]=my;// 终点栅格

第 7 步：势场扩散（整个函数的核心）：相当于另一篇讲navfn算法的第二步

planner_->setStart(map_goal);// ★ 反着传planner_->setGoal(map_start);if(use_astar_){planner_->calcNavFnAstar();}else{planner_->calcNavFnDijkstra(true);}

这个就是生成势场地图，每个格子都由自己的势值。

这个是最重要的，默认用的dijkstra算法，具体的算法逻辑在另外一篇博客中讲过了，本篇主要是讲源码，就不细讲算法逻辑了。

第 8 步：目标点可达性检查

p=goal;double potential=getPointPotential(p.position);// 查目标格子的势场值if(potential<POT_HIGH){best_pose=p;found_legal=true;// 目标点势场被更新过 = 可达}

getPointPotential 把目标点世界坐标转栅格，读 potarr 里那个格子的值。势场值 < 阈值（POT_HIGH=1e10）说明扩散到过它 = 从目标能到达。

第 9 步：容差搜索（目标不可达时的"妥协"）

// 目标点在障碍里/不可达时，在 tolerance 范围（默认 0.5m）网格扫一遍p.position.y=goal.position.y-tolerance;while(p.position.y<=goal.position.y+tolerance){p.position.x=goal.position.x-tolerance;while(p.position.x<=goal.position.x+tolerance){potential=getPointPotential(p.position);if(potential<POT_HIGH&&sdist<best_sdist){// 合法且离目标最近best_sdist=sdist;best_pose=p;found_legal=true;}p.position.x+=resolution;}p.position.y+=resolution;}

为什么需要：用户点的目标可能就卡在障碍边缘/墙里。这时在全图势场已算好的前提下，在目标周围 tolerance × tolerance 的方框里找最近的、势场可达的格子作为"替代终点"。所以路径会停在目标点附近最近的合法位置。

第 10 步：提取路径：相当于另一篇讲navfn算法的第三步

if(found_legal){if(getPlanFromPotential(best_pose,plan)){// 见下方smoothApproachToGoal(best_pose,plan);// 打磨末端if(use_final_approach_orientation_){...}// 终点朝向修正}else{RCLCPP_ERROR(...);}}return!plan.poses.empty();// 路径非空 = 成功

getPlanFromPotential（navfn_planner.cpp:371）内部：
把 best_pose 设成 NavFn 的起点（回溯起点）
planner_->calcPath(max_cycles) → 从起点沿梯度下山到目标，产出 pathx/pathy（浮点亚格子坐标）
转回世界坐标，填进 plan.poses
smoothApproachToGoal（navfn_planner.cpp:346）：若路径倒数第二个点离目标比离终点更近，就把终点替换成真正的 goal 点，消除离散网格的"锯齿尾巴"。

源码

/** * @brief NavFn规划器的底层路径生成函数 * * 该函数是NavFn规划器的核心算法实现，负责： * 1. 将起始点和目标点转换为地图坐标 * 2. 检查起点和终点是否在代价地图范围内 * 3. 构建导航函数（势场）并计算路径 * 4. 处理目标不可达的情况（寻找最近的可达点） * 5. 从势场中提取路径点 * 6. 平滑处理接近目标段的路径 * 7. 处理最终接近朝向 * * @param start 起始位姿（世界坐标） * @param goal 目标位姿（世界坐标） * @param tolerance 目标容忍半径（在范围内视为可达） * @param plan 输出的路径对象（引用） * @return bool 规划是否成功 */boolNavfnPlanner::makePlan(constgeometry_msgs::msg::Pose&start,constgeometry_msgs::msg::Pose&goal,double tolerance,nav_msgs::msg::Path&plan){// ========== 1. 初始化路径 ==========// 清空路径中可能存在的旧数据plan.poses.clear();// 设置路径的头信息plan.header.stamp=clock_->now();// 时间戳plan.header.frame_id=global_frame_;// 坐标系（地图坐标系）// ========== 2. 转换起始点坐标 ==========double wx=start.position.x;double wy=start.position.y;RCLCPP_DEBUG(logger_,"Making plan from (%.2f,%.2f) to (%.2f,%.2f)",start.position.x,start.position.y,goal.position.x,goal.position.y);// 将起始点从世界坐标转换为地图坐标（单元格坐标）unsigned int mx,my;if(!worldToMap(wx,wy,mx,my)){// 如果起始点在地图之外，无法规划RCLCPP_WARN(logger_,"Cannot create a plan: the robot's start position is off the global"" costmap. Planning will always fail, are you sure"" the robot has been properly localized?");returnfalse;}// ========== 3. 清除起始单元格的障碍物标记 ==========// 因为机器人当前位置不可能有障碍物，所以清除该单元格的障碍物标记// 这可以避免因地图更新延迟导致的误判clearRobotCell(mx,my);// ========== 4. 锁定代价地图并更新规划器 ==========// 使用互斥锁保护代价地图数据，防止在规划过程中被修改std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t>lock(*(costmap_->getMutex()));// 确保规划器的导航数组大小与代价地图匹配planner_->setNavArr(costmap_->getSizeInCellsX(),costmap_->getSizeInCellsY());// 将代价地图数据传递给规划器// allow_unknown_ 控制是否允许穿过未知区域planner_->setCostmap(costmap_->getCharMap(),true,allow_unknown_);// 提前解锁，允许其他线程访问代价地图lock.unlock();// ========== 5. 设置起始点和目标点 ==========int map_start[2];map_start[0]=mx;map_start[1]=my;// 转换目标点坐标wx=goal.position.x;wy=goal.position.y;if(!worldToMap(wx,wy,mx,my)){// 如果目标点在地图之外，无法规划RCLCPP_WARN(logger_,"The goal sent to the planner is off the global costmap."" Planning will always fail to this goal.");returnfalse;}int map_goal[2];map_goal[0]=mx;map_goal[1]=my;// 注意：这里start和goal在设置时是反的！// setStart接收的是目标点，setGoal接收的是起始点// 这是因为NavFn算法是从目标点向起始点传播势场// 这样计算出的势场可以直接用于路径提取planner_->setStart(map_goal);// 从目标点开始传播势场planner_->setGoal(map_start);// 传播到起始点结束// ========== 6. 计算导航函数（势场） ==========if(use_astar_){// 使用A*算法（启发式搜索）planner_->calcNavFnAstar();}else{// 使用Dijkstra算法（在栅格地图上，使用Dijkstra计算势场）// true表示使用潜在的场planner_->calcNavFnDijkstra(true);}// ========== 7. 处理目标点不可达的情况 ==========double resolution=costmap_->getResolution();geometry_msgs::msg::Pose p,best_pose;bool found_legal=false;// 7.1 检查目标点本身是否可达p=goal;double potential=getPointPotential(p.position);if(potential<POT_HIGH){// POT_HIGH表示无限大势能（不可达区域）// 目标点本身势能较低，可直接到达best_pose=p;found_legal=true;}else{// 7.2 目标点不可达，在容忍范围内搜索最近的可达点// 原理：在目标点周围（tolerance半径内）搜索势能最低的点double best_sdist=std::numeric_limits<double>::max();// 在目标点周围的正方形区域内进行搜索（步长为地图分辨率）p.position.y=goal.position.y-tolerance;while(p.position.y<=goal.position.y+tolerance){p.position.x=goal.position.x-tolerance;while(p.position.x<=goal.position.x+tolerance){potential=getPointPotential(p.position);double sdist=squared_distance(p,goal);// 到目标点的平方距离// 选择可达（势能低）且距离目标最近的候选点if(potential<POT_HIGH&&sdist<best_sdist){best_sdist=sdist;best_pose=p;found_legal=true;}p.position.x+=resolution;}p.position.y+=resolution;}}// ========== 8. 从势场中提取路径 ==========if(found_legal){// 8.1 从最佳目标点开始，沿着势能下降方向提取路径到起始点if(getPlanFromPotential(best_pose,plan)){// 8.2 平滑接近目标段smoothApproachToGoal(best_pose,plan);// ========== 9. 处理最终接近朝向 ==========// 如果配置了使用最终接近朝向if(use_final_approach_orientation_){size_t plan_size=plan.poses.size();if(plan_size==1){// 只有单个点（起点=终点），使用起始朝向plan.poses.back().pose.orientation=start.orientation;}elseif(plan_size>1){// 计算路径最后一段的方向作为最终朝向double dx,dy,theta;auto last_pose=plan.poses.back().pose.position;// 最后一个点auto approach_pose=plan.poses[plan_size-2].pose.position;// 倒数第二个点// 处理特殊情况：如果最后两个点重合（可能是算法产生的冗余点）if(std::abs(last_pose.x-approach_pose.x)<0.0001&&std::abs(last_pose.y-approach_pose.y)<0.0001&&plan_size>2){// 使用倒数第三个点来计算方向approach_pose=plan.poses[plan_size-3].pose.position;}// 计算方向角dx=last_pose.x-approach_pose.x;dy=last_pose.y-approach_pose.y;theta=atan2(dy,dx);// 将方向角转换为四元数，只绕Z轴旋转（保持水平）plan.poses.back().pose.orientation=nav2_util::geometry_utils::orientationAroundZAxis(theta);}}}else{RCLCPP_ERROR(logger_,"Failed to create a plan from potential when a legal"" potential was found. This shouldn't happen.");}}// ========== 10. 返回规划结果 ==========// 如果路径非空，表示规划成功return!plan.poses.empty();}

总结

这个模块是nav2默认的适配差速车的全局路径规划算法，负责从起点到终点的路径规划。