fix-collision checker must capture lethal before unknow (ros-navigati…

…on#2854)
EnjoyRobotics · Jun 30, 2022 · d3564f3 · d3564f3
1 parent 498c7fb
commit d3564f3
Showing 1 changed file with 5 additions and 5 deletions.
diff --git 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;
     }
   }