Adding base class for chained controllers: ChainedControllersInterface

controller_interface/CMakeLists.txt

@@ -14,6 +14,7 @@ add_library(
   SHARED
   src/controller_interface_base.cpp
   src/controller_interface.cpp
+  src/chainable_controller_interface.cpp
 )
 target_include_directories(
   controller_interface
@@ -52,6 +53,11 @@ if(BUILD_TESTING)
   target_link_libraries(test_controller_with_options controller_interface)
   target_include_directories(test_controller_with_options PRIVATE include)
 
+  ament_add_gmock(test_chainable_controller_interface test/test_chainable_controller_interface.cpp)
+  target_link_libraries(test_chainable_controller_interface controller_interface)
+  target_include_directories(test_chainable_controller_interface PRIVATE include)
+  ament_target_dependencies(test_chainable_controller_interface hardware_interface)
+
   ament_add_gmock(
     test_semantic_component_interface
     test/test_semantic_component_interface.cpp

controller_interface/include/controller_interface/chainable_controller_interface.hpp

@@ -0,0 +1,120 @@
+// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt)
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_
+#define CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_
+
+#include <vector>
+
+#include "controller_interface/controller_interface_base.hpp"
+#include "controller_interface/visibility_control.h"
+#include "hardware_interface/handle.hpp"
+
+namespace controller_interface
+{
+/// Virtual class to implement when integrating a controller that can be preceded by another
+/// controllers.
+/**
+ * Specialization of ControllerInterface class to force implementation of methods specific for
+ * "chainable" controller, i.e., controller that can be preceded by an another controller, for
+ * example inner controller of an control cascade.
+ *
+ */
+class ChainableControllerInterface : public ControllerInterfaceBase
+{
+public:
+  CONTROLLER_INTERFACE_PUBLIC
+  ChainableControllerInterface();
+
+  CONTROLLER_INTERFACE_PUBLIC
+  virtual ~ChainableControllerInterface() = default;
+
+  CONTROLLER_INTERFACE_PUBLIC
+  return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) final;
+
+  /**
+   * Controller is chainable.
+   *
+   * \returns true;
+   */
+  CONTROLLER_INTERFACE_PUBLIC
+  bool is_chainable() const final;
+
+  CONTROLLER_INTERFACE_PUBLIC
+  std::vector<hardware_interface::CommandInterface> export_reference_interfaces() final;
+
+  CONTROLLER_INTERFACE_PUBLIC
+  bool set_chained_mode(bool chained_mode) final;
+
+  CONTROLLER_INTERFACE_PUBLIC
+  bool is_in_chained_mode() const final;
+
+protected:
+  /// Virtual method that each chainable controller should implement to export its chainable
+  /// interfaces.
+  /**
+   * Each chainable controller implements this methods where all input (command) interfaces are
+   * 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.
+   */
+  virtual std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() = 0;
+
+  /// Virtual method that each chainable controller should implement to switch chained mode.
+  /**
+   * Each chainable controller implements this methods to switch between "chained" and "external"
+   * mode. In "chained" mode all external interfaces like subscriber and service servers are
+   * disabled to avoid potential concurrency in input commands.
+   *
+   * \param[in] flag marking a switch to or from chained mode.
+   *
+   * \returns true if controller successfully switched between "chained" and "external" mode. \default returns true so the method don't have to be overridden if controller can always switch chained mode.
+   */
+  virtual bool on_set_chained_mode(bool chained_mode);
+
+  /// Update reference from input topics when not in chained mode.
+  /**
+   * Each chainable controller implements this method to update reference from subscribers when not
+   * in chained mode.
+   *
+   * \returns return_type::OK if update is successfully, otherwise return_type::ERROR.
+   */
+  virtual return_type update_reference_from_subscribers() = 0;
+
+  /// Execute calculations of the controller and update command interfaces.
+  /**
+   * Update method for chainable controllers.
+   * In this method is valid to assume that \reference_interfaces_ hold the values for calculation
+   * of the commands in the current control step.
+   * This means that this method is called after \update_reference_from_subscribers if controller is
+   * not in chained mode.
+   *
+   * \returns return_type::OK if calculation and writing of interface is successfully, otherwise
+   * return_type::ERROR.
+   */
+  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_;
+
+private:
+  /// A flag marking is a chainable controller is currently preceded by another controller.
+  bool in_chained_mode_ = false;
+};
+
+}  // namespace controller_interface
+
+#endif  // CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_

controller_interface/include/controller_interface/controller_interface.hpp

@@ -57,6 +57,7 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase
    */
   CONTROLLER_INTERFACE_PUBLIC
   bool set_chained_mode(bool chained_mode) final;
+
   /**
    * Controller can not be in chained mode.
    *

controller_interface/src/chainable_controller_interface.cpp

@@ -0,0 +1,116 @@
+// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt)
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "controller_interface/chainable_controller_interface.hpp"
+
+#include <vector>
+
+#include "hardware_interface/types/lifecycle_state_names.hpp"
+#include "lifecycle_msgs/msg/state.hpp"
+
+namespace controller_interface
+{
+ChainableControllerInterface::ChainableControllerInterface() : ControllerInterfaceBase() {}
+
+bool ChainableControllerInterface::is_chainable() const { return true; }
+
+return_type ChainableControllerInterface::update(
+  const rclcpp::Time & time, const rclcpp::Duration & period)
+{
+  return_type ret = return_type::ERROR;
+
+  if (!is_in_chained_mode())
+  {
+    ret = update_reference_from_subscribers();
+    if (ret != return_type::OK)
+    {
+      return ret;
+    }
+  }
+
+  ret = update_and_write_commands(time, period);
+
+  return ret;
+}
+
+std::vector<hardware_interface::CommandInterface>
+ChainableControllerInterface::export_reference_interfaces()
+{
+  auto reference_interfaces = on_export_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_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_full_name().c_str(), get_node()->get_name());
+      reference_interfaces.clear();
+      break;
+    }
+  }
+
+  return reference_interfaces;
+}
+
+bool ChainableControllerInterface::set_chained_mode(bool chained_mode)
+{
+  bool result = false;
+
+  if (get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+  {
+    result = on_set_chained_mode(chained_mode);
+
+    if (result)
+    {
+      in_chained_mode_ = chained_mode;
+    }
+  }
+  else
+  {
+    RCLCPP_ERROR(
+      get_node()->get_logger(),
+      "Can not change controller's chained mode because it is no in '%s' state. "
+      "Current state is '%s'.",
+      hardware_interface::lifecycle_state_names::UNCONFIGURED, get_state().label().c_str());
+  }
+
+  return result;
+}
+
+bool ChainableControllerInterface::is_in_chained_mode() const { return in_chained_mode_; }
+
+bool ChainableControllerInterface::on_set_chained_mode(bool /*chained_mode*/) { return true; }
+
+}  // namespace controller_interface