规划器服务

nav2_planner | Nav2规划器
nav2_navfn_planner ｜ navfn规划器，Nav2规划器的一个实现，使用Dijkstra或A*，适用于全局路径规划，即规划整个路径从起点到终点，通常在静态环境中表现较好(熟悉，掌握使用)
nav2_smac_planner | smac规划器，Nav2规划器的一个实现，使用4或8个连接邻域实现2D A* 算法，具有更平滑和多分辨率的查询，适用于局部路径规划，即在已知的地图上规划避开障碍物的路径，通常用于动态环境中机器人的避障
nav2_theta_star_planner | theta_star规划器，Theta*的一种实现，使用任一视线创建非离散定向的路径段(了解)

插件	支持的机器人类型
NavFn规划器	圆形差速，圆形全向
Smac Planner 2D
Theta Star Planner
Smac Hybrid-A* Planner	非圆形或圆形阿克曼，非圆形或圆形腿式
Smac Lattice Planner	非圆形微速，非圆形全向

nav2_planner

PlannerServer：规划服务类，继承于nav2_util::LifecycleNode，实现行为树的 ComputePathToPose 接口action服务器，并托管不同算法的各种插件来计算和规划。
- 主要属性
  - 单点导航action_server_pose_ = nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose>;
  - 路点导航action_server_poses_ = nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathThroughPoses>;
  - 全局规划加载器：pluginlib::ClassLoader<nav2_core::GlobalPlanner> gp_loader_
- PlannerServer：构造函数
  - 节点名称：planner_server
  - 初始化成员变量
    - default_ids_ -> {GridBased}
    - default_types_ -> {nav2_navfn_planner/NavfnPlanner}
  - 初始化节点参数
    - planner_plugins -> default_ids_
    - expected_planner_frequency -> 1.0
    - 如果planner_plugins_是默认值default_ids_：GridBased.plugin -> nav2_navfn_planner/NavfnPlanner
  - 初始化代价地图： costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>("global_costmap", std::string{get_namespace()}, "global_costmap")
- getPlan：从所配置的插件获取到规划结果，根据传入的规划插件id，调用planners_[planner_id]->createPlan(start, goal)规划路径
- on_configure：配置成员变量和初始化规划器
  - 激活代价地图：costmap_ros_->configure()
  - 启动一个节点跑代价地图节点：costmap_thread_ = std::make_unique<nav2_util::NodeThread>(costmap_ros_)
  - 遍历planner_ids_ ，获取节点参数，创建插件实例：nav2_core::GlobalPlanner::Ptr planner = gp_loader_.createUniqueInstance(planner_types_[i]) ，并激活planner->configure
  - 获取规划频率expected_planner_frequency参数
  - 参数参数话题和服务
    - plan_publisher_ = create_publisher<nav_msgs::msg::Path>("plan", 1)
    - action_server_pose_ = std::make_unique<ActionServerToPose>(shared_from_this(), "compute_path_to_pose",...) ，绑定computePlan
    - action_server_poses_ = std::make_unique<ActionServerThroughPoses>(shared_from_this(), "compute_path_through_poses",...) ，绑定computePlanThroughPoses
- on_activate：激活成员变量
  - plan_publisher_->on_activate();
  - action_server_pose_->activate();
  - action_server_poses_->activate();
  - costmap_ros_->activate();
  - 遍历插件依次调用activate激活
  - 创建路径校验服务，is_path_valid_service_ = node->create_service<nav2_msgs::srv::IsPathValid>("is_path_valid",...) ，绑定isPathValid
  - 创建参数变更，绑定dynamicParametersCallback
  - 创建连接createBond()
- on_deactivate：取消激活成员变量，相关话题服务插件deactivate，destroyBond()
- on_cleanup：重置成员变量，相关话题服务属性reset，插件clearup
- on_shutdown：当关闭状态的时候调用
- isServerInactive：模板函数，action服务器是否激活可用
- isCancelRequested：模板函数，检查action服务器是否有待处理的取消请求
  - 调用传入的服务实例的action_server->is_cancel_requested()
  - 如果存在调用action_server->terminate_all()
