Dynamic reconfigure CM pointcloud min_height and max_height (#5366)

* [DEX] make CM pointcloud min_height and max_height

Signed-off-by: nelson <[email protected]>

* [DEX] fix CM dynamicParametersCallback override

Signed-off-by: nelson <[email protected]>

* adapt param validating style

Signed-off-by: nelson <[email protected]>

---------

Signed-off-by: nelson <[email protected]>
Co-authored-by: Guillaume Doisy <[email protected]>

Author: kaichie

nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp

@@ -88,6 +88,13 @@ class PointCloud : public Source
    */
   void dataCallback(sensor_msgs::msg::PointCloud2::ConstSharedPtr msg);
 
+  /**
+   * @brief Callback executed when a parameter change is detected
+   * @param event ParameterEvent message
+   */
+  rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(
+    std::vector<rclcpp::Parameter> parameters);
+
   // ----- Variables -----
 
   /// @brief PointCloud data subscriber

nav2_collision_monitor/src/pointcloud.cpp

@@ -64,6 +64,10 @@ void PointCloud::configure()
     source_topic,
     std::bind(&PointCloud::dataCallback, this, std::placeholders::_1),
     nav2::qos::SensorDataQoS());
+
+  // Add callback for dynamic parameters
+  dyn_params_handler_ = node->add_on_set_parameters_callback(
+    std::bind(&PointCloud::dynamicParametersCallback, this, std::placeholders::_1));
 }
 
 bool PointCloud::getData(
@@ -124,4 +128,32 @@ void PointCloud::dataCallback(sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)
   data_ = msg;
 }
 
+rcl_interfaces::msg::SetParametersResult
+PointCloud::dynamicParametersCallback(
+  std::vector<rclcpp::Parameter> parameters)
+{
+  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_name.find(source_name_ + ".") != 0) {
+      continue;
+    }
+    if (param_type == rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) {
+      if (param_name == source_name_ + "." + "min_height") {
+        min_height_ = parameter.as_double();
+      } else if (param_name == source_name_ + "." + "max_height") {
+        max_height_ = parameter.as_double();
+      }
+    } else if (param_type == rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) {
+      if (param_name == source_name_ + "." + "enabled") {
+        enabled_ = parameter.as_bool();
+      }
+    }
+  }
+  result.successful = true;
+  return result;
+}
+
 }  // namespace nav2_collision_monitor