Full variant support. not finished, showcase for what has to be changed

controller_interface/include/controller_interface/chainable_controller_interface.hpp

@@ -15,6 +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"
@@ -56,7 +60,8 @@ class ChainableControllerInterface : public ControllerInterfaceBase
   bool is_chainable() const 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;
@@ -114,8 +119,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 reference interfaces
-  std::vector<double> reference_interfaces_;
+  /// Storage of values for state interfaces
+  std::vector<std::string> exported_state_interface_names_;
+  // storage for the exported CommandInterfaces
+  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_;
+
+  // interface_names are in order they have been exported
+  std::vector<std::string> exported_reference_interface_names_;
+  // 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_ptrs_;
 
 private:
   /// A flag marking if a chainable controller is currently preceded by another controller.

controller_interface/include/controller_interface/controller_interface.hpp

@@ -48,7 +48,8 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase
    * \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

@@ -222,7 +222,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;
 
   /**
    * Set chained mode of a chainable controller. This method triggers internal processes to switch

controller_interface/src/chainable_controller_interface.cpp

@@ -44,44 +44,151 @@ return_type ChainableControllerInterface::update(
   return ret;
 }
 
-std::vector<hardware_interface::CommandInterface>
-ChainableControllerInterface::export_reference_interfaces()
+std::vector<std::shared_ptr<hardware_interface::StateInterface>>
+ChainableControllerInterface::export_state_interfaces()
 {
-  auto reference_interfaces = on_export_reference_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 (auto & descr : state_interfaces_descr)
+  {
+    if (descr.prefix_name != get_node()->get_name())
+    {
+      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.";
+      state_interfaces_ptrs_vec.clear();
+      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 if the "reference_interfaces_" variable is resized to number of interfaces
-  if (reference_interfaces_.size() != reference_interfaces.size())
+  // 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())
   {
-    // TODO(destogl): Should here be "FATAL"? It is fatal in terms of controller but not for the
-    // framework
-    RCLCPP_FATAL(
-      get_node()->get_logger(),
-      "The internal storage for reference values 'reference_interfaces_' variable has size '%zu', "
-      "but it is expected to have the size '%zu' equal to the number of exported reference "
-      "interfaces. No reference interface will be exported. Please correct and recompile "
-      "the controller with name '%s' and try again.",
-      reference_interfaces_.size(), reference_interfaces.size(), get_node()->get_name());
-    reference_interfaces.clear();
+    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()) +
+      ").";
+    throw std::runtime_error(error_msg);
   }
 
+  return state_interfaces_ptrs_vec;
+}
+
+std::vector<std::shared_ptr<hardware_interface::CommandInterface>>
+ChainableControllerInterface::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_ptrs_.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())
+    {
+      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)
     {
-      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 =
+        "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.";
+      reference_interfaces_ptrs_vec.clear();
+      exported_reference_interface_names_.clear();
+      ordered_reference_interfaces_.clear();
+      reference_interfaces_ptrs_.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_ptrs_.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_ptrs_ (" + std::to_string(reference_interfaces_ptrs_.size()) +
+      ").";
+    throw std::runtime_error(error_msg);
   }
 
-  return reference_interfaces;
+  return reference_interfaces_ptrs_vec;
 }
 
 bool ChainableControllerInterface::set_chained_mode(bool chained_mode)

controller_interface/src/controller_interface.cpp

@@ -28,7 +28,8 @@ ControllerInterface::ControllerInterface() : ControllerInterfaceBase() {}
 
 bool ControllerInterface::is_chainable() const { return false; }
 
-std::vector<hardware_interface::CommandInterface> ControllerInterface::export_reference_interfaces()
+std::vector<std::shared_ptr<hardware_interface::CommandInterface>>
+ControllerInterface::export_reference_interfaces()
 {
   return {};
 }

controller_interface/test/test_chainable_controller_interface.cpp

@@ -45,10 +45,10 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces)
   auto reference_interfaces = controller.export_reference_interfaces();
 
   ASSERT_EQ(reference_interfaces.size(), 1u);
-  EXPECT_EQ(reference_interfaces[0].get_prefix_name(), TEST_CONTROLLER_NAME);
-  EXPECT_EQ(reference_interfaces[0].get_interface_name(), "test_itf");
+  EXPECT_EQ(reference_interfaces[0]->get_prefix_name(), TEST_CONTROLLER_NAME);
+  EXPECT_EQ(reference_interfaces[0]->get_interface_name(), "test_itf");
 
-  EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE);
+  EXPECT_EQ(reference_interfaces[0]->get_value<double>(), INTERFACE_VALUE);
 }
 
 TEST_F(ChainableControllerInterfaceTest, reference_interfaces_storage_not_correct_size)
@@ -103,7 +103,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode)
   EXPECT_FALSE(controller.is_in_chained_mode());
 
   // Fail setting chained mode
-  EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE);
+  EXPECT_EQ(reference_interfaces[0]->get_value<double>(), INTERFACE_VALUE);
 
   EXPECT_FALSE(controller.set_chained_mode(true));
   EXPECT_FALSE(controller.is_in_chained_mode());
@@ -112,7 +112,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode)
   EXPECT_FALSE(controller.is_in_chained_mode());
 
   // Success setting chained mode
-  reference_interfaces[0].set_value(0.0);
+  reference_interfaces[0]->set_value(0.0);
 
   EXPECT_TRUE(controller.set_chained_mode(true));
   EXPECT_TRUE(controller.is_in_chained_mode());

controller_interface/test/test_chainable_controller_interface.hpp

@@ -22,6 +22,7 @@
 
 #include "controller_interface/chainable_controller_interface.hpp"
 #include "hardware_interface/handle.hpp"
+#include "hardware_interface/hardware_info.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 
 constexpr char TEST_CONTROLLER_NAME[] = "testable_chainable_controller";
@@ -30,18 +31,17 @@ constexpr double INTERFACE_VALUE_SUBSCRIBER_ERROR = 12345.0;
 constexpr double INTERFACE_VALUE_UPDATE_ERROR = 67890.0;
 constexpr double INTERFACE_VALUE_INITIAL_REF = 1984.0;
 
+using hardware_interface::InterfaceDescription;
+using hardware_interface::InterfaceInfo;
+
 class TestableChainableControllerInterface
 : public controller_interface::ChainableControllerInterface
 {
 public:
   FRIEND_TEST(ChainableControllerInterfaceTest, reference_interfaces_storage_not_correct_size);
   FRIEND_TEST(ChainableControllerInterfaceTest, test_update_logic);
 
-  TestableChainableControllerInterface()
-  {
-    reference_interfaces_.reserve(1);
-    reference_interfaces_.push_back(INTERFACE_VALUE);
-  }
+  TestableChainableControllerInterface() { update_ref_itf_full_name(); }
 
   controller_interface::CallbackReturn on_init() override
   {
@@ -68,15 +68,16 @@ class TestableChainableControllerInterface
   {
     std::vector<hardware_interface::CommandInterface> command_interfaces;
 
-    command_interfaces.push_back(hardware_interface::CommandInterface(
-      name_prefix_of_reference_interfaces_, "test_itf", &reference_interfaces_[0]));
+    command_interfaces.push_back(hardware_interface::CommandInterface(InterfaceDescription(
+      name_prefix_of_reference_interfaces_,
+      InterfaceInfo(ref_itf_name_, std::to_string(INTERFACE_VALUE), "double"))));
 
     return command_interfaces;
   }
 
   bool on_set_chained_mode(bool /*chained_mode*/) override
   {
-    if (reference_interfaces_[0] == 0.0)
+    if (reference_interfaces_ptrs_[ref_itf_full_name_]->get_value<double>() == 0.0)
     {
       return true;
     }
@@ -89,39 +90,52 @@ class TestableChainableControllerInterface
   controller_interface::return_type update_reference_from_subscribers(
     const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
   {
-    if (reference_interface_value_ == INTERFACE_VALUE_SUBSCRIBER_ERROR)
+    if (
+      reference_interfaces_ptrs_[ref_itf_full_name_]->get_value<double>() ==
+      INTERFACE_VALUE_SUBSCRIBER_ERROR)
     {
       return controller_interface::return_type::ERROR;
     }
 
-    reference_interfaces_[0] = reference_interface_value_;
+    reference_interfaces_ptrs_[ref_itf_full_name_]->set_value(reference_interface_value_);
     return controller_interface::return_type::OK;
   }
 
   controller_interface::return_type update_and_write_commands(
     const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
   {
-    if (reference_interfaces_[0] == INTERFACE_VALUE_UPDATE_ERROR)
+    if (
+      reference_interfaces_ptrs_[ref_itf_full_name_]->get_value<double>() ==
+      INTERFACE_VALUE_UPDATE_ERROR)
     {
       return controller_interface::return_type::ERROR;
     }
 
-    reference_interfaces_[0] -= 1;
+    reference_interfaces_ptrs_[ref_itf_full_name_]->operator-=(1);
 
     return controller_interface::return_type::OK;
   }
 
   void set_name_prefix_of_reference_interfaces(const std::string & prefix)
   {
     name_prefix_of_reference_interfaces_ = prefix;
+    update_ref_itf_full_name();
   }
 
   void set_new_reference_interface_value(const double ref_itf_value)
   {
     reference_interface_value_ = ref_itf_value;
+    update_ref_itf_full_name();
+  }
+
+  void update_ref_itf_full_name()
+  {
+    ref_itf_full_name_ = name_prefix_of_reference_interfaces_ + "/" + ref_itf_name_;
   }
 
   std::string name_prefix_of_reference_interfaces_;
+  std::string ref_itf_name_ = "test_itf";
+  std::string ref_itf_full_name_;
   double reference_interface_value_ = INTERFACE_VALUE_INITIAL_REF;
 };