ROS下用C++实现A* /JPS算法

在ROS下用C++实现A* /JPS算法

A* 算法流程

见文件:src/grid_path_searcheer/src/demo_node.cpp
主函数 main

1. int main(int argc, char** argv)
2. {
3. ......
4. //订阅到地图信息的回调函数
5. _map_sub = nh.subscribe( "map",
6. //订阅到终点信息的回调函数
7. _pts_sub = nh.subscribe( "waypoints", 1, rcvWaypointsCallback );
8. ......
1, rcvPointCloudCallBack );
9.
10.
//定义了结构体 AstarPathFinder 变量_astar_path_finder,该结构体存储、实现了 Astar 路径规划
所需的所有信息和功能
_astar_path_finder = new AstarPathFinder();
11.
12.
_astar_path_finder -> initGridMap(_resolution, _map_lower, _map_upper, _max_x_id, _max_y_id, _
max_z_id);
13.
14.
//定义了结构体 JPSPathFinder 变量_jps_path_finder,该结构体存储、实现了 JPS 路径规划所需
的所有信息和功能
9. _jps_path_finder = new JPSPathFinder();
10. _jps_path_finder -> initGridMap(_resolution, _map_lower, _map_upper, _max_x_id, _max_y_id, _
max_z_id);
17.
18.
......
11. }

回调函数 rcvPointCloudCallBack

1.void rcvPointCloudCallBack(const sensor_msgs::PointCloud2 & pointcloud_map)
2.{
12. ......
4.// 将障碍物信息设置进入栅格化地图,为后续路径规划做准备
5._astar_path_finder->setObs(pt.x, pt.y, pt.z);
6._jps_path_finder->setObs(pt.x, pt.y, pt.z);
7.
13. // 可视化地图部分
14. _grid_map_vis_pub.publish(map_vis);
15. ......
16. }

回调函数 rcvWaypointsCallback

1.void rcvWaypointsCallback(const nav_msgs::Path & wp)
2.{
3.  ......
4. //获取交互式界面给出的终点坐标
5. target_pt << wp.poses[0].pose.position.x,
6. wp.poses[0].pose.position.y,
7. wp.poses[0].pose.position.z;
8.  ......
9.  //输入起点、终点,调用 pathFind 函数
10. pathFinding(_start_pt, target_pt);  
11.} 

路径规划函数 pathFinding

在A*算法中的主要流程:
1.初始化各种变量
2.将起点加入openSet
3.进入while循环 将openSet里的第一个点设为currentPtr
4.对currentPtr进行扩展节点
5.进入扩展节点函数:扩展节点周围节点,将障碍物和在closelist里的节点剔除,并计算节点的边代价的edgecostSet代价
6.回到while循环里 对扩展的点进行for循环
1 判断是不是全新节点(未被扩展过的)
2 判断是不是已经在openSet里 并判断经过当前点 代价能否更小
7.当当前点=目标节点 回溯路径

二 运行结果

1 对比不同启发函数对效率影响

DiagonalHeu

ManhatanHeu

2 是否加入Tie Breaker对A*效率影响

三 遇到的问题及解决办法

四 对比JPS和A* 更适用于哪种场景