混合式A星代码解析_2 规划主程序Planner.cpp

3.规划程序 Planner.cpp

在main.cpp中,起主要作用的就是:

HybridAStar::Planner hy;
hy.plan();

规划算法都是在HybridAStar::Planner这个类内部进行运算的.所以主要分析以下Planner类.

3.0 实例化的对象

在planner.h中,定义了很多对象,用于最终生成规划路径.

hybridA星生成的路径:

Path path;

用于优化路径的平滑器:

Smoother smoother;

平滑后的路径:

Path smoothedPath = Path(true);

搜索的可视化:

Visualize visualization;

障碍物检测:

CollisionDetection configurationSpace;

voronoi图:

The voronoi diagram

规划时的珊格占用点:

nav_msgs::OccupancyGrid::Ptr grid;

3.1 构造函数 Planner::Planner( )

主要是订阅和发布消息

(1)订阅消息

• 规划起点:/initialpose
• 规划终点:/move_base_simple/goal
• 地图:/map

(2)发布消息

• 规划起点:/move_base_simple/start

3.2 (核心)规划函数 plan( )

规划路径主函数

3.2.1 外围框架

采用结构的是

if (validStart && validGoal) { }
else{ }

只有当规划的起点和终点都有效时,才会规划路径.

这里就涉及到如何判断规划的起点和终点是否有效的问题.

• 起点有效的条件有两个(有一个满足就ok):

1. 如果是在动态地图工作模式下,如果受到了地图消息,就会在回调函数 setMap()中,对start点的有效性进行判断,此时start点会设置成机器的坐标/base_link,如果机器人的坐标位于地图范围内,则start有效.否则无效.
2. 如果是在静态地图工作模式下,则只在回调函数setStart()中进行判断.

• 判断终点是否有效的条件就1个,那就是只在回调函数setGoal()中进行判断

3.2.2 定义栅格的尺寸和离散的数量

// ___________________________

// LISTS ALLOWCATED ROW MAJOR ORDER

int width = grid->info.width;

int height = grid->info.height;

int depth = Constants::headings;

int length = width * height * depth;

// define list pointers and initialize lists

Node3D