Full functionality of chainable controllers in controller manager (ro…

…s-controls#667)

* auto-switching of chained mode in controllers
* interface-matching approach for managing chaining controllers

controller_interface/include/controller_interface/controller_interface.hpp

@@ -53,7 +53,7 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase
   /**
    * Controller is not chainable, therefore no chained mode can be set.
    *
-   * \returns false;
+   * \returns false.
    */
   CONTROLLER_INTERFACE_PUBLIC
   bool set_chained_mode(bool chained_mode) final;

controller_interface/include/controller_interface/helpers.hpp

@@ -48,7 +48,11 @@ bool get_ordered_interfaces(
     {
       if (!interface_type.empty())
       {
-        if ((name == interface.get_name()) && (interface_type == interface.get_interface_name()))
+        // check case where:
+        // (<joint> == <joint> AND <interface> == <interface>) OR <joint>/<interface> == 'full name'
+        if (
+          ((name == interface.get_name()) && (interface_type == interface.get_interface_name())) ||
+          ((name + "/" + interface_type) == interface.get_full_name()))
         {
           ordered_interfaces.push_back(std::ref(interface));
         }

controller_manager/CMakeLists.txt

@@ -89,6 +89,16 @@ if(BUILD_TESTING)
     DESTINATION lib
   )
 
+  add_library(test_chainable_controller SHARED test/test_chainable_controller/test_chainable_controller.cpp)
+  target_include_directories(test_chainable_controller PRIVATE include)
+  target_link_libraries(test_chainable_controller controller_manager)
+  target_compile_definitions(test_chainable_controller PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL")
+  pluginlib_export_plugin_description_file(
+    controller_interface test/test_chainable_controller/test_chainable_controller.xml)
+  install(TARGETS test_chainable_controller
+    DESTINATION lib
+  )
+
   ament_add_gmock(
     test_controller_manager
     test/test_controller_manager.cpp
@@ -114,6 +124,14 @@ if(BUILD_TESTING)
   target_link_libraries(test_load_controller ${PROJECT_NAME} test_controller test_controller_failed_init)
   ament_target_dependencies(test_load_controller ros2_control_test_assets)
 
+  ament_add_gmock(
+    test_controllers_chaining_with_controller_manager
+    test/test_controllers_chaining_with_controller_manager.cpp
+  )
+  target_include_directories(test_controllers_chaining_with_controller_manager PRIVATE include)
+  target_link_libraries(test_controllers_chaining_with_controller_manager controller_manager test_chainable_controller test_controller)
+  ament_target_dependencies(test_controllers_chaining_with_controller_manager ros2_control_test_assets)
+
   ament_add_gmock(
     test_controller_manager_srvs
     test/test_controller_manager_srvs.cpp

controller_manager/include/controller_manager/controller_manager.hpp

@@ -15,9 +15,11 @@
 #ifndef CONTROLLER_MANAGER__CONTROLLER_MANAGER_HPP_
 #define CONTROLLER_MANAGER__CONTROLLER_MANAGER_HPP_
 
+#include <map>
 #include <memory>
 #include <string>
 #include <tuple>
+#include <unordered_map>
 #include <vector>
 
 #include "controller_interface/chainable_controller_interface.hpp"
@@ -40,15 +42,21 @@
 #include "controller_manager_msgs/srv/switch_controller.hpp"
 #include "controller_manager_msgs/srv/unload_controller.hpp"
 
+#include "hardware_interface/handle.hpp"
 #include "hardware_interface/resource_manager.hpp"
 
 #include "pluginlib/class_loader.hpp"
 
 #include "rclcpp/executor.hpp"
 #include "rclcpp/node.hpp"
+#include "rclcpp/node_interfaces/node_logging_interface.hpp"
+#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
+#include "rclcpp/parameter.hpp"
 
 namespace controller_manager
 {
+using ControllersListIterator = std::vector<controller_manager::ControllerSpec>::const_iterator;
+
 class ControllerManager : public rclcpp::Node
 {
 public:
@@ -169,6 +177,18 @@ class ControllerManager : public rclcpp::Node
   CONTROLLER_MANAGER_PUBLIC
   void stop_controllers();
 
+  /**
+   * Switch chained mode for all the controllers with respect to the following cases:
+   * - a preceding controller is getting activated --> switch controller to chained mode;
+   * - all preceding controllers are deactivated --> switch controller from chained mode.
+   *
+   * \param[in] chained_mode_switch_list list of controller to switch chained mode.
+   * \param[in] to_chained_mode flag if controller should be switched *to* or *from* chained mode.
+   */
+  CONTROLLER_MANAGER_PUBLIC
+  void switch_chained_mode(
+    const std::vector<std::string> & chained_mode_switch_list, bool to_chained_mode);
+
   CONTROLLER_MANAGER_PUBLIC
   void start_controllers();
 
@@ -243,11 +263,78 @@ class ControllerManager : public rclcpp::Node
   // Per controller update rate support
   unsigned int update_loop_counter_ = 0;
   unsigned int update_rate_ = 100;
+  std::vector<std::vector<std::string>> chained_controllers_configuration_;
+
+  std::unique_ptr<hardware_interface::ResourceManager> resource_manager_;
 
 private:
   std::vector<std::string> get_controller_names();
 
-  std::unique_ptr<hardware_interface::ResourceManager> resource_manager_;
+  /**
+   * Clear request lists used when switching controllers. The lists are shared between "callback" and
+   * "control loop" threads.
+   */
+  void clear_requests();
+
+  /**
+   * If a controller is deactivated all following controllers (if any exist) should be switched
+   * 'from' the chained mode.
+   *
+   * \param[in] controllers list with controllers.
+   */
+  void propagate_deactivation_of_chained_mode(const std::vector<ControllerSpec> & controllers);
+
+  /// Check if all the following controllers will be in active state and in the chained mode
+  /// after controllers' switch.
+  /**
+   * Check recursively that all following controllers of the @controller_it
+   * - are already active,
+   * - will not be deactivated,
+   * - or will be activated.
+   * The following controllers are added to the request to switch in the chained mode or removed
+   * from the request to switch from the chained mode.
+   *
+   * For each controller the whole chain of following controllers is checked.
+   *
+   * NOTE: The automatically adding of following controller into starting list is not implemented
+   * yet.
+   *
+   * \param[in] controllers list with controllers.
+   * \param[in] strictness if value is equal "MANIPULATE_CONTROLLERS_CHAIN" then all following
+   * controllers will be automatically added to the activate request list if they are not in the
+   * deactivate request.
+   * \param[in] controller_it iterator to the controller for which the following controllers are
+   * checked.
+   *
+   * \returns return_type::OK if all following controllers pass the checks, otherwise
+   * return_type::ERROR.
+   */
+  controller_interface::return_type check_following_controllers_for_activate(
+    const std::vector<ControllerSpec> & controllers, int strictness,
+    const ControllersListIterator controller_it);
+
+  /// Check if all the preceding controllers will be in inactive state after controllers' switch.
+  /**
+   * Check that all preceding controllers of the @controller_it
+   * - are inactive,
+   * - will be deactivated,
+   * - and will not be activated.
+   *
+   * NOTE: The automatically adding of preceding controllers into stopping list is not implemented
+   * yet.
+   *
+   * \param[in] controllers list with controllers.
+   * \param[in] strictness if value is equal "MANIPULATE_CONTROLLERS_CHAIN" then all preceding
+   * controllers will be automatically added to the deactivate request list.
+   * \param[in] controller_it iterator to the controller for which the preceding controllers are
+   * checked.
+   *
+   * \returns return_type::OK if all preceding controllers pass the checks, otherwise
+   * return_type::ERROR.
+   */
+  controller_interface::return_type check_preceeding_controllers_for_deactivate(
+    const std::vector<ControllerSpec> & controllers, int strictness,
+    const ControllersListIterator controller_it);
 
   std::shared_ptr<rclcpp::Executor> executor_;
 
@@ -366,6 +453,7 @@ class ControllerManager : public rclcpp::Node
     set_hardware_component_state_service_;
 
   std::vector<std::string> start_request_, stop_request_;
+  std::vector<std::string> to_chained_mode_request_, from_chained_mode_request_;
   std::vector<std::string> start_command_interface_request_, stop_command_interface_request_;
 
   struct SwitchParams