From b78df2895998db9462ba13e7b56976efa2c32b3f Mon Sep 17 00:00:00 2001 From: Dylan De Coeyer Date: Mon, 29 Sep 2025 18:25:21 +0200 Subject: [PATCH 1/3] nav2_costmap_2d: static_layer: add grid value interpretation for inscribed inflated obstacles Until now, the special OccupancyGrid value "99" attributed to the inscribed inflated obstacles was mapped to 251 on conversion to Costmap2D, using the default linear relation. This is incorrect since a special value exists in the Costmap2D for inscribed obstacles: 253. This commit makes sure that the correct value is used in case of inflated obstacle. Signed-off-by: Dylan De Coeyer --- nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp | 1 + nav2_costmap_2d/plugins/static_layer.cpp | 4 ++++ nav2_costmap_2d/src/costmap_2d_ros.cpp | 1 + 3 files changed, 6 insertions(+) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp index 0333f03577b..c8ac2c65bf9 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -201,6 +201,7 @@ class StaticLayer : public CostmapLayer bool track_unknown_space_; bool use_maximum_; unsigned char lethal_threshold_; + unsigned char inscribed_obstacle_cost_value_; unsigned char unknown_cost_value_; bool trinary_costmap_; bool map_received_{false}; diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 94aa69f87f8..009533e22c4 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -53,6 +53,7 @@ PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::StaticLayer, nav2_costmap_2d::Layer) using nav2_costmap_2d::NO_INFORMATION; using nav2_costmap_2d::LETHAL_OBSTACLE; +using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE; using nav2_costmap_2d::FREE_SPACE; using rcl_interfaces::msg::ParameterType; @@ -157,6 +158,7 @@ StaticLayer::getParameters() node->get_parameter("track_unknown_space", track_unknown_space_); node->get_parameter("use_maximum", use_maximum_); node->get_parameter("lethal_cost_threshold", temp_lethal_threshold); + node->get_parameter("inscribed_obstacle_cost_value", inscribed_obstacle_cost_value_); node->get_parameter("unknown_cost_value", unknown_cost_value_); node->get_parameter("trinary_costmap", trinary_costmap_); node->get_parameter("transform_tolerance", temp_tf_tol); @@ -267,6 +269,8 @@ StaticLayer::interpretValue(unsigned char value) return NO_INFORMATION; } else if (!track_unknown_space_ && value == unknown_cost_value_) { return FREE_SPACE; + } else if (value == inscribed_obstacle_cost_value_) { + return INSCRIBED_INFLATED_OBSTACLE; } else if (value >= lethal_threshold_) { return LETHAL_OBSTACLE; } else if (trinary_costmap_) { diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index f43373d286b..577dff9fb21 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -131,6 +131,7 @@ void Costmap2DROS::init() declare_parameter("initial_transform_timeout", rclcpp::ParameterValue(60.0)); declare_parameter("trinary_costmap", rclcpp::ParameterValue(true)); declare_parameter("unknown_cost_value", rclcpp::ParameterValue(static_cast(0xff))); + declare_parameter("inscribed_obstacle_cost_value", rclcpp::ParameterValue(99)); declare_parameter("update_frequency", rclcpp::ParameterValue(5.0)); declare_parameter("use_maximum", rclcpp::ParameterValue(false)); declare_parameter("subscribe_to_stamped_footprint", rclcpp::ParameterValue(false)); From 3773099cfc9d8a151af7bfe776480ddf9e13f0c3 Mon Sep 17 00:00:00 2001 From: Dylan De Coeyer Date: Wed, 1 Oct 2025 10:04:52 +0200 Subject: [PATCH 2/3] nav2_costmap_2d: fix unit tests following change Signed-off-by: Dylan De Coeyer --- nav2_costmap_2d/test/integration/inflation_tests.cpp | 2 +- nav2_costmap_2d/test/integration/plugin_container_tests.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_costmap_2d/test/integration/inflation_tests.cpp b/nav2_costmap_2d/test/integration/inflation_tests.cpp index 658791beeb5..e2aec4a93c0 100644 --- a/nav2_costmap_2d/test/integration/inflation_tests.cpp +++ b/nav2_costmap_2d/test/integration/inflation_tests.cpp @@ -494,7 +494,7 @@ TEST_F(TestNode, testInflation) // at <0, 1> ASSERT_EQ( countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE) + - countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 54u); + countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 98u); // Add an obstacle at <1, 9>. This will inflate obstacles around it addObservation(olayer, 1, 9); diff --git a/nav2_costmap_2d/test/integration/plugin_container_tests.cpp b/nav2_costmap_2d/test/integration/plugin_container_tests.cpp index dc8a4f4b3a2..126cf7f99c7 100644 --- a/nav2_costmap_2d/test/integration/plugin_container_tests.cpp +++ b/nav2_costmap_2d/test/integration/plugin_container_tests.cpp @@ -268,7 +268,7 @@ TEST_F(TestNode, testDifferentInflationLayers) { nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap(); ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 21); - ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 4); + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 77); } TEST_F(TestNode, testDifferentInflationLayers2) { @@ -317,7 +317,7 @@ TEST_F(TestNode, testDifferentInflationLayers2) { nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap(); ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 21); - ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 28); + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 77); } TEST_F(TestNode, testResetting) { @@ -447,7 +447,7 @@ TEST_F(TestNode, testClearing) { nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap(); ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 21); - ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 29); + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 77); ASSERT_EQ(olayer_b->getCost(9, 9), nav2_costmap_2d::LETHAL_OBSTACLE); pclayer_a->clearArea(-1, -1, 10, 10, false); From cd74efa0ba48b015b6b663459dd74c6b3e684351 Mon Sep 17 00:00:00 2001 From: Dylan De Coeyer Date: Mon, 13 Oct 2025 10:26:58 +0200 Subject: [PATCH 3/3] circleci: increment config version Signed-off-by: Dylan De Coeyer --- .circleci/config.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index 841d60042d1..df3ad800605 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,12 +33,12 @@ _commands: - restore_cache: name: Restore Cache << parameters.key >> keys: - - "<< parameters.key >>-v39\ + - "<< parameters.key >>-v40\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v39\ + - "<< parameters.key >>-v40\ -{{ arch }}\ -main\ -\ @@ -58,7 +58,7 @@ _commands: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v39\ + key: "<< parameters.key >>-v40\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\