Change interface export variant

hardware_interface/CMakeLists.txt

@@ -73,6 +73,7 @@ if(BUILD_TESTING)
 
  ament_add_gmock(test_component_interfaces test/test_component_interfaces.cpp)
  target_link_libraries(test_component_interfaces hardware_interface)
+ ament_target_dependencies(test_component_interfaces ros2_control_test_assets)
 
  ament_add_gmock(test_component_parser test/test_component_parser.cpp)
  target_link_libraries(test_component_parser hardware_interface)

hardware_interface/include/hardware_interface/actuator.hpp

@@ -64,11 +64,23 @@ class Actuator final
  HARDWARE_INTERFACE_PUBLIC
  const rclcpp_lifecycle::State & error();
 
+ [[deprecated(
+ "Replaced by vector<std::shared_ptr<StateInterface>> on_export_state_interfaces() method. "
+ "Exporting is handled by the Framework.")]] HARDWARE_INTERFACE_PUBLIC
+ std::vector<StateInterface>
+ export_state_interfaces();
+
  HARDWARE_INTERFACE_PUBLIC
- std::vector<StateInterface> export_state_interfaces();
+ std::vector<std::shared_ptr<StateInterface>> on_export_state_interfaces();
+
+ [[deprecated(
+ "Replaced by vector<std::shared_ptr<CommandInterface>> on_export_state_interfaces() method. "
+ "Exporting is handled by the Framework.")]] HARDWARE_INTERFACE_PUBLIC
+ std::vector<CommandInterface>
+ export_command_interfaces();
 
  HARDWARE_INTERFACE_PUBLIC
- std::vector<CommandInterface> export_command_interfaces();
+ std::vector<std::shared_ptr<CommandInterface>> on_export_command_interfaces();
 
  HARDWARE_INTERFACE_PUBLIC
  return_type prepare_command_mode_switch(

hardware_interface/include/hardware_interface/actuator_interface.hpp

@@ -15,10 +15,14 @@
 #ifndef HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_
 #define HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_
 
+#include <limits>
+#include <map>
 #include <memory>
 #include <string>
+#include <utility>
 #include <vector>
 
+#include "hardware_interface/component_parser.hpp"
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/hardware_info.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
@@ -97,31 +101,117 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
  virtual CallbackReturn on_init(const HardwareInfo & hardware_info)
  {
  info_ = hardware_info;
+ import_state_interface_descriptions(info_);
+ import_command_interface_descriptions(info_);
  return CallbackReturn::SUCCESS;
  };
 
+ /**
+ * Import the InterfaceDescription for the StateInterfaces from the HardwareInfo.
+ * Separate them into the possible types: Joint and store them.
+ */
+ virtual void import_state_interface_descriptions(const HardwareInfo & hardware_info)
+ {
+ auto joint_state_interface_descriptions =
+ parse_joint_state_interface_descriptions_from_hardware_info(hardware_info);
+ for (const auto & description : joint_state_interface_descriptions)
+ {
+ joint_state_interfaces_.insert(std::make_pair(description.get_name(), description));
+ }
+ }
+
+ /**
+ * Import the InterfaceDescription for the CommandInterfaces from the HardwareInfo.
+ * Separate them into the possible types: Joint and store them.
+ */
+ virtual void import_command_interface_descriptions(const HardwareInfo & hardware_info)
+ {
+ auto joint_command_interface_descriptions =
+ parse_joint_command_interface_descriptions_from_hardware_info(hardware_info);
+ for (const auto & description : joint_command_interface_descriptions)
+ {
+ joint_command_interfaces_.insert(std::make_pair(description.get_name(), description));
+ }
+ }
+
  /// Exports all state interfaces for this hardware interface.
  /**
+ * Default implementation for exporting the StateInterfaces. The StateInterfaces are created
+ * according to the InterfaceDescription. The memory accessed by the controllers and hardware is
+ * assigned here and resides in the system_interface.
+ *
+ * If overwritten:
  * The state interfaces have to be created and transferred according
  * to the hardware info passed in for the configuration.
  *
  * Note the ownership over the state interfaces is transferred to the caller.
  *
  * \return vector of state interfaces
  */
- virtual std::vector<StateInterface> export_state_interfaces() = 0;
+ [[deprecated(
+ "Replaced by vector<std::shared_ptr<StateInterface>> on_export_state_interfaces() method. "
+ "Exporting is "
+ "handled "
+ "by the Framework.")]] virtual std::vector<StateInterface>
+ export_state_interfaces()
+ {
+ // return empty vector by default. For backward compatibility we check if all vectors is empty
+ // and if so call on_export_state_interfaces()
+ std::vector<StateInterface> state_interfaces;
+ return state_interfaces;
+ }
+
+ std::vector<std::shared_ptr<StateInterface>> on_export_state_interfaces()
+ {
+ std::vector<std::shared_ptr<StateInterface>> state_interfaces;
+ state_interfaces.reserve(joint_state_interfaces_.size());
 
+ for (const auto & [name, descr] : joint_state_interfaces_)
+ {
+ actuator_states_.insert(std::make_pair(name, std::make_shared<StateInterface>(descr)));
+ state_interfaces.push_back(actuator_states_.at(name));
+ }
+ return state_interfaces;
+ }
  /// Exports all command interfaces for this hardware interface.
  /**
+ * Default implementation for exporting the CommandInterfaces. The CommandInterfaces are created
+ * according to the InterfaceDescription. The memory accessed by the controllers and hardware is
+ * assigned here and resides in the system_interface.
+ *
  * The command interfaces have to be created and transferred according
  * to the hardware info passed in for the configuration.
  *
  * Note the ownership over the state interfaces is transferred to the caller.
  *
  * \return vector of command interfaces
  */
- virtual std::vector<CommandInterface> export_command_interfaces() = 0;
+ [[deprecated(
+ "Replaced by vector<std::shared_ptr<CommandInterface>> on_export_command_interfaces() method. "
+ "Exporting is "
+ "handled "
+ "by the Framework.")]] virtual std::vector<CommandInterface>
+ export_command_interfaces()
+ {
+ // return empty vector by default. For backward compatibility we check if all vectors is empty
+ // and if so call on_export_command_interfaces()
+ std::vector<CommandInterface> command_interfaces;
+ return command_interfaces;
+ }
 
+ std::vector<std::shared_ptr<CommandInterface>> on_export_command_interfaces()
+ {
+ std::vector<std::shared_ptr<CommandInterface>> command_interfaces;
+ command_interfaces.reserve(joint_command_interfaces_.size());
+
+ for (const auto & [name, descr] : joint_command_interfaces_)
+ {
+ actuator_commands_.insert(std::make_pair(name, std::make_shared<CommandInterface>(descr)));
+ command_interfaces.push_back(actuator_commands_.at(name));
+ }
+
+ return command_interfaces;
+ }
  /// Prepare for a new command interface switch.
  /**
  * Prepare for any mode-switching required by the new command interface combination.
@@ -202,8 +292,35 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
  */
  void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; }
 
