add changes for chainable controllers (epxport of rerference interfaces)

Author: mamueluth

controller_interface/include/controller_interface/chainable_controller_interface.hpp

@@ -15,7 +15,9 @@
 #ifndef CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_
 #define CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_
 
+#include <memory>
 #include <string>
+#include <unordered_map>
 #include <vector>
 
 #include "controller_interface/controller_interface_base.hpp"
@@ -60,7 +62,11 @@ class ChainableControllerInterface : public ControllerInterfaceBase
   std::vector<hardware_interface::StateInterface> export_state_interfaces() final;
 
   CONTROLLER_INTERFACE_PUBLIC
-  std::vector<hardware_interface::CommandInterface> export_reference_interfaces() final;
+  std::vector<hardware_interface::StateInterface> export_state_interfaces() final;
+
+  CONTROLLER_INTERFACE_PUBLIC
+  std::vector<std::shared_ptr<hardware_interface::CommandInterface>> export_reference_interfaces()
+    final;
 
   CONTROLLER_INTERFACE_PUBLIC
   bool set_chained_mode(bool chained_mode) final;
@@ -135,7 +141,11 @@ class ChainableControllerInterface : public ControllerInterfaceBase
 
   /// Storage of values for reference interfaces
   std::vector<std::string> exported_reference_interface_names_;
+  // BEGIN (Handle export change): for backward compatibility
   std::vector<double> reference_interfaces_;
+  // END
+  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

@@ -55,7 +55,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

@@ -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

controller_interface/src/chainable_controller_interface.cpp

@@ -68,29 +68,87 @@ ChainableControllerInterface::export_state_interfaces()
   return state_interfaces;
 }
 
-std::vector<hardware_interface::CommandInterface>
-ChainableControllerInterface::export_reference_interfaces()
+std::vector<std::shared_ptr<hardware_interface::CommandInterface>>
+ChainableControllerInterface::export_state_interfaces()
 {
-  auto reference_interfaces = on_export_reference_interfaces();
+  auto state_interfaces = on_export_state_interfaces();
 
-  // check if the names of the reference interfaces begin with the controller's name
-  for (const auto & interface : reference_interfaces)
+  // check if the names of the controller state interfaces begin with the controller's name
+  for (const auto & interface : state_interfaces)
   {
     if (interface.get_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.",
+        "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());
-      reference_interfaces.clear();
+      state_interfaces.clear();
       break;
     }
   }
 
-  return reference_interfaces;
+  return state_interfaces;
+}
+
+std::vector<std::shared_ptr<hardware_interface::CommandInterface>>
+ChainableControllerInterface::export_reference_interfaces()
+{
+  auto reference_interfaces = on_export_reference_interfaces();
+  std::vector<std::shared_ptr<hardware_interface::CommandInterface>> reference_interfaces_ptrs_vec;
+  reference_interfaces_ptrs_vec.reserve(reference_interfaces.size());
+
+  // BEGIN (Handle export change): for backward compatibility
+  // check if the "reference_interfaces_" variable is resized to number of interfaces
+  if (reference_interfaces_.size() != reference_interfaces.size())
+  {
+    // TODO(destogl): Should here be "FATAL"? It is fatal in terms of controller but not for the
+    // framework
+    std::string error_msg =
+      "The internal storage for reference values 'reference_interfaces_' variable has size '" +
+      std::to_string(reference_interfaces_.size()) + "', but it is expected to have the size '" +
+      std::to_string(reference_interfaces.size()) +
+      "' equal to the number of exported reference interfaces. Please correct and recompile the "
+      "controller with name '" +
+      get_node()->get_name() + "' and try again.";
+    throw std::runtime_error(error_msg);
+  }
+  // END
+
+  // check if the names of the reference interfaces begin with the controller's name
+  const auto ref_interface_size = reference_interfaces.size();
+  for (auto & interface : reference_interfaces)
+  {
+    if (interface.get_prefix_name() != get_node()->get_name())
+    {
+      std::string error_msg = "The name of the interface " + interface.get_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);
+    }
+
+    std::shared_ptr<hardware_interface::CommandInterface> interface_ptr =
+      std::make_shared<hardware_interface::CommandInterface>(std::move(interface));
+    reference_interfaces_ptrs_vec.push_back(interface_ptr);
+    reference_interfaces_ptrs_.insert(std::make_pair(interface_ptr->get_name(), interface_ptr));
+  }
+
+  if (reference_interfaces_ptrs_.size() != ref_interface_size)
+  {
+    std::string error_msg =
+      "The internal storage for reference ptrs 'reference_interfaces_ptrs_' variable has size '" +
+      std::to_string(reference_interfaces_ptrs_.size()) +
+      "', but it is expected to have the size '" + std::to_string(ref_interface_size) +
+      "' equal to the number of exported reference interfaces. Please correct and recompile the "
+      "controller with name '" +
+      get_node()->get_name() + "' and try again.";
+    throw std::runtime_error(error_msg);
+  }
+
+  return reference_interfaces_ptrs_vec;
 }
 
 bool ChainableControllerInterface::set_chained_mode(bool chained_mode)

controller_interface/src/controller_interface.cpp

@@ -27,7 +27,8 @@ std::vector<hardware_interface::StateInterface> ControllerInterface::export_stat
   return {};
 }
 
-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

@@ -47,11 +47,11 @@ TEST_F(ChainableControllerInterfaceTest, export_state_interfaces)
 
   auto exported_state_interfaces = controller.export_state_interfaces();
 
