nav2_costmap_2d: static_layer: add grid value interpretation for inscribed inflated obstacles

.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\
                 -<no value>\
@@ -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 }}\

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};

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_) {

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<unsigned char>(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));

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);

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);