Refactor spawner to be able to reuse code for ros2controlcli (#1661)

(cherry picked from commit 0631e3e)

# Conflicts:
#	controller_manager/controller_manager/spawner.py
#	controller_manager/test/test_spawner_unspawner.cpp

controller_manager/controller_manager/__init__.py

@@ -23,6 +23,10 @@
     set_hardware_component_state,
     switch_controllers,
     unload_controller,
+    get_parameter_from_param_file,
+    set_controller_parameters,
+    set_controller_parameters_from_param_file,
+    bcolors,
 )
 
 __all__ = [
@@ -36,4 +40,8 @@
     "set_hardware_component_state",
     "switch_controllers",
     "unload_controller",
+    "get_parameter_from_param_file",
+    "set_controller_parameters",
+    "set_controller_parameters_from_param_file",
+    "bcolors",
 ]

controller_manager/controller_manager/controller_manager_services.py

@@ -26,6 +26,29 @@
 )
 
 import rclpy
+import yaml
+from rcl_interfaces.msg import Parameter
+
+# @note: The versions conditioning is added here to support the source-compatibility with Humble
+# The `get_parameter_value` function is moved to `rclpy.parameter` module from `ros2param.api` module from version 3.6.0
+try:
+    from rclpy.parameter import get_parameter_value
+except ImportError:
+    from ros2param.api import get_parameter_value
+from ros2param.api import call_set_parameters
+
+
+# from https://stackoverflow.com/a/287944
+class bcolors:
+    MAGENTA = "\033[95m"
+    OKBLUE = "\033[94m"
+    OKCYAN = "\033[96m"
+    OKGREEN = "\033[92m"
+    WARNING = "\033[93m"
+    FAIL = "\033[91m"
+    ENDC = "\033[0m"
+    BOLD = "\033[1m"
+    UNDERLINE = "\033[4m"
 
 
 class ServiceNotFoundError(Exception):
@@ -220,3 +243,97 @@ def unload_controller(node, controller_manager_name, controller_name, service_ti
         request,
         service_timeout,
     )
+
+
+def get_parameter_from_param_file(controller_name, namespace, parameter_file, parameter_name):
+    with open(parameter_file) as f:
+        namespaced_controller = (
+            controller_name if namespace == "/" else f"{namespace}/{controller_name}"
+        )
+        parameters = yaml.safe_load(f)
+        if namespaced_controller in parameters:
+            value = parameters[namespaced_controller]
+            if not isinstance(value, dict) or "ros__parameters" not in value:
+                raise RuntimeError(
+                    f"YAML file : {parameter_file} is not a valid ROS parameter file for controller : {namespaced_controller}"
+                )
+            if parameter_name in parameters[namespaced_controller]["ros__parameters"]:
+                return parameters[namespaced_controller]["ros__parameters"][parameter_name]
+            else:
+                return None
+        else:
+            return None
+
+
+def set_controller_parameters(
+    node, controller_manager_name, controller_name, parameter_name, parameter_value
+):
+    parameter = Parameter()
+    parameter.name = controller_name + "." + parameter_name
+    parameter_string = str(parameter_value)
+    parameter.value = get_parameter_value(string_value=parameter_string)
+
+    response = call_set_parameters(
+        node=node, node_name=controller_manager_name, parameters=[parameter]
+    )
+    assert len(response.results) == 1
+    result = response.results[0]
+    if result.successful:
+        node.get_logger().info(
+            bcolors.OKCYAN
+            + 'Setting controller param "'
+            + parameter_name
+            + '" to "'
+            + parameter_string
+            + '" for '
+            + bcolors.BOLD
+            + controller_name
+            + bcolors.ENDC
+        )
+    else:
+        node.get_logger().fatal(
+            bcolors.FAIL
+            + 'Could not set controller param "'
+            + parameter_name
+            + '" to "'
+            + parameter_string
+            + '" for '
+            + bcolors.BOLD
+            + controller_name
+            + bcolors.ENDC
+        )
+        return False
+    return True
+
+
+def set_controller_parameters_from_param_file(
+    node, controller_manager_name, controller_name, parameter_file, namespace=None
+):
+    if parameter_file:
+        spawner_namespace = namespace if namespace else node.get_namespace()
+        set_controller_parameters(
+            node, controller_manager_name, controller_name, "param_file", parameter_file
+        )
+
+        controller_type = get_parameter_from_param_file(
+            controller_name, spawner_namespace, parameter_file, "type"
+        )
+        if controller_type:
+            if not set_controller_parameters(
+                node, controller_manager_name, controller_name, "type", controller_type
+            ):
+                return False
+
+        fallback_controllers = get_parameter_from_param_file(
+            controller_name, spawner_namespace, parameter_file, "fallback_controllers"
+        )
+        if fallback_controllers:
+            if not set_controller_parameters(
+                node,
+                controller_manager_name,
+                controller_name,
+                "fallback_controllers",
+                fallback_controllers,
+            ):
+                return False
+    return True

