混合式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* nodes3D = new Node3D[length]();

Node2D* nodes2D = new Node2D[width * height]();

3.2.3 取出规划的goal和start

// ________________________

// retrieving goal position

float x = goal.pose.position.x / Constants::cellSize;

float y = goal.pose.position.y / Constants::cellSize;

float t = tf::getYaw(goal.pose.orientation);

// set theta to a value (0,2PI]

t = Helper::normalizeHeadingRad(t);

const Node3D nGoal(x, y, t, 0, 0, nullptr);

// _________________________

// retrieving start position

x = start.pose.pose.position.x / Constants::cellSize;

y = start.pose.pose.position.y / Constants::cellSize;

t = tf::getYaw(start.pose.pose.orientation);

// set theta to a value (0,2PI]

t = Helper::normalizeHeadingRad(t);

Node3D nStart(x, y, t, 0, 0, nullptr);

3.2.4 (核心)开始规划并计时

(1)计时:

ros::Time t0 = ros::Time::now();

(2)初始化

// CLEAR THE VISUALIZATION

visualization.clear();

// CLEAR THE PATH

path.clear();

smoothedPath.clear();

(3)(核心)寻找路径(hybridAstar的核心算法)
返回的是符合中goal要求的节点的指针.

// FIND THE PATH

Node3D* nSolution = Algorithm::hybridAStar(nStart, nGoal, nodes3D, nodes2D, width, height, configurationSpace, dubinsLookup, visualization);

(4)跟踪路径
根据goal节点的指针,回溯整个路径.

// TRACE THE PATH

smoother.tracePath(nSolution);

(5)更新路径

// CREATE THE UPDATED PATH

path.updatePath(smoother.getPath());

(6)平滑路径

// SMOOTH THE PATH

smoother.smoothPath(voronoiDiagram);

(7)更新路径

// CREATE THE UPDATED PATH

smoothedPath.updatePath(smoother.getPath());

(8)结束计时

ros::Time t1 = ros::Time::now();

ros::Duration d(t1 - t0);

std::cout << "TIME in ms: " << d * 1000 << std::endl;

(9)发布结果

// _________________________________

// PUBLISH THE RESULTS OF THE SEARCH

path.publishPath();

path.publishPathNodes();

path.publishPathVehicles();

smoothedPath.publishPath();

smoothedPath.publishPathNodes();

smoothedPath.publishPathVehicles();

visualization.publishNode3DCosts(nodes3D, width, height, depth);

visualization.publishNode2DCosts(nodes2D, width, height);

(10)删除节点

delete [] nodes3D;

delete [] nodes2D;

3.3 setGoal()

设置goal,同时判断是否有效.

3.4 setStart()

设置start,同时判断是否有效.

3.5 setMap()

存储地图,如果是动态地图,则判断start是否有效.

3.6 initializeLookups()

没有用到.