ROS globalplanner A*算法的改进
一直想对ros中的a*的改进,看源码astar.cpp中有四个add(),只有上下左右,没有斜方向。自己加了之后感觉还不如原来规划的,不知道为什么。
#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) {
}
bool AStarExpansion::calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y,
int cycles, float* potential) {
queue_.clear();
int start_i = toIndex(start_x, start_y);
queue_.push_back(Index(start_i, 0));
std::fill(potential, potential + ns_, POT_HIGH);
potential[start_i] = 0;
int goal_i = toIndex(end_x, end_y);
int cycle = 0;
while (queue_.size() > 0 && cycle < cycles) {
Index top = queue_[0];//min