Final variant support

controller_interface/include/controller_interface/chainable_controller_interface.hpp

@@ -15,7 +15,10 @@
 #ifndef CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_
 #define CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_
 
+#include <functional>
+#include <memory>
 #include <string>
+#include <unordered_map>
 #include <vector>
 
 #include "controller_interface/controller_interface_base.hpp"
@@ -57,10 +60,11 @@ class ChainableControllerInterface : public ControllerInterfaceBase
   bool is_chainable() const final;
 
   CONTROLLER_INTERFACE_PUBLIC
-  std::vector<hardware_interface::StateInterface> export_state_interfaces() final;
+  std::vector<std::shared_ptr<hardware_interface::StateInterface>> export_state_interfaces() final;
 
   CONTROLLER_INTERFACE_PUBLIC
-  std::vector<hardware_interface::CommandInterface> export_reference_interfaces() final;
+  std::vector<std::shared_ptr<hardware_interface::CommandInterface>> export_reference_interfaces()
+    final;
 
   CONTROLLER_INTERFACE_PUBLIC
   bool set_chained_mode(bool chained_mode) final;
@@ -76,9 +80,10 @@ class ChainableControllerInterface : public ControllerInterfaceBase
    * exported. The method has the same meaning as `export_state_interfaces` method from
    * hardware_interface::SystemInterface or hardware_interface::ActuatorInterface.
    *
-   * \returns list of StateInterfaces that other controller can use as their inputs.
+   * \returns list of StateInterfacesDescriptions that other controller can use as their inputs.
    */
-  virtual std::vector<hardware_interface::StateInterface> on_export_state_interfaces();
+  virtual std::vector<hardware_interface::InterfaceDescription>
+  export_state_interface_descriptions() = 0;
 
   /// Virtual method that each chainable controller should implement to export its read/write
   /// chainable interfaces.
@@ -87,9 +92,10 @@ class ChainableControllerInterface : public ControllerInterfaceBase
    * exported. The method has the same meaning as `export_command_interface` method from
    * hardware_interface::SystemInterface or hardware_interface::ActuatorInterface.
    *
-   * \returns list of CommandInterfaces that other controller can use as their outputs.
+   * \returns list of CommandInterfacesDescriptions that other controller can use as their outputs.
    */
-  virtual std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces();
+  virtual std::vector<hardware_interface::InterfaceDescription>
+  export_reference_interface_descriptions() = 0;
 
   /// Virtual method that each chainable controller should implement to switch chained mode.
   /**
@@ -129,13 +135,20 @@ class ChainableControllerInterface : public ControllerInterfaceBase
   virtual return_type update_and_write_commands(
     const rclcpp::Time & time, const rclcpp::Duration & period) = 0;
 
-  /// Storage of values for state interfaces
+  // interface_names are in order they have been exported
   std::vector<std::string> exported_state_interface_names_;
-  std::vector<double> state_interfaces_values_;
+  // storage for the exported StateInterfaces
+  std::vector<std::shared_ptr<hardware_interface::StateInterface>>
+    ordered_exported_state_interfaces_;
+  std::unordered_map<std::string, std::shared_ptr<hardware_interface::StateInterface>>
+    exported_state_interfaces_;
 
-  /// Storage of values for reference interfaces
+  // interface_names are in order they have been exported
   std::vector<std::string> exported_reference_interface_names_;
-  std::vector<double> reference_interfaces_;
+  // storage for the exported CommandInterfaces
+  std::vector<std::shared_ptr<hardware_interface::CommandInterface>> ordered_reference_interfaces_;
+  std::unordered_map<std::string, std::shared_ptr<hardware_interface::CommandInterface>>
+    reference_interfaces_;
 
 private:
   /// A flag marking if a chainable controller is currently preceded by another controller.

controller_interface/include/controller_interface/controller_interface.hpp

@@ -47,15 +47,16 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase
    * \returns empty list.
    */
   CONTROLLER_INTERFACE_PUBLIC
-  std::vector<hardware_interface::StateInterface> export_state_interfaces() final;
+  std::vector<std::shared_ptr<hardware_interface::StateInterface>> export_state_interfaces() final;
 
   /**
    * Controller has no reference interfaces.
    *
    * \returns empty list.
    */
   CONTROLLER_INTERFACE_PUBLIC
-  std::vector<hardware_interface::CommandInterface> export_reference_interfaces() final;
+  std::vector<std::shared_ptr<hardware_interface::CommandInterface>> export_reference_interfaces()
+    final;
 
   /**
    * Controller is not chainable, therefore no chained mode can be set.

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -221,7 +221,8 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
    * \returns list of command interfaces for preceding controllers.
    */
   CONTROLLER_INTERFACE_PUBLIC
