Skip to content

Commit

Permalink
Update spawner to accept controllers list and start them in sequence (r…
Browse files Browse the repository at this point in the history 
…os-controls#1139)
  • Loading branch information
@saikishor
saikishor authored Oct 30, 2023
1 parent 971ff39 commit 4e78813
Show file tree
Hide file tree
Showing 3 changed files with 221 additions and 102 deletions.
217 changes: 124 additions & 93 deletions controller_manager/controller_manager/spawner.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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",
Expand Down Expand Up @@ -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
Expand All @@ -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()
Expand All @@ -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
Expand All @@ -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(
Expand Down
19 changes: 10 additions & 9 deletions controller_manager/controller_manager/unspawner.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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",
Expand All @@ -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:
Expand Down
Loading

0 comments on commit 4e78813

Please sign in to comment.