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 <[email protected]>

Author: DylanDeCoeyer-Quimesis

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