Make forward controller chainable.

Enabling chanable mode for forward command controllers.

Fix all bugs in chained-controllers mode.

Remove debug output from forwarding controller.

Updated example ffwd chainable controller for new structure.

forward_command_controller/CMakeLists.txt

@@ -22,6 +22,8 @@ find_package(std_msgs REQUIRED)
 
 add_library(forward_command_controller
   SHARED
+  src/chainable_forward_controller.cpp
+  src/forward_controller.cpp
   src/forward_controllers_base.cpp
   src/forward_command_controller.cpp
   src/multi_interface_forward_command_controller.cpp
@@ -101,6 +103,31 @@ if(BUILD_TESTING)
   target_link_libraries(test_multi_interface_forward_command_controller
     forward_command_controller
   )
+
+  ament_add_gmock(
+    test_load_chained_forward_command_controller
+    test/test_load_chained_forward_command_controller.cpp
+  )
+  target_include_directories(test_load_chained_forward_command_controller PRIVATE include)
+  ament_target_dependencies(
+    test_load_chained_forward_command_controller
+    controller_manager
+    hardware_interface
+    ros2_control_test_assets
+  )
+
+  ament_add_gmock(
+    test_load_chained_multi_interface_forward_command_controller
+    test/test_load_chained_multi_interface_forward_command_controller.cpp
+  )
+  target_include_directories(test_load_chained_multi_interface_forward_command_controller PRIVATE include)
+  ament_target_dependencies(
+    test_load_chained_multi_interface_forward_command_controller
+    controller_manager
+    hardware_interface
+    ros2_control_test_assets
+  )
+
 endif()
 
 ament_export_dependencies(

forward_command_controller/forward_command_plugin.xml

@@ -1,11 +1,61 @@
+<!--<library path="forward_command_controller">
+  <class name="forward_command_controller/ForwardCommandController"
+         type="forward_command_controller::ForwardCommandController"
+         base_class_type="controller_interface::ControllerInterface">
+  <description>
+    The forward command controller commands a group of joints in a given interface
+  </description>
+  </class>
+  <class name="forward_command_controller/MultiInterfaceForwardCommandController"
+         type="forward_command_controller::MultiInterfaceForwardCommandController"
+         base_class_type="controller_interface::ControllerInterface">
+    <description>
+      MultiInterfaceForwardController ros2_control controller.
+    </description>
+  </class>
+
+  <class name="forward_command_controller/ChainableForwardCommandController"
+         type="forward_command_controller::ChainableForwardCommandController"
+         base_class_type="controller_interface::ControllerInterface">
+  <description>
+    The forward command controller commands a group of joints in a given interface
+  </description>
+  </class>
+  <class name="forward_command_controller/ChainableMultiInterfaceForwardCommandController"
+         type="forward_command_controller::ChainableMultiInterfaceForwardCommandController"
+         base_class_type="controller_interface::ControllerInterface">
+    <description>
+      MultiInterfaceForwardController ros2_control controller.
+    </description>
+  </class>
+</library>-->
+
 <library path="forward_command_controller">
-  <class name="forward_command_controller/ForwardCommandController" type="forward_command_controller::ForwardCommandController" base_class_type="controller_interface::ControllerInterface">
+  <class name="forward_command_controller/ForwardCommandController"
+         type="forward_command_controller::ForwardCommandController"
+         base_class_type="controller_interface::ControllerInterface">
   <description>
     The forward command controller commands a group of joints in a given interface
   </description>
   </class>
   <class name="forward_command_controller/MultiInterfaceForwardCommandController"
-         type="forward_command_controller::MultiInterfaceForwardCommandController" base_class_type="controller_interface::ControllerInterface">
+         type="forward_command_controller::MultiInterfaceForwardCommandController"
+         base_class_type="controller_interface::ControllerInterface">
+    <description>
+      MultiInterfaceForwardController ros2_control controller.
+    </description>
+  </class>
+
+  <class name="forward_command_controller/ChainableForwardCommandController"
+         type="forward_command_controller::ChainableForwardCommandController"
+         base_class_type="controller_interface::ChainableControllerInterface">
+  <description>
+    The forward command controller commands a group of joints in a given interface
+  </description>
+  </class>
+  <class name="forward_command_controller/ChainableMultiInterfaceForwardCommandController"
+         type="forward_command_controller::ChainableMultiInterfaceForwardCommandController"
+         base_class_type="controller_interface::ChainableControllerInterface">
     <description>
       MultiInterfaceForwardController ros2_control controller.
     </description>

forward_command_controller/include/forward_command_controller/chainable_forward_controller.hpp

@@ -0,0 +1,85 @@
+// Copyright 2021 Stogl Robotics Consulting UG (haftungsbescrhä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 FORWARD_COMMAND_CONTROLLER__CHAINABLE_FORWARD_CONTROLLER_HPP_
+#define FORWARD_COMMAND_CONTROLLER__CHAINABLE_FORWARD_CONTROLLER_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "controller_interface/chainable_controller_interface.hpp"
+#include "forward_command_controller/forward_controllers_base.hpp"
+#include "forward_command_controller/visibility_control.h"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+
+namespace forward_command_controller
+{
+using CmdType = std_msgs::msg::Float64MultiArray;
+
+/**
+ * \brief Forward command controller for a set of joints and interfaces.
+ *
+ * This class forwards the command signal down to a set of joints or interfaces.
+ *
+ * Subscribes to:
+ * - \b commands (std_msgs::msg::Float64MultiArray) : The commands to apply.
+ */
+class ChainableForwardController : public ForwardControllersBase,
+                                   public controller_interface::ChainableControllerInterface
+{
+public:
+  FORWARD_COMMAND_CONTROLLER_PUBLIC
+  ChainableForwardController();
+
+  FORWARD_COMMAND_CONTROLLER_PUBLIC
+  ~ChainableForwardController() = default;
+
+  FORWARD_COMMAND_CONTROLLER_PUBLIC
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  FORWARD_COMMAND_CONTROLLER_PUBLIC
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  FORWARD_COMMAND_CONTROLLER_PUBLIC
+  controller_interface::CallbackReturn on_init() override;
+
+  FORWARD_COMMAND_CONTROLLER_PUBLIC
+  controller_interface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  FORWARD_COMMAND_CONTROLLER_PUBLIC
+  controller_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  FORWARD_COMMAND_CONTROLLER_PUBLIC
+  controller_interface::CallbackReturn on_deactivate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+protected:
+  std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override;
+
+  bool on_set_chained_mode(bool chained_mode) override;
+
+  controller_interface::return_type update_reference_from_subscribers() override;
+
+  controller_interface::return_type update_and_write_commands(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+  std::vector<std::string> reference_interface_names_;
+};
+
+}  // namespace forward_command_controller
+
+#endif  // FORWARD_COMMAND_CONTROLLER__CHAINABLE_FORWARD_CONTROLLER_HPP_

forward_command_controller/include/forward_command_controller/forward_command_controller.hpp

@@ -18,8 +18,11 @@
 #include <string>
 #include <vector>
 
+#include "forward_command_controller/chainable_forward_controller.hpp"
+#include "forward_command_controller/forward_controller.hpp"
 #include "forward_command_controller/forward_controllers_base.hpp"
 #include "forward_command_controller/visibility_control.h"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
 
 namespace forward_command_controller
 {
@@ -34,20 +37,79 @@ namespace forward_command_controller
  * Subscribes to:
  * - \b commands (std_msgs::msg::Float64MultiArray) : The commands to apply.
  */
-class ForwardCommandController : public ForwardControllersBase
+template <
+  typename T,
+  typename std::enable_if<
+    std::is_convertible<T *, forward_command_controller::ForwardControllersBase *>::value,
+    T>::type * = nullptr,
+  typename std::enable_if<
+    std::is_convertible<T *, controller_interface::ControllerInterfaceBase *>::value, T>::type * =
+    nullptr>
+class BaseForwardCommandController : public T
 {
 public:
   FORWARD_COMMAND_CONTROLLER_PUBLIC
-  ForwardCommandController();
+  BaseForwardCommandController() : T() {}
 
 protected:
-  void declare_parameters() override;
-  controller_interface::CallbackReturn read_parameters() override;
+  void declare_parameters() override
+  {
+    controller_interface::ControllerInterfaceBase::auto_declare<std::vector<std::string>>(
+      "joints", std::vector<std::string>());
+    controller_interface::ControllerInterfaceBase::auto_declare<std::string>("interface_name", "");
+  };
+
+  controller_interface::CallbackReturn read_parameters() override
+  {
+    joint_names_ = T::get_node()->get_parameter("joints").as_string_array();
+
+    if (joint_names_.empty())
+    {
+      RCLCPP_ERROR(T::get_node()->get_logger(), "'joints' parameter was empty");
+      return controller_interface::CallbackReturn::ERROR;
+    }
+
+    // Specialized, child controllers set interfaces before calling configure function.
+    if (interface_name_.empty())
+    {
+      interface_name_ = T::get_node()->get_parameter("interface_name").as_string();
+    }
+
+    if (interface_name_.empty())
+    {
+      RCLCPP_ERROR(T::get_node()->get_logger(), "'interface_name' parameter was empty");
+      return controller_interface::CallbackReturn::ERROR;
+    }
+
+    for (const auto & joint : joint_names_)
+    {
+      T::command_interface_names_.push_back(joint + "/" + interface_name_);
+    }
+
+    return controller_interface::CallbackReturn::SUCCESS;
+  };
 
   std::vector<std::string> joint_names_;
   std::string interface_name_;
 };
 
+class ForwardCommandController : public BaseForwardCommandController<ForwardController>
+{
+public:
+  FORWARD_COMMAND_CONTROLLER_PUBLIC
+  ForwardCommandController() : BaseForwardCommandController<ForwardController>() {}
+};
+
+class ChainableForwardCommandController
+: public BaseForwardCommandController<ChainableForwardController>
+{
+public:
+  FORWARD_COMMAND_CONTROLLER_PUBLIC
+  ChainableForwardCommandController() : BaseForwardCommandController<ChainableForwardController>()
+  {
+  }
+};
+
 }  // namespace forward_command_controller
 
 #endif  // FORWARD_COMMAND_CONTROLLER__FORWARD_COMMAND_CONTROLLER_HPP_

forward_command_controller/include/forward_command_controller/forward_controller.hpp

@@ -0,0 +1,67 @@
+// Copyright 2021 Stogl Robotics Consulting UG (haftungsbescrhä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 FORWARD_COMMAND_CONTROLLER__FORWARD_CONTROLLER_HPP_
+#define FORWARD_COMMAND_CONTROLLER__FORWARD_CONTROLLER_HPP_
+
+#include "controller_interface/controller_interface.hpp"
+#include "forward_command_controller/forward_controllers_base.hpp"
+#include "forward_command_controller/visibility_control.h"
+
+namespace forward_command_controller
+{
+using CmdType = std_msgs::msg::Float64MultiArray;
+
+/**
+ * \brief Forward command controller for a set of joints and interfaces.
+ */
+class ForwardController : public ForwardControllersBase,
+                          public controller_interface::ControllerInterface
+{
+public:
+  FORWARD_COMMAND_CONTROLLER_PUBLIC
+  ForwardController();
+
+  FORWARD_COMMAND_CONTROLLER_PUBLIC
+  ~ForwardController() = default;
+
+  FORWARD_COMMAND_CONTROLLER_PUBLIC
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  FORWARD_COMMAND_CONTROLLER_PUBLIC
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  FORWARD_COMMAND_CONTROLLER_PUBLIC
+  controller_interface::CallbackReturn on_init() override;
+
+  FORWARD_COMMAND_CONTROLLER_PUBLIC
+  controller_interface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  FORWARD_COMMAND_CONTROLLER_PUBLIC
+  controller_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  FORWARD_COMMAND_CONTROLLER_PUBLIC
+  controller_interface::CallbackReturn on_deactivate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  FORWARD_COMMAND_CONTROLLER_PUBLIC
+  controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+};
+
+}  // namespace forward_command_controller
+
+#endif  // FORWARD_COMMAND_CONTROLLER__FORWARD_CONTROLLER_HPP_