Add grid value interpretation for inscribed inflated obstacles

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

@@ -133,6 +133,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

@@ -181,6 +181,7 @@ void TestNode::initNode(std::vector<rclcpp::Parameter> parameters)
   node_->declare_parameter("track_unknown_space", rclcpp::ParameterValue(false));
   node_->declare_parameter("use_maximum", rclcpp::ParameterValue(false));
   node_->declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100));
+  node_->declare_parameter("inscribed_obstacle_cost_value", rclcpp::ParameterValue(99));
   node_->declare_parameter(
     "unknown_cost_value",
     rclcpp::ParameterValue(static_cast<unsigned char>(0xff)));

nav2_costmap_2d/test/integration/obstacle_tests.cpp

@@ -101,6 +101,7 @@ class TestNode : public ::testing::Test
     node_->declare_parameter("track_unknown_space", rclcpp::ParameterValue(false));
     node_->declare_parameter("use_maximum", rclcpp::ParameterValue(false));
     node_->declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100));
+    node_->declare_parameter("inscribed_obstacle_cost_value", rclcpp::ParameterValue(99));
     node_->declare_parameter(
       "unknown_cost_value",
       rclcpp::ParameterValue(static_cast<unsigned char>(0xff)));
@@ -580,6 +581,7 @@ class TestNodeWithoutUnknownOverwrite : public ::testing::Test
     node_ = std::make_shared<TestLifecycleNode>("obstacle_test_node");
     node_->declare_parameter("track_unknown_space", rclcpp::ParameterValue(true));
     node_->declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100));
+    node_->declare_parameter("inscribed_obstacle_cost_value", rclcpp::ParameterValue(99));
     node_->declare_parameter("trinary_costmap", rclcpp::ParameterValue(true));
     node_->declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.3));
     node_->declare_parameter("observation_sources", rclcpp::ParameterValue(std::string("")));

nav2_costmap_2d/test/integration/plugin_container_tests.cpp

@@ -82,6 +82,7 @@ class TestNode : public ::testing::Test
     node_->declare_parameter("track_unknown_space", rclcpp::ParameterValue(false));
     node_->declare_parameter("use_maximum", rclcpp::ParameterValue(false));
     node_->declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100));
+    node_->declare_parameter("inscribed_obstacle_cost_value", rclcpp::ParameterValue(99));
     node_->declare_parameter(
       "unknown_cost_value",
       rclcpp::ParameterValue(static_cast<unsigned char>(0xff)));

nav2_costmap_2d/test/integration/range_tests.cpp

@@ -105,6 +105,7 @@ class TestNode : public ::testing::Test
     node_->declare_parameter("track_unknown_space", rclcpp::ParameterValue(false));
     node_->declare_parameter("use_maximum", rclcpp::ParameterValue(false));
     node_->declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100));
+    node_->declare_parameter("inscribed_obstacle_cost_value", rclcpp::ParameterValue(99));
     node_->declare_parameter(
       "unknown_cost_value",
       rclcpp::ParameterValue(static_cast<unsigned char>(0xff)));

nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp

@@ -93,6 +93,7 @@ class TestCollisionChecker : public nav2::LifecycleNode
     declare_parameter("track_unknown_space", rclcpp::ParameterValue(true));
     declare_parameter("use_maximum", rclcpp::ParameterValue(false));
     declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100));
+    declare_parameter("inscribed_obstacle_cost_value", rclcpp::ParameterValue(99));
     declare_parameter(
       "unknown_cost_value",
       rclcpp::ParameterValue(static_cast<unsigned char>(0xff)));

nav2_costmap_2d/test/unit/obstacle_layer_test.cpp

@@ -83,6 +83,7 @@ class ObstacleLayerTest : public ::testing::Test
     node_->declare_parameter("track_unknown_space", rclcpp::ParameterValue(false));
     node_->declare_parameter("use_maximum", rclcpp::ParameterValue(false));
     node_->declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100));
+    node_->declare_parameter("inscribed_obstacle_cost_value", rclcpp::ParameterValue(99));
     node_->declare_parameter(
       "unknown_cost_value",
       rclcpp::ParameterValue(static_cast<unsigned char>(0xff)));

nav2_planner/test/test_plan_through_poses.cpp

@@ -170,6 +170,8 @@ void Tester::setParameters()
 
   costmap->declare_parameter("static_layer.plugin", rclcpp::ParameterValue(COSTMAP_LAYER_NAME));
   costmap->declare_parameter("static_layer.lethal_cost_threshold", rclcpp::ParameterValue(100));
+  costmap->declare_parameter("static_layer.inscribed_obstacle_cost_value",
+    rclcpp::ParameterValue(99));
   costmap->declare_parameter(
     "static_layer.map_subscribe_transient_local", rclcpp::ParameterValue(true));