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{ }
只有当规划的起点和终点都有效时,才会规划路径.
这里就涉及到如何判断规划的起点和终点是否有效的问题.
- 如果是在
动态地图工作模式下,如果受到了地图消息,就会在回调函数 setMap()中,对start点的有效性进行判断,此时start点会设置成机器的坐标/base_link,如果机器人的坐标位于地图范围内,则start有效.否则无效.
- 如果是在
静态地图工作模式下,则只在回调函数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()
没有用到.