-  virtual std::vector<hardware_interface::CommandInterface> export_reference_interfaces() = 0;
+  virtual std::vector<std::shared_ptr<hardware_interface::CommandInterface>>
+  export_reference_interfaces() = 0;
 
   /**
    * Export interfaces for a chainable controller that can be used as state interface by other
@@ -230,7 +231,8 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
    * \returns list of state interfaces for preceding controllers.
    */
   CONTROLLER_INTERFACE_PUBLIC
-  virtual std::vector<hardware_interface::StateInterface> export_state_interfaces() = 0;
+  virtual std::vector<std::shared_ptr<hardware_interface::StateInterface>>
+  export_state_interfaces() = 0;
 
   /**
    * Set chained mode of a chainable controller. This method triggers internal processes to switch

controller_interface/include/semantic_components/force_torque_sensor.hpp

@@ -102,7 +102,7 @@ class ForceTorqueSensor : public SemanticComponentInterface<geometry_msgs::msg::
     {
       if (existing_axes_[i])
       {
-        forces_[i] = state_interfaces_[interface_counter].get().get_value();
+        forces_[i] = state_interfaces_[interface_counter].get().get_value<double>();
         ++interface_counter;
       }
     }
@@ -126,7 +126,7 @@ class ForceTorqueSensor : public SemanticComponentInterface<geometry_msgs::msg::
     {
       if (existing_axes_[i])
       {
-        torques_[i - 3] = state_interfaces_[torque_interface_counter].get().get_value();
+        torques_[i - 3] = state_interfaces_[torque_interface_counter].get().get_value<double>();
         ++torque_interface_counter;
       }
     }

controller_interface/include/semantic_components/imu_sensor.hpp

@@ -59,7 +59,7 @@ class IMUSensor : public SemanticComponentInterface<sensor_msgs::msg::Imu>
     size_t interface_offset = 0;
     for (size_t i = 0; i < orientation_.size(); ++i)
     {
-      orientation_[i] = state_interfaces_[interface_offset + i].get().get_value();
+      orientation_[i] = state_interfaces_[interface_offset + i].get().get_value<double>();
     }
     return orientation_;
   }
@@ -75,7 +75,7 @@ class IMUSensor : public SemanticComponentInterface<sensor_msgs::msg::Imu>
     size_t interface_offset = orientation_.size();
     for (size_t i = 0; i < angular_velocity_.size(); ++i)
     {
-      angular_velocity_[i] = state_interfaces_[interface_offset + i].get().get_value();
+      angular_velocity_[i] = state_interfaces_[interface_offset + i].get().get_value<double>();
     }
     return angular_velocity_;
   }
@@ -91,7 +91,7 @@ class IMUSensor : public SemanticComponentInterface<sensor_msgs::msg::Imu>
     size_t interface_offset = orientation_.size() + angular_velocity_.size();
     for (size_t i = 0; i < linear_acceleration_.size(); ++i)
     {
-      linear_acceleration_[i] = state_interfaces_[interface_offset + i].get().get_value();
+      linear_acceleration_[i] = state_interfaces_[interface_offset + i].get().get_value<double>();
     }
     return linear_acceleration_;
   }

controller_interface/include/semantic_components/range_sensor.hpp

@@ -38,7 +38,7 @@ class RangeSensor : public SemanticComponentInterface<sensor_msgs::msg::Range>
    *
    * \return value of the range in meters
    */
