diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 8463c11b19..3ca5600997 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -135,7 +135,7 @@ def is_controller_loaded(node, controller_manager, controller_name): def main(args=None): rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO) parser = argparse.ArgumentParser() - parser.add_argument("controller_name", help="Name of the controller") + parser.add_argument("controller_names", help="List of controllers", nargs="+") parser.add_argument( "-c", "--controller-manager", @@ -184,10 +184,17 @@ def main(args=None): 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, + ) 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 @@ -197,11 +204,7 @@ def main(args=None): if param_file and not os.path.isfile(param_file): raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file) - prefixed_controller_name = controller_name - if controller_namespace: - prefixed_controller_name = controller_namespace + "/" + controller_name - - node = Node("spawner_" + controller_name) + node = Node("spawner_" + controller_names[0]) if not controller_manager_name.startswith("/"): spawner_namespace = node.get_namespace() @@ -219,115 +222,142 @@ def main(args=None): ) return 1 - 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) + for controller_name in controller_names: + prefixed_controller_name = controller_name + if controller_namespace: + prefixed_controller_name = controller_namespace + "/" + controller_name - response = call_set_parameters( - node=node, node_name=controller_manager_name, parameters=[parameter] + 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 ) - 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: + 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] ) - else: + 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 - + 'Could not set controller type to "' - + controller_type - + '" for ' + + "Failed loading controller " + 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 " + node.get_logger().info( + bcolors.OKBLUE + + "Loaded " + 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 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 ) - 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 + # 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 ) - return 1 - if not args.inactive: - ret = switch_controllers( - node, controller_manager_name, [], [controller_name], True, True, 5.0 - ) + 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 activate controller" + bcolors.ENDC + bcolors.FAIL + "Failed to configure 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 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 + ) if not args.unload_on_kill: return 0 @@ -339,8 +369,9 @@ def main(args=None): except KeyboardInterrupt: 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_name], [], True, True, 5.0 + node, controller_manager_name, controller_names, [], True, True, 5.0 ) if not ret.ok: node.get_logger().error( diff --git a/controller_manager/controller_manager/unspawner.py b/controller_manager/controller_manager/unspawner.py index 766dc5d4f8..5acf05e65d 100644 --- a/controller_manager/controller_manager/unspawner.py +++ b/controller_manager/controller_manager/unspawner.py @@ -27,7 +27,7 @@ def main(args=None): rclpy.init(args=args) parser = argparse.ArgumentParser() - parser.add_argument("controller_name", help="Name of the controller") + parser.add_argument("controller_names", help="Name of the controller", nargs="+") parser.add_argument( "-c", "--controller-manager", @@ -38,22 +38,23 @@ 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 - node = Node("unspawner_" + controller_name) + 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_name], [], True, True, 5.0 + node, controller_manager_name, controller_names, [], True, True, 5.0 ) node.get_logger().info("Deactivated controller") - 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") + 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") return 0 finally: diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 4137386f72..1ecc3164b8 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -156,6 +156,93 @@ TEST_F(TestLoadController, spawner_test_type_in_param) } } +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 start_controllers = {}; + std::vector 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