From 40ea19176d47c63e9e873b7744cfad8eef366b41 Mon Sep 17 00:00:00 2001 From: Manuel Muth Date: Wed, 21 Aug 2024 17:41:57 +0200 Subject: [PATCH] Introduce Creation of Handles with InterfaceDescription (variant support) (#1679) --- .../hardware_interface/component_parser.hpp | 18 +++ .../include/hardware_interface/handle.hpp | 30 +++-- .../hardware_interface/hardware_info.hpp | 39 ++++++- hardware_interface/src/component_parser.cpp | 34 ++++++ .../test/test_component_parser.cpp | 110 ++++++++++++++++++ hardware_interface/test/test_handle.cpp | 31 +++++ 6 files changed, 252 insertions(+), 10 deletions(-) diff --git a/hardware_interface/include/hardware_interface/component_parser.hpp b/hardware_interface/include/hardware_interface/component_parser.hpp index 38ca0cf89d..2d0c067606 100644 --- a/hardware_interface/include/hardware_interface/component_parser.hpp +++ b/hardware_interface/include/hardware_interface/component_parser.hpp @@ -32,5 +32,23 @@ namespace hardware_interface HARDWARE_INTERFACE_PUBLIC std::vector parse_control_resources_from_urdf(const std::string & urdf); +/** + * \param[in] component_info information about a component (gpio, joint, sensor) + * \return vector filled with information about hardware's StateInterfaces for the component + * which are exported + */ +HARDWARE_INTERFACE_PUBLIC +std::vector parse_state_interface_descriptions( + const std::vector & component_info); + +/** + * \param[in] component_info information about a component (gpio, joint, sensor) + * \return vector filled with information about hardware's CommandInterfaces for the component + * which are exported + */ +HARDWARE_INTERFACE_PUBLIC +std::vector parse_command_interface_descriptions( + const std::vector & component_info); + } // namespace hardware_interface #endif // HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_ diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index bfe0ff1eb8..6f919033de 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -15,9 +15,11 @@ #ifndef HARDWARE_INTERFACE__HANDLE_HPP_ #define HARDWARE_INTERFACE__HANDLE_HPP_ +#include #include #include +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/macros.hpp" namespace hardware_interface @@ -29,6 +31,8 @@ using HANDLE_DATATYPE = std::variant; class Handle { public: + [[deprecated("Use InterfaceDescription for initializing the Interface")]] + Handle( const std::string & prefix_name, const std::string & interface_name, double * value_ptr = nullptr) @@ -36,11 +40,25 @@ class Handle { } + explicit Handle(const InterfaceDescription & interface_description) + : prefix_name_(interface_description.prefix_name), + interface_name_(interface_description.interface_info.name) + { + // As soon as multiple datatypes are used in HANDLE_DATATYPE + // we need to initialize according the type passed in interface description + value_ = std::numeric_limits::quiet_NaN(); + value_ptr_ = std::get_if(&value_); + } + + [[deprecated("Use InterfaceDescription for initializing the Interface")]] + explicit Handle(const std::string & interface_name) : interface_name_(interface_name), value_ptr_(nullptr) { } + [[deprecated("Use InterfaceDescription for initializing the Interface")]] + explicit Handle(const char * interface_name) : interface_name_(interface_name), value_ptr_(nullptr) { @@ -103,10 +121,8 @@ class Handle class StateInterface : public Handle { public: - explicit StateInterface( - const std::string & prefix_name, const std::string & interface_name, - double * value_ptr = nullptr) - : Handle(prefix_name, interface_name, value_ptr) + explicit StateInterface(const InterfaceDescription & interface_description) + : Handle(interface_description) { } @@ -120,10 +136,8 @@ class StateInterface : public Handle class CommandInterface : public Handle { public: - explicit CommandInterface( - const std::string & prefix_name, const std::string & interface_name, - double * value_ptr = nullptr) - : Handle(prefix_name, interface_name, value_ptr) + explicit CommandInterface(const InterfaceDescription & interface_description) + : Handle(interface_description) { } /// CommandInterface copy constructor is actively deleted. diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 2bd2099e69..b4e47c610e 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -40,9 +40,9 @@ struct InterfaceInfo std::string max; /// (Optional) Initial value of the interface. std::string initial_value; - /// (Optional) The datatype of the interface, e.g. "bool", "int". Used by GPIOs. + /// (Optional) The datatype of the interface, e.g. "bool", "int". std::string data_type; - /// (Optional) If the handle is an array, the size of the array. Used by GPIOs. + /// (Optional) If the handle is an array, the size of the array. int size; /// (Optional) enable or disable the limits for the command interfaces bool enable_limits; @@ -126,6 +126,41 @@ struct TransmissionInfo std::unordered_map parameters; }; +/** + * This structure stores information about an interface for a specific hardware which should be + * instantiated internally. + */ +struct InterfaceDescription +{ + InterfaceDescription(const std::string & prefix_name_in, const InterfaceInfo & interface_info_in) + : prefix_name(prefix_name_in), + interface_info(interface_info_in), + interface_name(prefix_name + "/" + interface_info.name) + { + } + + /** + * Name of the interface defined by the user. + */ + std::string prefix_name; + + /** + * Information about the Interface type (position, velocity,...) as well as limits and so on. + */ + InterfaceInfo interface_info; + + /** + * Name of the interface + */ + std::string interface_name; + + std::string get_prefix_name() const { return prefix_name; } + + std::string get_interface_name() const { return interface_info.name; } + + std::string get_name() const { return interface_name; } +}; + /// This structure stores information about hardware defined in a robot's URDF. struct HardwareInfo { diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 42432dda8d..0ef6c084f9 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -905,4 +905,38 @@ std::vector parse_control_resources_from_urdf(const std::string & return hardware_info; } +std::vector parse_state_interface_descriptions( + const std::vector & component_info) +{ + std::vector component_state_interface_descriptions; + component_state_interface_descriptions.reserve(component_info.size()); + + for (const auto & component : component_info) + { + for (const auto & state_interface : component.state_interfaces) + { + component_state_interface_descriptions.emplace_back( + InterfaceDescription(component.name, state_interface)); + } + } + return component_state_interface_descriptions; +} + +std::vector parse_command_interface_descriptions( + const std::vector & component_info) +{ + std::vector component_command_interface_descriptions; + component_command_interface_descriptions.reserve(component_info.size()); + + for (const auto & component : component_info) + { + for (const auto & command_interface : component.command_interfaces) + { + component_command_interface_descriptions.emplace_back( + InterfaceDescription(component.name, command_interface)); + } + } + return component_command_interface_descriptions; +} + } // namespace hardware_interface diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index be891787f3..6a78b0807d 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -1404,3 +1404,113 @@ TEST_F(TestComponentParser, urdf_incomplete_throws_error) std::string(ros2_control_test_assets::urdf_tail); ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); } + +TEST_F(TestComponentParser, parse_joint_state_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_system_multi_joints_transmission + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto joint_state_descriptions = + parse_state_interface_descriptions(control_hardware[0].joints); + EXPECT_EQ(joint_state_descriptions[0].get_prefix_name(), "joint1"); + EXPECT_EQ(joint_state_descriptions[0].get_interface_name(), "position"); + EXPECT_EQ(joint_state_descriptions[0].get_name(), "joint1/position"); + + EXPECT_EQ(joint_state_descriptions[1].get_prefix_name(), "joint2"); + EXPECT_EQ(joint_state_descriptions[1].get_interface_name(), "position"); + EXPECT_EQ(joint_state_descriptions[1].get_name(), "joint2/position"); +} + +TEST_F(TestComponentParser, parse_joint_command_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_system_multi_joints_transmission + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto joint_command_descriptions = + parse_command_interface_descriptions(control_hardware[0].joints); + EXPECT_EQ(joint_command_descriptions[0].get_prefix_name(), "joint1"); + EXPECT_EQ(joint_command_descriptions[0].get_interface_name(), "position"); + EXPECT_EQ(joint_command_descriptions[0].get_name(), "joint1/position"); + EXPECT_EQ(joint_command_descriptions[0].interface_info.min, "-1"); + EXPECT_EQ(joint_command_descriptions[0].interface_info.max, "1"); + + EXPECT_EQ(joint_command_descriptions[1].get_prefix_name(), "joint2"); + EXPECT_EQ(joint_command_descriptions[1].get_interface_name(), "position"); + EXPECT_EQ(joint_command_descriptions[1].get_name(), "joint2/position"); + EXPECT_EQ(joint_command_descriptions[1].interface_info.min, "-1"); + EXPECT_EQ(joint_command_descriptions[1].interface_info.max, "1"); +} + +TEST_F(TestComponentParser, parse_sensor_state_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_sensor_only + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto sensor_state_descriptions = + parse_state_interface_descriptions(control_hardware[0].sensors); + EXPECT_EQ(sensor_state_descriptions[0].get_prefix_name(), "sensor1"); + EXPECT_EQ(sensor_state_descriptions[0].get_interface_name(), "roll"); + EXPECT_EQ(sensor_state_descriptions[0].get_name(), "sensor1/roll"); + EXPECT_EQ(sensor_state_descriptions[1].get_prefix_name(), "sensor1"); + EXPECT_EQ(sensor_state_descriptions[1].get_interface_name(), "pitch"); + EXPECT_EQ(sensor_state_descriptions[1].get_name(), "sensor1/pitch"); + EXPECT_EQ(sensor_state_descriptions[2].get_prefix_name(), "sensor1"); + EXPECT_EQ(sensor_state_descriptions[2].get_interface_name(), "yaw"); + EXPECT_EQ(sensor_state_descriptions[2].get_name(), "sensor1/yaw"); + + EXPECT_EQ(sensor_state_descriptions[3].get_prefix_name(), "sensor2"); + EXPECT_EQ(sensor_state_descriptions[3].get_interface_name(), "image"); + EXPECT_EQ(sensor_state_descriptions[3].get_name(), "sensor2/image"); +} + +TEST_F(TestComponentParser, parse_gpio_state_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_gpio + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto gpio_state_descriptions = + parse_state_interface_descriptions(control_hardware[0].gpios); + EXPECT_EQ(gpio_state_descriptions[0].get_prefix_name(), "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[0].get_interface_name(), "analog_output1"); + EXPECT_EQ(gpio_state_descriptions[0].get_name(), "flange_analog_IOs/analog_output1"); + EXPECT_EQ(gpio_state_descriptions[1].get_prefix_name(), "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[1].get_interface_name(), "analog_input1"); + EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_analog_IOs/analog_input1"); + EXPECT_EQ(gpio_state_descriptions[2].get_prefix_name(), "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[2].get_interface_name(), "analog_input2"); + EXPECT_EQ(gpio_state_descriptions[2].get_name(), "flange_analog_IOs/analog_input2"); + + EXPECT_EQ(gpio_state_descriptions[3].get_prefix_name(), "flange_vacuum"); + EXPECT_EQ(gpio_state_descriptions[3].get_interface_name(), "vacuum"); + EXPECT_EQ(gpio_state_descriptions[3].get_name(), "flange_vacuum/vacuum"); +} + +TEST_F(TestComponentParser, parse_gpio_command_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_gpio + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto gpio_state_descriptions = + parse_command_interface_descriptions(control_hardware[0].gpios); + EXPECT_EQ(gpio_state_descriptions[0].get_prefix_name(), "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[0].get_interface_name(), "analog_output1"); + EXPECT_EQ(gpio_state_descriptions[0].get_name(), "flange_analog_IOs/analog_output1"); + + EXPECT_EQ(gpio_state_descriptions[1].get_prefix_name(), "flange_vacuum"); + EXPECT_EQ(gpio_state_descriptions[1].get_interface_name(), "vacuum"); + EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_vacuum/vacuum"); +} diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index da8258c643..7d79c032f0 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -14,8 +14,11 @@ #include #include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" using hardware_interface::CommandInterface; +using hardware_interface::InterfaceDescription; +using hardware_interface::InterfaceInfo; using hardware_interface::StateInterface; namespace @@ -64,3 +67,31 @@ TEST(TestHandle, value_methods_work_on_non_nullptr) EXPECT_NO_THROW(handle.set_value(0.0)); EXPECT_DOUBLE_EQ(handle.get_value(), 0.0); } + +TEST(TestHandle, interface_description_state_interface_name_getters_work) +{ + const std::string POSITION_INTERFACE = "position"; + const std::string JOINT_NAME_1 = "joint1"; + InterfaceInfo info; + info.name = POSITION_INTERFACE; + InterfaceDescription interface_descr(JOINT_NAME_1, info); + StateInterface handle{interface_descr}; + + EXPECT_EQ(handle.get_name(), JOINT_NAME_1 + "/" + POSITION_INTERFACE); + EXPECT_EQ(handle.get_interface_name(), POSITION_INTERFACE); + EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME_1); +} + +TEST(TestHandle, interface_description_command_interface_name_getters_work) +{ + const std::string POSITION_INTERFACE = "position"; + const std::string JOINT_NAME_1 = "joint1"; + InterfaceInfo info; + info.name = POSITION_INTERFACE; + InterfaceDescription interface_descr(JOINT_NAME_1, info); + CommandInterface handle{interface_descr}; + + EXPECT_EQ(handle.get_name(), JOINT_NAME_1 + "/" + POSITION_INTERFACE); + EXPECT_EQ(handle.get_interface_name(), POSITION_INTERFACE); + EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME_1); +}