controller_manager/controller_manager/hardware_spawner.py

@@ -19,6 +19,7 @@
 from controller_manager import (
     list_hardware_components,
     set_hardware_component_state,
+    bcolors,
 )
 from controller_manager.controller_manager_services import ServiceNotFoundError
 
@@ -28,19 +29,6 @@
 from rclpy.signals import SignalHandlerOptions
 
 
-# from https://stackoverflow.com/a/287944
-class bcolors:
-    HEADER = "\033[95m"
-    OKBLUE = "\033[94m"
-    OKCYAN = "\033[96m"
-    OKGREEN = "\033[92m"
-    WARNING = "\033[93m"
-    FAIL = "\033[91m"
-    ENDC = "\033[0m"
-    BOLD = "\033[1m"
-    UNDERLINE = "\033[4m"
-
-
 def first_match(iterable, predicate):
     return next((n for n in iterable if predicate(n)), None)
 

controller_manager/controller_manager/spawner.py

@@ -19,37 +19,25 @@
 import sys
 import time
 import warnings
-import yaml
 
 from controller_manager import (
     configure_controller,
     list_controllers,
     load_controller,
     switch_controllers,
     unload_controller,
+    set_controller_parameters_from_param_file,
+    bcolors,
 )
 from controller_manager.controller_manager_services import ServiceNotFoundError
 
 import rclpy
-from rcl_interfaces.msg import Parameter
 from rclpy.node import Node
+<<<<<<< HEAD
 from rclpy.parameter import get_parameter_value
