adapt controller interfaces for distributed chained control

Author: mamueluth

controller_interface/include/controller_interface/chainable_controller_interface.hpp

@@ -57,6 +57,10 @@ class ChainableControllerInterface : public ControllerInterfaceBase
   CONTROLLER_INTERFACE_PUBLIC
   std::vector<hardware_interface::CommandInterface> export_reference_interfaces() final;
 
+  CONTROLLER_INTERFACE_PUBLIC
+  std::vector<hardware_interface::DistributedCommandInterface>
+  export_distributed_reference_interfaces() final;
+
   CONTROLLER_INTERFACE_PUBLIC
   bool set_chained_mode(bool chained_mode) final;
 
@@ -75,6 +79,15 @@ class ChainableControllerInterface : public ControllerInterfaceBase
    */
   virtual std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() = 0;
 
+  // Only for fast poc should be pure virtual
+  /**
+   * Default returns emtpy list, so that not every chainable controller has to be updated for poc.
+   *
+   * \returns empty list.
+   */
+  virtual std::vector<hardware_interface::DistributedCommandInterface>
+  on_export_distributed_reference_interfaces();
+
   /// Virtual method that each chainable controller should implement to switch chained mode.
   /**
    * Each chainable controller implements this methods to switch between "chained" and "external"

controller_interface/include/controller_interface/controller_interface.hpp

@@ -43,13 +43,22 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase
   bool is_chainable() const final;
 
   /**
-   * Controller has no reference interfaces.
+   * Controller has no distributed reference interfaces.
    *
    * \returns empty list.
    */
   CONTROLLER_INTERFACE_PUBLIC
   std::vector<hardware_interface::CommandInterface> export_reference_interfaces() final;
 
+  /**
+   * Controller has no reference interfaces.
+   *
+   * \returns empty list.
+   */
+  CONTROLLER_INTERFACE_PUBLIC
+  std::vector<hardware_interface::DistributedCommandInterface>
+  export_distributed_reference_interfaces() final;
+
   /**
    * Controller is not chainable, therefore no chained mode can be set.
    *

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -197,6 +197,16 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
   CONTROLLER_INTERFACE_PUBLIC
   virtual std::vector<hardware_interface::CommandInterface> export_reference_interfaces() = 0;
 
+  /**
+   * Export interfaces for a chainable controller that can be used as command interface of other
+   * controllers if controller is distributed.
+   *
+   * \returns list of distributed command interfaces for preceding controllers.
+   */
+  CONTROLLER_INTERFACE_PUBLIC
+  virtual std::vector<hardware_interface::DistributedCommandInterface>
+  export_distributed_reference_interfaces() = 0;
+
   /**
    * Set chained mode of a chainable controller. This method triggers internal processes to switch
    * a chainable controller to "chained" mode and vice-versa. Setting controller to "chained" mode

controller_interface/src/chainable_controller_interface.cpp

@@ -14,6 +14,7 @@
 
 #include "controller_interface/chainable_controller_interface.hpp"
 
+#include <exception>
 #include <vector>
 
 #include "hardware_interface/types/lifecycle_state_names.hpp"
@@ -44,6 +45,46 @@ return_type ChainableControllerInterface::update(
   return ret;
 }
 
+std::vector<hardware_interface::DistributedCommandInterface>
+ChainableControllerInterface::export_distributed_reference_interfaces()
+{
+  auto reference_interfaces = on_export_distributed_reference_interfaces();
+
+  // 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
+    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();
+  }
+
+  // check if the names of the reference interfaces begin with the controller's name
+  for (const auto & interface : reference_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.",
+        interface.get_name().c_str(), get_node()->get_name());
+      reference_interfaces.clear();
+      break;
+    }
+  }
+
+  return reference_interfaces;
+}
+
 std::vector<hardware_interface::CommandInterface>
 ChainableControllerInterface::export_reference_interfaces()
 {
@@ -111,6 +152,12 @@ bool ChainableControllerInterface::set_chained_mode(bool chained_mode)
 
 bool ChainableControllerInterface::is_in_chained_mode() const { return in_chained_mode_; }
 
+std::vector<hardware_interface::DistributedCommandInterface>
+ChainableControllerInterface::on_export_distributed_reference_interfaces()
+{
+  return {};
+}
+
 bool ChainableControllerInterface::on_set_chained_mode(bool /*chained_mode*/) { return true; }
 
 }  // namespace controller_interface

controller_interface/src/controller_interface.cpp

@@ -33,6 +33,12 @@ std::vector<hardware_interface::CommandInterface> ControllerInterface::export_re
   return {};
 }
 
+std::vector<hardware_interface::DistributedCommandInterface>
+ControllerInterface::export_distributed_reference_interfaces()
+{
+  return {};
+}
+
 bool ControllerInterface::set_chained_mode(bool /*chained_mode*/) { return false; }
 
 bool ControllerInterface::is_in_chained_mode() const { return false; }