! there is a bug with multiple cleanupcalls! add exporting for all co…

…mponents, creatons of loans

hardware_interface/include/hardware_interface/actuator.hpp

@@ -73,7 +73,14 @@ class Actuator final
  std::vector<CommandInterface> export_command_interfaces();
 
  HARDWARE_INTERFACE_PUBLIC
- LoanedCommandInterface create_loaned_command_interface(const std::string & interface_name);
+ std::vector<InterfaceDescription> export_state_interface_descriptions();
+
+ HARDWARE_INTERFACE_PUBLIC
+ std::vector<InterfaceDescription> export_command_interface_descriptions();
+
+ HARDWARE_INTERFACE_PUBLIC
+ LoanedCommandInterface create_loaned_command_interface(
+ const std::string & interface_name, std::function<void(void)> && release_callback);
 
  HARDWARE_INTERFACE_PUBLIC
  LoanedStateInterface create_loaned_state_interface(const std::string & interface_name);

hardware_interface/include/hardware_interface/actuator_interface.hpp

@@ -18,6 +18,7 @@
 #include <map>
 #include <memory>
 #include <string>
+#include <utility>
 #include <vector>
 
 #include "hardware_interface/handle.hpp"
@@ -125,9 +126,30 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
  */
  virtual std::vector<CommandInterface> export_command_interfaces() = 0;
 
- virtual LoanedCommandInterface create_loaned_command_interface(const std::string & interface_name)
+ /**
+ * Exports all information about the available StateInterfaces for this hardware interface.
+ *
+ * \return vector of InterfaceDescription
+ */
+ virtual std::vector<InterfaceDescription> export_state_interface_descriptions() const
+ {
+ return joint_states_descriptions_;
+ }
+
+ /**
+ * Exports all information about the available CommandInterfaces for this hardware interface.
+ *
+ * \return vector of InterfaceDescription
+ */
+ virtual std::vector<InterfaceDescription> export_command_interface_descriptions() const
+ {
+ return joint_commands_descriptions_;
+ }
+
+ virtual LoanedCommandInterface create_loaned_command_interface(
+ const std::string & interface_name, std::function<void(void)> && release_callback)
  {
- return LoanedCommandInterface(joint_commands_.at(interface_name));
+ return LoanedCommandInterface(joint_commands_.at(interface_name), std::move(release_callback));
  }
 
  virtual LoanedStateInterface create_loaned_state_interface(const std::string & interface_name)
@@ -215,6 +237,26 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
  */
  void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; }
 
+ double joint_state_get_value(const InterfaceDescription & interface_descr) const
+ {
+ return joint_states_.at(interface_descr.get_name()).get_value();
+ }
+
+ void joint_state_set_value(const InterfaceDescription & interface_descr, const double & value)
+ {
+ joint_states_.at(interface_descr.get_name()).set_value(value);
+ }
+
+ double joint_command_get_value(const InterfaceDescription & interface_descr) const
+ {
+ return joint_commands_.at(interface_descr.get_name()).get_value();
+ }
+
+ void joint_command_set_value(const InterfaceDescription & interface_descr, const double & value)
+ {
+ joint_commands_.at(interface_descr.get_name()).set_value(value);
+ }
+
 protected:
  HardwareInfo info_;
  std::vector<InterfaceDescription> joint_states_descriptions_;

hardware_interface/include/hardware_interface/hardware_info.hpp

@@ -114,9 +114,9 @@ struct InterfaceDescription
  const InterfaceInfo & interface_info_in)
  : prefix_name(prefix_name_in),
  component_type(component_type_in),
- interface_info(interface_info_in)
+ interface_info(interface_info_in),
+ name(get_prefix_name() + "/" + get_interface_type())
  {
- name = get_prefix_name() + "/" + get_interface_type();
  }
 
  /**
@@ -130,22 +130,22 @@ struct InterfaceDescription
  std::string component_type;
 
  /**
- *  Full qualified name of the Interface
+ * Information about the Interface type (position, velocity,...) as well as limits and so on.
  */
- std::string name;
+ InterfaceInfo interface_info;
 
  /**
- * Information about the Interface type (position, velocity,...) as well as limits and so on.
+ *  Full qualified name of the Interface
  */
- InterfaceInfo interface_info;
+ std::string name;
 
- const std::string & get_prefix_name() const { return prefix_name; }
+ std::string get_prefix_name() const { return prefix_name; }
 
- const std::string & get_component_type() const { return component_type; }
+ std::string get_component_type() const { return component_type; }
 
- const std::string & get_name() const { return name; }
+ std::string get_name() const { return name; }
 
