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_