From 8091aac75645d9f68de74c83c04e4e8c977ab4bb Mon Sep 17 00:00:00 2001
From: Tiago Silva <15384781+siferati@users.noreply.github.com>
Date: Thu, 25 May 2023 08:37:32 +0900
Subject: [PATCH] Fix bug where global planner lethal cost is always 1 unit
 smaller than expected (#78)

---
 global_planner/include/global_planner/dijkstra.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/global_planner/include/global_planner/dijkstra.h b/global_planner/include/global_planner/dijkstra.h
index 89646ce92a..7fee5528bb 100644
--- 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;