Introduce Creation of Handles with InterfaceDescription (variant supp…

…ort) (#1679)

hardware_interface/include/hardware_interface/component_parser.hpp

@@ -32,5 +32,23 @@ namespace hardware_interface
 HARDWARE_INTERFACE_PUBLIC
 std::vector<HardwareInfo> 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<InterfaceDescription> parse_state_interface_descriptions(
+  const std::vector<ComponentInfo> & 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<InterfaceDescription> parse_command_interface_descriptions(
+  const std::vector<ComponentInfo> & component_info);
+
 }  // namespace hardware_interface
 #endif  // HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_

hardware_interface/include/hardware_interface/handle.hpp

@@ -15,9 +15,11 @@
 #ifndef HARDWARE_INTERFACE__HANDLE_HPP_
 #define HARDWARE_INTERFACE__HANDLE_HPP_
 
+#include <limits>
 #include <string>
 #include <variant>
 
+#include "hardware_interface/hardware_info.hpp"
 #include "hardware_interface/macros.hpp"
 
 namespace hardware_interface
@@ -29,18 +31,34 @@ using HANDLE_DATATYPE = std::variant<double>;
 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)
   : prefix_name_(prefix_name), interface_name_(interface_name), value_ptr_(value_ptr)
   {
   }
 
+  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<double>::quiet_NaN();
+    value_ptr_ = std::get_if<double>(&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.

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<std::string, std::string> 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
 {

hardware_interface/src/component_parser.cpp

@@ -905,4 +905,38 @@ std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string &
   return hardware_info;
 }
 
+std::vector<InterfaceDescription> parse_state_interface_descriptions(
+  const std::vector<ComponentInfo> & component_info)
+{
+  std::vector<InterfaceDescription> 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<InterfaceDescription> parse_command_interface_descriptions(
+  const std::vector<ComponentInfo> & component_info)
+{
+  std::vector<InterfaceDescription> 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

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");
+}

hardware_interface/test/test_handle.cpp

@@ -14,8 +14,11 @@
 
 #include <gmock/gmock.h>
 #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);
+}