From d3564f3d79289b46b2b0290996db2a6aaf5d6b58 Mon Sep 17 00:00:00 2001
From: Hossein Sheikhi Darani
 <64957461+HosseinSheikhi@users.noreply.github.com>
Date: Mon, 21 Mar 2022 08:50:23 -0700
Subject: [PATCH] fix-collision checker must capture lethal before unknow
 (#2854)

---
 nav2_costmap_2d/src/footprint_collision_checker.cpp | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/nav2_costmap_2d/src/footprint_collision_checker.cpp b/nav2_costmap_2d/src/footprint_collision_checker.cpp
index 51c1fe7841..f51ec286be 100644
--- a/nav2_costmap_2d/src/footprint_collision_checker.cpp
+++ b/nav2_costmap_2d/src/footprint_collision_checker.cpp
@@ -94,13 +94,13 @@ double FootprintCollisionChecker<CostmapT>::lineCost(int x0, int x1, int y0, int
   for (nav2_util::LineIterator line(x0, y0, x1, y1); line.isValid(); line.advance()) {
     point_cost = pointCost(line.getX(), line.getY());   // Score the current point
 
-    if (line_cost < point_cost) {
-      line_cost = point_cost;
+    // if in collision, no need to continue
+    if (point_cost == static_cast<double>(LETHAL_OBSTACLE)) {
+      return point_cost;
     }
 
-    // if in collision, no need to continue
-    if (line_cost == static_cast<double>(LETHAL_OBSTACLE)) {
-      return line_cost;
+    if (line_cost < point_cost) {
+      line_cost = point_cost;
     }
   }