Fix bug where global planner lethal cost is always 1 unit smaller tha…

…n expected (#78)
rapyuta-robotics · May 24, 2023 · 8091aac · 8091aac
1 parent 4e6cb5e
commit 8091aac
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/global_planner/include/global_planner/dijkstra.h b/global_planner/include/global_planner/dijkstra.h
@@ -85,7 +85,7 @@ class DijkstraExpansion : public Expander {
 
         float getCost(unsigned char* costs, int n) {
             float c = costs[n];
-            if (c < lethal_cost_ - 1 || (unknown_ && c==255)) {
+            if (c < lethal_cost_ || (unknown_ && c==255)) {
                 c = c * factor_ + neutral_cost_;
                 if (c >= lethal_cost_)
                     c = lethal_cost_ - 1;