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 246ffc7275..b98bc8c560 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);