Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

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

Merged
merged 7 commits into from
Nov 4, 2023
Merged

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

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
2125380
Update spawner to accept controllers list and start them in sequence …
saikishor Oct 30, 2023
56991d4
fix spawner conflicts
bmagyar Oct 31, 2023
2e598c8
fix spawner conflicts - contd
bmagyar Oct 31, 2023
c87ccb4
fix unspawner conflicts
bmagyar Oct 31, 2023
b61600f
fix missing imports
bmagyar Oct 31, 2023
7da8e89
Also handle "stopped" arg
bmagyar Nov 3, 2023
09f8cf6
++stopped
bmagyar Nov 3, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
219 changes: 150 additions & 69 deletions controller_manager/controller_manager/spawner.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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
Expand All @@ -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}"
Expand All @@ -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
)
bmagyar marked this conversation as resolved.
Show resolved Hide resolved
if args.stopped:
node.get_logger().warn('"--stopped" flag is deprecated use "--inactive" instead')

if not args.unload_on_kill:
return 0
Expand All @@ -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
Expand Down
29 changes: 12 additions & 17 deletions controller_manager/controller_manager/unspawner.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
Loading