- waitForCostmap：等待成本图随着更新的传感器数据而生效，或者在清除恢复后重新填充。阻塞直到true且没有超时。
- getPreemptedGoalIfRequested：模板函数，检查Action Server是否有抢占请求，并用新的抢占目标替换当前目标。
  - 调用传入的服务实例的action_server->is_preempt_requested()
  - 存在调用goal = action_server->accept_pending_goal()
- getStartPose：模板函数，从代价地图获取开始位姿
  - goal->use_start -> start = goal->start; -> return true;
  - 否则!costmap_ros_->getRobotPose(start) -> action_server->terminate_current() -> return false;
- transformPosesToGlobalFrame：模板函数，调用costmap_ros_->transformPoseToGlobalFrame将开始和目标位姿变换到成本图全局框架中，供路径规划插件使用
- validatePath：模板函数，验证路径是否包含有意义的路径，看点个数，打印相关日志
- computePlan：ComputePathToPose服务回调函数，获取规划结果
  - 未激活或取消请求，isServerInactive(action_server_pose_) || isCancelRequested(action_server_pose_)，直接return
  - 调用waitForCostmap(), getPreemptedGoalIfRequested(action_server_pose_, goal)
  - 调用getStartPose(action_server_pose_, goal, start)，如果提供了开始坐标就使用，否则使用机器人当前坐标
  - 转成全局坐标transformPosesToGlobalFrame(action_server_pose_, curr_start, curr_goal)
  - 获取路径curr_path = getPlan(curr_start, curr_goal, goal->planner_id)
  - 校验是否有效validatePath(action_server_pose_, curr_goal, curr_path, goal->planner_id)
  - 发布显示publishPlan(result->path)
  - 调用action_server_pose_->succeeded_current(result)
- computePlanThroughPoses：ComputePathThroughPoses服务回调函数，获取规划结果
  - 未激活或取消请求，isServerInactive(action_server_poses_) || isCancelRequested(action_server_poses_)，直接return
  - 调用waitForCostmap(), getPreemptedGoalIfRequested(action_server_poses_, goal)
  - 调用getStartPose(action_server_poses_, goal, start)，如果提供了开始坐标就使用，否则使用机器人当前坐标
  - 遍历goal->goals获取通过这些点的连续路径
    - 转成全局坐标transformPosesToGlobalFrame(action_server_poses_, curr_start, curr_goal)
    - 获取路径curr_path = getPlan(curr_start, curr_goal, goal->planner_id)
    - 校验是否有效validatePath(action_server_poses_, curr_goal, curr_path, goal->planner_id)
    - 将路径连接在一起
    - 发布显示publishPlan(result->path)
    - 调用action_server_poses_->succeeded_current(result)
- isPathValid：用于确定路径是否仍然是有效的服务回调
  - 遍历路径点，找到欧式距离最近的点及其索引
  - 致命障碍物检查从最近的点开始，以避免已经通过且可能已被占领的点
    - 调用costmap_->worldToMap，全局坐标转代价地图坐标
    - 调用cost = costmap_->getCost(mx, my)，获取代价值，cost == 254 或 253，路径无效
- publishPlan：发布路径用于展示(rviz)，如果发布话题激活并且有订阅者，调用plan_publisher_->publish
- dynamicParametersCallback：参数变化回调

nav2_navfn_planner