- const std::string & get_interface_type() const { return 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.

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -99,7 +99,9 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
  */
  bool is_urdf_already_loaded() const;
 
- bool component_creates_loaned_state(const std::string & key);
+ bool component_creates_loaned_state_interface(const std::string & key);
+
+ bool component_creates_loaned_command_interface(const std::string & key);
 
  /// Claim a state interface given its key.
  /**

hardware_interface/include/hardware_interface/sensor.hpp

@@ -69,6 +69,9 @@ class Sensor final
  HARDWARE_INTERFACE_PUBLIC
  std::vector<StateInterface> export_state_interfaces();
 
+ HARDWARE_INTERFACE_PUBLIC
+ std::vector<InterfaceDescription> export_state_interface_descriptions();
+
  HARDWARE_INTERFACE_PUBLIC
  LoanedStateInterface create_loaned_state_interface(const std::string & interface_name);
 

hardware_interface/include/hardware_interface/sensor_interface.hpp

@@ -113,6 +113,16 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
  */
  virtual std::vector<StateInterface> export_state_interfaces() = 0;
 
+ /**
+ * Exports all information about the available StateInterfaces for this hardware interface.
+ *
+ * \return vector of InterfaceDescription
+ */
+ virtual std::vector<InterfaceDescription> export_state_interface_descriptions() const
+ {
+ return sensor_states_descriptions_;
+ }
+
  virtual LoanedStateInterface create_loaned_state_interface(const std::string & interface_name)
  {
  return LoanedStateInterface(sensor_states_.at(interface_name));
@@ -148,6 +158,16 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
  */
  void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; }
 
+ double sensor_state_get_value(const InterfaceDescription & interface_descr) const
+ {
+ return sensor_states_.at(interface_descr.get_name()).get_value();
+ }
+
+ void sensor_state_set_value(const InterfaceDescription & interface_descr, const double & value)
+ {
+ sensor_states_.at(interface_descr.get_name()).set_value(value);
+ }
+
 protected:
  HardwareInfo info_;
  std::vector<InterfaceDescription> sensor_states_descriptions_;

hardware_interface/include/hardware_interface/system.hpp

@@ -80,7 +80,8 @@ class System final
  std::vector<InterfaceDescription> export_command_interface_descriptions();
 
  HARDWARE_INTERFACE_PUBLIC
- LoanedCommandInterface create_loaned_command_interface(const std::string & interface_name);
+ LoanedCommandInterface create_loaned_command_interface(
+ const std::string & interface_name, std::function<void(void)> && release_callback);
 
  HARDWARE_INTERFACE_PUBLIC
  LoanedStateInterface create_loaned_state_interface(const std::string & interface_name);

hardware_interface/include/hardware_interface/system_interface.hpp

@@ -158,56 +158,6 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
  }
  }
 
