diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 28488a6776..62e95ebc73 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -100,7 +100,7 @@ class AmclNode : public nav2::LifecycleNode * @return rcl_interfaces::msg::SetParametersResult Result indicating whether the update is accepted. */ rcl_interfaces::msg::SetParametersResult validateParameterUpdatesCallback( - std::vector parameters); + const std::vector & parameters); /** * @brief Apply parameter updates after validation @@ -108,7 +108,7 @@ class AmclNode : public nav2::LifecycleNode * It updates the internal configuration of the node with the new parameter values. * @param parameters List of parameters that have been updated. */ - void updateParametersCallback(std::vector parameters); + void updateParametersCallback(const std::vector & parameters); // Dynamic parameters handler rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr post_set_params_handler_; diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 15c572d2ed..c7c6bf89f2 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -993,11 +993,11 @@ AmclNode::initParameters() } rcl_interfaces::msg::SetParametersResult AmclNode::validateParameterUpdatesCallback( - std::vector parameters) + const std::vector & parameters) { rcl_interfaces::msg::SetParametersResult result; result.successful = true; - for (auto parameter : parameters) { + for (const auto & parameter : parameters) { const auto & param_type = parameter.get_type(); const auto & param_name = parameter.get_name(); if (param_name.find('.') != std::string::npos) { @@ -1043,7 +1043,7 @@ rcl_interfaces::msg::SetParametersResult AmclNode::validateParameterUpdatesCallb void AmclNode::updateParametersCallback( - std::vector parameters) + const std::vector & parameters) { std::lock_guard cfl(mutex_); diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp index b8e976a02f..2bbbf6b4f8 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp +++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp @@ -168,7 +168,7 @@ class KinematicsHandler * @return rcl_interfaces::msg::SetParametersResult Result indicating whether the update is accepted. */ rcl_interfaces::msg::SetParametersResult validateParameterUpdatesCallback( - std::vector parameters); + const std::vector & parameters); /** * @brief Apply parameter updates after validation @@ -176,7 +176,7 @@ class KinematicsHandler * It updates the internal configuration of the node with the new parameter values. * @param parameters List of parameters that have been updated. */ - void updateParametersCallback(std::vector parameters); + void updateParametersCallback(const std::vector & parameters); void update_kinematics(KinematicParameters kinematics); std::string plugin_name_; }; diff --git a/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp b/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp index b6512272e8..5c379c1173 100644 --- a/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp @@ -189,11 +189,11 @@ void KinematicsHandler::setSpeedLimit( } rcl_interfaces::msg::SetParametersResult KinematicsHandler::validateParameterUpdatesCallback( - std::vector parameters) + const std::vector & parameters) { rcl_interfaces::msg::SetParametersResult result; result.successful = true; - for (auto parameter : parameters) { + for (const auto & parameter : parameters) { const auto & param_type = parameter.get_type(); const auto & param_name = parameter.get_name(); if (param_name.find(plugin_name_ + ".") != 0) { @@ -229,12 +229,12 @@ rcl_interfaces::msg::SetParametersResult KinematicsHandler::validateParameterUpd } void -KinematicsHandler::updateParametersCallback(std::vector parameters) +KinematicsHandler::updateParametersCallback(const std::vector & parameters) { rcl_interfaces::msg::SetParametersResult result; KinematicParameters kinematics(*kinematics_.load()); - for (auto parameter : parameters) { + for (const auto & parameter : parameters) { const auto & param_type = parameter.get_type(); const auto & param_name = parameter.get_name(); if (param_name.find(plugin_name_ + ".") != 0) { diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp index 4262d54fed..f5fe81ac46 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp @@ -21,8 +21,7 @@ #include #include -#include "nav2_util/odometry_utils.hpp" -#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/parameter_handler.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_ros_common/node_utils.hpp" @@ -59,39 +58,18 @@ struct Parameters * @class nav2_graceful_controller::ParameterHandler * @brief Handles parameters and dynamic parameters for GracefulMotionController */ -class ParameterHandler +class ParameterHandler : public nav2_util::ParameterHandler { public: /** * @brief Constructor for nav2_graceful_controller::ParameterHandler */ ParameterHandler( - nav2::LifecycleNode::SharedPtr node, + const nav2::LifecycleNode::SharedPtr & node, std::string & plugin_name, rclcpp::Logger & logger, const double costmap_size_x); - /** - * @brief Destructor for nav2_graceful_controller::ParameterHandler - */ - ~ParameterHandler(); - - std::mutex & getMutex() {return mutex_;} - - Parameters * getParams() {return ¶ms_;} - - /** - * @brief Registers callbacks for dynamic parameter handling. - */ - void activate(); - - /** - * @brief Resets callbacks for dynamic parameter handling. - */ - void deactivate(); - protected: - nav2::LifecycleNode::WeakPtr node_; - /** * @brief Validate incoming parameter updates before applying them. * This callback is triggered when one or more parameters are about to be updated. @@ -101,7 +79,7 @@ class ParameterHandler * @return rcl_interfaces::msg::SetParametersResult Result indicating whether the update is accepted. */ rcl_interfaces::msg::SetParametersResult validateParameterUpdatesCallback( - std::vector parameters); + const std::vector & parameters) override; /** * @brief Apply parameter updates after validation @@ -109,15 +87,9 @@ class ParameterHandler * It updates the internal configuration of the node with the new parameter values. * @param parameters List of parameters that have been updated. */ - void updateParametersCallback(std::vector parameters); + void updateParametersCallback(const std::vector & parameters) override; - // Dynamic parameters handler - std::mutex mutex_; - rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr post_set_params_handler_; - rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_params_handler_; - Parameters params_; std::string plugin_name_; - rclcpp::Logger logger_ {rclcpp::get_logger("GracefulMotionController")}; }; } // namespace nav2_graceful_controller diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp index e5a3a4fead..b13844f4b6 100644 --- a/nav2_graceful_controller/src/graceful_controller.cpp +++ b/nav2_graceful_controller/src/graceful_controller.cpp @@ -345,12 +345,12 @@ bool GracefulController::simulateTrajectory( } double distance = std::numeric_limits::max(); - double resolution_ = costmap_ros_->getCostmap()->getResolution(); - double dt = (params_->v_linear_max > 0.0) ? resolution_ / params_->v_linear_max : 0.0; + double resolution = costmap_ros_->getCostmap()->getResolution(); + double dt = (params_->v_linear_max > 0.0) ? resolution / params_->v_linear_max : 0.0; // Set max iter to avoid infinite loop unsigned int max_iter = 3 * - std::hypot(motion_target.pose.position.x, motion_target.pose.position.y) / resolution_; + std::hypot(motion_target.pose.position.x, motion_target.pose.position.y) / resolution; // Generate path do{ @@ -397,7 +397,7 @@ bool GracefulController::simulateTrajectory( // Check if we reach the goal distance = nav2_util::geometry_utils::euclidean_distance(motion_target.pose, next_pose.pose); - }while(distance > resolution_ && trajectory.poses.size() < max_iter); + }while(distance > resolution && trajectory.poses.size() < max_iter); return true; } diff --git a/nav2_graceful_controller/src/parameter_handler.cpp b/nav2_graceful_controller/src/parameter_handler.cpp index 708f32e9d7..38057676b5 100644 --- a/nav2_graceful_controller/src/parameter_handler.cpp +++ b/nav2_graceful_controller/src/parameter_handler.cpp @@ -28,13 +28,11 @@ using nav2::declare_parameter_if_not_declared; using rcl_interfaces::msg::ParameterType; ParameterHandler::ParameterHandler( - nav2::LifecycleNode::SharedPtr node, std::string & plugin_name, + const nav2::LifecycleNode::SharedPtr & node, std::string & plugin_name, rclcpp::Logger & logger, const double costmap_size_x) +: nav2_util::ParameterHandler(node, logger) { - node_ = node; plugin_name_ = plugin_name; - logger_ = logger; - declare_parameter_if_not_declared( node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1)); declare_parameter_if_not_declared( @@ -116,42 +114,12 @@ ParameterHandler::ParameterHandler( } } -void ParameterHandler::activate() -{ - auto node = node_.lock(); - post_set_params_handler_ = node->add_post_set_parameters_callback( - std::bind( - &ParameterHandler::updateParametersCallback, - this, std::placeholders::_1)); - on_set_params_handler_ = node->add_on_set_parameters_callback( - std::bind( - &ParameterHandler::validateParameterUpdatesCallback, - this, std::placeholders::_1)); -} - -void ParameterHandler::deactivate() -{ - auto node = node_.lock(); - if (post_set_params_handler_ && node) { - node->remove_post_set_parameters_callback(post_set_params_handler_.get()); - } - post_set_params_handler_.reset(); - if (on_set_params_handler_ && node) { - node->remove_on_set_parameters_callback(on_set_params_handler_.get()); - } - on_set_params_handler_.reset(); -} - -ParameterHandler::~ParameterHandler() -{ -} - rcl_interfaces::msg::SetParametersResult ParameterHandler::validateParameterUpdatesCallback( - std::vector parameters) + const std::vector & parameters) { rcl_interfaces::msg::SetParametersResult result; result.successful = true; - for (auto parameter : parameters) { + for (const auto & parameter : parameters) { const auto & param_type = parameter.get_type(); const auto & param_name = parameter.get_name(); if (param_name.find(plugin_name_ + ".") != 0) { @@ -187,7 +155,7 @@ rcl_interfaces::msg::SetParametersResult ParameterHandler::validateParameterUpda } void ParameterHandler::updateParametersCallback( - std::vector parameters) + const std::vector & parameters) { std::lock_guard lock_reinit(mutex_); diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp index f2219c6094..e6626c2244 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp @@ -23,8 +23,7 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_ros_common/lifecycle_node.hpp" -#include "nav2_util/odometry_utils.hpp" -#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/parameter_handler.hpp" #include "nav2_ros_common/node_utils.hpp" namespace nav2_regulated_pure_pursuit_controller @@ -69,39 +68,18 @@ struct Parameters * @class nav2_regulated_pure_pursuit_controller::ParameterHandler * @brief Handles parameters and dynamic parameters for RPP */ -class ParameterHandler +class ParameterHandler : public nav2_util::ParameterHandler { public: /** * @brief Constructor for nav2_regulated_pure_pursuit_controller::ParameterHandler */ ParameterHandler( - nav2::LifecycleNode::SharedPtr node, + const nav2::LifecycleNode::SharedPtr & node, std::string & plugin_name, rclcpp::Logger & logger, const double costmap_size_x); - /** - * @brief Destrructor for nav2_regulated_pure_pursuit_controller::ParameterHandler - */ - ~ParameterHandler(); - - std::mutex & getMutex() {return mutex_;} - - Parameters * getParams() {return ¶ms_;} - - /** - * @brief Registers callbacks for dynamic parameter handling. - */ - void activate(); - - /** - * @brief Resets callbacks for dynamic parameter handling. - */ - void deactivate(); - protected: - nav2::LifecycleNode::WeakPtr node_; - /** * @brief Apply parameter updates after validation * This callback is executed when parameters have been successfully updated. @@ -109,7 +87,7 @@ class ParameterHandler * @param parameters List of parameters that have been updated. */ void - updateParametersCallback(std::vector parameters); + updateParametersCallback(const std::vector & parameters) override; /** * @brief Validate incoming parameter updates before applying them. @@ -120,15 +98,9 @@ class ParameterHandler * @return rcl_interfaces::msg::SetParametersResult Result indicating whether the update is accepted. */ rcl_interfaces::msg::SetParametersResult - validateParameterUpdatesCallback(std::vector parameters); + validateParameterUpdatesCallback(const std::vector & parameters) override; - // Dynamic parameters handler - std::mutex mutex_; - rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr post_set_params_handler_; - rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_params_handler_; - Parameters params_; std::string plugin_name_; - rclcpp::Logger logger_ {rclcpp::get_logger("RegulatedPurePursuitController")}; }; } // namespace nav2_regulated_pure_pursuit_controller diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index edfbdb7d5f..4962c5b9e4 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -28,13 +28,12 @@ using nav2::declare_parameter_if_not_declared; using rcl_interfaces::msg::ParameterType; ParameterHandler::ParameterHandler( - nav2::LifecycleNode::SharedPtr node, + const nav2::LifecycleNode::SharedPtr & node, std::string & plugin_name, rclcpp::Logger & logger, const double costmap_size_x) +: nav2_util::ParameterHandler(node, logger) { - node_ = node; plugin_name_ = plugin_name; - logger_ = logger; declare_parameter_if_not_declared( node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5)); @@ -199,42 +198,12 @@ ParameterHandler::ParameterHandler( } } -void ParameterHandler::activate() -{ - auto node = node_.lock(); - post_set_params_handler_ = node->add_post_set_parameters_callback( - std::bind( - &ParameterHandler::updateParametersCallback, - this, std::placeholders::_1)); - on_set_params_handler_ = node->add_on_set_parameters_callback( - std::bind( - &ParameterHandler::validateParameterUpdatesCallback, - this, std::placeholders::_1)); -} - -void ParameterHandler::deactivate() -{ - auto node = node_.lock(); - if (post_set_params_handler_ && node) { - node->remove_post_set_parameters_callback(post_set_params_handler_.get()); - } - post_set_params_handler_.reset(); - if (on_set_params_handler_ && node) { - node->remove_on_set_parameters_callback(on_set_params_handler_.get()); - } - on_set_params_handler_.reset(); -} - -ParameterHandler::~ParameterHandler() -{ -} - rcl_interfaces::msg::SetParametersResult ParameterHandler::validateParameterUpdatesCallback( - std::vector parameters) + const std::vector & parameters) { rcl_interfaces::msg::SetParametersResult result; result.successful = true; - for (auto parameter : parameters) { + for (const auto & parameter : parameters) { const auto & param_type = parameter.get_type(); const auto & param_name = parameter.get_name(); if (param_name.find(plugin_name_ + ".") != 0) { @@ -270,7 +239,7 @@ rcl_interfaces::msg::SetParametersResult ParameterHandler::validateParameterUpda } void ParameterHandler::updateParametersCallback( - std::vector parameters) + const std::vector & parameters) { std::lock_guard lock_reinit(mutex_); diff --git a/nav2_util/include/nav2_util/parameter_handler.hpp b/nav2_util/include/nav2_util/parameter_handler.hpp new file mode 100644 index 0000000000..607899dda5 --- /dev/null +++ b/nav2_util/include/nav2_util/parameter_handler.hpp @@ -0,0 +1,121 @@ +// Copyright (c) 2025 Maurice Alexander Purnawan +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_UTIL__PARAMETER_HANDLER_HPP_ +#define NAV2_UTIL__PARAMETER_HANDLER_HPP_ + +#include +#include +#include +#include +#include + +#include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_ros_common/node_utils.hpp" + +namespace nav2_util +{ + +/** + * @class ParameterHandler + * Handles parameters and dynamic parameters + */ +template +class ParameterHandler +{ +public: + /** + * @brief Constructor for nav2_util::ParameterHandler + */ + ParameterHandler( + const nav2::LifecycleNode::SharedPtr & node, + rclcpp::Logger & logger) + : node_(node), logger_(logger) {} + + /** + * @brief Destructor for nav2_util::ParameterHandler + */ + virtual ~ParameterHandler() = default; + + /** + * @brief Get the internal mutex used for thread-safe parameter access. + * @return Reference to the mutex. + */ + std::mutex & getMutex() {return mutex_;} + + /** + * @brief Get a pointer to the internal parameter structure. + * @return Pointer to the stored parameter structure. + */ + ParamsT * getParams() {return ¶ms_;} + + /** + * @brief Registers callbacks for dynamic parameter handling. + */ + void activate() + { + auto node = node_.lock(); + post_set_params_handler_ = node->add_post_set_parameters_callback( + std::bind(&ParameterHandler::updateParametersCallback, this, std::placeholders::_1)); + on_set_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&ParameterHandler::validateParameterUpdatesCallback, this, std::placeholders::_1)); + } + + /** + * @brief Resets callbacks for dynamic parameter handling. + */ + void deactivate() + { + auto node = node_.lock(); + if (post_set_params_handler_ && node) { + node->remove_post_set_parameters_callback(post_set_params_handler_.get()); + } + if (on_set_params_handler_ && node) { + node->remove_on_set_parameters_callback(on_set_params_handler_.get()); + } + post_set_params_handler_.reset(); + on_set_params_handler_.reset(); + } + +protected: + nav2::LifecycleNode::WeakPtr node_; + std::mutex mutex_; + ParamsT params_; + rclcpp::Logger logger_; + rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr post_set_params_handler_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_params_handler_; + + /** + * @brief Validate incoming parameter updates before applying them. + * This callback is triggered when one or more parameters are about to be updated. + * It checks the validity of parameter values and rejects updates that would lead + * to invalid or inconsistent configurations + * @param parameters List of parameters that are being updated. + * @return rcl_interfaces::msg::SetParametersResult Result indicating whether the update is accepted. + */ + virtual rcl_interfaces::msg::SetParametersResult validateParameterUpdatesCallback( + const std::vector & parameters) = 0; + + /** + * @brief Apply parameter updates after validation + * This callback is executed when parameters have been successfully updated. + * It updates the internal configuration of the node with the new parameter values. + * @param parameters List of parameters that have been updated. + */ + virtual void updateParametersCallback(const std::vector & parameters) = 0; +}; + +} // namespace nav2_util + +#endif // NAV2_UTIL__PARAMETER_HANDLER_HPP_ diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index 68483963be..b11f2ee353 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -40,3 +40,6 @@ target_link_libraries(test_twist_subscriber ${library_name} rclcpp::rclcpp ${geo ament_add_gtest(test_path_utils test_path_utils.cpp) target_link_libraries(test_path_utils ${library_name} rclcpp::rclcpp ${geometry_msgs_TARGETS}) + +ament_add_gtest(test_parameter_handler test_parameter_handler.cpp) +target_link_libraries(test_parameter_handler ${library_name} rclcpp::rclcpp) diff --git a/nav2_util/test/test_parameter_handler.cpp b/nav2_util/test/test_parameter_handler.cpp new file mode 100644 index 0000000000..4f3962595c --- /dev/null +++ b/nav2_util/test/test_parameter_handler.cpp @@ -0,0 +1,97 @@ +// Copyright (c) 2023 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include "nav2_util/parameter_handler.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" + +using namespace std::chrono_literals; + +struct DummyParams +{ + double value = 0.0; +}; + +class TestParameterHandler : public nav2_util::ParameterHandler +{ +public: + TestParameterHandler(const nav2::LifecycleNode::SharedPtr & node, rclcpp::Logger & logger) + : ParameterHandler(node, logger) {} + + bool validated = false; + bool updated = false; + + rcl_interfaces::msg::SetParametersResult validateParameterUpdatesCallback( + const std::vector & /*parameters*/) override + { + validated = true; + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; + } + + void updateParametersCallback(const std::vector & /*parameters*/) override + { + updated = true; + } +}; + +TEST(ParameterHandlerTest, Construction) +{ + rclcpp::init(0, nullptr); + auto node = std::make_shared("test_node"); + rclcpp::Logger logger = node->get_logger(); + TestParameterHandler handler(node, logger); + EXPECT_NE(handler.getParams(), nullptr); + rclcpp::shutdown(); +} + +TEST(ParameterHandlerTest, ActivateAndDeactivate) +{ + rclcpp::init(0, nullptr); + auto node = std::make_shared("test_node"); + rclcpp::Logger logger = node->get_logger(); + + TestParameterHandler handler(node, logger); + handler.activate(); + EXPECT_TRUE(handler.getMutex().try_lock()); + handler.getMutex().unlock(); + handler.deactivate(); + rclcpp::shutdown(); +} + +TEST(ParameterHandlerTest, DynamicCallbacksAreInvoked) +{ + rclcpp::init(0, nullptr); + auto node = std::make_shared("test_node"); + rclcpp::Logger logger = node->get_logger(); + TestParameterHandler handler(node, logger); + + handler.activate(); + + // Declare a parameter and change it to trigger callbacks + node->declare_parameter("test_param", 1.0); + node->set_parameter(rclcpp::Parameter("test_param", 2.0)); + + // Manually invoke callbacks + std::vector params = {rclcpp::Parameter("test_param", 3.0)}; + handler.validateParameterUpdatesCallback(params); + handler.updateParametersCallback(params); + + EXPECT_TRUE(handler.validated); + EXPECT_TRUE(handler.updated); + handler.deactivate(); + rclcpp::shutdown(); +}