NavFn：规划类，缓存代价地图和NavFn地图，地图是基于像素的，原点在左上，x向右，y向下
- 主要成员变量
  - COSTTYPE * costarr：用于存储2D配置空间中每个网格点的代价值。这些代价值通常表示了该点的通行代价，例如障碍物会有较高的代价值，而可通行区域则有较低的代价值
  - float * potarr：用于存储导航函数的潜在数组。导航函数通常被用于路径规划中，它表示了从起点到每个点的最小代价值，用于指导路径规划。
  - bool * pending：用于标记在路径规划中尚未被处理的网格点。这些点可能需要在后续的路径规划过程中被进一步处理
  - int nobs：表示障碍物的数量，用于路径规划算法中的优化和统计
  - int * pb1, * pb2, * pb3：用于存储优先级缓冲区的数据。这些缓冲区通常用于路径规划算法中的优先级队列的实现
  - int * curP, * nextP, * overP：表示当前、下一个和超出优先级缓冲区的指针。这些指针通常用于管理优先级队列中的元素
  - int curPe, nextPe, overPe：表示当前、下一个和超出优先级缓冲区的终点索引。这些索引通常用于管理优先级队列中的元素
  - float curT：表示当前的优先级阈值。这个阈值通常用于控制路径规划算法的执行顺序和策略。
  - float priInc：表示优先级阈值的增量。这个增量通常用于调整路径规划算法的执行策略和性能。
  - gradx：梯度数组，存储了与潜在函数数组大小相同的梯度值。通常用于路径规划算法中计算梯度信息。
  - grady：梯度数组，与 gradx 类似，用于存储垂直方向上的梯度值。
  - pathx：路径点数组，存储了路径的子像素单元格的 x 坐标。
  - pathy：路径点数组，存储了路径的子像素单元格的 y 坐标。
  - npath：路径点数目，表示路径中包含的点的数量。
  - npathbuf：pathx 和 pathy 数组的缓冲区大小，即分配的内存空间大小。
- NavFn：构造函数，入参地图的x和y大小
  - 属性成员变量初始化
  - 调用setNavArr
- setNavArr：设置或重置NavFn地图，初始化相关数组
- setCostmap：为planner设置代价地图数组，可能来自ROS或PGM，迭代赋值，通常来自ROS订阅
- calcNavFnAstar：使用A*规划一条路径，返回true or false
  - 调用setupNavFn(true)
  - 调用propNavFnAstar(std::max(nx * ny / 20, nx + ny))
- calcNavFnDijkstra：使用Dijkstra规划一条路径，返回true or false
  - 调用setupNavFn(true)
  - 调用propNavFnDijkstra(std::max(nx * ny / 20, nx + ny), atStart)
- getPathX：返回路径的x坐标
- getPathY：返回路径的y坐标
- getPathLen：返回路径长，如果未找到，返回0
- getLastPathCost：获取上次A*计算的路径成本last_path_cost_
- setGoal：设置规划的目标位置，注意：计算的导航成本字段是给出了从目标（而不是从起点）到达给定点的成本。
- setStart：设置规划的起点坐标
- initCost：初始化代价单元格，potarr[k] = v;push_cur(上下左右)
- updateCell：根据索引更新单元格(需要花时间结合代码看)
  - 定义了四个变量 u、d、l、r，分别表示当前单元格的上、下、左、右邻居的潜在代价值（potential）。通过 potarr 数组获取邻居单元格的潜在代价值。
  - 计算当前单元格的上、下、左、右邻居的最低代价值，并将其存储在 ta 和 tc 变量中。
  - 根据当前单元格和其邻居单元格的代价值以及遍历代价的关系，更新当前单元格的代价值。如果当前单元格的代价值小于障碍物的遍历代价（COST_OBS），则执行以下操作：
    - a. 计算两个邻居单元格代价值之间的相对差值 dc，并确定哪一个邻居单元格的代价值更低。
    - b. 根据相对差值和当前单元格的遍历代价，使用线性插值或二次插值的方式计算出新的代价值 pot。
  - 如果新计算的代价值小于当前单元格的代价值，则将当前单元格的代价值更新为新的代价值，并将受影响的邻居单元格添加到适当的优先级缓冲区中。添加到优先级缓冲区的方式取决于当前单元格的代价值是否小于当前优先级阈值 curT。
    - a. 如果当前单元格的代价值小于当前优先级阈值 curT，则将受影响的邻居单元格添加到next低成本缓冲区中。
    - b. 如果当前单元格的代价值大于等于当前优先级阈值 curT，则将受影响的邻居单元格添加到溢出缓冲区中。
