Improved parameter_handler usage when re-configure

ros-controls · Mar 9, 2023 · 528d4ea · 528d4ea
1 parent 9cee59e
commit 528d4ea
Show file tree

Hide file tree

Showing 2 changed files with 25 additions and 16 deletions.
diff --git 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
@@ -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()));
 }