Update spawner to accept controllers list and start them in sequence (backport #1139)

controller_manager/controller_manager/spawner.py

@@ -120,6 +120,10 @@ def main(args=None):
 
     rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO)
     parser = argparse.ArgumentParser()
+<<<<<<< HEAD
+=======
+    parser.add_argument("controller_names", help="List of controllers", nargs="+")
+>>>>>>> 4e78813 (Update spawner to accept controllers list and start them in sequence (#1139))
     parser.add_argument(
         'controller_name', help='Name of the controller')
     parser.add_argument(
@@ -143,6 +147,7 @@ def main(args=None):
         '--inactive', help='Load and configure the controller, however do not activate them',
         action='store_true', required=False)
     parser.add_argument(
+<<<<<<< HEAD
         '-t', '--controller-type',
         help='If not provided it should exist in the controller manager namespace',
         default=None, required=False)
@@ -153,10 +158,25 @@ def main(args=None):
     parser.add_argument(
         '--controller-manager-timeout',
         help='Time to wait for the controller manager', required=False, default=10, type=int)
+=======
+        "--controller-manager-timeout",
+        help="Time to wait for the controller manager",
+        required=False,
+        default=10,
+        type=int,
+    )
+    parser.add_argument(
+        "--activate-as-group",
+        help="Activates all the parsed controllers list together instead of one by one."
+        " Useful for activating all chainable controllers altogether",
+        action="store_true",
+        required=False,
+    )
+>>>>>>> 4e78813 (Update spawner to accept controllers list and start them in sequence (#1139))
 
     command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:]
     args = parser.parse_args(command_line_args)
-    controller_name = args.controller_name
+    controller_names = args.controller_names
     controller_manager_name = args.controller_manager
     controller_namespace = args.namespace
     param_file = args.param_file
@@ -166,12 +186,18 @@ def main(args=None):
     if param_file and not os.path.isfile(param_file):
         raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file)
 
+<<<<<<< HEAD
     prefixed_controller_name = controller_name
     if controller_namespace:
         prefixed_controller_name = controller_namespace + '/' + controller_name
 
     node = Node('spawner_' + controller_name)
     if not controller_manager_name.startswith('/'):
+=======
+    node = Node("spawner_" + controller_names[0])
+
+    if not controller_manager_name.startswith("/"):
+>>>>>>> 4e78813 (Update spawner to accept controllers list and start them in sequence (#1139))
         spawner_namespace = node.get_namespace()
         if spawner_namespace != '/':
             controller_manager_name = f"{spawner_namespace}/{controller_manager_name}"
@@ -184,6 +210,7 @@ def main(args=None):
             node.get_logger().error('Controller manager not available')
             return 1
 
+<<<<<<< HEAD
         if is_controller_loaded(node, controller_manager_name, prefixed_controller_name):
             node.get_logger().warn('Controller already loaded, skipping load_controller')
         else:
@@ -242,6 +269,144 @@ def main(args=None):
                                        bcolors.BOLD + prefixed_controller_name + bcolors.ENDC)
             elif args.stopped:
                 node.get_logger().warn('"--stopped" flag is deprecated use "--inactive" instead')
+=======
+        for controller_name in controller_names:
+            prefixed_controller_name = controller_name
+            if controller_namespace:
+                prefixed_controller_name = controller_namespace + "/" + controller_name
+
+            if is_controller_loaded(node, controller_manager_name, prefixed_controller_name):
+                node.get_logger().warn(
+                    bcolors.WARNING
+                    + "Controller already loaded, skipping load_controller"
+                    + bcolors.ENDC
+                )
+            else:
+                if controller_type:
+                    parameter = Parameter()
+                    parameter.name = prefixed_controller_name + ".type"
+                    parameter.value = get_parameter_value(string_value=controller_type)
+
+                    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
+                            + 'Set controller type to "'
+                            + controller_type
+                            + '" for '
+                            + bcolors.BOLD
+                            + prefixed_controller_name
+                            + bcolors.ENDC
+                        )
+                    else:
+                        node.get_logger().fatal(
+                            bcolors.FAIL
+                            + 'Could not set controller type to "'
+                            + controller_type
+                            + '" for '
+                            + bcolors.BOLD
+                            + prefixed_controller_name
+                            + bcolors.ENDC
+                        )
+                        return 1
+
+                ret = load_controller(node, controller_manager_name, controller_name)
+                if not ret.ok:
+                    node.get_logger().fatal(
+                        bcolors.FAIL
+                        + "Failed loading controller "
+                        + bcolors.BOLD
+                        + prefixed_controller_name
+                        + bcolors.ENDC
+                    )
+                    return 1
+                node.get_logger().info(
+                    bcolors.OKBLUE
+                    + "Loaded "
+                    + bcolors.BOLD
+                    + prefixed_controller_name
+                    + bcolors.ENDC
+                )
+
+            if param_file:
+                # load_parameter_file writes to stdout/stderr. Here we capture that and use node logging instead
+                with redirect_stdout(io.StringIO()) as f_stdout, redirect_stderr(
+                    io.StringIO()
+                ) as f_stderr:
+                    load_parameter_file(
+                        node=node,
+                        node_name=prefixed_controller_name,
+                        parameter_file=param_file,
+                        use_wildcard=True,
+                    )
+                if f_stdout.getvalue():
+                    node.get_logger().info(bcolors.OKCYAN + f_stdout.getvalue() + bcolors.ENDC)
+                if f_stderr.getvalue():
+                    node.get_logger().error(bcolors.FAIL + f_stderr.getvalue() + bcolors.ENDC)
+                node.get_logger().info(
+                    bcolors.OKCYAN
+                    + 'Loaded parameters file "'
+                    + param_file
+                    + '" for '
+                    + bcolors.BOLD
+                    + prefixed_controller_name
+                    + bcolors.ENDC
+                )
+                # TODO(destogl): use return value when upstream return value is merged
+                # ret =
+                # if ret.returncode != 0:
+                #     Error message printed by ros2 param
+                #     return ret.returncode
+                node.get_logger().info(
+                    "Loaded " + param_file + " into " + prefixed_controller_name
+                )
+
+            if not args.load_only:
+                ret = configure_controller(node, controller_manager_name, controller_name)
+                if not ret.ok:
+                    node.get_logger().error(
+                        bcolors.FAIL + "Failed to configure controller" + bcolors.ENDC
+                    )
+                    return 1
+
+                if not args.inactive and not args.activate_as_group:
+                    ret = switch_controllers(
+                        node, controller_manager_name, [], [controller_name], True, True, 5.0
+                    )
+                    if not ret.ok:
+                        node.get_logger().error(
+                            bcolors.FAIL + "Failed to activate controller" + bcolors.ENDC
+                        )
+                        return 1
+
+                    node.get_logger().info(
+                        bcolors.OKGREEN
+                        + "Configured and activated "
+                        + bcolors.BOLD
+                        + prefixed_controller_name
+                        + bcolors.ENDC
+                    )
+
+        if not args.inactive and args.activate_as_group:
+            ret = switch_controllers(
+                node, controller_manager_name, [], controller_names, True, True, 5.0
+            )
+            if not ret.ok:
+                node.get_logger().error(
+                    bcolors.FAIL + "Failed to activate the parsed controllers list" + bcolors.ENDC
+                )
+                return 1
+
+            node.get_logger().info(
+                bcolors.OKGREEN
+                + "Configured and activated all the parsed controllers list!"
+                + bcolors.ENDC
+            )
+>>>>>>> 4e78813 (Update spawner to accept controllers list and start them in sequence (#1139))
 
         if not args.unload_on_kill:
             return 0
@@ -251,6 +416,7 @@ def main(args=None):
             while True:
                 time.sleep(1)
         except KeyboardInterrupt:
+<<<<<<< HEAD
             if not args.stopped and not args.inactive:
                 node.get_logger().info('Interrupt captured, deactivating and unloading controller')
                 ret = switch_controllers(
@@ -261,6 +427,14 @@ def main(args=None):
                     True,
                     True,
                     5.0)
+=======
+            if not args.inactive:
+                node.get_logger().info("Interrupt captured, deactivating and unloading controller")
+                # TODO(saikishor) we might have an issue in future, if any of these controllers is in chained mode
+                ret = switch_controllers(
+                    node, controller_manager_name, controller_names, [], True, True, 5.0
+                )
+>>>>>>> 4e78813 (Update spawner to accept controllers list and start them in sequence (#1139))
                 if not ret.ok:
                     node.get_logger().error('Failed to deactivate controller')
                     return 1

controller_manager/controller_manager/unspawner.py

@@ -28,6 +28,10 @@ def main(args=None):
 
     rclpy.init(args=args)
     parser = argparse.ArgumentParser()
+<<<<<<< HEAD
+=======
+    parser.add_argument("controller_names", help="Name of the controller", nargs="+")
+>>>>>>> 4e78813 (Update spawner to accept controllers list and start them in sequence (#1139))
     parser.add_argument(
         'controller_name', help='Name of the controller')
     parser.add_argument(
@@ -36,9 +40,10 @@ def main(args=None):
 
     command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:]
     args = parser.parse_args(command_line_args)
-    controller_name = args.controller_name
+    controller_names = args.controller_names
     controller_manager_name = args.controller_manager
 
+<<<<<<< HEAD
     node = Node('unspawner_' + controller_name)
     try:
         # Ignore returncode, because message is already printed and we'll try to unload anyway
@@ -57,6 +62,22 @@ def main(args=None):
             node.get_logger().info('Failed to unload controller')
             return 1
         node.get_logger().info('Unloaded controller')
+=======
+    node = Node("unspawner_" + controller_names[0])
+    try:
+        # Ignore returncode, because message is already printed and we'll try to unload anyway
+        ret = switch_controllers(
+            node, controller_manager_name, controller_names, [], True, True, 5.0
+        )
+        node.get_logger().info("Deactivated controller")
+
+        for controller_name in controller_names:
+            ret = unload_controller(node, controller_manager_name, controller_name)
+            if not ret.ok:
+                node.get_logger().info("Failed to unload controller")
+                return 1
+            node.get_logger().info("Unloaded controller")
+>>>>>>> 4e78813 (Update spawner to accept controllers list and start them in sequence (#1139))
 
         return 0
     finally:

controller_manager/test/test_spawner_unspawner.cpp

@@ -125,6 +125,93 @@ TEST_F(TestLoadController, spawner_test_type_in_param)
   ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
 }
 
+TEST_F(TestLoadController, multi_ctrls_test_type_in_param)
+{
+  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 ctrl_2 -c test_controller_manager"), 0);
+
+  auto validate_loaded_controllers = [&]()
+  {
+    auto loaded_controllers = cm_->get_loaded_controllers();
+    for (size_t i = 0; i < loaded_controllers.size(); i++)
+    {
+      auto ctrl = loaded_controllers[i];
+      const std::string controller_name = "ctrl_" + std::to_string(i + 1);
+
+      RCLCPP_ERROR(rclcpp::get_logger("test_controller_manager"), "%s", controller_name.c_str());
+      auto it = std::find_if(
+        loaded_controllers.begin(), loaded_controllers.end(),
+        [&](const auto & controller) { return controller.info.name == controller_name; });
+      ASSERT_TRUE(it != loaded_controllers.end());
+      ASSERT_EQ(ctrl.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
+      ASSERT_EQ(ctrl.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+    }
+  };
+
+  ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul);
+  {
+    validate_loaded_controllers();
+  }
+
+  // Try to spawn again multiple but one of them is already active, it should fail because already
+  // active
+  EXPECT_NE(call_spawner("ctrl_1 ctrl_3 -c test_controller_manager"), 0)
+    << "Cannot configure from active";
+
+  std::vector<std::string> start_controllers = {};
+  std::vector<std::string> stop_controllers = {"ctrl_1"};
+  cm_->switch_controller(
+    start_controllers, stop_controllers,
+    controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
+
+  // We should be able to reconfigure and activate a configured controller
+  EXPECT_EQ(call_spawner("ctrl_1 ctrl_3 -c test_controller_manager"), 0);
+
+  ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul);
+  {
+    validate_loaded_controllers();
+  }
+
+  stop_controllers = {"ctrl_1", "ctrl_2"};
+  cm_->switch_controller(
+    start_controllers, stop_controllers,
+    controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
+  for (auto ctrl : stop_controllers)
+  {
+    cm_->unload_controller(ctrl);
+    cm_->load_controller(ctrl);
+  }
+
+  // We should be able to reconfigure and activate am unconfigured loaded controller
+  EXPECT_EQ(call_spawner("ctrl_1 ctrl_2 -c test_controller_manager"), 0);
+  ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul);
+  {
+    validate_loaded_controllers();
+  }
+
+  // Unload and reload
+  EXPECT_EQ(call_unspawner("ctrl_1 ctrl_3 -c test_controller_manager"), 0);
+  ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul) << "Controller should have been unloaded";
+  EXPECT_EQ(call_spawner("ctrl_1 ctrl_3 -c test_controller_manager"), 0);
+  ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul) << "Controller should have been loaded";
+  {
+    validate_loaded_controllers();
+  }
+
+  // Now unload everything and load them as a group in a single operation
+  EXPECT_EQ(call_unspawner("ctrl_1 ctrl_2 ctrl_3 -c test_controller_manager"), 0);
+  ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul) << "Controller should have been unloaded";
+  EXPECT_EQ(call_spawner("ctrl_1 ctrl_2 ctrl_3 -c test_controller_manager --activate-as-group"), 0);
+  ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul) << "Controller should have been loaded";
+  {
+    validate_loaded_controllers();
+  }
+}
+
 TEST_F(TestLoadController, spawner_test_type_in_arg)
 {
   // Provide controller type via -t argument