Skip to content

Commit

Permalink
Revert "[ForceTorqueSensorBroadcaster] Create ParamListener and get p…
Browse files Browse the repository at this point in the history 
…arameters on configure (#698)" (#988) (#1004)
  • Loading branch information
@mergify
mergify[bot] authored Jan 31, 2024
1 parent a41d29a commit 6285b43
Show file tree
Hide file tree
Showing 2 changed files with 39 additions and 44 deletions.
16 changes: 9 additions & 7 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,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
{
Expand All @@ -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() &&
Expand Down
67 changes: 30 additions & 37 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 @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand Down

0 comments on commit 6285b43

Please sign in to comment.