From 74d75d6a8dcedbd13c6903b45cf9bb22c0b509c7 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 2 Apr 2026 00:14:17 +0200 Subject: [PATCH] Migrate hardware components to new handle API (#2987) (cherry picked from commit 4c8ff6bcfa36b581e7080458fb59c9c6ce9e06e3) --- .../test_chainable_controller_interface.cpp | 20 +- .../test_chainable_controller.cpp | 5 +- .../include/hardware_interface/handle.hpp | 8 - .../loaned_command_interface.hpp | 2 +- .../loaned_state_interface.hpp | 2 +- .../hardware_interface/resource_manager.hpp | 3 - .../mock_components/generic_system.hpp | 4 - .../mock_components/test_generic_system.cpp | 3 - .../test/test_component_interfaces.cpp | 177 ++++++++---------- hardware_interface/test/test_handle.cpp | 3 - .../test_force_torque_sensor.cpp | 65 +++---- .../test_imu_sensor.cpp | 93 ++++----- .../test_single_joint_actuator.cpp | 48 ++--- .../test_system_with_command_modes.cpp | 75 ++++---- .../test_two_joint_system.cpp | 37 ++-- .../test/test_components/test_actuator.cpp | 104 +++++----- .../test_actuator_exclusive_interfaces.cpp | 69 ++++--- .../test/test_components/test_sensor.cpp | 14 +- .../test/test_components/test_system.cpp | 100 +++++----- .../test/test_resource_manager.cpp | 25 ++- 20 files changed, 410 insertions(+), 447 deletions(-) diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index 75e1eac057..8e3a5c9517 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -91,8 +91,9 @@ TEST_F(ChainableControllerInterfaceTest, export_state_interfaces_list_only) ASSERT_TRUE( controller.exported_state_interfaces_.find("testable_chainable_controller/test_state_ptr") != controller.exported_state_interfaces_.end()); - controller.exported_state_interfaces_.at("testable_chainable_controller/test_state_ptr") - ->set_value(EXPORTED_STATE_INTERFACE_VALUE); + std::ignore = + controller.exported_state_interfaces_.at("testable_chainable_controller/test_state_ptr") + ->set_value(EXPORTED_STATE_INTERFACE_VALUE); EXPECT_EQ(exported_state_interfaces[0]->get_optional().value(), EXPORTED_STATE_INTERFACE_VALUE); // calling export_state_interfaces again should return the same interface and shouldn't throw @@ -134,8 +135,9 @@ TEST_F(ChainableControllerInterfaceTest, export_state_interfaces_list_plus_legac ASSERT_TRUE( controller.exported_state_interfaces_.find("testable_chainable_controller/test_state_ptr") != controller.exported_state_interfaces_.end()); - controller.exported_state_interfaces_.at("testable_chainable_controller/test_state_ptr") - ->set_value(EXPORTED_STATE_INTERFACE_VALUE); + std::ignore = + controller.exported_state_interfaces_.at("testable_chainable_controller/test_state_ptr") + ->set_value(EXPORTED_STATE_INTERFACE_VALUE); EXPECT_EQ(exported_state_interfaces[0]->get_optional().value(), EXPORTED_STATE_INTERFACE_VALUE); // calling export_state_interfaces again should return the same interface and shouldn't throw @@ -203,8 +205,9 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces_list_only) ASSERT_TRUE( controller.exported_reference_interfaces_.find("testable_chainable_controller/test_itf_ptr") != controller.exported_reference_interfaces_.end()); - controller.exported_reference_interfaces_.at("testable_chainable_controller/test_itf_ptr") - ->set_value(INTERFACE_VALUE); + std::ignore = + controller.exported_reference_interfaces_.at("testable_chainable_controller/test_itf_ptr") + ->set_value(INTERFACE_VALUE); EXPECT_EQ(reference_interfaces[0]->get_optional().value(), INTERFACE_VALUE); // calling export_reference_interfaces again should return the same interface and shouldn't throw @@ -246,8 +249,9 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces_list_plus_l ASSERT_TRUE( controller.exported_reference_interfaces_.find("testable_chainable_controller/test_itf_ptr") != controller.exported_reference_interfaces_.end()); - controller.exported_reference_interfaces_.at("testable_chainable_controller/test_itf_ptr") - ->set_value(INTERFACE_VALUE); + std::ignore = + controller.exported_reference_interfaces_.at("testable_chainable_controller/test_itf_ptr") + ->set_value(INTERFACE_VALUE); EXPECT_EQ(reference_interfaces[0]->get_optional().value(), INTERFACE_VALUE); // calling export_reference_interfaces again should return the same interface and shouldn't throw diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp index 87b2c48fce..1849f4a12a 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -148,7 +148,7 @@ controller_interface::return_type TestChainableController::update_and_write_comm // If there is a command interface then integrate and set it to the exported state interface data for (size_t i = 0; i < state_interfaces_to_export_.size() && i < command_interfaces_.size(); ++i) { - ordered_exported_state_interfaces_[i]->set_value( + std::ignore = ordered_exported_state_interfaces_[i]->set_value( command_interfaces_[i].get_optional().value() * CONTROLLER_DT); } // If there is no command interface and if there is a state interface then just forward the same @@ -157,7 +157,8 @@ controller_interface::return_type TestChainableController::update_and_write_comm command_interfaces_.empty(); ++i) { - ordered_exported_state_interfaces_[i]->set_value(state_interfaces_[i].get_optional().value()); + std::ignore = + ordered_exported_state_interfaces_[i]->set_value(state_interfaces_[i].get_optional().value()); } return update_return_value; diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 66d418ec43..00ed421cf6 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -722,11 +722,7 @@ class StateInterface : public Handle StateInterface(StateInterface && other) = default; -// Disable deprecated warnings -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" using Handle::Handle; -#pragma GCC diagnostic pop using SharedPtr = std::shared_ptr; using ConstSharedPtr = std::shared_ptr; @@ -814,11 +810,7 @@ class CommandInterface : public Handle } } -// Disable deprecated warnings -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" using Handle::Handle; -#pragma GCC diagnostic pop using SharedPtr = std::shared_ptr; diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index af03e9a7df..b20cebc06f 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -221,8 +221,8 @@ class LoanedCommandInterface protected: CommandInterface & command_interface_; - Deleter deleter_; std::string interface_name_; + Deleter deleter_; private: struct HandleRTStatistics diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index 87144249c8..52a4df8503 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -181,8 +181,8 @@ class LoanedStateInterface protected: const StateInterface & state_interface_; - Deleter deleter_; std::string interface_name_; + Deleter deleter_; private: struct HandleRTStatistics diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 2d73c9a7e8..1593df70bc 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -23,11 +23,8 @@ #include "hardware_interface/actuator.hpp" #include "hardware_interface/hardware_component_info.hpp" #include "hardware_interface/hardware_info.hpp" -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#pragma GCC diagnostic pop #include "hardware_interface/sensor.hpp" #include "hardware_interface/system.hpp" #include "hardware_interface/system_interface.hpp" diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index e739bd425f..25c3219dde 100644 --- a/hardware_interface/include/mock_components/generic_system.hpp +++ b/hardware_interface/include/mock_components/generic_system.hpp @@ -19,11 +19,7 @@ #include #include - -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" #include "hardware_interface/handle.hpp" -#pragma GCC diagnostic pop #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 969e88340b..a8052448c0 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -20,11 +20,8 @@ #include #include "gmock/gmock.h" -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#pragma GCC diagnostic pop #include "hardware_interface/resource_manager.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 78166e5d8e..8f2f656a74 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -53,7 +53,6 @@ namespace test_components { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; -// BEGIN (Handle export change): for backward compatibility class DummyActuator : public hardware_interface::ActuatorInterface { CallbackReturn on_init( @@ -65,12 +64,12 @@ class DummyActuator : public hardware_interface::ActuatorInterface CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override { - position_state_ = 0.0; - velocity_state_ = 0.0; + std::ignore = position_state_->set_value(0.0); + std::ignore = velocity_state_->set_value(0.0); if (recoverable_error_happened_) { - velocity_command_ = 0.0; + std::ignore = velocity_command_->set_value(0.0); } read_calls_ = 0; @@ -79,32 +78,29 @@ class DummyActuator : public hardware_interface::ActuatorInterface return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces() override + std::vector on_export_state_interfaces() + override { // We can read a position and a velocity - std::vector state_interfaces; -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - state_interfaces.emplace_back( - hardware_interface::StateInterface( - "joint1", hardware_interface::HW_IF_POSITION, &position_state_)); - state_interfaces.emplace_back( - hardware_interface::StateInterface( - "joint1", hardware_interface::HW_IF_VELOCITY, &velocity_state_)); -#pragma GCC diagnostic pop + std::vector state_interfaces; + position_state_ = std::make_shared( + "joint1", hardware_interface::HW_IF_POSITION); + velocity_state_ = std::make_shared( + "joint1", hardware_interface::HW_IF_VELOCITY); + state_interfaces.push_back(position_state_); + state_interfaces.push_back(velocity_state_); return state_interfaces; } - std::vector export_command_interfaces() override + std::vector on_export_command_interfaces() + override { // We can command in velocity -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - std::vector command_interfaces; - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - "joint1", hardware_interface::HW_IF_VELOCITY, &velocity_command_)); -#pragma GCC diagnostic pop + std::vector command_interfaces; + velocity_command_ = std::make_shared( + "joint1", hardware_interface::HW_IF_VELOCITY); + std::ignore = velocity_command_->set_value(0.0); + command_interfaces.push_back(velocity_command_); return command_interfaces; } @@ -130,15 +126,19 @@ class DummyActuator : public hardware_interface::ActuatorInterface return hardware_interface::return_type::ERROR; } - position_state_ += velocity_command_; - velocity_state_ = velocity_command_; + double velocity_cmd = 0.0; + std::ignore = velocity_command_->get_value(velocity_cmd, false); + double position = 0.0; + std::ignore = position_state_->get_value(position, false); + std::ignore = position_state_->set_value(position + velocity_cmd); + std::ignore = velocity_state_->set_value(velocity_cmd); return hardware_interface::return_type::OK; } CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/) override { - velocity_state_ = 0; + std::ignore = velocity_state_->set_value(0.0); return CallbackReturn::SUCCESS; } @@ -157,16 +157,15 @@ class DummyActuator : public hardware_interface::ActuatorInterface } private: - double position_state_ = std::numeric_limits::quiet_NaN(); - double velocity_state_ = std::numeric_limits::quiet_NaN(); - double velocity_command_ = 0.0; + hardware_interface::StateInterface::SharedPtr position_state_; + hardware_interface::StateInterface::SharedPtr velocity_state_; + hardware_interface::CommandInterface::SharedPtr velocity_command_; // Helper variables to initiate error on read unsigned int read_calls_ = 0; unsigned int write_calls_ = 0; bool recoverable_error_happened_ = false; }; -// END class DummyActuatorDefault : public hardware_interface::ActuatorInterface { @@ -266,7 +265,6 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface bool recoverable_error_happened_ = false; }; -// BEGIN (Handle export change): for backward compatibility class DummySensor : public hardware_interface::SensorInterface { CallbackReturn on_init( @@ -278,20 +276,18 @@ class DummySensor : public hardware_interface::SensorInterface CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override { - voltage_level_ = 0.0; + std::ignore = voltage_level_->set_value(0.0); read_calls_ = 0; return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces() override + std::vector on_export_state_interfaces() + override { // We can read some voltage level - std::vector state_interfaces; -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - state_interfaces.emplace_back( - hardware_interface::StateInterface("sens1", "voltage", &voltage_level_)); -#pragma GCC diagnostic pop + std::vector state_interfaces; + voltage_level_ = std::make_shared("sens1", "voltage"); + state_interfaces.push_back(voltage_level_); return state_interfaces; } @@ -305,7 +301,7 @@ class DummySensor : public hardware_interface::SensorInterface } // no-op, static value - voltage_level_ = voltage_level_hw_value_; + std::ignore = voltage_level_->set_value(voltage_level_hw_value_); return hardware_interface::return_type::OK; } @@ -324,14 +320,13 @@ class DummySensor : public hardware_interface::SensorInterface } private: - double voltage_level_ = std::numeric_limits::quiet_NaN(); + hardware_interface::StateInterface::SharedPtr voltage_level_; double voltage_level_hw_value_ = 0x666; // Helper variables to initiate error on read int read_calls_ = 0; bool recoverable_error_happened_ = false; }; -// END class DummySensorDefault : public hardware_interface::SensorInterface { @@ -459,7 +454,6 @@ class DummySensorJointDefault : public hardware_interface::SensorInterface bool recoverable_error_happened_ = false; }; -// BEGIN (Handle export change): for backward compatibility class DummySystem : public hardware_interface::SystemInterface { CallbackReturn on_init( @@ -473,15 +467,15 @@ class DummySystem : public hardware_interface::SystemInterface { for (auto i = 0ul; i < 3; ++i) { - position_state_[i] = 0.0; - velocity_state_[i] = 0.0; + std::ignore = position_state_[i]->set_value(0.0); + std::ignore = velocity_state_[i]->set_value(0.0); } // reset command only if error is initiated if (recoverable_error_happened_) { for (auto i = 0ul; i < 3; ++i) { - velocity_command_[i] = 0.0; + std::ignore = velocity_command_[i]->set_value(0.0); } } @@ -491,50 +485,46 @@ class DummySystem : public hardware_interface::SystemInterface return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces() override + std::vector on_export_state_interfaces() + override { // We can read a position and a velocity - std::vector state_interfaces; -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - state_interfaces.emplace_back( - hardware_interface::StateInterface( - "joint1", hardware_interface::HW_IF_POSITION, &position_state_[0])); - state_interfaces.emplace_back( - hardware_interface::StateInterface( - "joint1", hardware_interface::HW_IF_VELOCITY, &velocity_state_[0])); - state_interfaces.emplace_back( - hardware_interface::StateInterface( - "joint2", hardware_interface::HW_IF_POSITION, &position_state_[1])); - state_interfaces.emplace_back( - hardware_interface::StateInterface( - "joint2", hardware_interface::HW_IF_VELOCITY, &velocity_state_[1])); - state_interfaces.emplace_back( - hardware_interface::StateInterface( - "joint3", hardware_interface::HW_IF_POSITION, &position_state_[2])); - state_interfaces.emplace_back( - hardware_interface::StateInterface( - "joint3", hardware_interface::HW_IF_VELOCITY, &velocity_state_[2])); -#pragma GCC diagnostic pop + std::vector state_interfaces; + position_state_[0] = std::make_shared( + "joint1", hardware_interface::HW_IF_POSITION); + velocity_state_[0] = std::make_shared( + "joint1", hardware_interface::HW_IF_VELOCITY); + position_state_[1] = std::make_shared( + "joint2", hardware_interface::HW_IF_POSITION); + velocity_state_[1] = std::make_shared( + "joint2", hardware_interface::HW_IF_VELOCITY); + position_state_[2] = std::make_shared( + "joint3", hardware_interface::HW_IF_POSITION); + velocity_state_[2] = std::make_shared( + "joint3", hardware_interface::HW_IF_VELOCITY); + state_interfaces.push_back(position_state_[0]); + state_interfaces.push_back(velocity_state_[0]); + state_interfaces.push_back(position_state_[1]); + state_interfaces.push_back(velocity_state_[1]); + state_interfaces.push_back(position_state_[2]); + state_interfaces.push_back(velocity_state_[2]); return state_interfaces; } - std::vector export_command_interfaces() override + std::vector on_export_command_interfaces() + override { // We can command in velocity - std::vector command_interfaces; -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - "joint1", hardware_interface::HW_IF_VELOCITY, &velocity_command_[0])); - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - "joint2", hardware_interface::HW_IF_VELOCITY, &velocity_command_[1])); - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - "joint3", hardware_interface::HW_IF_VELOCITY, &velocity_command_[2])); -#pragma GCC diagnostic pop + std::vector command_interfaces; + velocity_command_[0] = std::make_shared( + "joint1", hardware_interface::HW_IF_VELOCITY); + velocity_command_[1] = std::make_shared( + "joint2", hardware_interface::HW_IF_VELOCITY); + velocity_command_[2] = std::make_shared( + "joint3", hardware_interface::HW_IF_VELOCITY); + command_interfaces.push_back(velocity_command_[0]); + command_interfaces.push_back(velocity_command_[1]); + command_interfaces.push_back(velocity_command_[2]); return command_interfaces; } @@ -560,10 +550,14 @@ class DummySystem : public hardware_interface::SystemInterface return hardware_interface::return_type::ERROR; } + double velocity_cmd = 0.0; + std::ignore = velocity_command_[0]->get_value(velocity_cmd, false); for (size_t i = 0; i < 3; ++i) { - position_state_[i] += velocity_command_[0]; - velocity_state_[i] = velocity_command_[0]; + double position = 0.0; + std::ignore = position_state_[i]->get_value(position, false); + std::ignore = position_state_[i]->set_value(position + velocity_cmd); + std::ignore = velocity_state_[i]->set_value(velocity_cmd); } return hardware_interface::return_type::OK; } @@ -572,7 +566,7 @@ class DummySystem : public hardware_interface::SystemInterface { for (size_t i = 0; i < 3; ++i) { - velocity_state_[i] = 0.0; + std::ignore = velocity_state_[i]->set_value(0.0); } return CallbackReturn::SUCCESS; } @@ -592,20 +586,15 @@ class DummySystem : public hardware_interface::SystemInterface } private: - std::array position_state_ = { - {std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN()}}; - std::array velocity_state_ = { - {std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN()}}; - std::array velocity_command_ = {{0.0, 0.0, 0.0}}; + std::array position_state_; + std::array velocity_state_; + std::array velocity_command_; // Helper variables to initiate error on read unsigned int read_calls_ = 0; unsigned int write_calls_ = 0; bool recoverable_error_happened_ = false; }; -// END class DummySystemDefault : public hardware_interface::SystemInterface { diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index 88c8cab50f..098005ee37 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -15,10 +15,7 @@ #include #include "gmock/gmock.h" -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" #include "hardware_interface/handle.hpp" -#pragma GCC diagnostic pop #include "hardware_interface/hardware_info.hpp" using hardware_interface::CommandInterface; diff --git a/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp b/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp index e973ecfa10..27a311c3a4 100644 --- a/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp +++ b/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include "hardware_interface/sensor_interface.hpp" @@ -54,52 +55,44 @@ class TestForceTorqueSensor : public SensorInterface return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces() override + std::vector on_export_state_interfaces() override { - std::vector state_interfaces; - const auto & sensor_name = get_hardware_info().sensors[0].name; -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "fx", &values_.fx)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "fy", &values_.fy)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "fz", &values_.fz)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "tx", &values_.tx)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "ty", &values_.ty)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "tz", &values_.tz)); -#pragma GCC diagnostic pop - return state_interfaces; + fx_interface_ = std::make_shared(sensor_name, "fx"); + fy_interface_ = std::make_shared(sensor_name, "fy"); + fz_interface_ = std::make_shared(sensor_name, "fz"); + tx_interface_ = std::make_shared(sensor_name, "tx"); + ty_interface_ = std::make_shared(sensor_name, "ty"); + tz_interface_ = std::make_shared(sensor_name, "tz"); + return {fx_interface_, fy_interface_, fz_interface_, + tx_interface_, ty_interface_, tz_interface_}; } return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { - values_.fx = fmod((values_.fx + 1.0), 10); - values_.fy = fmod((values_.fy + 1.0), 10); - values_.fz = fmod((values_.fz + 1.0), 10); - values_.tx = fmod((values_.tx + 1.0), 10); - values_.ty = fmod((values_.ty + 1.0), 10); - values_.tz = fmod((values_.tz + 1.0), 10); + double fx = 0.0, fy = 0.0, fz = 0.0, tx = 0.0, ty = 0.0, tz = 0.0; + std::ignore = fx_interface_->get_value(fx, true); + std::ignore = fy_interface_->get_value(fy, true); + std::ignore = fz_interface_->get_value(fz, true); + std::ignore = tx_interface_->get_value(tx, true); + std::ignore = ty_interface_->get_value(ty, true); + std::ignore = tz_interface_->get_value(tz, true); + std::ignore = fx_interface_->set_value(fmod((fx + 1.0), 10), true); + std::ignore = fy_interface_->set_value(fmod((fy + 1.0), 10), true); + std::ignore = fz_interface_->set_value(fmod((fz + 1.0), 10), true); + std::ignore = tx_interface_->set_value(fmod((tx + 1.0), 10), true); + std::ignore = ty_interface_->set_value(fmod((ty + 1.0), 10), true); + std::ignore = tz_interface_->set_value(fmod((tz + 1.0), 10), true); return return_type::OK; } private: - struct FTValues - { - double fx = 0.0; - double fy = 0.0; - double fz = 0.0; - double tx = 0.0; - double ty = 0.0; - double tz = 0.0; - }; - - FTValues values_; + StateInterface::SharedPtr fx_interface_; + StateInterface::SharedPtr fy_interface_; + StateInterface::SharedPtr fz_interface_; + StateInterface::SharedPtr tx_interface_; + StateInterface::SharedPtr ty_interface_; + StateInterface::SharedPtr tz_interface_; }; } // namespace test_hardware_components diff --git a/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp b/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp index aa1848fe6a..27c6496521 100644 --- a/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp +++ b/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp @@ -21,6 +21,7 @@ #define _USE_MATH_DEFINES #endif #include +#include #include #include @@ -65,38 +66,23 @@ class TestIMUSensor : public SensorInterface return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces() override + std::vector on_export_state_interfaces() override { - std::vector state_interfaces; - const std::string & sensor_name = get_hardware_info().sensors[0].name; -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "orientation.x", &orientation_.x)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "orientation.y", &orientation_.y)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "orientation.z", &orientation_.z)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "orientation.w", &orientation_.w)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "angular_velocity.x", &angular_velocity_.x)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "angular_velocity.y", &angular_velocity_.y)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "angular_velocity.z", &angular_velocity_.z)); - state_interfaces.emplace_back( - hardware_interface::StateInterface( - sensor_name, "linear_acceleration.x", &linear_acceleration_.x)); - state_interfaces.emplace_back( - hardware_interface::StateInterface( - sensor_name, "linear_acceleration.y", &linear_acceleration_.y)); - state_interfaces.emplace_back( - hardware_interface::StateInterface( - sensor_name, "linear_acceleration.z", &linear_acceleration_.z)); -#pragma GCC diagnostic pop - return state_interfaces; + orientation_x_ = std::make_shared(sensor_name, "orientation.x"); + orientation_y_ = std::make_shared(sensor_name, "orientation.y"); + orientation_z_ = std::make_shared(sensor_name, "orientation.z"); + orientation_w_ = std::make_shared(sensor_name, "orientation.w"); + angular_velocity_x_ = std::make_shared(sensor_name, "angular_velocity.x"); + angular_velocity_y_ = std::make_shared(sensor_name, "angular_velocity.y"); + angular_velocity_z_ = std::make_shared(sensor_name, "angular_velocity.z"); + linear_acceleration_x_ = std::make_shared(sensor_name, "linear_acceleration.x"); + linear_acceleration_y_ = std::make_shared(sensor_name, "linear_acceleration.y"); + linear_acceleration_z_ = std::make_shared(sensor_name, "linear_acceleration.z"); + return {orientation_x_, orientation_y_, orientation_z_, + orientation_w_, angular_velocity_x_, angular_velocity_y_, + angular_velocity_z_, linear_acceleration_x_, linear_acceleration_y_, + linear_acceleration_z_}; } return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override @@ -106,42 +92,35 @@ class TestIMUSensor : public SensorInterface const double u1 = distribution_1(generator_); const double u2 = distribution_1(generator_); const double u3 = distribution_1(generator_); - orientation_.w = std::sqrt(1. - u1) * std::sin(2 * M_PI * u2); - orientation_.x = std::sqrt(1. - u1) * std::cos(2 * M_PI * u2); - orientation_.y = std::sqrt(u1) * std::sin(2 * M_PI * u3); - orientation_.z = std::sqrt(u1) * std::cos(2 * M_PI * u3); + std::ignore = orientation_w_->set_value(std::sqrt(1. - u1) * std::sin(2 * M_PI * u2), true); + std::ignore = orientation_x_->set_value(std::sqrt(1. - u1) * std::cos(2 * M_PI * u2), true); + std::ignore = orientation_y_->set_value(std::sqrt(u1) * std::sin(2 * M_PI * u3), true); + std::ignore = orientation_z_->set_value(std::sqrt(u1) * std::cos(2 * M_PI * u3), true); // generate random angular velocities and linear accelerations std::uniform_real_distribution distribution_2(0.0, 0.1); - angular_velocity_.x = distribution_2(generator_); - angular_velocity_.y = distribution_2(generator_); - angular_velocity_.z = distribution_2(generator_); + std::ignore = angular_velocity_x_->set_value(distribution_2(generator_), true); + std::ignore = angular_velocity_y_->set_value(distribution_2(generator_), true); + std::ignore = angular_velocity_z_->set_value(distribution_2(generator_), true); - linear_acceleration_.x = distribution_2(generator_); - linear_acceleration_.y = distribution_2(generator_); - linear_acceleration_.z = distribution_2(generator_); + std::ignore = linear_acceleration_x_->set_value(distribution_2(generator_), true); + std::ignore = linear_acceleration_y_->set_value(distribution_2(generator_), true); + std::ignore = linear_acceleration_z_->set_value(distribution_2(generator_), true); return return_type::OK; } private: - struct QuaternionValues - { - double x = 0.0; - double y = 0.0; - double z = 0.0; - double w = 1.0; - }; - struct AxisValues - { - double x = 0.0; - double y = 0.0; - double z = 0.0; - }; - std::default_random_engine generator_; - QuaternionValues orientation_; - AxisValues angular_velocity_; - AxisValues linear_acceleration_; + StateInterface::SharedPtr orientation_x_; + StateInterface::SharedPtr orientation_y_; + StateInterface::SharedPtr orientation_z_; + StateInterface::SharedPtr orientation_w_; + StateInterface::SharedPtr angular_velocity_x_; + StateInterface::SharedPtr angular_velocity_y_; + StateInterface::SharedPtr angular_velocity_z_; + StateInterface::SharedPtr linear_acceleration_x_; + StateInterface::SharedPtr linear_acceleration_y_; + StateInterface::SharedPtr linear_acceleration_z_; }; } // namespace test_hardware_components diff --git a/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp b/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp index 53ddc05dcc..a04e5b0f7c 100644 --- a/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp +++ b/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp @@ -68,36 +68,22 @@ class TestSingleJointActuator : public ActuatorInterface return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces() override + std::vector on_export_state_interfaces() override { - std::vector state_interfaces; - const auto & joint_name = get_hardware_info().joints[0].name; -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - state_interfaces.emplace_back( - hardware_interface::StateInterface( - joint_name, hardware_interface::HW_IF_POSITION, &position_state_)); - state_interfaces.emplace_back( - hardware_interface::StateInterface( - joint_name, hardware_interface::HW_IF_VELOCITY, &velocity_state_)); -#pragma GCC diagnostic pop - return state_interfaces; + position_state_interface_ = + std::make_shared(joint_name, hardware_interface::HW_IF_POSITION); + velocity_state_interface_ = + std::make_shared(joint_name, hardware_interface::HW_IF_VELOCITY); + return {position_state_interface_, velocity_state_interface_}; } - std::vector export_command_interfaces() override + std::vector on_export_command_interfaces() override { - std::vector command_interfaces; - const auto & joint_name = get_hardware_info().joints[0].name; -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - joint_name, hardware_interface::HW_IF_POSITION, &position_command_)); -#pragma GCC diagnostic pop - - return command_interfaces; + position_command_interface_ = + std::make_shared(joint_name, hardware_interface::HW_IF_POSITION); + return {position_command_interface_}; } return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override @@ -107,15 +93,19 @@ class TestSingleJointActuator : public ActuatorInterface return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { - velocity_state_ = position_command_ - position_state_; - position_state_ = position_command_; + double position_state = 0.0; + double position_command = 0.0; + std::ignore = position_state_interface_->get_value(position_state, true); + std::ignore = position_command_interface_->get_value(position_command, true); + std::ignore = velocity_state_interface_->set_value(position_command - position_state, true); + std::ignore = position_state_interface_->set_value(position_command, true); return return_type::OK; } private: - double position_state_ = 0.0; - double velocity_state_ = 0.0; - double position_command_ = 0.0; + StateInterface::SharedPtr position_state_interface_; + StateInterface::SharedPtr velocity_state_interface_; + CommandInterface::SharedPtr position_command_interface_; }; } // namespace test_hardware_components diff --git a/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp b/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp index 6a9b320bcf..528a72a257 100644 --- a/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp +++ b/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include @@ -75,47 +76,43 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces() override + std::vector on_export_state_interfaces() + override { - std::vector state_interfaces; + std::vector state_interfaces; for (auto i = 0u; i < get_hardware_info().joints.size(); i++) { -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - state_interfaces.emplace_back( - hardware_interface::StateInterface( - get_hardware_info().joints[i].name, hardware_interface::HW_IF_POSITION, - &position_state_[i])); - state_interfaces.emplace_back( - hardware_interface::StateInterface( - get_hardware_info().joints[i].name, hardware_interface::HW_IF_VELOCITY, - &velocity_state_[i])); - state_interfaces.emplace_back( - hardware_interface::StateInterface( - get_hardware_info().joints[i].name, hardware_interface::HW_IF_ACCELERATION, - &acceleration_state_[i])); -#pragma GCC diagnostic pop + position_state_interfaces_[i] = std::make_shared( + get_hardware_info().joints[i].name, hardware_interface::HW_IF_POSITION); + velocity_state_interfaces_[i] = std::make_shared( + get_hardware_info().joints[i].name, hardware_interface::HW_IF_VELOCITY); + acceleration_state_interfaces_[i] = std::make_shared( + get_hardware_info().joints[i].name, hardware_interface::HW_IF_ACCELERATION); + std::ignore = position_state_interfaces_[i]->set_value(0.0); + std::ignore = velocity_state_interfaces_[i]->set_value(0.0); + std::ignore = acceleration_state_interfaces_[i]->set_value(0.0); + state_interfaces.push_back(position_state_interfaces_[i]); + state_interfaces.push_back(velocity_state_interfaces_[i]); + state_interfaces.push_back(acceleration_state_interfaces_[i]); } return state_interfaces; } - std::vector export_command_interfaces() override + std::vector on_export_command_interfaces() + override { - std::vector command_interfaces; + std::vector command_interfaces; for (auto i = 0u; i < get_hardware_info().joints.size(); i++) { -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - get_hardware_info().joints[i].name, hardware_interface::HW_IF_POSITION, - &position_command_[i])); - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - get_hardware_info().joints[i].name, hardware_interface::HW_IF_VELOCITY, - &velocity_command_[i])); -#pragma GCC diagnostic pop + position_command_interfaces_[i] = std::make_shared( + get_hardware_info().joints[i].name, hardware_interface::HW_IF_POSITION); + velocity_command_interfaces_[i] = std::make_shared( + get_hardware_info().joints[i].name, hardware_interface::HW_IF_VELOCITY); + std::ignore = position_command_interfaces_[i]->set_value(0.0); + std::ignore = velocity_command_interfaces_[i]->set_value(0.0); + command_interfaces.push_back(position_command_interfaces_[i]); + command_interfaces.push_back(velocity_command_interfaces_[i]); } return command_interfaces; @@ -137,7 +134,9 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface const std::vector & start_interfaces, const std::vector & stop_interfaces) override { - acceleration_state_[0] += 1.0; + double accel = 0.0; + std::ignore = acceleration_state_interfaces_[0]->get_value(accel, true); + std::ignore = acceleration_state_interfaces_[0]->set_value(accel + 1.0, true); // Starting interfaces start_modes_.clear(); @@ -185,7 +184,9 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface const std::vector & start_interfaces, const std::vector & /*stop_interfaces*/) override { - acceleration_state_[0] += 100.0; + double accel = 0.0; + std::ignore = acceleration_state_interfaces_[0]->get_value(accel, true); + std::ignore = acceleration_state_interfaces_[0]->set_value(accel + 100.0, true); // Test of failure in perform command mode switch // Fail if given an empty list. // This should never occur in a real system as the same start_interfaces list is sent to both @@ -201,11 +202,11 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface std::vector start_modes_ = {"position", "position"}; std::vector stop_modes_ = {false, false}; - std::array position_command_ = {{0.0, 0.0}}; - std::array velocity_command_ = {{0.0, 0.0}}; - std::array position_state_ = {{0.0, 0.0}}; - std::array velocity_state_ = {{0.0, 0.0}}; - std::array acceleration_state_ = {{0.0, 0.0}}; + std::array position_command_interfaces_; + std::array velocity_command_interfaces_; + std::array position_state_interfaces_; + std::array velocity_state_interfaces_; + std::array acceleration_state_interfaces_; }; } // namespace test_hardware_components diff --git a/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp b/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp index c5b1cf74bb..68667cb06f 100644 --- a/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp +++ b/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include "hardware_interface/system_interface.hpp" @@ -68,41 +69,29 @@ class TestTwoJointSystem : public SystemInterface return CallbackReturn::SUCCESS; } -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - std::vector export_state_interfaces() override + std::vector on_export_state_interfaces() override { - std::vector state_interfaces; + std::vector state_interfaces; for (auto i = 0u; i < get_hardware_info().joints.size(); ++i) { - state_interfaces.emplace_back( - hardware_interface::StateInterface( - get_hardware_info().joints[i].name, hardware_interface::HW_IF_POSITION, - &position_state_[i])); + position_state_interfaces_[i] = std::make_shared( + get_hardware_info().joints[i].name, hardware_interface::HW_IF_POSITION); + state_interfaces.push_back(position_state_interfaces_[i]); } - return state_interfaces; } -#pragma GCC diagnostic pop -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - std::vector export_command_interfaces() override + std::vector on_export_command_interfaces() override { - std::vector command_interfaces; + std::vector command_interfaces; for (auto i = 0u; i < get_hardware_info().joints.size(); ++i) { -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - get_hardware_info().joints[i].name, hardware_interface::HW_IF_POSITION, - &position_command_[i])); + position_command_interfaces_[i] = std::make_shared( + get_hardware_info().joints[i].name, hardware_interface::HW_IF_POSITION); + command_interfaces.push_back(position_command_interfaces_[i]); } - return command_interfaces; } -#pragma GCC diagnostic pop return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { @@ -115,8 +104,8 @@ class TestTwoJointSystem : public SystemInterface } private: - std::array position_command_ = {{0.0, 0.0}}; - std::array position_state_ = {{0.0, 0.0}}; + std::array position_state_interfaces_; + std::array position_command_interfaces_; }; } // namespace test_hardware_components diff --git a/hardware_interface_testing/test/test_components/test_actuator.cpp b/hardware_interface_testing/test/test_components/test_actuator.cpp index d26cf863f8..13d1881f64 100644 --- a/hardware_interface_testing/test/test_components/test_actuator.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator.cpp @@ -62,38 +62,39 @@ class TestActuator : public ActuatorInterface return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces() override + std::vector on_export_state_interfaces() override { - std::vector state_interfaces; - state_interfaces.emplace_back( - hardware_interface::StateInterface( - get_hardware_info().joints[0].name, get_hardware_info().joints[0].state_interfaces[0].name, - &position_state_)); - state_interfaces.emplace_back( - hardware_interface::StateInterface( - get_hardware_info().joints[0].name, get_hardware_info().joints[0].state_interfaces[1].name, - &velocity_state_)); - state_interfaces.emplace_back( - hardware_interface::StateInterface( - get_hardware_info().joints[0].name, "some_unlisted_interface", &unlisted_interface_)); + std::vector state_interfaces; + position_state_ = std::make_shared( + get_hardware_info().joints[0].name, get_hardware_info().joints[0].state_interfaces[0].name); + std::ignore = position_state_->set_value(0.0, false); + velocity_state_ = std::make_shared( + get_hardware_info().joints[0].name, get_hardware_info().joints[0].state_interfaces[1].name); + std::ignore = velocity_state_->set_value(0.0, false); + unlisted_interface_ = std::make_shared( + get_hardware_info().joints[0].name, "some_unlisted_interface"); + state_interfaces.push_back(position_state_); + state_interfaces.push_back(velocity_state_); + state_interfaces.push_back(unlisted_interface_); return state_interfaces; } - std::vector export_command_interfaces() override + std::vector on_export_command_interfaces() override { - std::vector command_interfaces; - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - get_hardware_info().joints[0].name, - get_hardware_info().joints[0].command_interfaces[0].name, &velocity_command_)); + std::vector command_interfaces; + velocity_command_ = std::make_shared( + get_hardware_info().joints[0].name, get_hardware_info().joints[0].command_interfaces[0].name); + std::ignore = velocity_command_->set_value(0.0, false); + command_interfaces.push_back(velocity_command_); if (get_hardware_info().joints[0].command_interfaces.size() > 1) { - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - get_hardware_info().joints[0].name, - get_hardware_info().joints[0].command_interfaces[1].name, &max_velocity_command_)); + max_velocity_command_ = std::make_shared( + get_hardware_info().joints[0].name, + get_hardware_info().joints[0].command_interfaces[1].name); + std::ignore = max_velocity_command_->set_value(0.0, false); + command_interfaces.push_back(max_velocity_command_); } return command_interfaces; @@ -103,7 +104,9 @@ class TestActuator : public ActuatorInterface const std::vector & /*start_interfaces*/, const std::vector & /*stop_interfaces*/) override { - position_state_ += 0.001; + double pos = 0.0; + std::ignore = position_state_->get_value(pos, false); + std::ignore = position_state_->set_value(pos + 0.001, false); return hardware_interface::return_type::OK; } @@ -120,7 +123,9 @@ class TestActuator : public ActuatorInterface return hardware_interface::return_type::ERROR; } } - position_state_ += 0.1; + double pos = 0.0; + std::ignore = position_state_->get_value(pos, false); + std::ignore = position_state_->set_value(pos + 0.1, false); return hardware_interface::return_type::OK; } @@ -131,21 +136,26 @@ class TestActuator : public ActuatorInterface std::this_thread::sleep_for( std::chrono::milliseconds(1000 / (3 * get_hardware_info().rw_rate))); } + double vel_cmd = 0.0; + std::ignore = velocity_command_->get_value(vel_cmd, false); // simulate error on read - if (velocity_command_ == test_constants::READ_FAIL_VALUE) + if (vel_cmd == test_constants::READ_FAIL_VALUE) { // reset value to get out from error on the next call - simplifies CM // tests - velocity_command_ = 0.0; + std::ignore = velocity_command_->set_value(0.0, false); return return_type::ERROR; } // simulate deactivate on read - if (velocity_command_ == test_constants::READ_DEACTIVATE_VALUE) + if (vel_cmd == test_constants::READ_DEACTIVATE_VALUE) { return return_type::DEACTIVATE; } // simulate exception on read - if (velocity_command_ == test_constants::READ_THROW_VALUE) + const auto velocity_command_value = velocity_command_->get_optional(); + if ( + velocity_command_value.has_value() && + velocity_command_value.value() == test_constants::READ_THROW_VALUE) { throw std::runtime_error("Exception from TestActuator::read() as requested."); } @@ -154,13 +164,16 @@ class TestActuator : public ActuatorInterface // working as it should. This makes value checks clearer and confirms there // is no "state = command" line or some other mixture of interfaces // somewhere in the test stack. - velocity_state_ = velocity_command_ / 2; - position_state_ += velocity_state_ * period.seconds(); + double vel_state = vel_cmd / 2; + std::ignore = velocity_state_->set_value(vel_state, false); + double pos_state = 0.0; + std::ignore = position_state_->get_value(pos_state, false); + std::ignore = position_state_->set_value(pos_state + vel_state * period.seconds(), false); - if (velocity_command_ == test_constants::RESET_STATE_INTERFACES_VALUE) + if (vel_cmd == test_constants::RESET_STATE_INTERFACES_VALUE) { - position_state_ = 0.0; - velocity_state_ = 0.0; + std::ignore = position_state_->set_value(0.0, false); + std::ignore = velocity_state_->set_value(0.0, false); } return return_type::OK; } @@ -172,21 +185,26 @@ class TestActuator : public ActuatorInterface std::this_thread::sleep_for( std::chrono::milliseconds(1000 / (6 * get_hardware_info().rw_rate))); } + double vel_cmd = 0.0; + std::ignore = velocity_command_->get_value(vel_cmd, false); // simulate error on write - if (velocity_command_ == test_constants::WRITE_FAIL_VALUE) + if (vel_cmd == test_constants::WRITE_FAIL_VALUE) { // reset value to get out from error on the next call - simplifies CM // tests - velocity_command_ = 0.0; + std::ignore = velocity_command_->set_value(0.0, false); return return_type::ERROR; } // simulate deactivate on write - if (velocity_command_ == test_constants::WRITE_DEACTIVATE_VALUE) + if (vel_cmd == test_constants::WRITE_DEACTIVATE_VALUE) { return return_type::DEACTIVATE; } // simulate exception on write - if (velocity_command_ == test_constants::WRITE_THROW_VALUE) + const auto velocity_command_value = velocity_command_->get_optional(); + if ( + velocity_command_value.has_value() && + velocity_command_value.value() == test_constants::WRITE_THROW_VALUE) { throw std::runtime_error("Exception from TestActuator::write() as requested."); } @@ -194,11 +212,11 @@ class TestActuator : public ActuatorInterface } private: - double position_state_ = 0.0; - double velocity_state_ = 0.0; - double velocity_command_ = 0.0; - double max_velocity_command_ = 0.0; - double unlisted_interface_ = std::numeric_limits::quiet_NaN(); + StateInterface::SharedPtr position_state_; + StateInterface::SharedPtr velocity_state_; + CommandInterface::SharedPtr velocity_command_; + CommandInterface::SharedPtr max_velocity_command_; + StateInterface::SharedPtr unlisted_interface_; }; class TestUninitializableActuator : public TestActuator diff --git a/hardware_interface_testing/test/test_components/test_actuator_exclusive_interfaces.cpp b/hardware_interface_testing/test/test_components/test_actuator_exclusive_interfaces.cpp index 53b132adaa..aed74d7b12 100644 --- a/hardware_interface_testing/test/test_components/test_actuator_exclusive_interfaces.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator_exclusive_interfaces.cpp @@ -47,12 +47,6 @@ static std::pair extract_joint_and_interface( return {joint_name, interface_name}; } -struct JointState -{ - double pos; - double vel; - double effort; -}; class TestActuatorExclusiveInterfaces : public ActuatorInterface { @@ -65,53 +59,53 @@ class TestActuatorExclusiveInterfaces : public ActuatorInterface } verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); - for (const auto & j : get_hardware_info().joints) - { - (void)j; // Suppress unused warning - current_states_.emplace_back(JointState{}); - } - return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces() override + std::vector on_export_state_interfaces() override { verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); - std::vector state_interfaces; + std::vector state_interfaces; for (std::size_t i = 0; i < info_.joints.size(); ++i) { const auto & joint = info_.joints[i]; - state_interfaces.emplace_back( - hardware_interface::StateInterface( - joint.name, hardware_interface::HW_IF_POSITION, ¤t_states_.at(i).pos)); - state_interfaces.emplace_back( - hardware_interface::StateInterface( - joint.name, hardware_interface::HW_IF_VELOCITY, ¤t_states_.at(i).vel)); - state_interfaces.emplace_back( - hardware_interface::StateInterface( - joint.name, hardware_interface::HW_IF_EFFORT, ¤t_states_.at(i).effort)); + position_states_.push_back( + std::make_shared(joint.name, hardware_interface::HW_IF_POSITION)); + std::ignore = position_states_.back()->set_value(0.0, false); + velocity_states_.push_back( + std::make_shared(joint.name, hardware_interface::HW_IF_VELOCITY)); + std::ignore = velocity_states_.back()->set_value(0.0, false); + effort_states_.push_back( + std::make_shared(joint.name, hardware_interface::HW_IF_EFFORT)); + std::ignore = effort_states_.back()->set_value(0.0, false); + state_interfaces.push_back(position_states_.back()); + state_interfaces.push_back(velocity_states_.back()); + state_interfaces.push_back(effort_states_.back()); } return state_interfaces; } - std::vector export_command_interfaces() override + std::vector on_export_command_interfaces() override { verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); - std::vector command_interfaces; + std::vector command_interfaces; for (std::size_t i = 0; i < info_.joints.size(); ++i) { const auto & joint = info_.joints[i]; - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - joint.name, hardware_interface::HW_IF_POSITION, ¤t_states_.at(i).pos)); - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - joint.name, hardware_interface::HW_IF_VELOCITY, ¤t_states_.at(i).vel)); - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - joint.name, hardware_interface::HW_IF_EFFORT, ¤t_states_.at(i).effort)); + position_commands_.push_back( + std::make_shared(joint.name, hardware_interface::HW_IF_POSITION)); + std::ignore = position_commands_.back()->set_value(0.0, false); + velocity_commands_.push_back( + std::make_shared(joint.name, hardware_interface::HW_IF_VELOCITY)); + std::ignore = velocity_commands_.back()->set_value(0.0, false); + effort_commands_.push_back( + std::make_shared(joint.name, hardware_interface::HW_IF_EFFORT)); + std::ignore = effort_commands_.back()->set_value(0.0, false); + command_interfaces.push_back(position_commands_.back()); + command_interfaces.push_back(velocity_commands_.back()); + command_interfaces.push_back(effort_commands_.back()); } return command_interfaces; } @@ -200,7 +194,12 @@ class TestActuatorExclusiveInterfaces : public ActuatorInterface private: std::vector currently_claimed_joints_; - std::vector current_states_; + std::vector position_states_; + std::vector velocity_states_; + std::vector effort_states_; + std::vector position_commands_; + std::vector velocity_commands_; + std::vector effort_commands_; }; #include "pluginlib/class_list_macros.hpp" // NOLINT diff --git a/hardware_interface_testing/test/test_components/test_sensor.cpp b/hardware_interface_testing/test/test_components/test_sensor.cpp index a0445e6365..a4cfb5131f 100644 --- a/hardware_interface_testing/test/test_components/test_sensor.cpp +++ b/hardware_interface_testing/test/test_components/test_sensor.cpp @@ -46,13 +46,13 @@ class TestSensor : public SensorInterface return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces() override + std::vector on_export_state_interfaces() override { - std::vector state_interfaces; - state_interfaces.emplace_back( - hardware_interface::StateInterface( - get_hardware_info().sensors[0].name, - get_hardware_info().sensors[0].state_interfaces[0].name, &velocity_state_)); + std::vector state_interfaces; + velocity_state_ = std::make_shared( + get_hardware_info().sensors[0].name, get_hardware_info().sensors[0].state_interfaces[0].name); + std::ignore = velocity_state_->set_value(0.0, false); + state_interfaces.push_back(velocity_state_); return state_interfaces; } @@ -63,7 +63,7 @@ class TestSensor : public SensorInterface } private: - double velocity_state_ = 0.0; + StateInterface::SharedPtr velocity_state_; }; class TestUninitializableSensor : public TestSensor diff --git a/hardware_interface_testing/test/test_components/test_system.cpp b/hardware_interface_testing/test/test_components/test_system.cpp index 389f235805..a10a2939e1 100644 --- a/hardware_interface_testing/test/test_components/test_system.cpp +++ b/hardware_interface_testing/test/test_components/test_system.cpp @@ -66,58 +66,64 @@ class TestSystem : public SystemInterface return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces() override + std::vector on_export_state_interfaces() override { verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); const auto info = get_hardware_info(); - std::vector state_interfaces; + std::vector state_interfaces; for (auto i = 0u; i < info.joints.size(); ++i) { - state_interfaces.emplace_back( - hardware_interface::StateInterface( - info.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i])); - state_interfaces.emplace_back( - hardware_interface::StateInterface( - info.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_state_[i])); - state_interfaces.emplace_back( - hardware_interface::StateInterface( - info.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &acceleration_state_[i])); + position_state_[i] = + std::make_shared(info.joints[i].name, hardware_interface::HW_IF_POSITION); + std::ignore = position_state_[i]->set_value(0.0, false); + velocity_state_[i] = + std::make_shared(info.joints[i].name, hardware_interface::HW_IF_VELOCITY); + std::ignore = velocity_state_[i]->set_value(0.0, false); + acceleration_state_[i] = std::make_shared( + info.joints[i].name, hardware_interface::HW_IF_ACCELERATION); + std::ignore = acceleration_state_[i]->set_value(0.0, false); + state_interfaces.push_back(position_state_[i]); + state_interfaces.push_back(velocity_state_[i]); + state_interfaces.push_back(acceleration_state_[i]); } if (info.gpios.size() > 0) { // Add configuration/max_tcp_jerk interface - state_interfaces.emplace_back( - hardware_interface::StateInterface( - info.gpios[0].name, info.gpios[0].state_interfaces[0].name, &configuration_state_)); + configuration_state_ = std::make_shared( + info.gpios[0].name, info.gpios[0].state_interfaces[0].name); + std::ignore = configuration_state_->set_value(0.0, false); + state_interfaces.push_back(configuration_state_); } return state_interfaces; } - std::vector export_command_interfaces() override + std::vector on_export_command_interfaces() override { verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); const auto info = get_hardware_info(); - std::vector command_interfaces; + std::vector command_interfaces; for (auto i = 0u; i < info.joints.size(); ++i) { - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - info.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_command_[i])); + velocity_command_[i] = + std::make_shared(info.joints[i].name, hardware_interface::HW_IF_VELOCITY); + std::ignore = velocity_command_[i]->set_value(0.0, false); + command_interfaces.push_back(velocity_command_[i]); } // Add max_acceleration command interface - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - info.joints[0].name, info.joints[0].command_interfaces[1].name, - &max_acceleration_command_)); + max_acceleration_command_ = std::make_shared( + info.joints[0].name, info.joints[0].command_interfaces[1].name); + std::ignore = max_acceleration_command_->set_value(0.0, false); + command_interfaces.push_back(max_acceleration_command_); if (info.gpios.size() > 0) { // Add configuration/max_tcp_jerk interface - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - info.gpios[0].name, info.gpios[0].command_interfaces[0].name, &configuration_command_)); + configuration_command_ = std::make_shared( + info.gpios[0].name, info.gpios[0].command_interfaces[0].name); + std::ignore = configuration_command_->set_value(0.0, false); + command_interfaces.push_back(configuration_command_); } return command_interfaces; @@ -131,21 +137,26 @@ class TestSystem : public SystemInterface std::this_thread::sleep_for( std::chrono::milliseconds(1000 / (3 * get_hardware_info().rw_rate))); } + double vel_cmd = 0.0; + std::ignore = velocity_command_[0]->get_value(vel_cmd, false); // simulate error on read - if (velocity_command_[0] == test_constants::READ_FAIL_VALUE) + if (vel_cmd == test_constants::READ_FAIL_VALUE) { // reset value to get out from error on the next call - simplifies CM // tests - velocity_command_[0] = 0.0; + std::ignore = velocity_command_[0]->set_value(0.0, false); return return_type::ERROR; } // simulate deactivate on read - if (velocity_command_[0] == test_constants::READ_DEACTIVATE_VALUE) + if (vel_cmd == test_constants::READ_DEACTIVATE_VALUE) { return return_type::DEACTIVATE; } // simulate exception on read - if (velocity_command_[0] == test_constants::READ_THROW_VALUE) + const auto velocity_command_value = velocity_command_[0]->get_optional(); + if ( + velocity_command_value.has_value() && + velocity_command_value.value() == test_constants::READ_THROW_VALUE) { throw std::runtime_error("Exception from TestSystem::read() as requested."); } @@ -154,7 +165,7 @@ class TestSystem : public SystemInterface // working as it should. This makes value checks clearer and confirms there // is no "state = command" line or some other mixture of interfaces // somewhere in the test stack. - velocity_state_[0] = velocity_command_[0] / 2.0; + std::ignore = velocity_state_[0]->set_value(vel_cmd / 2.0, false); return return_type::OK; } @@ -166,21 +177,26 @@ class TestSystem : public SystemInterface std::this_thread::sleep_for( std::chrono::milliseconds(1000 / (6 * get_hardware_info().rw_rate))); } + double vel_cmd = 0.0; + std::ignore = velocity_command_[0]->get_value(vel_cmd, false); // simulate error on write - if (velocity_command_[0] == test_constants::WRITE_FAIL_VALUE) + if (vel_cmd == test_constants::WRITE_FAIL_VALUE) { // reset value to get out from error on the next call - simplifies CM // tests - velocity_command_[0] = 0.0; + std::ignore = velocity_command_[0]->set_value(0.0, false); return return_type::ERROR; } // simulate deactivate on write - if (velocity_command_[0] == test_constants::WRITE_DEACTIVATE_VALUE) + if (vel_cmd == test_constants::WRITE_DEACTIVATE_VALUE) { return return_type::DEACTIVATE; } // simulate exception on write - if (velocity_command_[0] == test_constants::WRITE_THROW_VALUE) + const auto velocity_command_value = velocity_command_[0]->get_optional(); + if ( + velocity_command_value.has_value() && + velocity_command_value.value() == test_constants::WRITE_THROW_VALUE) { throw std::runtime_error("Exception from TestSystem::write() as requested."); } @@ -188,13 +204,13 @@ class TestSystem : public SystemInterface } private: - std::array velocity_command_ = {{0.0, 0.0}}; - std::array position_state_ = {{0.0, 0.0}}; - std::array velocity_state_ = {{0.0, 0.0}}; - std::array acceleration_state_ = {{0.0, 0.0}}; - double max_acceleration_command_ = 0.0; - double configuration_state_ = 0.0; - double configuration_command_ = 0.0; + std::array velocity_command_; + std::array position_state_; + std::array velocity_state_; + std::array acceleration_state_; + CommandInterface::SharedPtr max_acceleration_command_; + StateInterface::SharedPtr configuration_state_; + CommandInterface::SharedPtr configuration_command_; }; class TestUninitializableSystem : public TestSystem diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 1a7a2daa0a..36e2574867 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -373,21 +373,23 @@ TEST_F(ResourceManagerTest, resource_claiming) class ExternalComponent : public hardware_interface::ActuatorInterface { - std::vector export_state_interfaces() override + std::vector on_export_state_interfaces() + override { - std::vector state_interfaces; - state_interfaces.emplace_back( - hardware_interface::StateInterface("external_joint", "external_state_interface", nullptr)); - + std::vector state_interfaces; + state_interface_ = std::make_shared( + "external_joint", "external_state_interface"); + state_interfaces.emplace_back(state_interface_); return state_interfaces; } - std::vector export_command_interfaces() override + std::vector on_export_command_interfaces() + override { - std::vector command_interfaces; - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - "external_joint", "external_command_interface", nullptr)); + std::vector command_interfaces; + command_interface_ = std::make_shared( + "external_joint", "external_command_interface"); + command_interfaces.emplace_back(command_interface_); return command_interfaces; } @@ -403,6 +405,9 @@ class ExternalComponent : public hardware_interface::ActuatorInterface { return hardware_interface::return_type::OK; } + + hardware_interface::StateInterface::SharedPtr state_interface_; + hardware_interface::CommandInterface::SharedPtr command_interface_; }; TEST_F(ResourceManagerTest, post_initialization_add_components)