- /**
- * Exports all information about the available StateInterfaces for this hardware interface.
- *
- * \return vector of InterfaceDescription
- */
- virtual std::vector<InterfaceDescription> export_state_interface_descriptions() const
- {
- std::vector<InterfaceDescription> state_interface_descriptions;
- state_interface_descriptions.reserve(
- joint_states_descriptions_.size() + sensor_states_descriptions_.size() +
- gpio_states_descriptions_.size());
-
- // append InterfaceDescription of joint_states
- state_interface_descriptions.insert(
- state_interface_descriptions.end(), joint_states_descriptions_.begin(),
- joint_states_descriptions_.end());
- // append InterfaceDescription of sensor_states
- state_interface_descriptions.insert(
- state_interface_descriptions.end(), sensor_states_descriptions_.begin(),
- sensor_states_descriptions_.end());
- // append InterfaceDescription of gpio_states
- state_interface_descriptions.insert(
- state_interface_descriptions.end(), gpio_states_descriptions_.begin(),
- gpio_states_descriptions_.end());
-
- return state_interface_descriptions;
- }
-
- /**
- * Exports all information about the available CommandInterfaces for this hardware interface.
- *
- * \return vector of InterfaceDescription
- */
- virtual std::vector<InterfaceDescription> export_command_interface_descriptions() const
- {
- std::vector<InterfaceDescription> command_interface_descriptions;
- command_interface_descriptions.reserve(
- joint_commands_descriptions_.size() + gpio_commands_descriptions_.size());
-
- // append InterfaceDescription of joint_commands
- command_interface_descriptions.insert(
- command_interface_descriptions.end(), joint_commands_descriptions_.begin(),
- joint_commands_descriptions_.end());
- // append InterfaceDescription of gpio_commands
- command_interface_descriptions.insert(
- command_interface_descriptions.end(), gpio_commands_descriptions_.begin(),
- gpio_commands_descriptions_.end());
-
- return command_interface_descriptions;
- }
  /// Exports all state interfaces for this hardware interface.
  /**
  * The state interfaces have to be created and transferred according
@@ -267,9 +217,61 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
  return command_interfaces;
  }
 
- virtual LoanedCommandInterface create_loaned_command_interface(const std::string & interface_name)
+ /**
+ * Exports all information about the available StateInterfaces for this hardware interface.
+ *
+ * \return vector of InterfaceDescription
+ */
+ virtual std::vector<InterfaceDescription> export_state_interface_descriptions() const
+ {
+ std::vector<InterfaceDescription> state_interface_descriptions;
+ state_interface_descriptions.reserve(
+ joint_states_descriptions_.size() + sensor_states_descriptions_.size() +
+ gpio_states_descriptions_.size());
+
+ // append InterfaceDescription of joint_states
+ state_interface_descriptions.insert(
+ state_interface_descriptions.end(), joint_states_descriptions_.begin(),
+ joint_states_descriptions_.end());
+ // append InterfaceDescription of sensor_states
+ state_interface_descriptions.insert(
+ state_interface_descriptions.end(), sensor_states_descriptions_.begin(),
+ sensor_states_descriptions_.end());
+ // append InterfaceDescription of gpio_states
+ state_interface_descriptions.insert(
+ state_interface_descriptions.end(), gpio_states_descriptions_.begin(),
+ gpio_states_descriptions_.end());
+
+ return state_interface_descriptions;
+ }
+
+ /**
+ * Exports all information about the available CommandInterfaces for this hardware interface.
+ *
+ * \return vector of InterfaceDescription
+ */
+ virtual std::vector<InterfaceDescription> export_command_interface_descriptions() const
+ {
+ std::vector<InterfaceDescription> command_interface_descriptions;
+ command_interface_descriptions.reserve(
+ joint_commands_descriptions_.size() + gpio_commands_descriptions_.size());
+
+ // append InterfaceDescription of joint_commands
+ command_interface_descriptions.insert(
+ command_interface_descriptions.end(), joint_commands_descriptions_.begin(),
+ joint_commands_descriptions_.end());
+ // append InterfaceDescription of gpio_commands
+ command_interface_descriptions.insert(
+ command_interface_descriptions.end(), gpio_commands_descriptions_.begin(),
+ gpio_commands_descriptions_.end());
+
+ return command_interface_descriptions;
+ }
+
+ virtual LoanedCommandInterface create_loaned_command_interface(
+ const std::string & interface_name, std::function<void(void)> && release_callback)
  {
- return LoanedCommandInterface(joint_commands_.at(interface_name));
+ return LoanedCommandInterface(joint_commands_.at(interface_name), std::move(release_callback));
  }
 
  virtual LoanedStateInterface create_loaned_state_interface(const std::string & interface_name)
@@ -316,7 +318,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
  return return_type::OK;
  }
 
- /// Read the current state values from the actuator.
+ /// Read the current state values from the system.
  /**
  * The data readings from the physical hardware has to be updated
  * and reflected accordingly in the exported state interfaces.
@@ -328,7 +330,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
  */
  virtual return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) = 0;
 
- /// Write the current command values to the actuator.
+ /// Write the current command values to the system.
  /**
  * The physical hardware shall be updated with the latest value from
  * the exported command interfaces.
@@ -339,19 +341,19 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
  */
  virtual return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) = 0;
 
- /// Get name of the actuator hardware.
+ /// Get name of the system hardware.
  /**
  * \return name.
  */
  virtual std::string get_name() const { return info_.name; }
 
- /// Get life-cycle state of the actuator hardware.
+ /// Get life-cycle state of the system hardware.
  /**
  * \return state.
  */
  const rclcpp_lifecycle::State & get_state() const { return lifecycle_state_; }
 
- /// Set life-cycle state of the actuator hardware.
+ /// Set life-cycle state of the system hardware.
  /**
  * \return state.
  */

hardware_interface/src/actuator.cpp

@@ -198,9 +198,20 @@ std::vector<CommandInterface> Actuator::export_command_interfaces()
  return impl_->export_command_interfaces();
 }
 
-LoanedCommandInterface Actuator::create_loaned_command_interface(const std::string & interface_name)
+std::vector<InterfaceDescription> Actuator::export_state_interface_descriptions()
 {
- return impl_->create_loaned_command_interface(interface_name);
+ return impl_->export_state_interface_descriptions();
+}
+
+std::vector<InterfaceDescription> Actuator::export_command_interface_descriptions()
+{
+ return impl_->export_command_interface_descriptions();
+}
+
+LoanedCommandInterface Actuator::create_loaned_command_interface(
+ const std::string & interface_name, std::function<void(void)> && release_callback)
+{
+ return impl_->create_loaned_command_interface(interface_name, std::move(release_callback));
 }
 
 LoanedStateInterface Actuator::create_loaned_state_interface(const std::string & interface_name)