ROS - move_base全局路径规划之A*程序分析

最新推荐文章于 2024-05-04 21:08:47 发布

原创

最新推荐文章于 2024-05-04 21:08:47 发布 · 4.1k 阅读

82 ·

CC 4.0 BY-SA版权

其中A*的的实现代码的详细分析如下：

#include<global_planner/astar.h>
#include<costmap_2d/cost_values.h>

namespace global_planner {
   
   

AStarExpansion::AStarExpansion(PotentialCalculator* p_calc, int xs, int ys) :
        Expander(p_calc, xs, ys) {
   
   
}

/*	function：计算规划代价的函数
	potential: 代价数组， 
	costs： 地图指针， 
	cycles：循环次数； 
*/
bool AStarExpansion::calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y,  int cycles, float* potential) {
   
   
	//queue_为启发式搜索到的向量队列：<i , cost>
    queue_.clear();
	//起点对应的序号
    int start_i = toIndex(start_x, start_y);
	//1 将起点放入队列
    queue_.push_back(Index(start_i, 0