Obstacle layer dynamic parameters

.gitignore

@@ -51,3 +51,6 @@ sphinx_doc/_build
 # CLion artifacts
 .idea
 cmake-build-debug/
+
+# doxygen docs
+doc/html/

nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp

@@ -131,6 +131,13 @@ class ObstacleLayer : public CostmapLayer
    */
   virtual bool isClearable() {return true;}
 
+  /**
+   * @brief Callback executed when a parameter change is detected
+   * @param event ParameterEvent message
+   */
+  rcl_interfaces::msg::SetParametersResult
+  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
+
   /**
    * @brief triggers the update of observations buffer
    */
@@ -234,6 +241,9 @@ class ObstacleLayer : public CostmapLayer
   /// @brief Used to store observation buffers used for clearing obstacles
   std::vector<std::shared_ptr<nav2_costmap_2d::ObservationBuffer>> clearing_buffers_;
 
+  /// @brief Dynamic parameters handler
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
+
   // Used only for testing purposes
   std::vector<nav2_costmap_2d::Observation> static_clearing_observations_;
   std::vector<nav2_costmap_2d::Observation> static_marking_observations_;

nav2_costmap_2d/plugins/obstacle_layer.cpp

@@ -55,12 +55,14 @@ using nav2_costmap_2d::FREE_SPACE;
 
 using nav2_costmap_2d::ObservationBuffer;
 using nav2_costmap_2d::Observation;
+using rcl_interfaces::msg::ParameterType;
 
 namespace nav2_costmap_2d
 {
 
 ObstacleLayer::~ObstacleLayer()
 {
+  dyn_params_handler_.reset();
   for (auto & notifier : observation_notifiers_) {
     notifier.reset();
   }
@@ -74,7 +76,6 @@ void ObstacleLayer::onInitialize()
   // The topics that we'll subscribe to from the parameter server
   std::string topics_string;
 
-  // TODO(mjeronimo): these four are candidates for dynamic update
   declareParameter("enabled", rclcpp::ParameterValue(true));
   declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(true));
   declareParameter("max_obstacle_height", rclcpp::ParameterValue(2.0));
@@ -94,6 +95,12 @@ void ObstacleLayer::onInitialize()
   node->get_parameter("transform_tolerance", transform_tolerance);
   node->get_parameter(name_ + "." + "observation_sources", topics_string);
 
+  dyn_params_handler_ = node->add_on_set_parameters_callback(
+    std::bind(
+      &ObstacleLayer::dynamicParametersCallback,
+      this,
+      std::placeholders::_1));
+
   RCLCPP_INFO(
     logger_,
     "Subscribed to Topics: %s", topics_string.c_str());
@@ -272,6 +279,38 @@ void ObstacleLayer::onInitialize()
   }
 }
 
+rcl_interfaces::msg::SetParametersResult
+ObstacleLayer::dynamicParametersCallback(
+  std::vector<rclcpp::Parameter> parameters)
+{
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
+  rcl_interfaces::msg::SetParametersResult result;
+
+  for (auto parameter : parameters) {
+    const auto & param_type = parameter.get_type();
+    const auto & param_name = parameter.get_name();
+
+    if (param_type == ParameterType::PARAMETER_DOUBLE) {
+      if (param_name == name_ + "." + "max_obstacle_height") {
+        max_obstacle_height_ = parameter.as_double();
+      }
+    } else if (param_type == ParameterType::PARAMETER_BOOL) {
+      if (param_name == name_ + "." + "enabled") {
+        enabled_ = parameter.as_bool();
+      } else if (param_name == name_ + "." + "footprint_clearing_enabled") {
+        footprint_clearing_enabled_ = parameter.as_bool();
+      }
+    } else if (param_type == ParameterType::PARAMETER_INTEGER) {
+      if (param_name == name_ + "." + "combination_method") {
+        combination_method_ = parameter.as_int();
+      }
+    }
+  }
+
+  result.successful = true;
+  return result;
+}
+
 void
 ObstacleLayer::laserScanCallback(
   sensor_msgs::msg::LaserScan::ConstSharedPtr message,
@@ -365,6 +404,7 @@ ObstacleLayer::updateBounds(
   double robot_x, double robot_y, double robot_yaw, double * min_x,
   double * min_y, double * max_x, double * max_y)
 {
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
   if (rolling_window_) {
     updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
   }
@@ -455,7 +495,6 @@ ObstacleLayer::updateFootprint(
   double * max_x,
   double * max_y)
 {
-  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
   if (!footprint_clearing_enabled_) {return;}
   transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_);
 

nav2_costmap_2d/test/integration/obstacle_tests.cpp

@@ -408,6 +408,53 @@ TEST_F(TestNode, testRaytracing) {
   ASSERT_EQ(lethal_count, 1);
 }
 
+/**
+ * Test dynamic parameter setting of obstacle layer
+ */
+TEST_F(TestNode, testDynParamsSetObstacle)
+{
+  auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_costmap");
+
+  // Add obstacle layer
+  std::vector<std::string> plugins_str;
+  plugins_str.push_back("obstacle_layer");
+  costmap->set_parameter(rclcpp::Parameter("plugins", plugins_str));
+  costmap->declare_parameter(
+    "obstacle_layer.plugin",
+    rclcpp::ParameterValue(std::string("nav2_costmap_2d::ObstacleLayer")));
+
+  costmap->set_parameter(rclcpp::Parameter("global_frame", std::string("base_link")));
+  costmap->on_configure(rclcpp_lifecycle::State());
+
+  costmap->on_activate(rclcpp_lifecycle::State());
+
+  auto parameter_client = std::make_shared<rclcpp::AsyncParametersClient>(
+    costmap->get_node_base_interface(), costmap->get_node_topics_interface(),
+    costmap->get_node_graph_interface(),
+    costmap->get_node_services_interface());
+
+  auto results = parameter_client->set_parameters_atomically(
+  {
+    rclcpp::Parameter("obstacle_layer.combination_method", 5),
+    rclcpp::Parameter("obstacle_layer.max_obstacle_height", 4.0),
+    rclcpp::Parameter("obstacle_layer.enabled", false),
+    rclcpp::Parameter("obstacle_layer.footprint_clearing_enabled", false)
+  });
+
+  rclcpp::spin_until_future_complete(
+    costmap->get_node_base_interface(),
+    results);
+
+  EXPECT_EQ(costmap->get_parameter("obstacle_layer.combination_method").as_int(), 5);
+  EXPECT_EQ(costmap->get_parameter("obstacle_layer.max_obstacle_height").as_double(), 4.0);
+  EXPECT_EQ(costmap->get_parameter("obstacle_layer.enabled").as_bool(), false);
+  EXPECT_EQ(costmap->get_parameter("obstacle_layer.footprint_clearing_enabled").as_bool(), false);
+
+  costmap->on_deactivate(rclcpp_lifecycle::State());
+  costmap->on_cleanup(rclcpp_lifecycle::State());
+  costmap->on_shutdown(rclcpp_lifecycle::State());
+}
+
 /**
  * Test dynamic parameter setting of voxel layer
  */