-  float get_range() { return state_interfaces_[0].get().get_value(); }
+  float get_range() { return state_interfaces_[0].get().get_value<double>(); }
 
   /// Return Range message with range in meters
   /**

controller_interface/include/semantic_components/semantic_component_interface.hpp

@@ -87,7 +87,7 @@ class SemanticComponentInterface
     // insert all the values
     for (size_t i = 0; i < state_interfaces_.size(); ++i)
     {
-      values.emplace_back(state_interfaces_[i].get().get_value());
+      values.emplace_back(state_interfaces_[i].get().get_value<double>());
     }
     return true;
   }

controller_interface/src/chainable_controller_interface.cpp

@@ -44,53 +44,154 @@ return_type ChainableControllerInterface::update(
   return ret;
 }
 
-std::vector<hardware_interface::StateInterface>
+std::vector<std::shared_ptr<hardware_interface::StateInterface>>
 ChainableControllerInterface::export_state_interfaces()
 {
-  auto state_interfaces = on_export_state_interfaces();
+  auto state_interfaces_descr = export_state_interface_descriptions();
+  std::vector<std::shared_ptr<hardware_interface::StateInterface>> state_interfaces_ptrs_vec;
+  state_interfaces_ptrs_vec.reserve(state_interfaces_descr.size());
+  exported_state_interface_names_.reserve(state_interfaces_descr.size());
+  ordered_exported_state_interfaces_.reserve(state_interfaces_descr.size());
+  exported_state_interfaces_.reserve(state_interfaces_descr.size());
 
   // check if the names of the controller state interfaces begin with the controller's name
-  for (const auto & interface : state_interfaces)
+  for (auto & descr : state_interfaces_descr)
   {
-    if (interface.get_prefix_name() != get_node()->get_name())
+    if (descr.prefix_name != get_node()->get_name())
     {
-      RCLCPP_FATAL(
-        get_node()->get_logger(),
-        "The name of the interface '%s' does not begin with the controller's name. This is "
-        "mandatory for state interfaces. No state interface will be exported. Please "
-        "correct and recompile the controller with name '%s' and try again.",
-        interface.get_name().c_str(), get_node()->get_name());
-      state_interfaces.clear();
-      break;
+      std::string error_msg =
+        "The prefix of the interface description'" + descr.prefix_name +
+        "' does not equal the controller's name '" + get_node()->get_name() +
+        "'. This is mandatory for state interfaces. No state interface will be exported. Please "
+        "correct and recompile the controller with name '" +
+        get_node()->get_name() + "' and try again.";
+      throw std::runtime_error(error_msg);
     }
+
+    auto state_interface = std::make_shared<hardware_interface::StateInterface>(descr);
+    const auto inteface_name = state_interface->get_name();
+    // check the exported interface name is unique
+    auto [it, succ] = exported_state_interfaces_.insert({inteface_name, state_interface});
+    // either we have name duplicate which we want to avoid under all circumstances since interfaces
+    // need to be uniquely identify able or something else really went wrong. In any case abort and
+    // inform cm by throwing exception
+    if (!succ)
+    {
+      std::string error_msg =
+        "Could not insert StateInterface<" + inteface_name +
+        "> into exported_state_interfaces_ map. Check if you export duplicates. The "
+        "map returned iterator with interface_name<" +
+        it->second->get_name() +
+        ">. If its a duplicate adjust exportation of InterfacesDescription so that all the "
+        "interface names are unique.";
+      exported_state_interface_names_.clear();
+      ordered_exported_state_interfaces_.clear();
+      exported_state_interfaces_.clear();
+      throw std::runtime_error(error_msg);
+    }
+    state_interfaces_ptrs_vec.push_back(state_interface);
+    exported_state_interface_names_.push_back(inteface_name);
+    ordered_exported_state_interfaces_.push_back(state_interface);
+  }
+
+  // check that all are equal
+  if (
+    state_interfaces_descr.size() != state_interfaces_ptrs_vec.size() ||
+    state_interfaces_descr.size() != exported_state_interface_names_.size() ||
+    state_interfaces_descr.size() != ordered_exported_state_interfaces_.size() ||
+    state_interfaces_descr.size() != exported_state_interfaces_.size())
+  {
+    std::string error_msg =
+      "The size of the exported StateInterfaceDescriptions (" +
+      std::to_string(state_interfaces_descr.size()) + ") of controller <" + get_node()->get_name() +
+      "> does not match with the size of one of the following: state_interfaces_ptrs_vec (" +
+      std::to_string(state_interfaces_ptrs_vec.size()) + "), exported_state_interface_names_ (" +
+      std::to_string(exported_state_interface_names_.size()) +
+      "), ordered_exported_state_interfaces_ (" +
+      std::to_string(ordered_exported_state_interfaces_.size()) +
+      ") or exported_state_interfaces_ (" + std::to_string(exported_state_interfaces_.size()) +
+      ").";
+    exported_state_interface_names_.clear();
+    ordered_exported_state_interfaces_.clear();
+    exported_state_interfaces_.clear();
+    throw std::runtime_error(error_msg);
   }
 
-  return state_interfaces;
+  return state_interfaces_ptrs_vec;
 }
 
-std::vector<hardware_interface::CommandInterface>
+std::vector<std::shared_ptr<hardware_interface::CommandInterface>>
 ChainableControllerInterface::export_reference_interfaces()
 {
-  auto reference_interfaces = on_export_reference_interfaces();
+  auto reference_interface_descr = export_reference_interface_descriptions();
+  std::vector<std::shared_ptr<hardware_interface::CommandInterface>> reference_interfaces_ptrs_vec;
+  reference_interfaces_ptrs_vec.reserve(reference_interface_descr.size());
+  exported_reference_interface_names_.reserve(reference_interface_descr.size());
+  ordered_reference_interfaces_.reserve(reference_interface_descr.size());
+  reference_interfaces_.reserve(reference_interface_descr.size());
 
   // check if the names of the reference interfaces begin with the controller's name
-  for (const auto & interface : reference_interfaces)
+  for (auto & descr : reference_interface_descr)
   {
-    if (interface.get_prefix_name() != get_node()->get_name())
+    if (descr.prefix_name != get_node()->get_name())
     {
-      RCLCPP_FATAL(
-        get_node()->get_logger(),
-        "The name of the interface '%s' does not begin with the controller's name. This is "
-        "mandatory "
-        " for reference interfaces. No reference interface will be exported. Please correct and "
-        "recompile the controller with name '%s' and try again.",
-        interface.get_name().c_str(), get_node()->get_name());
-      reference_interfaces.clear();
-      break;
+      std::string error_msg = "The name of the interface descr " + descr.prefix_name +
+                              " does not begin with the controller's name. This is mandatory for "
+                              "reference interfaces. Please "
+                              "correct and recompile the controller with name " +
+                              get_node()->get_name() + " and try again.";
+      throw std::runtime_error(error_msg);
     }
+
+    auto reference_interface = std::make_shared<hardware_interface::CommandInterface>(descr);
+    const auto inteface_name = reference_interface->get_name();
+    // check the exported interface name is unique
+    auto [it, succ] = reference_interfaces_.insert({inteface_name, reference_interface});
+    // either we have name duplicate which we want to avoid under all circumstances since interfaces
+    // need to be uniquely identify able or something else really went wrong. In any case abort and
+    // inform cm by throwing exception
+    if (!succ)
+    {
+      std::string error_msg =
+        "Could not insert Reference interface<" + inteface_name +
+        "> into reference_interfaces_ map. Check if you export duplicates. The "
+        "map returned iterator with interface_name<" +
+        it->second->get_name() +
+        ">. If its a duplicate adjust exportation of InterfacesDescription so that all the "
+        "interface names are unique.";
+      exported_reference_interface_names_.clear();
+      ordered_reference_interfaces_.clear();
+      reference_interfaces_.clear();
+      throw std::runtime_error(error_msg);
+    }
+    reference_interfaces_ptrs_vec.push_back(reference_interface);
+    exported_reference_interface_names_.push_back(inteface_name);
+    ordered_reference_interfaces_.push_back(reference_interface);
+  }
+
+  if (
+    reference_interface_descr.size() != reference_interfaces_ptrs_vec.size() ||
+    reference_interface_descr.size() != exported_reference_interface_names_.size() ||
+    reference_interface_descr.size() != ordered_reference_interfaces_.size() ||
+    reference_interface_descr.size() != reference_interfaces_.size())
+  {
+    std::string error_msg =
+      "The size of the exported ReferenceInterfaceDescriptions (" +
+      std::to_string(reference_interface_descr.size()) + ") of controller <" +
+      get_node()->get_name() +
+      "> does not match with the size of one of the following: reference_interfaces_ptrs_vec (" +
+      std::to_string(reference_interfaces_ptrs_vec.size()) +
+      "), exported_reference_interface_names_ (" +
+      std::to_string(exported_reference_interface_names_.size()) +
+      "), ordered_reference_interfaces_ (" + std::to_string(ordered_reference_interfaces_.size()) +
+      ") or reference_interfaces_ (" + std::to_string(reference_interfaces_.size()) + ").";
+    exported_reference_interface_names_.clear();
+    ordered_reference_interfaces_.clear();
+    reference_interfaces_.clear();
+    throw std::runtime_error(error_msg);
   }
 
-  return reference_interfaces;
+  return reference_interfaces_ptrs_vec;
 }
 
 bool ChainableControllerInterface::set_chained_mode(bool chained_mode)
@@ -122,30 +223,4 @@ bool ChainableControllerInterface::is_in_chained_mode() const { return in_chaine
 
 bool ChainableControllerInterface::on_set_chained_mode(bool /*chained_mode*/) { return true; }
 
-std::vector<hardware_interface::StateInterface>
-ChainableControllerInterface::on_export_state_interfaces()
-{
-  state_interfaces_values_.resize(exported_state_interface_names_.size(), 0.0);
-  std::vector<hardware_interface::StateInterface> state_interfaces;
-  for (size_t i = 0; i < exported_state_interface_names_.size(); ++i)
-  {
-    state_interfaces.emplace_back(hardware_interface::StateInterface(
-      get_node()->get_name(), exported_state_interface_names_[i], &state_interfaces_values_[i]));
-  }
-  return state_interfaces;
-}
-
-std::vector<hardware_interface::CommandInterface>
-ChainableControllerInterface::on_export_reference_interfaces()
-{
-  reference_interfaces_.resize(exported_reference_interface_names_.size(), 0.0);
-  std::vector<hardware_interface::CommandInterface> reference_interfaces;
-  for (size_t i = 0; i < exported_reference_interface_names_.size(); ++i)
-  {
-    reference_interfaces.emplace_back(hardware_interface::CommandInterface(
-      get_node()->get_name(), exported_reference_interface_names_[i], &reference_interfaces_[i]));
-  }
-  return reference_interfaces;
-}
-
 }  // namespace controller_interface