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 )