Add switch and unload controller functionality

controller_manager/CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(ament_cmake)
 find_package(ament_cmake_core REQUIRED)
 find_package(ament_index_cpp REQUIRED)
 find_package(controller_interface REQUIRED)
+find_package(controller_manager_msgs REQUIRED)
 find_package(hardware_interface REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
@@ -23,6 +24,7 @@ target_include_directories(controller_manager PRIVATE include)
 ament_target_dependencies(controller_manager
   ament_index_cpp
   controller_interface
+  controller_manager_msgs
   hardware_interface
   pluginlib
   rclcpp

controller_manager/include/controller_manager/controller_manager.hpp

@@ -52,6 +52,10 @@ class ControllerManager : public rclcpp::Node
     const std::string & controller_name,
     const std::string & controller_type);
 
+  CONTROLLER_MANAGER_PUBLIC
+  controller_interface::return_type unload_controller(
+    const std::string & controller_name);
+
   CONTROLLER_MANAGER_PUBLIC
   std::vector<std::shared_ptr<controller_interface::ControllerInterface>>
   get_loaded_controllers() const;
@@ -75,6 +79,13 @@ class ControllerManager : public rclcpp::Node
     return add_controller_impl(controller, controller_name);
   }
 
+  CONTROLLER_MANAGER_PUBLIC
+  controller_interface::return_type
+  switchController(
+    const std::vector<std::string> & start_controllers,
+    const std::vector<std::string> & stop_controllers,
+    int strictness, bool start_asap, double timeout);
+
   CONTROLLER_MANAGER_PUBLIC
   controller_interface::return_type
   update();
@@ -102,11 +113,57 @@ class ControllerManager : public rclcpp::Node
     std::shared_ptr<controller_interface::ControllerInterface> controller,
     const std::string & controller_name);
 
+  CONTROLLER_MANAGER_PUBLIC
+  controller_interface::ControllerInterface * getControllerByName(const std::string & name);
+
+  CONTROLLER_MANAGER_PUBLIC
+  void manageSwitch();
+
+  CONTROLLER_MANAGER_PUBLIC
+  void stopControllers();
+
+  CONTROLLER_MANAGER_PUBLIC
+  void startControllers();
+
+  CONTROLLER_MANAGER_PUBLIC
+  void startControllersAsap();
+
 private:
   std::shared_ptr<hardware_interface::RobotHardware> hw_;
   std::shared_ptr<rclcpp::Executor> executor_;
   std::vector<ControllerLoaderInterfaceSharedPtr> loaders_;
-  std::vector<std::shared_ptr<controller_interface::ControllerInterface>> loaded_controllers_;
+
+  /** \name Controllers List
+   * The controllers list is double-buffered to avoid needing to lock the
+   * real-time thread when switching controllers in the non-real-time thread.
+   *\{*/
+  /// Mutex protecting the current controllers list
+  std::recursive_mutex controllers_lock_;
+  std::vector<std::shared_ptr<controller_interface::ControllerInterface>> controllers_lists_[2];
+  /// The index of the current controllers list
+  int current_controllers_list_ = {0};
+  /// The index of the controllers list being used in the real-time thread.
+  int used_by_realtime_ = {-1};
+
+
+  std::vector<controller_interface::ControllerInterface *> start_request_, stop_request_;
+#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING
+//  std::list<hardware_interface::ControllerInfo> switch_start_list_, switch_stop_list_;
+#endif
+
+  struct SwitchParams
+  {
+    bool do_switch = {false};
+    bool started = {false};
+    rclcpp::Time init_time = {rclcpp::Time::max()};
+
+    // Switch options
+    int strictness = {0};
+    bool start_asap = {false};
+    double timeout = {0.0};
+  };
+
+  SwitchParams switch_params_;
 };
 
 }  // namespace controller_manager

controller_manager/package.xml

@@ -11,6 +11,7 @@
 
   <depend>ament_index_cpp</depend>
   <depend>controller_interface</depend>
+  <depend>controller_manager_msgs</depend>
   <depend>hardware_interface</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>