From c11cecbb34db1941764844f5d8d5f6d10f3fe338 Mon Sep 17 00:00:00 2001 From: Leif Terry Date: Mon, 25 Sep 2023 17:59:39 -0400 Subject: [PATCH] Don't assign bad values to radius, even temporarily. --- nav2_costmap_2d/src/footprint.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/nav2_costmap_2d/src/footprint.cpp b/nav2_costmap_2d/src/footprint.cpp index 608a494a9d..b5015b7929 100644 --- a/nav2_costmap_2d/src/footprint.cpp +++ b/nav2_costmap_2d/src/footprint.cpp @@ -42,12 +42,14 @@ namespace nav2_costmap_2d void calculateMinAndMaxDistances( const std::vector & footprint, - double & min_dist, double & max_dist) + double & min_dist_out, double & max_dist_out) { - min_dist = std::numeric_limits::max(); - max_dist = 0.0; + double min_dist = std::numeric_limits::max(); + double max_dist = 0.0; if (footprint.size() <= 2) { + min_dist_out = 0.0; + max_dist_out = 0.0; return; } @@ -68,6 +70,9 @@ void calculateMinAndMaxDistances( footprint.front().x, footprint.front().y); min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist)); max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist)); + + min_dist_out = min_dist; + max_dist_out = max_dist; } geometry_msgs::msg::Point32 toPoint32(geometry_msgs::msg::Point pt)