Fix the failure of GetPath in GlobalPlanner

global_planner/include/global_planner/astar.h

@@ -67,7 +67,7 @@ class AStarExpansion : public Expander {
         bool calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y, int cycles,
                                 float* potential);
     private:
-        void add(unsigned char* costs, float* potential, float prev_potential, int next_i, int end_x, int end_y);
+        void add(unsigned char* costs, float* potential, float prev_potential, int next_i, double end_x, double end_y);
         std::vector<Index> queue_;
 };
 

global_planner/include/global_planner/potential_calculator.h

@@ -48,7 +48,7 @@ class PotentialCalculator {
             setSize(nx, ny);
         }
         virtual ~PotentialCalculator() {}
-        virtual float calculatePotential(float* potential, unsigned char cost, int n, float prev_potential=-1){
+        virtual float calculatePotential(float* potential, float cost, int n, float prev_potential=-1){
             if(prev_potential < 0){
                 // get min of neighbors
                 float min_h = std::min( potential[n - 1], potential[n + 1] ),

global_planner/include/global_planner/quadratic_calculator.h

@@ -46,7 +46,7 @@ class QuadraticCalculator : public PotentialCalculator {
     public:
         QuadraticCalculator(int nx, int ny): PotentialCalculator(nx,ny) {}
 
-        float calculatePotential(float* potential, unsigned char cost, int n, float prev_potential);
+        float calculatePotential(float* potential, float cost, int n, float prev_potential);
 };
 
 

global_planner/src/astar.cpp

@@ -47,13 +47,13 @@ AStarExpansion::AStarExpansion(PotentialCalculator* p_calc, int xs, int 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);
+    int start_i = toIndex(round(start_x), round(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 goal_i = toIndex(round(end_x), round(end_y));
     int cycle = 0;
 
     while (queue_.size() > 0 && cycle < cycles) {
@@ -76,8 +76,8 @@ bool AStarExpansion::calculatePotentials(unsigned char* costs, double start_x, d
     return false;
 }
 
-void AStarExpansion::add(unsigned char* costs, float* potential, float prev_potential, int next_i, int end_x,
-                         int end_y) {
+void AStarExpansion::add(unsigned char* costs, float* potential, float prev_potential, int next_i, double end_x,
+                         double end_y) {
     if (next_i < 0 || next_i >= ns_)
         return;
 
@@ -86,10 +86,11 @@ void AStarExpansion::add(unsigned char* costs, float* potential, float prev_pote
 
     if(costs[next_i]>=lethal_cost_ && !(unknown_ && costs[next_i]==costmap_2d::NO_INFORMATION))
         return;
-
-    potential[next_i] = p_calc_->calculatePotential(potential, costs[next_i] + neutral_cost_, next_i, prev_potential);
+    float c = costs[next_i] * factor_ + neutral_cost_;
+    potential[next_i] =
+        p_calc_->calculatePotential(potential, c, next_i, prev_potential);
     int x = next_i % nx_, y = next_i / nx_;
-    float distance = abs(end_x - x) + abs(end_y - y);
+    float distance = std::hypot(end_x - x, end_y - y);
 
     queue_.push_back(Index(next_i, potential[next_i] + distance * neutral_cost_));
     std::push_heap(queue_.begin(), queue_.end(), greater1());

global_planner/src/gradient_path.cpp

@@ -81,7 +81,7 @@ bool GradientPath::getPath(float* potential, double start_x, double start_y, dou
         // check if near goal
         double nx = stc % xs_ + dx, ny = stc / xs_ + dy;
 
-        if (fabs(nx - start_x) < .5 && fabs(ny - start_y) < .5) {
+        if (fabs(nx - start_x) < 1. && fabs(ny - start_y) < 1.) {
             current.first = start_x;
             current.second = start_y;
             path.push_back(current);

global_planner/src/grid_path.cpp

@@ -38,27 +38,28 @@
 #include <global_planner/grid_path.h>
 #include <algorithm>
 #include <stdio.h>
+#include <math.h>
 namespace global_planner {
 
 bool GridPath::getPath(float* potential, double start_x, double start_y, double end_x, double end_y, std::vector<std::pair<float, float> >& path) {
     std::pair<float, float> current;
     current.first = end_x;
     current.second = end_y;
 
-    int start_index = getIndex(start_x, start_y);
-
     path.push_back(current);
     int c = 0;
     int ns = xs_ * ys_;
-    
-    while (getIndex(current.first, current.second) != start_index) {
+
+    while (!(fabs(current.first - start_x) < 1. && fabs(current.second - start_y) < 1.)) {
         float min_val = 1e10;
-        int min_x = 0, min_y = 0;
+        int min_x = -1, min_y = -1;
         for (int xd = -1; xd <= 1; xd++) {
             for (int yd = -1; yd <= 1; yd++) {
                 if (xd == 0 && yd == 0)
                     continue;
                 int x = current.first + xd, y = current.second + yd;
+                if(x < 0 || x > xs_ - 1 || y < 0 || y > ys_ - 1)
+                    continue;
                 int index = getIndex(x, y);
                 if (potential[index] < min_val) {
                     min_val = potential[index];
@@ -67,15 +68,14 @@ bool GridPath::getPath(float* potential, double start_x, double start_y, double
                 }
             }
         }
-        if (min_x == 0 && min_y == 0)
+        if (min_x == -1 && min_y == -1)
             return false;
         current.first = min_x;
         current.second = min_y;
         path.push_back(current);
 
-        if(c++>ns*4){
+        if(c++>ns*4)
             return false;
-        }
 
     }
     return true;

global_planner/src/planner_core.cpp

@@ -103,7 +103,7 @@ void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D* costmap,
 
         private_nh.param("old_navfn_behavior", old_navfn_behavior_, false);
         if(!old_navfn_behavior_)
-            convert_offset_ = 0.5;
+            convert_offset_ = 0.0;
         else
             convert_offset_ = 0.0;
 

global_planner/src/quadratic_calculator.cpp

@@ -30,7 +30,7 @@
 #include <global_planner/quadratic_calculator.h>
 
 namespace global_planner {
-float QuadraticCalculator::calculatePotential(float* potential, unsigned char cost, int n, float prev_potential) {
+float QuadraticCalculator::calculatePotential(float* potential, float cost, int n, float prev_potential) {
     // get neighbors
     float u, d, l, r;
     l = potential[n - 1];