[CM] Set default strictness of switch_controllers using parameters (#2168)

(cherry picked from commit ad2efc6)

Author: saikishor

controller_manager/controller_manager/controller_manager_services.py

@@ -275,18 +275,15 @@ def switch_controllers(
     controller_manager_name,
     deactivate_controllers,
     activate_controllers,
-    strict,
+    strictness,
     activate_asap,
     timeout,
     call_timeout=10.0,
 ):
     request = SwitchController.Request()
     request.activate_controllers = activate_controllers
     request.deactivate_controllers = deactivate_controllers
-    if strict:
-        request.strictness = SwitchController.Request.STRICT
-    else:
-        request.strictness = SwitchController.Request.BEST_EFFORT
+    request.strictness = strictness
     request.activate_asap = activate_asap
     request.timeout = rclpy.duration.Duration(seconds=timeout).to_msg()
     return service_caller(

controller_manager/controller_manager/spawner.py

@@ -30,6 +30,7 @@
     set_controller_parameters_from_param_files,
     bcolors,
 )
+from controller_manager_msgs.srv import SwitchController
 from controller_manager.controller_manager_services import ServiceNotFoundError
 
 from filelock import Timeout, FileLock
@@ -164,6 +165,7 @@ def main(args=None):
     controller_manager_timeout = args.controller_manager_timeout
     service_call_timeout = args.service_call_timeout
     switch_timeout = args.switch_timeout
+    strictness = SwitchController.Request.STRICT
 
     if param_files:
         for param_file in param_files:
@@ -299,7 +301,7 @@ def main(args=None):
                         controller_manager_name,
                         [],
                         [controller_name],
-                        True,
+                        strictness,
                         True,
                         switch_timeout,
                         service_call_timeout,
@@ -324,7 +326,7 @@ def main(args=None):
                 controller_manager_name,
                 [],
                 controller_names,
-                True,
+                strictness,
                 True,
                 switch_timeout,
                 service_call_timeout,
@@ -358,7 +360,7 @@ def main(args=None):
                     controller_manager_name,
                     controller_names,
                     [],
-                    True,
+                    strictness,
                     True,
                     switch_timeout,
                     service_call_timeout,

controller_manager/controller_manager/unspawner.py

@@ -19,6 +19,7 @@
 import warnings
 
 from controller_manager import switch_controllers, unload_controller
+from controller_manager_msgs.srv import SwitchController
 from controller_manager.controller_manager_services import ServiceNotFoundError
 
 import rclpy
@@ -51,6 +52,7 @@ def main(args=None):
     controller_names = args.controller_names
     controller_manager_name = args.controller_manager
     switch_timeout = args.switch_timeout
+    strictness = SwitchController.Request.STRICT
 
     node = Node("unspawner_" + controller_names[0])
     try:
@@ -60,7 +62,7 @@ def main(args=None):
             controller_manager_name,
             controller_names,
             [],
-            True,
+            strictness,
             True,
             switch_timeout,
         )

controller_manager/src/controller_manager.cpp

@@ -1321,14 +1321,22 @@ controller_interface::return_type ControllerManager::switch_controller_cb(
   }
   if (strictness == 0)
   {
-    RCLCPP_WARN(
+    std::string default_strictness = params_->defaults.switch_controller.strictness;
+    // Convert to uppercase
+    std::transform(
+      default_strictness.begin(), default_strictness.end(), default_strictness.begin(),
+      [](unsigned char c) { return std::toupper(c); });
+    RCLCPP_WARN_ONCE(
       get_logger(),
       "Controller Manager: to switch controllers you need to specify a "
       "strictness level of controller_manager_msgs::SwitchController::STRICT "
-      "(%d) or ::BEST_EFFORT (%d). Defaulting to ::BEST_EFFORT",
+      "(%d) or ::BEST_EFFORT (%d). When unspecified, the default is %s",
       controller_manager_msgs::srv::SwitchController::Request::STRICT,
-      controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT);
-    strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;
+      controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT,
+      default_strictness.c_str());
+    strictness = params_->defaults.switch_controller.strictness == "strict"
+                   ? controller_manager_msgs::srv::SwitchController::Request::STRICT
+                   : controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;
   }
 
   std::string activate_list, deactivate_list;

