diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 250357c6c8..27823c8cfd 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -19,6 +19,8 @@ import sys import time import warnings +import io +from contextlib import redirect_stdout, redirect_stderr from controller_manager import configure_controller, list_controllers, \ load_controller, switch_controllers, unload_controller @@ -120,8 +122,7 @@ 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', help='Name of the controller manager ROS node', default='controller_manager', required=False) @@ -153,10 +154,17 @@ def main(args=None): parser.add_argument( '--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, + ) 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 +174,9 @@ 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_names[0]) - node = Node('spawner_' + controller_name) - if not controller_manager_name.startswith('/'): + if not controller_manager_name.startswith("/"): spawner_namespace = node.get_namespace() if spawner_namespace != '/': controller_manager_name = f"{spawner_namespace}/{controller_manager_name}" @@ -184,64 +189,144 @@ def main(args=None): node.get_logger().error('Controller manager not available') return 1 - if is_controller_loaded(node, controller_manager_name, prefixed_controller_name): - node.get_logger().warn('Controller already loaded, skipping load_controller') - 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) + 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 - 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(node=node, node_name=prefixed_controller_name, parameter_file=param_file, - use_wildcard=True) - 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 args.stopped and 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.stopped and 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('Failed to configure controller') + node.get_logger().error( + bcolors.FAIL + "Failed to activate the parsed controllers list" + bcolors.ENDC + ) return 1 - if not args.stopped and not args.inactive: - ret = switch_controllers( - node, - controller_manager_name, - [], - [controller_name], - True, - True, - 5.0) - if not ret.ok: - node.get_logger().error('Failed to activate controller') - return 1 - - node.get_logger().info(bcolors.OKGREEN + 'Configured and activated ' + - bcolors.BOLD + prefixed_controller_name + bcolors.ENDC) - elif args.stopped: - node.get_logger().warn('"--stopped" flag is deprecated use "--inactive" instead') + node.get_logger().info( + bcolors.OKGREEN + + "Configured and activated all the parsed controllers list!" + + bcolors.ENDC + ) + if args.stopped: + node.get_logger().warn('"--stopped" flag is deprecated use "--inactive" instead') if not args.unload_on_kill: return 0 @@ -252,15 +337,11 @@ def main(args=None): time.sleep(1) except KeyboardInterrupt: if not args.stopped and not args.inactive: - node.get_logger().info('Interrupt captured, deactivating and unloading controller') + 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('Failed to deactivate controller') return 1 diff --git a/controller_manager/controller_manager/unspawner.py b/controller_manager/controller_manager/unspawner.py index 4020e2ad06..efa385e0a5 100644 --- a/controller_manager/controller_manager/unspawner.py +++ b/controller_manager/controller_manager/unspawner.py @@ -28,35 +28,30 @@ 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', help='Name of the controller manager ROS node', default='/controller_manager', 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 - 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.get_logger().info('Deactivated controller') + 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 12d5566889..7c03a8862d 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/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 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