+=======
+>>>>>>> 0631e3e (Refactor spawner to be able to reuse code for ros2controlcli (#1661))
 from rclpy.signals import SignalHandlerOptions
-from ros2param.api import call_set_parameters
-
-# from https://stackoverflow.com/a/287944
-
-
-class bcolors:
-    MAGENTA = "\033[95m"
-    OKBLUE = "\033[94m"
-    OKCYAN = "\033[96m"
-    OKGREEN = "\033[92m"
-    WARNING = "\033[93m"
-    FAIL = "\033[91m"
-    ENDC = "\033[0m"
-    BOLD = "\033[1m"
-    UNDERLINE = "\033[4m"
 
 
 def first_match(iterable, predicate):
@@ -81,24 +69,6 @@ def is_controller_loaded(node, controller_manager, controller_name, service_time
     return any(c.name == controller_name for c in controllers)
 
 
-def get_parameter_from_param_file(controller_name, namespace, parameter_file, parameter_name):
-    with open(parameter_file) as f:
-        namespaced_controller = (
-            controller_name if namespace == "/" else f"{namespace}/{controller_name}"
-        )
-        parameters = yaml.safe_load(f)
-        if namespaced_controller in parameters:
-            value = parameters[namespaced_controller]
-            if not isinstance(value, dict) or "ros__parameters" not in value:
-                raise RuntimeError(
-                    f"YAML file : {parameter_file} is not a valid ROS parameter file for controller : {namespaced_controller}"
-                )
-            if parameter_name in parameters[namespaced_controller]["ros__parameters"]:
-                return parameters[namespaced_controller]["ros__parameters"][parameter_name]
-            else:
-                return None
-
-
 def main(args=None):
     rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO)
     parser = argparse.ArgumentParser()
@@ -190,6 +160,10 @@ def main(args=None):
 
     try:
         for controller_name in controller_names:
+<<<<<<< HEAD
+=======
+
+>>>>>>> 0631e3e (Refactor spawner to be able to reuse code for ros2controlcli (#1661))
             if is_controller_loaded(
                 node, controller_manager_name, controller_name, controller_manager_timeout
             ):
@@ -199,6 +173,7 @@ def main(args=None):
                     + bcolors.ENDC
                 )
             else:
+<<<<<<< HEAD
                 controller_type = (
                     args.controller_type
                     if param_file is None
@@ -268,6 +243,16 @@ def main(args=None):
                             + controller_name
                             + bcolors.ENDC
                         )
+=======
+                if param_file:
+                    if not set_controller_parameters_from_param_file(
+                        node,
+                        controller_manager_name,
+                        controller_name,
+                        param_file,
+                        spawner_namespace,
+                    ):
+>>>>>>> 0631e3e (Refactor spawner to be able to reuse code for ros2controlcli (#1661))
                         return 1
 
                 ret = load_controller(node, controller_manager_name, controller_name)

controller_manager/test/test_spawner_unspawner.cpp

@@ -283,6 +283,57 @@ TEST_F(TestLoadController, unload_on_kill)
   ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul);
 }
 
+<<<<<<< HEAD
+=======
+TEST_F(TestLoadController, spawner_test_fallback_controllers)
+{
+  const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") +
+                                     "/test/test_controller_spawner_with_fallback_controllers.yaml";
+
+  cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
+  cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
+  cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
+
+  ControllerManagerRunner cm_runner(this);
+  EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager --load-only -p " + test_file_path), 0);
+
+  ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul);
+  {
+    auto ctrl_1 = cm_->get_loaded_controllers()[0];
+    ASSERT_EQ(ctrl_1.info.name, "ctrl_1");
+    ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
+    ASSERT_TRUE(ctrl_1.info.fallback_controllers_names.empty());
+    ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+  }
+
+  // Try to spawn now the controller with fallback controllers inside the yaml
+  EXPECT_EQ(
+    call_spawner("ctrl_2 ctrl_3 -c test_controller_manager --load-only -p " + test_file_path), 0);
+
+  ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul);
+  {
+    auto ctrl_1 = cm_->get_loaded_controllers()[0];
+    ASSERT_EQ(ctrl_1.info.name, "ctrl_1");
+    ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
+    ASSERT_TRUE(ctrl_1.info.fallback_controllers_names.empty());
+    ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+
+    auto ctrl_2 = cm_->get_loaded_controllers()[1];
+    ASSERT_EQ(ctrl_2.info.name, "ctrl_2");
+    ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
+    ASSERT_THAT(
+      ctrl_2.info.fallback_controllers_names, testing::ElementsAre("ctrl_6", "ctrl_7", "ctrl_8"));
+    ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+
+    auto ctrl_3 = cm_->get_loaded_controllers()[2];
+    ASSERT_EQ(ctrl_3.info.name, "ctrl_3");
+    ASSERT_EQ(ctrl_3.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
+    ASSERT_THAT(ctrl_3.info.fallback_controllers_names, testing::ElementsAre("ctrl_9"));
+    ASSERT_EQ(ctrl_3.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+  }
+}
+
+>>>>>>> 0631e3e (Refactor spawner to be able to reuse code for ros2controlcli (#1661))
 TEST_F(TestLoadController, spawner_with_many_controllers)
 {
   std::stringstream ss;

ros2controlcli/ros2controlcli/verb/list_controllers.py

@@ -12,8 +12,7 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-from controller_manager import list_controllers
-from controller_manager.spawner import bcolors
+from controller_manager import list_controllers, bcolors
 
 from ros2cli.node.direct import add_arguments
 from ros2cli.node.strategy import NodeStrategy

ros2controlcli/ros2controlcli/verb/list_hardware_components.py

@@ -12,8 +12,7 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-from controller_manager import list_hardware_components
-from controller_manager.spawner import bcolors
+from controller_manager import list_hardware_components, bcolors
 
 from lifecycle_msgs.msg import State
 

ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py

@@ -12,8 +12,7 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-from controller_manager import list_hardware_interfaces
-from controller_manager.spawner import bcolors
+from controller_manager import list_hardware_interfaces, bcolors
 
 from ros2cli.node.direct import add_arguments
 from ros2cli.node.strategy import NodeStrategy