ROS : Navigation - global_planner 功能包源码分析

global_planner 功能包在 planner_core.cpp 中规定一个功能函数 makePlan ，这是整个功能包最重要的函数，而且 move_base 功能包也是通过调用该函数进行全局路径规划。该函数给定 起始 跟 目标 ，输出 路径（一系列 pose ） 。

功能函数 makePlan 输入输出

bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
                           double tolerance, std::vector<geometry_msgs::PoseStamped>& plan)

该函数有 4 个输入，其中 start 、 goal 、 plan 这三个参数都属于 引用传入函数参数 （这是 & 的作用），当 makePlan 中这些变量的值改变了，那么调用该函数时对应传入的变量值也跟着改变了：

• const geometry_msgs::PoseStamped& start ：起始点的位姿
• const geometry_msgs::PoseStamped& goal ：目标点的位姿
• double tolerance ：路径规划器规划出的终点容错距离
• std::vector<geometry_msgs::PoseStamped>& plan ：初始时会清除路径 plan.clear(); ，执行完 makePlan 后会规划出路径（一系列 pose ）

该函数的输出值为 bool 类型，如能规划出非空路径则返回 True ，即 return !plan.empty(); 。

这里主要用到了 geometry_msgs/PoseStamped 消息类型：

root@eln-pc:~/catkin_ws# rosmsg show geometry_msgs/PoseStamped
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Pose pose
  geometry_msgs/Point position
    float64 x
    float64 y
    float64 z
  geometry_msgs/Quaternion orientation
    float64 x
    float64 y
    float64 z
    float64 w

功能函数 makePlan 的整体流程

功能函数 makePlan 会根据几个关键配置参数决定所要调用的算法：

• use_quadratic (bool, default: true) ：如果设置为 true ，使用 二次函数近似势函数（ quadratic_calculator.cpp ） 。否则使用 更加简单的计算方式（ potential_calculator.h ）
• use_dijkstra (bool, default: true) ：如果设置为 true ，则使用 dijkstra 算法（ dijkstra.cpp ） ，否则使用 A-Star 算法（ astar.cpp ）
• use_grid_path (bool, default: false) ：如果设置为 true ，则 创建一条沿着网格边界的路径（ grid_path.cpp ） ，否则使用 梯度下降法（ gradient_path.cpp ）