mppi: return NO_INFORMATION when the checked point is outside the cos…

…tmap (#3816) (#3818) otherwise the controller crashes at ObstaclesCritic::costAtPose because x_i and y_i isn't initialized. (cherry picked from commit 6b250a7) Co-authored-by: Chuanhong Guo <[email protected]>
ros-navigation · Sep 14, 2023 · 9e63d43 · 9e63d43
1 parent aaee5ae
commit 9e63d43
Showing 1 changed file with 4 additions and 1 deletion.
diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp
@@ -192,7 +192,10 @@ CollisionCost ObstaclesCritic::costAtPose(float x, float y, float theta)
   float & cost = collision_cost.cost;
   collision_cost.using_footprint = false;
   unsigned int x_i, y_i;
-  collision_checker_.worldToMap(x, y, x_i, y_i);
+  if (!collision_checker_.worldToMap(x, y, x_i, y_i)) {
+    cost = nav2_costmap_2d::NO_INFORMATION;
+    return collision_cost;
+  }
   cost = collision_checker_.pointCost(x_i, y_i);
 
   if (consider_footprint_ && (cost >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1)) {