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(self, *, args):
with NodeStrategy(args) as node:
response = configure_controller(node, args.controller_manager,
args.controller_name)
if not response.ok:
'Error configuring controller, check controller_manager logs'
print(f'Successfully configured controller {args.controller_name}')
return 0
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(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()