From d8bb9ce2cdea283196cf22da9e2dc308fc5006a4 Mon Sep 17 00:00:00 2001
From: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>
Date: Thu, 25 Jan 2024 19:39:27 +0100
Subject: [PATCH] Revert "[ForceTorqueSensorBroadcaster] Create ParamListener
 and get parameters on configure (#698)"

This reverts commit 32aaef7552638826aba0b3f3a72b1c1453739afa.
---
 .../src/force_torque_sensor_broadcaster.cpp   | 16 +++--
 .../test_force_torque_sensor_broadcaster.cpp  | 67 +++++++++----------
 2 files changed, 39 insertions(+), 44 deletions(-)

diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp
index e9a173380b..9b570d353f 100644
--- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp
+++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp
@@ -29,12 +29,6 @@ ForceTorqueSensorBroadcaster::ForceTorqueSensorBroadcaster()
 }
 
 controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_init()
-{
-  return controller_interface::CallbackReturn::SUCCESS;
-}
-
-controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure(
-  const rclcpp_lifecycle::State & /*previous_state*/)
 {
   try
   {
@@ -43,10 +37,18 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure(
   }
   catch (const std::exception & e)
   {
-    fprintf(stderr, "Exception thrown during configure stage with message: %s \n", e.what());
+    fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
     return controller_interface::CallbackReturn::ERROR;
   }
 
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure(
+  const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  params_ = param_listener_->get_params();
+
   const bool no_interface_names_defined =
     params_.interface_names.force.x.empty() && params_.interface_names.force.y.empty() &&
     params_.interface_names.force.z.empty() && params_.interface_names.torque.x.empty() &&
diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp
index ce6a04ec1f..f06252d83a 100644
--- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp
+++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp
@@ -113,12 +113,11 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_InterfaceNames_Set)
   SetUpFTSBroadcaster();
 
   // set the 'sensor_name'
-  fts_broadcaster_->get_node()->declare_parameter("sensor_name", sensor_name_);
+  fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_});
 
   // set the 'interface_names'
-  fts_broadcaster_->get_node()->declare_parameter("interface_names.force.x", "fts_sensor/force.x");
-  fts_broadcaster_->get_node()->declare_parameter(
-    "interface_names.torque.z", "fts_sensor/torque.z");
+  fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"});
+  fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"});
 
   // configure failed, both 'sensor_name' and 'interface_names' supplied
   ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR);
@@ -129,7 +128,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_IsEmpty_InterfaceNames_NotSe
   SetUpFTSBroadcaster();
 
   // set the 'sensor_name' empty
-  fts_broadcaster_->get_node()->declare_parameter("sensor_name", "");
+  fts_broadcaster_->get_node()->set_parameter({"sensor_name", ""});
 
   // configure failed, 'sensor_name' parameter empty
   ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR);
@@ -140,8 +139,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_IsEmpty_SensorName_NotSe
   SetUpFTSBroadcaster();
 
   // set the 'interface_names' empty
-  fts_broadcaster_->get_node()->declare_parameter("interface_names.force.x", "");
-  fts_broadcaster_->get_node()->declare_parameter("interface_names.torque.z", "");
+  fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", ""});
+  fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", ""});
 
   // configure failed, 'interface_name' parameter empty
   ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR);
@@ -152,10 +151,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Configure_Success)
   SetUpFTSBroadcaster();
 
   // set the 'sensor_name'
-  fts_broadcaster_->get_node()->declare_parameter("sensor_name", sensor_name_);
+  fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_});
 
   // set the 'frame_id'
-  fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_);
+  fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_});
 
   // configure passed
   ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
@@ -172,12 +171,11 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Configure_Success)
   SetUpFTSBroadcaster();
 
   // set the 'interface_names'
-  fts_broadcaster_->get_node()->declare_parameter("interface_names.force.x", "fts_sensor/force.x");
-  fts_broadcaster_->get_node()->declare_parameter(
-    "interface_names.torque.z", "fts_sensor/torque.z");
+  fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"});
+  fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"});
 
   // set the 'frame_id'
-  fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_);
+  fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_});
 
   // configure passed
   ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
@@ -188,8 +186,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_ActivateDeactivate_Success)
   SetUpFTSBroadcaster();
 
   // set the params 'sensor_name' and 'frame_id'
-  fts_broadcaster_->get_node()->declare_parameter("sensor_name", sensor_name_);
-  fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_);
+  fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_});
+  fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_});
 
   // configure and activate success
   ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
@@ -216,8 +214,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Update_Success)
   SetUpFTSBroadcaster();
 
   // set the params 'sensor_name' and 'frame_id'
-  fts_broadcaster_->get_node()->declare_parameter("sensor_name", sensor_name_);
-  fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_);
+  fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_});
+  fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_});
 
   ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
   ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
@@ -232,10 +230,9 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Success)
   SetUpFTSBroadcaster();
 
   // set the params 'interface_names' and 'frame_id'
-  fts_broadcaster_->get_node()->declare_parameter("interface_names.force.x", "fts_sensor/force.x");
-  fts_broadcaster_->get_node()->declare_parameter(
-    "interface_names.torque.z", "fts_sensor/torque.z");
-  fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_);
+  fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"});
+  fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"});
+  fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_});
 
   ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
   ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
@@ -249,8 +246,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success)
   SetUpFTSBroadcaster();
 
   // set the params 'sensor_name' and 'frame_id'
-  fts_broadcaster_->get_node()->declare_parameter("sensor_name", sensor_name_);
-  fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_);
+  fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_});
+  fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_});
 
   ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
   ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
@@ -272,10 +269,9 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success)
   SetUpFTSBroadcaster();
 
   // set the params 'interface_names' and 'frame_id'
-  fts_broadcaster_->get_node()->declare_parameter("interface_names.force.x", "fts_sensor/force.x");
-  fts_broadcaster_->get_node()->declare_parameter(
-    "interface_names.torque.z", "fts_sensor/torque.z");
-  fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_);
+  fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"});
+  fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"});
+  fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_});
 
   ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
   ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
@@ -297,16 +293,13 @@ TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success)
   SetUpFTSBroadcaster();
 
   // set all the params 'interface_names' and 'frame_id'
-  fts_broadcaster_->get_node()->declare_parameter("interface_names.force.x", "fts_sensor/force.x");
-  fts_broadcaster_->get_node()->declare_parameter("interface_names.force.y", "fts_sensor/force.y");
-  fts_broadcaster_->get_node()->declare_parameter("interface_names.force.z", "fts_sensor/force.z");
-  fts_broadcaster_->get_node()->declare_parameter(
-    "interface_names.torque.x", "fts_sensor/torque.x");
-  fts_broadcaster_->get_node()->declare_parameter(
-    "interface_names.torque.y", "fts_sensor/torque.y");
-  fts_broadcaster_->get_node()->declare_parameter(
-    "interface_names.torque.z", "fts_sensor/torque.z");
-  fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_);
+  fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"});
+  fts_broadcaster_->get_node()->set_parameter({"interface_names.force.y", "fts_sensor/force.y"});
+  fts_broadcaster_->get_node()->set_parameter({"interface_names.force.z", "fts_sensor/force.z"});
+  fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.x", "fts_sensor/torque.x"});
+  fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.y", "fts_sensor/torque.y"});
+  fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"});
+  fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_});
 
   ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
   ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);