Show error if inflation radius is smaller than circumscribed radius (#5148)

* Warn if inflation radius is smaller than circumscribed radius

Signed-off-by: Tony Najjar <[email protected]>

* Update nav2_mppi_controller/src/critics/cost_critic.cpp

Co-authored-by: Steve Macenski <[email protected]>
Signed-off-by: Tony Najjar <[email protected]>

* Update nav2_smac_planner/include/nav2_smac_planner/utils.hpp

Co-authored-by: Steve Macenski <[email protected]>
Signed-off-by: Tony Najjar <[email protected]>

---------

Signed-off-by: Tony Najjar <[email protected]>
Signed-off-by: Tony Najjar <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>

Author: tonynajjar

nav2_mppi_controller/src/critics/cost_critic.cpp

@@ -95,6 +95,19 @@ float CostCritic::findCircumscribedCost(
     inflation_layer_name_);
   if (inflation_layer != nullptr) {
     const double resolution = costmap->getCostmap()->getResolution();
+    double inflation_radius = inflation_layer->getInflationRadius();
+    if (inflation_radius < circum_radius) {
+      RCLCPP_ERROR(
+        rclcpp::get_logger("computeCircumscribedCost"),
+        "The inflation radius (%f) is smaller than the circumscribed radius (%f) "
+        "If this is an SE2-collision checking plugin, it cannot use costmap potential "
+        "field to speed up collision checking by only checking the full footprint "
+        "when robot is within possibly-inscribed radius of an obstacle. This may "
+        "significantly slow down planning times!",
+        inflation_radius, circum_radius);
+      result = 0.0;
+      return result;
+    }
     result = inflation_layer->computeCost(circum_radius / resolution);
   } else {
     RCLCPP_WARN(

nav2_smac_planner/include/nav2_smac_planner/utils.hpp

@@ -83,6 +83,19 @@ inline double findCircumscribedCost(std::shared_ptr<nav2_costmap_2d::Costmap2DRO
   if (inflation_layer != nullptr) {
     double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius();
     double resolution = costmap->getCostmap()->getResolution();
+    double inflation_radius = inflation_layer->getInflationRadius();
+    if (inflation_radius < circum_radius) {
+      RCLCPP_ERROR(
+        rclcpp::get_logger("computeCircumscribedCost"),
+        "The inflation radius (%f) is smaller than the circumscribed radius (%f) "
+        "If this is an SE2-collision checking plugin, it cannot use costmap potential "
+        "field to speed up collision checking by only checking the full footprint "
+        "when robot is within possibly-inscribed radius of an obstacle. This may "
+        "significantly slow down planning times!",
+        inflation_radius, circum_radius);
+      result = 0.0;
+      return result;
+    }
     result = static_cast<double>(inflation_layer->computeCost(circum_radius / resolution));
   } else {
     RCLCPP_WARN(