use interfacedescription for creation of handle, depricate old way

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_from_hardware_info(
+ 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_from_hardware_info(
+ const std::vector<ComponentInfo> & component_info);
+
 } // namespace hardware_interface
 #endif // HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_

hardware_interface/include/hardware_interface/handle.hpp

@@ -15,42 +15,67 @@
 #ifndef HARDWARE_INTERFACE__HANDLE_HPP_
 #define HARDWARE_INTERFACE__HANDLE_HPP_
 
+#include <limits>
+#include <memory>
 #include <string>
+#include <utility>
+#include <variant>
 
+#include "hardware_interface/hardware_info.hpp"
 #include "hardware_interface/macros.hpp"
+#include "hardware_interface/visibility_control.h"
 
 namespace hardware_interface
 {
+
+typedef std::variant<double> HANDLE_DATATYPE;
+
 /// A handle used to get and set a value on a given interface.
-class ReadOnlyHandle
+class Handle
 {
 public:
- ReadOnlyHandle(
+ [[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 ReadOnlyHandle(const std::string & interface_name)
+ 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)
  {
  }
 
- explicit ReadOnlyHandle(const char * interface_name)
+ [[deprecated("Use InterfaceDescription for initializing the Interface")]]
+
+ explicit Handle(const char * interface_name)
  : interface_name_(interface_name), value_ptr_(nullptr)
  {
  }
 
- ReadOnlyHandle(const ReadOnlyHandle & other) = default;
+ Handle(const Handle & other) = default;
 
- ReadOnlyHandle(ReadOnlyHandle && other) = default;
+ Handle(Handle && other) = default;
 
- ReadOnlyHandle & operator=(const ReadOnlyHandle & other) = default;
+ Handle & operator=(const Handle & other) = default;
 
- ReadOnlyHandle & operator=(ReadOnlyHandle && other) = default;
+ Handle & operator=(Handle && other) = default;
 
- virtual ~ReadOnlyHandle() = default;
+ virtual ~Handle() = default;
 
  /// Returns true if handle references a value.
  inline operator bool() const { return value_ptr_ != nullptr; }
@@ -70,60 +95,54 @@ class ReadOnlyHandle
 
  double get_value() const
  {
+ // BEGIN (Handle export change): for backward compatibility
+ // TODO(Manuel) return value_ if old functionality is removed
  THROW_ON_NULLPTR(value_ptr_);
  return *value_ptr_;
+ // END
+ }
+
+ void set_value(double value)
+ {
+ // BEGIN (Handle export change): for backward compatibility
+ // TODO(Manuel) set value_ directly if old functionality is removed
+ THROW_ON_NULLPTR(this->value_ptr_);
+ *this->value_ptr_ = value;
+ // END
  }
 
 protected:
  std::string prefix_name_;
  std::string interface_name_;
+ HANDLE_DATATYPE value_;
+ // BEGIN (Handle export change): for backward compatibility
+ // TODO(Manuel) redeclare as HANDLE_DATATYPE * value_ptr_ if old functionality is removed
  double * value_ptr_;
+ // END
 };
 
-class ReadWriteHandle : public ReadOnlyHandle
+class StateInterface : public Handle
 {
 public:
- ReadWriteHandle(
- const std::string & prefix_name, const std::string & interface_name,
- double * value_ptr = nullptr)
- : ReadOnlyHandle(prefix_name, interface_name, value_ptr)
- {
- }
-
- explicit ReadWriteHandle(const std::string & interface_name) : ReadOnlyHandle(interface_name) {}
-
- explicit ReadWriteHandle(const char * interface_name) : ReadOnlyHandle(interface_name) {}
-
- ReadWriteHandle(const ReadWriteHandle & other) = default;
-
- ReadWriteHandle(ReadWriteHandle && other) = default;
-
- ReadWriteHandle & operator=(const ReadWriteHandle & other) = default;
-
- ReadWriteHandle & operator=(ReadWriteHandle && other) = default;
-
- virtual ~ReadWriteHandle() = default;
-
- void set_value(double value)
+ explicit StateInterface(const InterfaceDescription & interface_description)
+ : Handle(interface_description)
  {
- THROW_ON_NULLPTR(this->value_ptr_);
- *this->value_ptr_ = value;
  }
-};
 
-class StateInterface : public ReadOnlyHandle
-{
-public:
  StateInterface(const StateInterface & other) = default;
 
  StateInterface(StateInterface && other) = default;
 
- using ReadOnlyHandle::ReadOnlyHandle;
+ using Handle::Handle;
 };
 
-class CommandInterface : public ReadWriteHandle
+class CommandInterface : public Handle
 {
 public:
+ explicit CommandInterface(const InterfaceDescription & interface_description)
+ : Handle(interface_description)
+ {
+ }
  /// CommandInterface copy constructor is actively deleted.
  /**
  * Command interfaces are having a unique ownership and thus
@@ -134,7 +153,7 @@ class CommandInterface : public ReadWriteHandle
 
  CommandInterface(CommandInterface && other) = default;
 
- using ReadWriteHandle::ReadWriteHandle;
+ using Handle::Handle;
 };
 
 } // namespace hardware_interface

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,32 @@ 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)
+ {
+ }
+
+ /**
+ * 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;
+
+ std::string get_name() const { return prefix_name + "/" + interface_info.name; }
+
+ std::string get_interface_type() const { return interface_info.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_from_hardware_info(
+ 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_from_hardware_info(
+ 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_from_hardware_info(control_hardware[0].joints);
+ EXPECT_EQ(joint_state_descriptions[0].prefix_name, "joint1");
+ EXPECT_EQ(joint_state_descriptions[0].get_interface_type(), "position");
+ EXPECT_EQ(joint_state_descriptions[0].get_name(), "joint1/position");
+
+ EXPECT_EQ(joint_state_descriptions[1].prefix_name, "joint2");
+ EXPECT_EQ(joint_state_descriptions[1].get_interface_type(), "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_from_hardware_info(control_hardware[0].joints);
+ EXPECT_EQ(joint_command_descriptions[0].prefix_name, "joint1");
+ EXPECT_EQ(joint_command_descriptions[0].get_interface_type(), "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].prefix_name, "joint2");
+ EXPECT_EQ(joint_command_descriptions[1].get_interface_type(), "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_from_hardware_info(control_hardware[0].sensors);
+ EXPECT_EQ(sensor_state_descriptions[0].prefix_name, "sensor1");
+ EXPECT_EQ(sensor_state_descriptions[0].get_interface_type(), "roll");
+ EXPECT_EQ(sensor_state_descriptions[0].get_name(), "sensor1/roll");
+ EXPECT_EQ(sensor_state_descriptions[1].prefix_name, "sensor1");
+ EXPECT_EQ(sensor_state_descriptions[1].get_interface_type(), "pitch");
+ EXPECT_EQ(sensor_state_descriptions[1].get_name(), "sensor1/pitch");
+ EXPECT_EQ(sensor_state_descriptions[2].prefix_name, "sensor1");
+ EXPECT_EQ(sensor_state_descriptions[2].get_interface_type(), "yaw");
+ EXPECT_EQ(sensor_state_descriptions[2].get_name(), "sensor1/yaw");
+
+ EXPECT_EQ(sensor_state_descriptions[3].prefix_name, "sensor2");
+ EXPECT_EQ(sensor_state_descriptions[3].get_interface_type(), "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_from_hardware_info(control_hardware[0].gpios);
+ EXPECT_EQ(gpio_state_descriptions[0].prefix_name, "flange_analog_IOs");
+ EXPECT_EQ(gpio_state_descriptions[0].get_interface_type(), "analog_output1");
+ EXPECT_EQ(gpio_state_descriptions[0].get_name(), "flange_analog_IOs/analog_output1");
+ EXPECT_EQ(gpio_state_descriptions[1].prefix_name, "flange_analog_IOs");
+ EXPECT_EQ(gpio_state_descriptions[1].get_interface_type(), "analog_input1");
+ EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_analog_IOs/analog_input1");
+ EXPECT_EQ(gpio_state_descriptions[2].prefix_name, "flange_analog_IOs");
+ EXPECT_EQ(gpio_state_descriptions[2].get_interface_type(), "analog_input2");
+ EXPECT_EQ(gpio_state_descriptions[2].get_name(), "flange_analog_IOs/analog_input2");
+
+ EXPECT_EQ(gpio_state_descriptions[3].prefix_name, "flange_vacuum");
+ EXPECT_EQ(gpio_state_descriptions[3].get_interface_type(), "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_from_hardware_info(control_hardware[0].gpios);
+ EXPECT_EQ(gpio_state_descriptions[0].prefix_name, "flange_analog_IOs");
+ EXPECT_EQ(gpio_state_descriptions[0].get_interface_type(), "analog_output1");
+ EXPECT_EQ(gpio_state_descriptions[0].get_name(), "flange_analog_IOs/analog_output1");
+
+ EXPECT_EQ(gpio_state_descriptions[1].prefix_name, "flange_vacuum");
+ EXPECT_EQ(gpio_state_descriptions[1].get_interface_type(), "vacuum");
+ EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_vacuum/vacuum");
+}