Ability to switch controllers in non-realtime loop

controller_manager/controller_manager/spawner.py

@@ -141,6 +141,13 @@ def main(args=None):
         action="store_true",
         required=False,
     )
+    parser.add_argument(
+        "--switch-asap",
+        help="Option to switch the controllers in the realtime loop at the earliest possible time or in the non-realtime loop.",
+        required=False,
+        default=False,
+        action=argparse.BooleanOptionalAction,
+    )
     parser.add_argument(
         "--controller-ros-args",
         help="The --ros-args to be passed to the controller node, e.g., for remapping topics. "
@@ -160,6 +167,7 @@ def main(args=None):
     switch_timeout = args.switch_timeout
     strictness = SwitchController.Request.STRICT
     unload_controllers_upon_exit = False
+    switch_asap = args.switch_asap
     node = None
 
     if param_files:
@@ -282,7 +290,7 @@ def main(args=None):
                         [],
                         [controller_name],
                         strictness,
-                        True,
+                        switch_asap,
                         switch_timeout,
                         service_call_timeout,
                     )
@@ -307,7 +315,7 @@ def main(args=None):
                 [],
                 controller_names,
                 strictness,
-                True,
+                switch_asap,
                 switch_timeout,
                 service_call_timeout,
             )
@@ -341,7 +349,7 @@ def main(args=None):
                     controller_names,
                     [],
                     strictness,
-                    True,
+                    switch_asap,
                     switch_timeout,
                     service_call_timeout,
                 )

controller_manager/doc/userdoc.rst

@@ -163,9 +163,9 @@ There are two scripts to interact with controller manager from launch files:
 .. code-block:: console
 
     $ ros2 run controller_manager spawner -h
-    usage: spawner [-h] [-c CONTROLLER_MANAGER] [-p PARAM_FILE] [--load-only] [--inactive] [-u] [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT] [--switch-timeout SWITCH_TIMEOUT] [--service-call-timeout SERVICE_CALL_TIMEOUT] [--activate-as-group]
-                  [--controller-ros-args CONTROLLER_ROS_ARGS]
-                  controller_names [controller_names ...]
+    usage: spawner [-h] [-c CONTROLLER_MANAGER] [-p PARAM_FILE] [--load-only] [--inactive] [-u] [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT] [--switch-timeout SWITCH_TIMEOUT]
+               [--service-call-timeout SERVICE_CALL_TIMEOUT] [--activate-as-group] [--switch-asap | --no-switch-asap] [--controller-ros-args CONTROLLER_ROS_ARGS]
+               controller_names [controller_names ...]
 
     positional arguments:
       controller_names      List of controllers
@@ -186,6 +186,8 @@ There are two scripts to interact with controller manager from launch files:
       --service-call-timeout SERVICE_CALL_TIMEOUT
                             Time to wait for the service response from the controller manager
       --activate-as-group   Activates all the parsed controllers list together instead of one by one. Useful for activating all chainable controllers altogether
+      --switch-asap, --no-switch-asap
+                            Option to switch the controllers in the realtime loop at the earliest possible time or in the non-realtime loop.
       --controller-ros-args CONTROLLER_ROS_ARGS
                             The --ros-args to be passed to the controller node, e.g., for remapping topics. Pass multiple times for every argument.
 

controller_manager/include/controller_manager/controller_manager.hpp

@@ -40,6 +40,7 @@
 #include "controller_manager_msgs/srv/unload_controller.hpp"
 
 #include "diagnostic_updater/diagnostic_updater.hpp"
+#include "hardware_interface/helpers.hpp"
 #include "hardware_interface/resource_manager.hpp"
 
 #include "pluginlib/class_loader.hpp"
@@ -259,7 +260,7 @@ class ControllerManager : public rclcpp::Node
    */
   void deactivate_controllers(
     const std::vector<ControllerSpec> & rt_controller_list,
-    const std::vector<std::string> controllers_to_deactivate);
+    const std::vector<std::string> & controllers_to_deactivate);
 
   /**
    * Switch chained mode for all the controllers with respect to the following cases:
@@ -282,22 +283,7 @@ class ControllerManager : public rclcpp::Node
    */
   void activate_controllers(
     const std::vector<ControllerSpec> & rt_controller_list,
-    const std::vector<std::string> controllers_to_activate);
-
-  /// Activate chosen controllers from real-time controller list.
-  /**
-   * Activate controllers with names \p controllers_to_activate from list \p rt_controller_list.
-   * The controller list will be iterated as many times as there are controller names.
-   *
-   * *NOTE*: There is currently not difference to `activate_controllers` method.
-   * Check https://github.com/ros-controls/ros2_control/issues/263 for more information.
-   *
-   * \param[in] rt_controller_list controllers in the real-time list.
-   * \param[in] controllers_to_activate names of the controller that have to be activated.
-   */
-  void activate_controllers_asap(
-    const std::vector<ControllerSpec> & rt_controller_list,
-    const std::vector<std::string> controllers_to_activate);
+    const std::vector<std::string> & controllers_to_activate);
 
   void list_controllers_srv_cb(
     const std::shared_ptr<controller_manager_msgs::srv::ListControllers::Request> request,
@@ -661,11 +647,6 @@ class ControllerManager : public rclcpp::Node
   rclcpp::Service<controller_manager_msgs::srv::SetHardwareComponentState>::SharedPtr
     set_hardware_component_state_service_;
 
-  std::vector<std::string> activate_request_, deactivate_request_;
-  std::vector<std::string> to_chained_mode_request_, from_chained_mode_request_;
-  std::vector<std::string> activate_command_interface_request_,
-    deactivate_command_interface_request_;
-
   std::map<std::string, std::vector<std::string>> controller_chained_reference_interfaces_cache_;
   std::map<std::string, std::vector<std::string>> controller_chained_state_interfaces_cache_;
 
@@ -686,17 +667,34 @@ class ControllerManager : public rclcpp::Node
       activate_asap = false;
     }
 
-    bool do_switch;
+    std::atomic_bool do_switch;
     bool started;
 
     // Switch options
     int strictness;
-    bool activate_asap;
+    std::atomic_bool activate_asap;
     std::chrono::nanoseconds timeout;
 
     // conditional variable and mutex to wait for the switch to complete
     std::condition_variable cv;
     std::mutex mutex;
+
+    bool skip_cycle(const controller_manager::ControllerSpec & spec) const
+    {
+      const std::string controller_name = spec.info.name;
+      return ros2_control::has_item(activate_request, controller_name) ||
+             ros2_control::has_item(deactivate_request, controller_name) ||
+             ros2_control::has_item(to_chained_mode_request, controller_name) ||
+             ros2_control::has_item(from_chained_mode_request, controller_name);
+    }
+
+    // The controllers list to activate and deactivate
+    std::vector<std::string> activate_request;
+    std::vector<std::string> deactivate_request;
+    std::vector<std::string> to_chained_mode_request;
+    std::vector<std::string> from_chained_mode_request;
+    std::vector<std::string> activate_command_interface_request;
+    std::vector<std::string> deactivate_command_interface_request;
   };
 
   SwitchParams switch_params_;