Enabling chanable mode for forward command controllers.

forward_command_controller/CMakeLists.txt

@@ -23,6 +23,7 @@ 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

forward_command_controller/forward_command_plugin.xml

@@ -1,4 +1,4 @@
-<library path="forward_command_controller">
+<!--<library path="forward_command_controller">
   <class name="forward_command_controller/ForwardCommandController"
          type="forward_command_controller::ForwardCommandController<forward_command_controller::ForwardController>"
          base_class_type="controller_interface::ControllerInterface">
@@ -28,4 +28,36 @@
       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">
+  <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>

forward_command_controller/include/forward_command_controller/chainable_forward_controller.hpp

@@ -22,11 +22,7 @@
 #include "controller_interface/chainable_controller_interface.hpp"
 #include "forward_command_controller/forward_controllers_base.hpp"
 #include "forward_command_controller/visibility_control.h"
-#include "rclcpp/subscription.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
-#include "rclcpp_lifecycle/state.hpp"
-#include "realtime_tools/realtime_buffer.h"
-#include "std_msgs/msg/float64_multi_array.hpp"
 
 namespace forward_command_controller
 {
@@ -46,17 +42,29 @@ class ChainableForwardController : public ForwardControllersBase,
 {
 public:
   FORWARD_COMMAND_CONTROLLER_PUBLIC
-  ChainableForwardController() = default;
+  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
+  CallbackReturn on_init() override;
+
   FORWARD_COMMAND_CONTROLLER_PUBLIC
   CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
 
   FORWARD_COMMAND_CONTROLLER_PUBLIC
   CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
 
+  FORWARD_COMMAND_CONTROLLER_PUBLIC
+  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;

forward_command_controller/include/forward_command_controller/forward_command_controller.hpp

@@ -18,10 +18,10 @@
 #include <string>
 #include <vector>
 
-#include "controller_interface/controller_interface.hpp"
+#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/logging.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 
 namespace forward_command_controller
@@ -43,49 +43,43 @@ template <
   typename T,
   typename std::enable_if<
     std::is_convertible<T *, forward_command_controller::ForwardControllersBase *>::value,
-    T>::type * = nullptr>
-class ForwardCommandController : public T
+    T>::type * = nullptr,
+  typename std::enable_if<
+    std::is_convertible<T *, controller_interface::ControllerInterface *>::value, T>::type * =
+    nullptr>
+class BaseForwardCommandController : public T
 {
 public:
   FORWARD_COMMAND_CONTROLLER_PUBLIC
-  ForwardCommandController() : controller_interface::ControllerInterface(), T(){};
+  BaseForwardCommandController() : T() {}
 
 protected:
   void declare_parameters() override
   {
-    controller_interface::ControllerInterface::get_node()
-      ->declare_parameter<std::vector<std::string>>("joints", std::vector<std::string>());
-    controller_interface::ControllerInterface::get_node()->declare_parameter<std::string>(
-      "interface_name", "");
+    controller_interface::ControllerInterface::auto_declare<std::vector<std::string>>(
+      "joints", std::vector<std::string>());
+    controller_interface::ControllerInterface::auto_declare<std::string>("interface_name", "");
   };
 
   CallbackReturn read_parameters() override
   {
-    joint_names_ = controller_interface::ControllerInterface::get_node()
-                     ->get_parameter("joints")
-                     .as_string_array();
+    joint_names_ = T::get_node()->get_parameter("joints").as_string_array();
 
     if (joint_names_.empty())
     {
-      RCLCPP_ERROR(
-        controller_interface::ControllerInterface::get_node()->get_logger(),
-        "'joints' parameter was empty");
+      RCLCPP_ERROR(T::get_node()->get_logger(), "'joints' parameter was empty");
       return CallbackReturn::ERROR;
     }
 
     // Specialized, child controllers set interfaces before calling configure function.
     if (interface_name_.empty())
     {
-      interface_name_ = controller_interface::ControllerInterface::get_node()
-                          ->get_parameter("interface_name")
-                          .as_string();
+      interface_name_ = T::get_node()->get_parameter("interface_name").as_string();
     }
 
     if (interface_name_.empty())
     {
-      RCLCPP_ERROR(
-        controller_interface::ControllerInterface::get_node()->get_logger(),
-        "'interface_name' parameter was empty");
+      RCLCPP_ERROR(T::get_node()->get_logger(), "'interface_name' parameter was empty");
       return CallbackReturn::ERROR;
     }
 
@@ -101,6 +95,23 @@ class ForwardCommandController : public T
   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

@@ -15,8 +15,8 @@
 #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
@@ -26,20 +26,38 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface
 
 /**
  * \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 ForwardController : public ForwardControllersBase
+class ForwardController : public ForwardControllersBase,
+                          public controller_interface::ControllerInterface
 {
 public:
   FORWARD_COMMAND_CONTROLLER_PUBLIC
-  ForwardController() = default;
+  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
+  CallbackReturn on_init() override;
+
+  FORWARD_COMMAND_CONTROLLER_PUBLIC
+  CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
+
+  FORWARD_COMMAND_CONTROLLER_PUBLIC
+  CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
+
+  FORWARD_COMMAND_CONTROLLER_PUBLIC
+  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

forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp

@@ -33,14 +33,14 @@ using CmdType = std_msgs::msg::Float64MultiArray;
 using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
 
 /**
- * \brief Forward command controller for a set of joints and interfaces.
+ * \brief Forward command controller base class for shared implementation.
  *
  * 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 ForwardControllersBase : virtual public controller_interface::ControllerInterface
+class ForwardControllersBase
 {
 public:
   FORWARD_COMMAND_CONTROLLER_PUBLIC
@@ -50,26 +50,24 @@ class ForwardControllersBase : virtual public controller_interface::ControllerIn
   ~ForwardControllersBase() = default;
 
   FORWARD_COMMAND_CONTROLLER_PUBLIC
-  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+  controller_interface::InterfaceConfiguration get_command_interface_configuration() const;
 
   FORWARD_COMMAND_CONTROLLER_PUBLIC
-  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+  controller_interface::InterfaceConfiguration get_state_interface_configuration() const;
 
   FORWARD_COMMAND_CONTROLLER_PUBLIC
-  CallbackReturn on_init() override;
+  CallbackReturn execute_init(const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node);
 
   FORWARD_COMMAND_CONTROLLER_PUBLIC
-  CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
+  CallbackReturn execute_configure(const rclcpp_lifecycle::State & previous_state);
 
   FORWARD_COMMAND_CONTROLLER_PUBLIC
-  CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
+  CallbackReturn execute_activate(
+    const rclcpp_lifecycle::State & previous_state,
+    std::vector<hardware_interface::LoanedCommandInterface> & command_interfaces);
 
   FORWARD_COMMAND_CONTROLLER_PUBLIC
-  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;
+  CallbackReturn execute_deactivate(const rclcpp_lifecycle::State & previous_state);
 
 protected:
   /**
@@ -97,6 +95,9 @@ class ForwardControllersBase : virtual public controller_interface::ControllerIn
 
   realtime_tools::RealtimeBuffer<std::shared_ptr<CmdType>> rt_command_ptr_;
   rclcpp::Subscription<CmdType>::SharedPtr joints_command_subscriber_;
+
+private:
+  std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
 };
 
 }  // namespace forward_command_controller