From 8b49c374acf640770c9a9e02b41be5d3517a6058 Mon Sep 17 00:00:00 2001 From: Chuanhong Guo Date: Thu, 14 Sep 2023 22:19:55 +0800 Subject: [PATCH] mppi: return NO_INFORMATION when the checked point is outside the costmap (#3816) otherwise the controller crashes at ObstaclesCritic::costAtPose because x_i and y_i isn't initialized. (cherry picked from commit 6b250a7c57536ee43a402c9820ac2a2acdb8bc13) --- nav2_mppi_controller/src/critics/obstacles_critic.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index fa98d569739..4568e1eb14f 100644 --- 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)) {