From a1543f72b8fed9a155fb8d816c59bd84139d27cc Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 12 Jun 2024 11:21:33 +0200 Subject: [PATCH 1/2] Warn if inflation_radius_ < inscribed_radius_ Signed-off-by: Tony Najjar --- nav2_costmap_2d/plugins/inflation_layer.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 067877e548..025e28bf73 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -170,6 +170,14 @@ InflationLayer::onFootprintChanged() computeCaches(); need_reinflation_ = true; + if (inflation_radius_ < inscribed_radius_) { + RCLCPP_WARN( + 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", From d3d79712742c119a5249c09c87349d36f502d40c Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Sun, 16 Jun 2024 18:05:34 +0200 Subject: [PATCH 2/2] convert to error Signed-off-by: Tony Najjar --- nav2_costmap_2d/plugins/inflation_layer.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 025e28bf73..90068f463f 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -171,10 +171,12 @@ InflationLayer::onFootprintChanged() need_reinflation_ = true; if (inflation_radius_ < inscribed_radius_) { - RCLCPP_WARN( + 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", + "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_); }