-  ASSERT_THAT(exported_state_interfaces, SizeIs(1));
-  EXPECT_EQ(exported_state_interfaces[0].get_prefix_name(), TEST_CONTROLLER_NAME);
-  EXPECT_EQ(exported_state_interfaces[0].get_interface_name(), "test_state");
+  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(exported_state_interfaces[0].get_value(), EXPORTED_STATE_INTERFACE_VALUE);
+  EXPECT_EQ(reference_interfaces[0]->get_value(), INTERFACE_VALUE);
 }
 
 TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces)
@@ -114,8 +114,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(exported_state_interfaces[0].get_value(), EXPORTED_STATE_INTERFACE_VALUE);
+  EXPECT_EQ(reference_interfaces[0]->get_value(), INTERFACE_VALUE);
 
   EXPECT_FALSE(controller.set_chained_mode(true));
   EXPECT_FALSE(controller.is_in_chained_mode());
@@ -124,7 +123,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_manager/src/controller_manager.cpp

@@ -752,14 +752,26 @@ controller_interface::return_type ControllerManager::configure_controller(
       "Controller '%s' is chainable. Interfaces are being exported to resource manager.",
       controller_name.c_str());
     auto state_interfaces = controller->export_state_interfaces();
-    auto ref_interfaces = controller->export_reference_interfaces();
-    if (ref_interfaces.empty() && state_interfaces.empty())
+    std::vector<std::shared_ptr<hardware_interface::CommandInterface>> interfaces;
+    try
     {
-      // TODO(destogl): Add test for this!
-      RCLCPP_ERROR(
-        get_logger(),
-        "Controller '%s' is chainable, but does not export any state or reference interfaces.",
-        controller_name.c_str());
+      interfaces = controller->export_reference_interfaces();
+      if (interfaces.empty() && state_interfaces.empty())
+      {
+        // TODO(destogl): Add test for this!
+        RCLCPP_ERROR(
+          get_logger(),
+          "Controller '%s' is chainable, but does not export any reference interfaces. Did you "
+          "override the on_export_method() correctly?",
+          controller_name.c_str());
+        return controller_interface::return_type::ERROR;
+      }
+    }
+    catch (const std::runtime_error & e)
+    {
+      RCLCPP_FATAL(
+        get_logger(), "Creation of the reference interfaces failed with following error: %s",
+        e.what());
       return controller_interface::return_type::ERROR;
     }
     resource_manager_->import_controller_reference_interfaces(controller_name, ref_interfaces);

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -194,7 +194,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
    * \param[in] interfaces list of controller's reference interfaces as CommandInterfaces.
    */
   void import_controller_reference_interfaces(
-    const std::string & controller_name, std::vector<CommandInterface> & interfaces);
+    const std::string & controller_name,
+    const std::vector<std::shared_ptr<hardware_interface::CommandInterface>> & interfaces);
 
   /// Get list of reference interface of a controller.
   /**

hardware_interface/src/resource_manager.cpp

@@ -642,7 +642,7 @@ class ResourceStorage
     }
   }
 
-  void insert_command_interface(std::shared_ptr<CommandInterface> command_interface)
+  void insert_command_interface(const std::shared_ptr<CommandInterface> command_interface)
   {
     const auto [it, success] = command_interface_map_.insert(
       std::make_pair(command_interface->get_name(), command_interface));
@@ -770,11 +770,11 @@ class ResourceStorage
   }
 
   std::vector<std::string> add_command_interfaces(
-    std::vector<std::shared_ptr<CommandInterface>> & interfaces)
+    const std::vector<std::shared_ptr<CommandInterface>> & interfaces)
   {
     std::vector<std::string> interface_names;
     interface_names.reserve(interfaces.size());
-    for (auto & interface : interfaces)
+    for (const auto & interface : interfaces)
     {
       auto key = interface->get_name();
       insert_command_interface(interface);
@@ -1305,7 +1305,8 @@ void ResourceManager::remove_controller_exported_state_interfaces(
 
 // CM API: Called in "callback/slow"-thread
 void ResourceManager::import_controller_reference_interfaces(
-  const std::string & controller_name, std::vector<CommandInterface> & interfaces)
+  const std::string & controller_name,
+  const std::vector<std::shared_ptr<hardware_interface::CommandInterface>> & interfaces)
 {
   std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_);
   auto interface_names = resource_storage_->add_command_interfaces(interfaces);

hardware_interface_testing/test/test_resource_manager.cpp

@@ -1230,12 +1230,12 @@ TEST_F(ResourceManagerTest, managing_controllers_reference_interfaces)
     CONTROLLER_NAME + "/" + REFERENCE_INTERFACE_NAMES[1],
     CONTROLLER_NAME + "/" + REFERENCE_INTERFACE_NAMES[2]};
 
-  std::vector<hardware_interface::CommandInterface> reference_interfaces;
+  std::vector<std::shared_ptr<hardware_interface::CommandInterface>> reference_interfaces;
   std::vector<double> reference_interface_values = {1.0, 2.0, 3.0};
 
   for (size_t i = 0; i < REFERENCE_INTERFACE_NAMES.size(); ++i)
   {
-    reference_interfaces.push_back(hardware_interface::CommandInterface(
+    reference_interfaces.push_back(std::make_shared<hardware_interface::CommandInterface>(
       CONTROLLER_NAME, REFERENCE_INTERFACE_NAMES[i], &(reference_interface_values[i])));
   }