- updateCellAstar：使用A*根据索引更新单元格(需要花时间结合代码看)
  - 定义了四个变量 u、d、l、r，分别表示当前单元格的上、下、左、右邻居的潜在代价值（potential）。通过 potarr 数组获取邻居单元格的潜在代价值。
  - 计算当前单元格的上、下、左、右邻居的最低代价值，并将其存储在 ta 和 tc 变量中。
  - 根据当前单元格和其邻居单元格的代价值以及遍历代价的关系，更新当前单元格的代价值。如果当前单元格的代价值小于障碍物的遍历代价（COST_OBS），则执行以下操作：
    - a. 计算两个邻居单元格代价值之间的相对差值 dc，并确定哪一个邻居单元格的代价值更低。
    - b. 根据相对差值和当前单元格的遍历代价，使用线性插值或二次插值的方式计算出新的代价值 pot。
  - 如果新计算的代价值小于当前单元格的代价值，则将当前单元格的代价值更新为新的代价值，并将受影响的邻居单元格添加到适当的优先级缓冲区中。添加到优先级缓冲区的方式取决于当前单元格的代价值加上当前点到起点的距离 是否小于当前优先级阈值 curT。
    - a. 如果当前单元格的代价值小于当前优先级阈值 curT，则将受影响的邻居单元格添加到next低成本缓冲区中。
    - b. 如果当前单元格的代价值大于等于当前优先级阈值 curT，则将受影响的邻居单元格添加到溢出缓冲区中。
- setupNavFn：为新的传播设置数组
  - 重置传播数组中的潜在代价值设置为POT_HIGH最大
  - 设置边界为障碍物，即地图的四个边界都被标记为障碍物
    - 初始化优先级缓冲区
    - curT用于记录当前的最小代价值。
    - curP、nextP、overP是用于存储优先级信息的缓冲区。
    - pending是一个布尔数组，用于标记每个点是否在处理队列中。
  - 设置目标代价值为0，调用initCost(k, 0)，k为目标索引，把目标的潜在代价值设置为0
  - 找到为障碍物的单元格数量
- propNavFnDijkstra：Dijkstra规划(需要花时间结合代码看)
  - 初始化了一些变量，包括 nwv（最大优先级块大小）、nc（放入优先级块的单元格数量）和 cycle（循环计数器）。
  - 将起点的索引转换为一维数组中的位置。
  - 使用一个循环，执行给定的循环次数。在每次循环中：
    - a. 如果当前和下一个优先级块都为空，则退出循环。
    - b. 统计当前优先级块中的单元格数量，并更新最大优先级块大小。
    - c. 重置当前优先级缓冲区中的单元格的挂起标志pending[*(pb++)] = false。
    - d. 处理当前优先级缓冲区中的单元格，通过调用 updateCell(*pb++) 函数来更新它们的状态。
    - e. 交换当前和下一个优先级块，并根据情况更新当前优先级缓冲区。
    - f. 如果当前优先级块为空，则增加当前优先级阈值并切换到溢出块。
    - g. 如果 atStart 为真，则检查是否达到了起点，起点的潜在代价值<POT_HIGH，小于表示循环迭代到起点了，并在达到起点时退出循环。
  - 返回一个布尔值，表示是否在给定的循环次数内完成了计算。
