From 5715e8f605570d7f404731ac4c23c4d3842bca3b Mon Sep 17 00:00:00 2001 From: Guillaume Walck <guihome@wanadoo.fr> Date: Thu, 9 Mar 2023 16:08:19 +0100 Subject: [PATCH] Improved parameter_handler usage when re-configure --- .../control_filters/gravity_compensation.hpp | 31 ++++++++++--------- .../test_gravity_compensation.cpp | 10 ++++-- 2 files changed, 25 insertions(+), 16 deletions(-) diff --git a/include/control_filters/gravity_compensation.hpp b/include/control_filters/gravity_compensation.hpp index 9d749376..b93fc1d9 100644 --- a/include/control_filters/gravity_compensation.hpp +++ b/include/control_filters/gravity_compensation.hpp @@ -100,21 +100,24 @@ bool GravityCompensation<T>::configure() logger_.reset( new rclcpp::Logger(this->logging_interface_->get_logger().get_child(this->filter_name_))); - // Initialize the parameters - try + // Initialize the parameter_listener once + if (!parameter_handler_) { - parameter_handler_ = - std::make_shared<gravity_compensation_filter::ParamListener>(this->params_interface_); - } - catch (rclcpp::exceptions::ParameterUninitializedException & ex) { - RCLCPP_ERROR((*logger_), "GravityCompensation filter cannot be configured: %s", ex.what()); - parameter_handler_.reset(); - return false; - } - catch (rclcpp::exceptions::InvalidParameterValueException & ex) { - RCLCPP_ERROR((*logger_), "GravityCompensation filter cannot be configured: %s", ex.what()); - parameter_handler_.reset(); - return false; + try + { + parameter_handler_ = + std::make_shared<gravity_compensation_filter::ParamListener>(this->params_interface_); + } + catch (rclcpp::exceptions::ParameterUninitializedException & ex) { + RCLCPP_ERROR((*logger_), "GravityCompensation filter cannot be configured: %s", ex.what()); + parameter_handler_.reset(); + return false; + } + catch (rclcpp::exceptions::InvalidParameterValueException & ex) { + RCLCPP_ERROR((*logger_), "GravityCompensation filter cannot be configured: %s", ex.what()); + parameter_handler_.reset(); + return false; + } } parameters_ = parameter_handler_->get_params(); compute_internal_params(); diff --git a/test/control_filters/test_gravity_compensation.cpp b/test/control_filters/test_gravity_compensation.cpp index 538890fe..d6df13ad 100644 --- a/test/control_filters/test_gravity_compensation.cpp +++ b/test/control_filters/test_gravity_compensation.cpp @@ -31,7 +31,7 @@ TEST_F(GravityCompensationTest, TestGravityCompensationMissingParameters) */ } -TEST_F(GravityCompensationTest, TestGravityCompensationInvalidParameters) +TEST_F(GravityCompensationTest, TestGravityCompensationParameters) { std::shared_ptr<filters::FilterBase<geometry_msgs::msg::WrenchStamped>> filter_ = std::make_shared<control_filters::GravityCompensation<geometry_msgs::msg::WrenchStamped>>(); @@ -49,7 +49,13 @@ TEST_F(GravityCompensationTest, TestGravityCompensationInvalidParameters) node_->get_node_logging_interface(), node_->get_node_parameters_interface())); node_->set_parameter(rclcpp::Parameter("CoG.pos", std::vector<double>({0.0, 0.0, 0.0}))); - // all parameters correctly set + // all parameters correctly set AND second call to yet unconfigured filter + ASSERT_TRUE(filter_->configure("", "TestGravityCompensationFilter", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + + // change a parameter + node_->set_parameter(rclcpp::Parameter("CoG.pos", std::vector<double>({0.0, 0.0, 0.2}))); + // accept second call to configure with valid parameters to already configured filter ASSERT_TRUE(filter_->configure("", "TestGravityCompensationFilter", node_->get_node_logging_interface(), node_->get_node_parameters_interface())); }