+ void set_state(const std::string & interface_name, const double & value)
+ {
+ actuator_states_.at(interface_name)->set_value(value);
+ }
+
+ double get_state(const std::string & interface_name) const
+ {
+ return actuator_states_.at(interface_name)->get_value();
+ }
+
+ void set_command(const std::string & interface_name, const double & value)
+ {
+ actuator_commands_.at(interface_name)->set_value(value);
+ }
+
+ double get_command(const std::string & interface_name) const
+ {
+ return actuator_commands_.at(interface_name)->get_value();
+ }
+
 protected:
  HardwareInfo info_;
+ std::map<std::string, InterfaceDescription> joint_state_interfaces_;
+ std::map<std::string, InterfaceDescription> joint_command_interfaces_;
+
+private:
+ std::map<std::string, std::shared_ptr<StateInterface>> actuator_states_;
+ std::map<std::string, std::shared_ptr<CommandInterface>> actuator_commands_;
+
  rclcpp_lifecycle::State lifecycle_state_;
 };
 

hardware_interface/include/hardware_interface/component_parser.hpp

@@ -33,6 +33,51 @@ namespace hardware_interface
 HARDWARE_INTERFACE_PUBLIC
 std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string & urdf);
 
+/**
+ * \param[in] hw_info the hardware description
+ * \return vector filled with information about robot's SommandInterfaces for the joints
+ * which are exported
+ */
+HARDWARE_INTERFACE_PUBLIC
+std::vector<InterfaceDescription> parse_joint_state_interface_descriptions_from_hardware_info(
+ const HardwareInfo & hw_info);
+
+/**
+ * \param[in] hw_info the hardware description
+ * \return vector filled with information about robot's SommandInterfaces for the sensors
+ * which are exported
+ */
+HARDWARE_INTERFACE_PUBLIC
+std::vector<InterfaceDescription> parse_sensor_state_interface_descriptions_from_hardware_info(
+ const HardwareInfo & hw_info);
+
+/**
+ * \param[in] hw_info the hardware description
+ * \return vector filled with information about robot's SommandInterfaces for the gpios
+ * which are exported
+ */
+HARDWARE_INTERFACE_PUBLIC
+std::vector<InterfaceDescription> parse_gpio_state_interface_descriptions_from_hardware_info(
+ const HardwareInfo & hw_info);
+
+/**
+ * \param[in] hw_info the hardware description
+ * \return vector filled with information about robot's CommandInterfaces for the joints
+ * which are exported
+ */
+HARDWARE_INTERFACE_PUBLIC
+std::vector<InterfaceDescription> parse_joint_command_interface_descriptions_from_hardware_info(
+ const HardwareInfo & hw_info);
+
+/**
+ * \param[in] hw_info the hardware description
+ * \return vector filled with information about robot's CommandInterfaces for the gpios
+ * which are exported
+ */
+HARDWARE_INTERFACE_PUBLIC
+std::vector<InterfaceDescription> parse_gpio_command_interface_descriptions_from_hardware_info(
+ const HardwareInfo & hw_info);
+
 HARDWARE_INTERFACE_PUBLIC
 bool parse_bool(const std::string & bool_string);