- propNavFnAstar：A*规划(需要花时间结合代码看)
  - 初始化了一些变量，包括 nwv（最大优先级块大小）、nc（放入优先级块的单元格数量）和 cycle（循环计数器）。
  - 根据距离(起点到目标点) * COST_NEUTRAL(50)设置初始阈值，curT = dist + curT
  - 将起点的索引转换为一维数组中的位置。
  - 使用一个循环，执行给定的循环次数。在每次循环中：
    - a. 如果当前和下一个优先级块都为空，则退出循环。
    - b. 统计当前优先级块中的单元格数量，并更新最大优先级块大小。
    - c. 重置当前优先级缓冲区中的单元格的挂起标志pending[*(pb++)] = false。
    - d. 处理当前优先级缓冲区中的单元格，通过调用 updateCellAstar(*pb++) 函数来更新它们的状态。
    - e. 交换当前和下一个优先级块，并根据情况更新当前优先级缓冲区。
    - f. 如果当前优先级块为空，则增加当前优先级阈值并切换到溢出块。
    - g. 检查是否达到了起点，起点的潜在代价值<POT_HIGH，小于表示循环迭代到起点了，并在达到起点时退出循环，并设置一下最后的代价值last_path_cost_ = potarr[startCell]
  - 返回一个布尔值，表示是否达到了起点。
- calcPath：计算至少 n 个周期的路径
  - 设置起点位置：接着根据传入的起点位置st，计算起点单元格的索引位置，并初始化偏移量dx和dy为0，并将路径点数目npath初始化为0。
  - 循环搜索路径：使用一个循环来搜索路径，最多进行n次循环。
  - 检查是否接近目标点：在每次循环中，首先检查当前位置是否接近目标点。如果当前位置的最近点的潜在值小于设定的阈值（COST_NEUTRAL），则认为已经到达目标点，将目标点坐标加入路径数组中，并返回路径点数目。
  - 检查是否越界：然后检查当前位置是否越界。如果当前位置超出地图边界，则打印警告信息并返回0表示未找到路径。
  - 添加当前位置到路径中：将当前位置的坐标加入到路径数组中，并增加路径点数目npath。
  - 检测振荡：如果已经有超过两个路径点，并且最后两个路径点与倒数第三个路径点相同，则表示发生了振荡，打印警告信息，并尝试修复它。
  - 检查潜在函数边界：然后检查当前位置周围的八个相邻单元格的潜在值是否过高，或者是否发生了振荡。如果是，则重新选择路径，并更新当前位置。
  - 计算插值梯度：如果当前位置没有超出边界并且潜在函数值较低，则调用gradCell计算当前位置附近四个位置的梯度，并使用双线性插值计算当前位置的梯度。
  - 移动：根据插值得到的梯度信息，更新偏移量dx和dy，并检查是否超出边界，如果超出边界则调整当前位置。
  - 返回结果：如果完成了指定次数的循环仍未找到路径，则打印警告信息并返回0表示未找到路径。
- gradCell：计算单元格的梯度
  - 检查梯度和：首先，函数检查给定单元格 n 处的梯度和是否大于零。如果大于零，表明这个单元格处于一个高潜在值的区域，因此将返回1.0表示梯度的模长，并不进行后续计算。
  - 检查是否越界：接着，函数检查给定单元格 n 是否超出了地图边界。如果超出了地图边界，将返回0.0表示梯度的模长，并不进行后续计算。
  - 计算梯度：如果给定单元格 n 既不处于高潜在值区域，也没有越界，那么就根据该单元格及其相邻单元格的潜在值来计算梯度。梯度的计算分为两种情况：
    - 如果当前单元格处于障碍物区域（潜在值大于等于 POT_HIGH），则梯度的计算基于相邻单元格是否处于障碍物区域。如果相邻单元格处于障碍物区域，则在对应方向上的梯度增加或减少一个固定的值 COST_OBS ，表示对应方向上的梯度方向被障碍物阻挡。
    - 如果当前单元格不处于障碍物区域，则梯度的计算基于当前单元格及其相邻单元格的潜在值之差。如果相邻单元格的潜在值小于 POT_HIGH，则将当前单元格和相邻单元格的潜在值之差加入到对应方向上的梯度中。
  - 梯度归一化：最后，对计算得到的梯度进行归一化处理，即将梯度向量除以其模长，从而得到单位向量，并将归一化后的梯度存储到 gradx 和 grady 数组中。
  - 返回结果：函数最后返回计算得到的梯度的模长
