From 8feaa42d53c7718a573be87ec89cc2e0a876f353 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 18 Jun 2024 17:48:24 +0200 Subject: [PATCH] Warn if inflation_radius_ < inscribed_radius_ (#4423) * Warn if inflation_radius_ < inscribed_radius_ Signed-off-by: Tony Najjar * convert to error Signed-off-by: Tony Najjar --------- Signed-off-by: Tony Najjar --- nav2_costmap_2d/plugins/inflation_layer.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 796c2fd62f..8cbe8dacc9 100644 --- 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",