Controller restart by switch_controller

controller_manager/CMakeLists.txt

@@ -72,6 +72,17 @@ if(BUILD_TESTING)
     DESTINATION lib
   )
 
+  add_library(test_controller_with_command SHARED
+    test/test_controller_with_command/test_controller_with_command.cpp)
+  target_link_libraries(test_controller_with_command PUBLIC controller_manager)
+  target_compile_definitions(test_controller_with_command PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL")
+  pluginlib_export_plugin_description_file(
+    controller_interface test/test_controller_with_command/test_controller_with_command.xml)
+  install(
+    TARGETS test_controller_with_command
+    DESTINATION lib
+  )
+
   add_library(test_chainable_controller SHARED
     test/test_chainable_controller/test_chainable_controller.cpp
   )
@@ -123,6 +134,16 @@ if(BUILD_TESTING)
     ros2_control_test_assets::ros2_control_test_assets
   )
 
+  ament_add_gmock(test_restart_controller
+    test/test_restart_controller.cpp
+    APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path}_$<CONFIG>
+  )
+  target_link_libraries(test_restart_controller
+    controller_manager
+    test_controller_with_command
+    ros2_control_test_assets::ros2_control_test_assets
+  )
+
   ament_add_gmock(test_controllers_chaining_with_controller_manager
     test/test_controllers_chaining_with_controller_manager.cpp
   )

controller_manager/include/controller_manager/controller_manager.hpp

@@ -340,73 +340,81 @@ class ControllerManager : public rclcpp::Node
    */
   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);
+  void extract_de_activate_command_interface_request(
+    const std::vector<ControllerSpec> & controllers,
+    const std::vector<std::string> & deactivate_request,
+    const std::vector<std::string> & activate_request,
+    std::vector<std::string> & deactivate_command_interface_request,
+    std::vector<std::string> & activate_command_interface_request) const;
+
+  void cache_controller_interfaces_in_activate_request(
+    const std::vector<ControllerSpec> & controllers,
+    const std::vector<std::string> & activate_request);
 
-  /// 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.
+   * @brief Check for conflicts with the (de)activate request. If conflicts are detected, handle
+   * them by either raising an error (STRICT) or removing the conflicting controller from the
+   * request (BEST_EFFORT).
    *
-   * For each controller the whole chain of following controllers is checked.
+   * NOTE: The input argument controllers must be sorted in the order of preceding and following.
+   * NOTE: strictness type "MANIPULATE_CONTROLLERS_CHAIN" is not implemented yet.
    *
-   * NOTE: The automatically adding of following controller into activate 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.
+   * \param[in] controllers list with controllers.
+   * \param[in] controllers_map map with controllers.
+   * \param[in,out] deactivate_request A list of controller names requested for deactivation. If the
+   * strictness value is equal to "BEST_EFFORT", conflicting controllers found will be erased from
+   * the list.
+   * \param[in,out] activate_request A list of controller names requested for activation. If the
+   * strictness value is equal to "BEST_EFFORT", conflicting controllers found will be erased from
+   * the list.
    *
-   * \returns return_type::OK if all following controllers pass the checks, otherwise
+   * \returns return_type::OK if all (de)activate requests 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);
+  controller_interface::return_type check_controllers_for_de_activate(
+    const int strictness, const std::vector<controller_manager::ControllerSpec> & controllers,
+    const controller_manager::ControllersMap & controllers_map,
+    std::vector<std::string> & deactivate_request,
+    std::vector<std::string> & activate_request) const;
 
-  /// 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.
+   * @brief Determine if a change chained mode is necessary for the following controllers based on
+   * the request, and add them to the chained mode request if necessary. Additionally, if only
+   * preceding performs the activation state transition, a restart of the following controller
+   * is necessary to switch the chained mode, so add them to the (de)activate request.
    *
-   * NOTE: The automatically adding of preceding controllers into deactivate list is not implemented
-   * yet.
+   * NOTE: The input argument controllers must be sorted in the order of preceding and following.
    *
    * \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.
+   * \param[in] controllers_map map with controllers.
+   * \param[in,out] deactivate_request A list of controller names requested for deactivation. If a
+   * following controller requiring a restart due to chained mode is found, it will be automatically
+   * added to the list.
+   * \param[in,out] activate_request A list of controller names requested for activation. If a
+   * following controller requiring a restart due to chained mode is found, it will be automatically
+   * added to the list.
+   * \param[out] from_chained_mode_request A list containing the names of following controllers that
+   * require switching from chained mode to non-chained mode, based on the requested (de)activation.
+   * \param[out] to_chained_mode_request A list containing the names of following controllers that
+   * require switching from non-chained mode to chained mode, based on the requested (de)activation.
    */
-  controller_interface::return_type check_preceeding_controllers_for_deactivate(
-    const std::vector<ControllerSpec> & controllers, int strictness,
-    const ControllersListIterator controller_it);
+  void create_from_to_chained_mode_request(
+    const std::vector<controller_manager::ControllerSpec> & controllers,
+    const controller_manager::ControllersMap & controllers_map,
+    std::vector<std::string> & deactivate_request, std::vector<std::string> & activate_request,
+    std::vector<std::string> & from_chained_mode_request,
+    std::vector<std::string> & to_chained_mode_request) const;
 
   /**
    * @brief Inserts a controller into an ordered list based on dependencies to compute the
    * controller chain.
    *
    * This method computes the controller chain by inserting the provided controller name into an
-   * ordered list of controllers based on dependencies. It ensures that controllers are inserted in
-   * the correct order so that dependencies are satisfied.
+   * ordered list of controllers based on dependencies. It ensures that controllers are inserted
+   * in the correct order so that dependencies are satisfied.
    *
    * @param ctrl_name The name of the controller to be inserted into the chain.
    * @param controller_iterator An iterator pointing to the position in the ordered list where the

controller_manager/include/controller_manager/controller_spec.hpp

@@ -22,6 +22,7 @@
 #include <map>
 #include <memory>
 #include <string>
+#include <unordered_map>
 #include <vector>
 #include "controller_interface/controller_interface.hpp"
 #include "hardware_interface/controller_info.hpp"
@@ -46,5 +47,10 @@ struct ControllerChainSpec
   std::vector<std::string> following_controllers;
   std::vector<std::string> preceding_controllers;
 };
+
+using ControllersMap = std::unordered_map<std::string, const controller_manager::ControllerSpec &>;
+using ControllerChainSpecsMap =
+  std::unordered_map<std::string, controller_manager::ControllerChainSpec>;
+
 }  // namespace controller_manager
 #endif  // CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_