ros全局路径规划分享

move_base如何调用全局路径规划

在move_base.cpp中使用planThread函数生成全局路径规划,是一个独立运行的线程.

在函数planThread中调用makePlan,在makePlan中调用planner_的makePlan方法.planner_的初始化在move_base中的133行,和字符串global_planner的内容有关.

global_planner的内容是通过参数base_global_planner的内容有关,由参数决定全局路径规划使用的包(现在使用的是最新的global_planner/GlobalPlanner)

planner_core全局路径规划的核心

makePlan方法声明在planner_core.cpp的第222行,把传入的世界地图坐标全部转换成地图坐标后调用planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y,nx * ny * 2, potential_array_);

迪杰斯特拉的实现

如果外部参数use_dijkstra为true则planner_->calculatePotentials调用dijkstra.cpp中的函数.这里会把potential_array_中所有像素点赋为最大值,

在potential_array_中从起点一层一层的添加”等代价”的点,起点代价为0,中间每相差一个像素则代价加一,直到超过nx*ny*2这个最大寻找值或者找到goal点.

超过会返回失败,找到则返回新生成的potential_array_代价地图.

A*的实现

A*的实现主要是通过堆(最小二叉树)实现的.

1.初始化把代价地图各个像素对应的分数赋值为最大值,同时在堆queue中加入原点.

2.获得二叉树树顶的点,修改该像素点对应的分数,并且在树中删除它,把它周围4个点和点的分数加入树中.

3.判断树顶点是不是终点,是则完成,不是则继续第二部.

生成路径

获得各个像素点对应的分数,从终点找附近评分最小的像素,一直循环直到找到起点.这条连线就是全局路径.