controller_manager/src/controller_manager_parameters.yaml

@@ -25,6 +25,21 @@ controller_manager:
       }
     }
 
+  defaults:
+    switch_controller:
+      strictness: {
+        type: string,
+        default_value: "best_effort",
+        description: "The default switch controller strategy. This strategy is used when no strategy is specified in the switch_controller service call.",
+        validation: {
+          not_empty<>: null,
+          one_of<>: [[
+            "strict",
+            "best_effort",
+          ]],
+        }
+      }
+
   diagnostics:
     threshold:
       controller_manager:

doc/release_notes.rst

@@ -183,6 +183,10 @@ Changes from `(PR #1688) <https://github.com/ros-controls/ros2_control/pull/1688
 * To access the automatically created ``Command-/StateInterfaces`` we provide the ``std::unordered_map<std::string, InterfaceDescription>``, where the string is the fully qualified name of the interface and the ``InterfaceDescription`` is the configuration of the interface. The ``std::unordered_map<>`` are divided into ``type_state_interfaces_`` and ``type_command_interfaces_`` where type can be: ``joint``, ``sensor``, ``gpio`` and ``unlisted``. E.g. the ``CommandInterfaces`` for all joints can be found in the  ``joint_command_interfaces_`` map. The ``unlisted`` includes all interfaces not listed in the ``ros2_control`` XML-tag but were created by overriding the ``export_unlisted_command_interfaces()`` or ``export_unlisted_state_interfaces()`` function by creating some custom ``Command-/StateInterfaces``.
 
 
+controller_manager
+******************
+* The default strictness of the ``swtich_controllers`` can now we be chosen using ROS 2 parameters. The default behaviour is still left to ``BEST_EFFORT`` (`#2168 <https://github.com/ros-controls/ros2_control/pull/2168>`_).
+
 ros2controlcli
 **************
 * Spawner colours were added to ``list_controllers`` depending upon active or inactive (`#1409 <https://github.com/ros-controls/ros2_control/pull/1409>`_)

ros2controlcli/ros2controlcli/verb/switch_controllers.py

@@ -13,6 +13,7 @@
 # limitations under the License.
 
 from controller_manager import switch_controllers, bcolors
+from controller_manager_msgs.srv import SwitchController
 
 from ros2cli.node.direct import add_arguments
 from ros2cli.node.strategy import NodeStrategy
@@ -40,7 +41,17 @@ def add_arguments(self, parser, cli_name):
             help="Name of the controllers to be activated",
         )
         arg.completer = LoadedControllerNameCompleter(["inactive"])
-        parser.add_argument("--strict", action="store_true", help="Strict switch")
+        strictness_group = parser.add_mutually_exclusive_group(required=False)
+        strictness_group.add_argument(
+            "--strict",
+            help="Set the switch_controllers service strictness to strict",
+            action="store_true",
+        )
+        strictness_group.add_argument(
+            "--best-effort",
+            help="Set the switch_controllers service strictness to best effort",
+            action="store_true",
+        )
         parser.add_argument("--activate-asap", action="store_true", help="Start asap controllers")
         parser.add_argument(
             "--switch-timeout",
@@ -53,12 +64,17 @@ def add_arguments(self, parser, cli_name):
 
     def main(self, *, args):
         with NodeStrategy(args).direct_node as node:
+            strictness = 0
+            if args.strict:
+                strictness = SwitchController.Request.STRICT
+            elif args.best_effort:
+                strictness = SwitchController.Request.BEST_EFFORT
             response = switch_controllers(
                 node,
                 args.controller_manager,
                 args.deactivate,
                 args.activate,
-                args.strict,
+                strictness,
                 args.activate_asap,
                 args.switch_timeout,
             )