[ForceTorqueSensorBroadcaster] Create ParamListener and get parameter…

…s on configure (#698) * Create ParamListener and get parameters on configure * Declare parameters for test_force_torque_sensor_broadcaster Since the parameters are not declared on init anymore, they cannot be set without declaring them before --------- Co-authored-by: Bence Magyar <[email protected]> (cherry picked from commit 32aaef7)
ros-controls · Aug 18, 2023 · 4b37bd9 · 4b37bd9
1 parent 4992fd4
commit 4b37bd9
Show file tree

Hide file tree

Showing 2 changed files with 44 additions and 39 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
@@ -29,6 +29,12 @@ 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
   {
@@ -37,18 +43,10 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_init()
   }
   catch (const std::exception & e)
   {
-    fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
+    fprintf(stderr, "Exception thrown during configure 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
@@ -111,11 +111,12 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_InterfaceNames_Set)
   SetUpFTSBroadcaster();
 
   // set the 'sensor_name'
-  fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_});
+  fts_broadcaster_->get_node()->declare_parameter("sensor_name", sensor_name_);
 
   // set the 'interface_names'
-  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()->declare_parameter("interface_names.force.x", "fts_sensor/force.x");
+  fts_broadcaster_->get_node()->declare_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);
@@ -126,7 +127,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_IsEmpty_InterfaceNames_NotSe
   SetUpFTSBroadcaster();
 
   // set the 'sensor_name' empty
-  fts_broadcaster_->get_node()->set_parameter({"sensor_name", ""});
+  fts_broadcaster_->get_node()->declare_parameter("sensor_name", "");
 
   // configure failed, 'sensor_name' parameter empty
   ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR);
@@ -137,8 +138,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_IsEmpty_SensorName_NotSe
   SetUpFTSBroadcaster();
 
   // set the 'interface_names' empty
-  fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", ""});
-  fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", ""});
+  fts_broadcaster_->get_node()->declare_parameter("interface_names.force.x", "");
+  fts_broadcaster_->get_node()->declare_parameter("interface_names.torque.z", "");
 
   // configure failed, 'interface_name' parameter empty
   ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR);
@@ -149,10 +150,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Configure_Success)
   SetUpFTSBroadcaster();
 
   // set the 'sensor_name'
-  fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_});
+  fts_broadcaster_->get_node()->declare_parameter("sensor_name", sensor_name_);
 
   // set the 'frame_id'
-  fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_});
+  fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_);
 
   // configure passed
   ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
@@ -163,11 +164,12 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Configure_Success)
   SetUpFTSBroadcaster();
 
   // set the 'interface_names'
-  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()->declare_parameter("interface_names.force.x", "fts_sensor/force.x");
+  fts_broadcaster_->get_node()->declare_parameter(
+    "interface_names.torque.z", "fts_sensor/torque.z");
 
   // set the 'frame_id'
-  fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_});
+  fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_);
 
   // configure passed
   ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
@@ -178,8 +180,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Activate_Success)
   SetUpFTSBroadcaster();
 
   // set the params 'sensor_name' and 'frame_id'
-  fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_});
-  fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_});
+  fts_broadcaster_->get_node()->declare_parameter("sensor_name", sensor_name_);
+  fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_);
 
   // configure and activate success
   ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
@@ -191,8 +193,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Update_Success)
   SetUpFTSBroadcaster();
 
   // set the params 'sensor_name' and 'frame_id'
-  fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_});
-  fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_});
+  fts_broadcaster_->get_node()->declare_parameter("sensor_name", sensor_name_);
+  fts_broadcaster_->get_node()->declare_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);
@@ -207,9 +209,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Success)
   SetUpFTSBroadcaster();
 
   // set the params 'interface_names' and '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_});
+  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_);
 
   ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
   ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
@@ -223,8 +226,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success)
   SetUpFTSBroadcaster();
 
   // set the params 'sensor_name' and 'frame_id'
-  fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_});
-  fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_});
+  fts_broadcaster_->get_node()->declare_parameter("sensor_name", sensor_name_);
+  fts_broadcaster_->get_node()->declare_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);
@@ -246,9 +249,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success)
   SetUpFTSBroadcaster();
 
   // set the params 'interface_names' and '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_});
+  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_);
 
   ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
   ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
@@ -270,13 +274,16 @@ TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success)
   SetUpFTSBroadcaster();
 
   // set all the params 'interface_names' and '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_});
+  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_);
 
   ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
   ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);