diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp index 1d6b6e27f86..997031b825f 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 af584a8e9cc..998001508bb 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/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 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