Skip to content

Commit

Permalink
[ForceTorqueSensorBroadcaster] Create ParamListener and get parameter…
Browse files Browse the repository at this point in the history 
…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 <bence.magyar.robotics@gmail.com>
  • Loading branch information
@Noel215 @bmagyar
Noel215 and bmagyar authored Aug 18, 2023
1 parent 102c1cf commit 32aaef7
Show file tree
Hide file tree
Showing 2 changed files with 44 additions and 39 deletions.
16 changes: 7 additions & 9 deletions force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand All @@ -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() &&
Expand Down
67 changes: 37 additions & 30 deletions force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand Down

0 comments on commit 32aaef7

Please sign in to comment.