From 528d4ea76ea0076b63b447488084d7d6f04ab895 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..dc57d721 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()));
 }