Warn if inflation_radius_ < inscribed_radius_ (#4423)

* Warn if inflation_radius_ < inscribed_radius_ Signed-off-by: Tony Najjar <[email protected]> * convert to error Signed-off-by: Tony Najjar <[email protected]> --------- Signed-off-by: Tony Najjar <[email protected]>
ros-navigation · Jun 18, 2024 · 91b4f8e · 91b4f8e
1 parent d512820
commit 91b4f8e
Showing 1 changed file with 10 additions and 0 deletions.
diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp
@@ -170,6 +170,16 @@ InflationLayer::onFootprintChanged()
   computeCaches();
   need_reinflation_ = true;
 
+  if (inflation_radius_ < inscribed_radius_) {
+    RCLCPP_ERROR(
+      logger_,
+      "The configured inflation radius (%.3f) is smaller than "
+      "the computed inscribed radius (%.3f) of your footprint, "
+      "it is highly recommended to set inflation radius to be at "
+      "least as big as the inscribed radius to avoid collisions",
+      inflation_radius_, inscribed_radius_);
+  }
+
   RCLCPP_DEBUG(
     logger_, "InflationLayer::onFootprintChanged(): num footprint points: %zu,"
     " inscribed_radius_ = %.3f, inflation_radius_ = %.3f",