diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp index bfe8d09ed33..0d6cdfe61f2 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp +++ b/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 parameters); + // ----- Variables ----- /// @brief PointCloud data subscriber diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index 817fb48257c..863af6923db 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -63,6 +63,10 @@ void PointCloud::configure() data_sub_ = node->create_subscription( source_topic, pointcloud_qos, std::bind(&PointCloud::dataCallback, this, std::placeholders::_1)); + + // 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( @@ -123,4 +127,26 @@ void PointCloud::dataCallback(sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) data_ = msg; } +rcl_interfaces::msg::SetParametersResult +PointCloud::dynamicParametersCallback( + std::vector 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_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(); + } + } + } + result.successful = true; + return result; +} + } // namespace nav2_collision_monitor