Controller restart by switch_controller (minimal implementation)

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

@@ -329,6 +329,13 @@ class ControllerManager : public rclcpp::Node
   std::unique_ptr<hardware_interface::ResourceManager> resource_manager_;
 
 private:
+  enum class CheckDeActivateRequestResult
+  {
+    OK,
+    ERROR,
+    RETRY
+  };
+
   std::vector<std::string> get_controller_names();
   std::pair<std::string, std::string> split_command_interface(
     const std::string & command_interface);
@@ -339,6 +346,7 @@ class ControllerManager : public rclcpp::Node
    * and "control loop" threads.
    */
   void clear_requests();
+  void clear_chained_mode_requests();
 
   /**
    * If a controller is deactivated all following controllers (if any exist) should be switched
@@ -377,6 +385,23 @@ class ControllerManager : public rclcpp::Node
     const std::vector<ControllerSpec> & controllers, int strictness,
     const ControllersListIterator controller_it);
 
+  /**
+   * Check if all activate requests are valid.
+   * Perform a detailed check internally by calling check_following_controllers_for_activate.
+   *
+   * \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.
+   *
+   * \returns If all activate requests pass the check, return CheckDeActivateRequestResult::OK. If
+   * an error occurs during the check, the processing will differ based on the value of strictness:
+   * if BEST_EFFORT, erase the target controller from the activate_request and return
+   * CheckDeActivateRequestResult::RETRY; if STRICT, return CheckDeActivateRequestResult::ERROR.
+   */
+  CheckDeActivateRequestResult check_activate_requests(
+    const std::vector<ControllerSpec> & controllers, int strictness);
+
   /// Check if all the preceding controllers will be in inactive state after controllers' switch.
   /**
    * Check that all preceding controllers of the @controller_it
@@ -396,10 +421,27 @@ class ControllerManager : public rclcpp::Node
    * \returns return_type::OK if all preceding controllers pass the checks, otherwise
    * return_type::ERROR.
    */
-  controller_interface::return_type check_preceeding_controllers_for_deactivate(
+  controller_interface::return_type check_preceding_controllers_for_deactivate(
     const std::vector<ControllerSpec> & controllers, int strictness,
     const ControllersListIterator controller_it);
 
+  /**
+   * Check if all deactivate requests are valid.
+   * Perform a detailed check internally by calling check_preceding_controllers_for_deactivate.
+   *
+   * \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.
+   *
+   * \returns If all deactivate requests pass the check, return CheckDeActivateRequestResult::OK. If
+   * an error occurs during the check, the processing will differ based on the value of strictness:
+   * if BEST_EFFORT, erase the target controller from the deactivate_request and return
+   * CheckDeActivateRequestResult::RETRY; if STRICT, return CheckDeActivateRequestResult::ERROR.
+   */
+  CheckDeActivateRequestResult check_deactivate_requests(
+    const std::vector<ControllerSpec> & controllers, int strictness);
+
   /**
    * @brief Inserts a controller into an ordered list based on dependencies to compute the
    * controller chain.