NavfnPlanner：NavFn规划节点类，继承于nav2_core::GlobalPlanner
- NavfnPlanner：构造函数
- configure：重写函数
  - 初始化成员变量
    - tf_,node_
    - costmap_ = costmap_ros->getCostmap()
    - global_frame_ = costmap_ros->getGlobalFrameID()
    - declare_parameter_if_not_declared初始化声明参数
      - name + .tolerance -> 0.5
      - name + .use_astar -> false
      - name + .allow_unknown -> true
      - name + .use_final_approach_orientation -> false
  - 创建NavFn，planner_ = std::make_unique<NavFn>(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY())
- cleanup：重写函数，属性reset
- activate：重写函数，绑定参数回调dynamicParametersCallback
- deactivate：重写函数，属性reset
- createPlan：重写函数，生成一个从起点到目标位置的规划
  - 调用isPlannerOutOfDate() 查看是否过时，过时调用planner_->setNavArr(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY())
  - 检查起点和目标点是否位置是否一致，一致调用costmap_->worldToMap，检测是否存在障碍物，不存在的话更新路径
  - 否则调用makePlan
- makePlan：根据给定的全局起点和目标位置计算一个规划
  - 调用worldToMap，把起点坐标转换到代价地图坐标
  - 调用clearRobotCell(mx, my)，把代价地图中，当前机器人位置置为空闲状态
  - 调用planner_->setNavArr(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY())
  - 调用planner_->setCostmap(costmap_->getCharMap(), true, allow_unknown_)
  - 调用worldToMap，把目标坐标转换到代价地图坐标
  - 调用planner_->setStart(map_goal)和planner_->setGoal(map_start)
  - 如果参数use_astar_ == true，调用planner_->calcNavFnAstar()，否则planner_->calcNavFnDijkstra(true)
  - 调用getPointPotential(goal)返回是否小于POT_HIGH
    - 小于，已到达目标位置，最好的位姿就是best_pose = goal
    - 否则，找目标位置的旁边8个点中潜在代价小于POT_HIGH并且距离最小的点best_pose
  - 调用getPlanFromPotential，规划成功
    - 调用smoothApproachToGoal
    - 根据参数和规划的点坐标数量，确认最后目标位置的车头朝向
- computePotential：定义后未实现？
- getPlanFromPotential：从潜在代价数组中计算一条规划到目标位置
  - 调用worldToMap
  - 调用planner_->setStart(map_goal)
  - 调用planner_->calcPath(max_cycles)返回0，直接返回false
  - 逆序遍历规划局部坐标数组，调用mapToWorld转换成全局坐标
- smoothApproachToGoal：删除路径末端的点位，如果最后一个坐标和倒数第二个坐标的距离大于目标位置到倒数第二个坐标的距离，则最后一个坐标替换为目标位置坐标
- getPointPotential：计算世界上给定点的潜在代价或导航成本，调用worldToMap -> planner_->potarr[index]
- squared_distance：计算两点之间的平方距离
- worldToMap：世界坐标转地图坐标
- mapToWorld：地图坐标转世界坐标
- clearRobotCell：将对应的单元格设置为空闲可用状态，调用costmap_->setCost(mx, my, nav2_costmap_2d::FREE_SPACE)
- isPlannerOutOfDate：确定是否应创建新的规划器对象，如果过期则返回true，过时条件：规划未初始化或规划的地图边界xy与代价地图的边界不相等了
- dynamicParametersCallback：参数变更回调

规划器服务

datetime:2024/03/11 11:35

author:nzb

导航

规划器服务

nav2_planner

nav2_navfn_planner

results matching ""

No results matching ""