def main(self, *, args): with NodeStrategy(args) as node: controllers = list_controllers(node, args.controller_manager).controller try: matched_controller = [c for c in controllers if c.name == args.controller_name][0] except IndexError: return 'controller {} does not seem to be loaded'.format(args.controller_name) if args.state == 'configure': if matched_controller.state != 'unconfigured': return "can't configure {} from its current state {}{}".format( matched_controller.name, matched_controller.state) response = configure_controller( node, args.controller_manager, args.controller_name) if not response.ok: return 'Error configuring controller, check controller_manager logs' return 'successfully configured {}'.format(args.controller_name) if args.state == 'start': if matched_controller.state != 'inactive': return "can't start {} from its current state {}".format( matched_controller.name, matched_controller.state) response = switch_controllers( node, args.controller_manager, [], [args.controller_name], True, True, 5.0) if not response.ok: return 'Error starting controller, check controller_manager logs' return 'successfully started {}'.format(args.controller_name) if args.state == 'stop': if matched_controller.state != 'active': return "can't stop {} from its current state {}".format( matched_controller.name, matched_controller.state) response = switch_controllers( node, args.controller_manager, [args.controller_name], [], True, True, 5.0) if not response.ok: return 'Error stopping controller, check controller_manager logs' return 'successfully stopped {}'.format(args.controller_name)
def main(self, *, args): with NodeStrategy(args) as node: response = load_controller(node, args.controller_manager, args.controller_name) if not response.ok: return 'Error loading controller, check controller_manager logs' if not args.state: return 'Successfully loaded controller {}'.format( args.controller_name) # we in any case configure the controller response = configure_controller(node, args.controller_manager, args.controller_name) if not response.ok: return 'Error configuring controller' if args.state == 'start': response = switch_controllers(node, args.controller_manager, [], [args.controller_name], True, True, 5.0) if not response.ok: return 'Error starting controller, check controller_manager logs' return 'Sucessfully loaded controller {} into state {}'.format( args.controller_name, ('inactive' if args.state == 'configure' else 'active'))
def main(self, *, args): with NodeStrategy(args) as node: response = load_controller(node, args.controller_manager, args.controller_name) if not response.ok: return 'Error loading controller, check controller_manager logs' if not args.state: print(f'Successfully loaded controller {args.controller_name}') return 0 # we in any case configure the controller response = configure_controller(node, args.controller_manager, args.controller_name) if not response.ok: return 'Error configuring controller' if args.state == 'start': response = switch_controllers(node, args.controller_manager, [], [args.controller_name], True, True, 5.0) if not response.ok: return 'Error starting controller, check controller_manager logs' print( f'Sucessfully loaded controller {args.controller_name} into ' f'state { "inactive" if args.state == "configure" else "active" }' ) return 0
def main(args=None): rclpy.init(args=args) parser = argparse.ArgumentParser() parser.add_argument('controller_name', help='Name of the controller') 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_manager_name = args.controller_manager node = Node('unspawner_' + controller_name) 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('Stopped 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') return 0 finally: rclpy.shutdown()
def main(self, *, args): with NodeStrategy(args) as node: response = switch_controllers(node, args.controller_manager, args.stop_controllers, args.start_controllers, args.strict, args.start_asap, args.switch_timeout) if not response.ok: return 'Error switching controllers, check controller_manager logs' return 'Successfully switched controllers'
def main(args=None): rclpy.init(args=args) parser = argparse.ArgumentParser() parser.add_argument('controller_name', help='Name of the controller') parser.add_argument('-c', '--controller-manager', help='Name of the controller manager ROS node', default='/controller_manager', required=False) parser.add_argument( '-p', '--param-file', help= 'Controller param file to be loaded into controller node before configure', required=False) parser.add_argument( '--stopped', help='Load and configure the controller, however do not start them', action='store_true', required=False) parser.add_argument( '-t', '--controller-type', help= 'If not provided it should exist in the controller manager namespace', default=None, required=False) parser.add_argument( '-u', '--unload-on-kill', help='Wait until this application is interrupted and unload controller', action='store_true') parser.add_argument('--controller-manager-timeout', help='Time to wait for the controller manager', required=False, default=10, type=int) 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_manager_name = make_absolute(args.controller_manager) param_file = args.param_file controller_type = args.controller_type controller_manager_timeout = args.controller_manager_timeout if param_file and not os.path.isfile(param_file): raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file) node = Node('spawner_' + controller_name) try: if not wait_for_controller_manager(node, controller_manager_name, controller_manager_timeout): node.get_logger().error('Controller manager not available') return 1 if is_controller_loaded(node, controller_manager_name, controller_name): node.get_logger().info( 'Controller already loaded, skipping load_controller') else: if controller_type: ret = subprocess.run([ 'ros2', 'param', 'set', controller_manager_name, controller_name + '.type', controller_type ]) ret = load_controller(node, controller_manager_name, controller_name) if not ret.ok: # Error message printed by ros2 control return 1 node.get_logger().info('Loaded ' + controller_name) if param_file: ret = subprocess.run( ['ros2', 'param', 'load', controller_name, param_file]) if ret.returncode != 0: # Error message printed by ros2 param return ret.returncode node.get_logger().info('Loaded ' + param_file + ' into ' + controller_name) ret = configure_controller(node, controller_manager_name, controller_name) if not ret.ok: node.get_logger().info('Failed to configure controller') return 1 if not args.stopped: ret = switch_controllers(node, controller_manager_name, [], [controller_name], True, True, 5.0) if not ret.ok: node.get_logger().info('Failed to start controller') return 1 node.get_logger().info('Configured and started ' + controller_name) if not args.unload_on_kill: return 0 try: node.get_logger().info( 'Waiting until interrupt to unload controllers') while True: time.sleep(1) except KeyboardInterrupt: if not args.stopped: node.get_logger().info( 'Interrupt captured, stopping and unloading controller') ret = switch_controllers(node, controller_manager_name, [controller_name], [], True, True, 5.0) if not ret.ok: node.get_logger().info('Failed to stop controller') return 1 node.get_logger().info('Stopped 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') return 0 finally: rclpy.shutdown()