From 898943155aaf890cfbbf83ab1d297dbe98384107 Mon Sep 17 00:00:00 2001 From: arw Date: Thu, 18 Oct 2018 06:53:06 -0700 Subject: [PATCH 1/6] check for currently running controllers on shutdown --- controller_manager/scripts/spawner | 24 ++++++---- .../controller_manager_interface.py | 46 ++++++++----------- 2 files changed, 36 insertions(+), 34 deletions(-) diff --git a/controller_manager/scripts/spawner b/controller_manager/scripts/spawner index 831dad6fd..b334bea3c 100755 --- a/controller_manager/scripts/spawner +++ b/controller_manager/scripts/spawner @@ -41,6 +41,7 @@ import yaml from controller_manager_msgs.srv import * from std_msgs.msg import * import argparse +import controller_manager.controller_manager_interface loaded = [] @@ -49,27 +50,37 @@ load_controller_service = "" switch_controller_service = "" unload_controller_service = "" +counter = 0 + def shutdown(): global loaded,unload_controller_service,load_controller_service,switch_controller_service rospy.loginfo("Shutting down spawner. Stopping and unloading controllers...") try: - # unloader + # get list of running controllers + _running_controllers = [] + _loaded_controllers = controller_manager.controller_manager_interface.list_controllers() + for c in _loaded_controllers: + if c.state == "running": + _running_controllers.append(c.name) + + # create unloader and switcher unload_controller = rospy.ServiceProxy(unload_controller_service, UnloadController) - - # switcher switch_controller = rospy.ServiceProxy(switch_controller_service, SwitchController) rospy.loginfo("Stopping all controllers..."); - switch_controller([], loaded, SwitchControllerRequest.STRICT) + switch_controller([], _running_controllers, SwitchControllerRequest.STRICT) rospy.loginfo("Unloading all loaded controllers..."); - for name in reversed(loaded): + for name in reversed(_running_controllers): rospy.logout("Trying to unload %s" % name) unload_controller(name) rospy.logout("Succeeded in unloading %s" % name) except (rospy.ServiceException, rospy.exceptions.ROSException) as exc: rospy.logwarn("Controller Spawner error while taking down controllers: %s" % (exc)) + counter += 1 + if counter < 3: + shutdown() def parse_args(args=None): parser = argparse.ArgumentParser(description='Controller spawner') @@ -191,9 +202,6 @@ def main(): rospy.loginfo("Controller Spawner: Loaded controllers: %s" % ', '.join(loaded)) - if rospy.is_shutdown(): - return - # start controllers is requested if autostart: resp = switch_controller(loaded, [], 2) diff --git a/controller_manager/src/controller_manager/controller_manager_interface.py b/controller_manager/src/controller_manager/controller_manager_interface.py index 9cac44a0c..5058749b3 100644 --- a/controller_manager/src/controller_manager/controller_manager_interface.py +++ b/controller_manager/src/controller_manager/controller_manager_interface.py @@ -1,18 +1,15 @@ #! /usr/bin/env python -from __future__ import print_function import rospy from controller_manager_msgs.srv import * - def list_controller_types(): rospy.wait_for_service('controller_manager/list_controller_types') s = rospy.ServiceProxy('controller_manager/list_controller_types', ListControllerTypes) resp = s.call(ListControllerTypesRequest()) for t in resp.types: - print(t) - + print t -def reload_libraries(force_kill, restore=False): +def reload_libraries(force_kill, restore = False): rospy.wait_for_service('controller_manager/reload_controller_libraries') s = rospy.ServiceProxy('controller_manager/reload_controller_libraries', ReloadControllerLibraries) @@ -20,16 +17,16 @@ def reload_libraries(force_kill, restore=False): load_srv = rospy.ServiceProxy('controller_manager/load_controller', LoadController) switch_srv = rospy.ServiceProxy('controller_manager/switch_controller', SwitchController) - print("Restore:", restore) + print "Restore:", restore if restore: originally = list_srv.call(ListControllersRequest()) resp = s.call(ReloadControllerLibrariesRequest(force_kill)) if resp.ok: - print("Successfully reloaded libraries") + print "Successfully reloaded libraries" result = True else: - print("Failed to reload libraries. Do you still have controllers loaded?") + print "Failed to reload libraries. Do you still have controllers loaded?" result = False if restore: @@ -39,20 +36,23 @@ def reload_libraries(force_kill, restore=False): switch_srv(start_controllers=to_start, stop_controllers=[], strictness=SwitchControllerRequest.BEST_EFFORT) - print("Controllers restored to original state") + print "Controllers restored to original state" return result def list_controllers(): + controller_list = [] rospy.wait_for_service('controller_manager/list_controllers') s = rospy.ServiceProxy('controller_manager/list_controllers', ListControllers) resp = s.call(ListControllersRequest()) if len(resp.controller) == 0: - print("No controllers are loaded in mechanism control") + print "No controllers are loaded in mechanism control" else: for c in resp.controller: - hwi = list(set(r.hardware_interface for r in c.claimed_resources)) - print('%s - %s ( %s )' % (c.name, '+'.join(hwi), c.state)) + hwi = list(set(r.hardware_interface for r in c.claimed_resources)) + print '%s - %s ( %s )'%(c.name, '+'.join(hwi), c.state) + controller_list.append(c) + return controller_list def load_controller(name): @@ -60,41 +60,35 @@ def load_controller(name): s = rospy.ServiceProxy('controller_manager/load_controller', LoadController) resp = s.call(LoadControllerRequest(name)) if resp.ok: - print("Loaded", name) + print "Loaded", name return True else: - print("Error when loading", name) + print "Error when loading", name return False - def unload_controller(name): rospy.wait_for_service('controller_manager/unload_controller') s = rospy.ServiceProxy('controller_manager/unload_controller', UnloadController) resp = s.call(UnloadControllerRequest(name)) if resp.ok == 1: - print("Unloaded %s successfully" % name) + print "Unloaded %s successfully" % name return True else: - print("Error when unloading", name) + print "Error when unloading", name return False - def start_controller(name): return start_stop_controllers([name], True) - def start_controllers(names): return start_stop_controllers(names, True) - def stop_controller(name): return start_stop_controllers([name], False) - def stop_controllers(names): return start_stop_controllers(names, False) - def start_stop_controllers(names, st): rospy.wait_for_service('controller_manager/switch_controller') s = rospy.ServiceProxy('controller_manager/switch_controller', SwitchController) @@ -108,13 +102,13 @@ def start_stop_controllers(names, st): resp = s.call(SwitchControllerRequest(start, stop, strictness)) if resp.ok == 1: if st: - print("Started {} successfully".format(names)) + print "Started {} successfully".format(names) else: - print("Stopped {} successfully".format(names)) + print "Stopped {} successfully".format(names) return True else: if st: - print("Error when starting ", names) + print "Error when starting ", names else: - print("Error when stopping ", names) + print "Error when stopping ", names return False From 471da15e8098c5589ab83d778e7069d807505d19 Mon Sep 17 00:00:00 2001 From: arw Date: Thu, 18 Oct 2018 06:56:15 -0700 Subject: [PATCH 2/6] cleaned up --- controller_manager/scripts/spawner | 5 ----- 1 file changed, 5 deletions(-) diff --git a/controller_manager/scripts/spawner b/controller_manager/scripts/spawner index b334bea3c..6a6a58679 100755 --- a/controller_manager/scripts/spawner +++ b/controller_manager/scripts/spawner @@ -50,8 +50,6 @@ load_controller_service = "" switch_controller_service = "" unload_controller_service = "" -counter = 0 - def shutdown(): global loaded,unload_controller_service,load_controller_service,switch_controller_service @@ -78,9 +76,6 @@ def shutdown(): rospy.logout("Succeeded in unloading %s" % name) except (rospy.ServiceException, rospy.exceptions.ROSException) as exc: rospy.logwarn("Controller Spawner error while taking down controllers: %s" % (exc)) - counter += 1 - if counter < 3: - shutdown() def parse_args(args=None): parser = argparse.ArgumentParser(description='Controller spawner') From e54267ccce93fff4e591bd301a6bc30b3650eb1e Mon Sep 17 00:00:00 2001 From: arw Date: Thu, 18 Oct 2018 11:09:13 -0700 Subject: [PATCH 3/6] fixed prints --- .../controller_manager_interface.py | 30 +++++++++---------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/controller_manager/src/controller_manager/controller_manager_interface.py b/controller_manager/src/controller_manager/controller_manager_interface.py index 5058749b3..5204e87a3 100644 --- a/controller_manager/src/controller_manager/controller_manager_interface.py +++ b/controller_manager/src/controller_manager/controller_manager_interface.py @@ -7,7 +7,7 @@ def list_controller_types(): s = rospy.ServiceProxy('controller_manager/list_controller_types', ListControllerTypes) resp = s.call(ListControllerTypesRequest()) for t in resp.types: - print t + print(t) def reload_libraries(force_kill, restore = False): rospy.wait_for_service('controller_manager/reload_controller_libraries') @@ -17,16 +17,16 @@ def reload_libraries(force_kill, restore = False): load_srv = rospy.ServiceProxy('controller_manager/load_controller', LoadController) switch_srv = rospy.ServiceProxy('controller_manager/switch_controller', SwitchController) - print "Restore:", restore + print("Restore:", restore) if restore: originally = list_srv.call(ListControllersRequest()) resp = s.call(ReloadControllerLibrariesRequest(force_kill)) if resp.ok: - print "Successfully reloaded libraries" + print("Successfully reloaded libraries") result = True else: - print "Failed to reload libraries. Do you still have controllers loaded?" + print("Failed to reload libraries. Do you still have controllers loaded?") result = False if restore: @@ -36,7 +36,7 @@ def reload_libraries(force_kill, restore = False): switch_srv(start_controllers=to_start, stop_controllers=[], strictness=SwitchControllerRequest.BEST_EFFORT) - print "Controllers restored to original state" + print("Controllers restored to original state") return result @@ -46,11 +46,11 @@ def list_controllers(): s = rospy.ServiceProxy('controller_manager/list_controllers', ListControllers) resp = s.call(ListControllersRequest()) if len(resp.controller) == 0: - print "No controllers are loaded in mechanism control" + print("No controllers are loaded in mechanism control") else: for c in resp.controller: hwi = list(set(r.hardware_interface for r in c.claimed_resources)) - print '%s - %s ( %s )'%(c.name, '+'.join(hwi), c.state) + print('%s - %s ( %s )'%(c.name, '+'.join(hwi), c.state)) controller_list.append(c) return controller_list @@ -60,10 +60,10 @@ def load_controller(name): s = rospy.ServiceProxy('controller_manager/load_controller', LoadController) resp = s.call(LoadControllerRequest(name)) if resp.ok: - print "Loaded", name + print("Loaded", name) return True else: - print "Error when loading", name + print("Error when loading", name) return False def unload_controller(name): @@ -71,10 +71,10 @@ def unload_controller(name): s = rospy.ServiceProxy('controller_manager/unload_controller', UnloadController) resp = s.call(UnloadControllerRequest(name)) if resp.ok == 1: - print "Unloaded %s successfully" % name + print("Unloaded %s successfully" % name) return True else: - print "Error when unloading", name + print("Error when unloading", name) return False def start_controller(name): @@ -102,13 +102,13 @@ def start_stop_controllers(names, st): resp = s.call(SwitchControllerRequest(start, stop, strictness)) if resp.ok == 1: if st: - print "Started {} successfully".format(names) + print("Started {} successfully".format(names)) else: - print "Stopped {} successfully".format(names) + print("Stopped {} successfully".format(names)) return True else: if st: - print "Error when starting ", names + print("Error when starting ", names) else: - print "Error when stopping ", names + print("Error when stopping ", names) return False From 6bcfd75ae15b56a48d33125291de25ee8402530b Mon Sep 17 00:00:00 2001 From: arw Date: Fri, 19 Oct 2018 08:16:42 -0700 Subject: [PATCH 4/6] check for intersection of running and loaded controllers --- controller_manager/scripts/spawner | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/controller_manager/scripts/spawner b/controller_manager/scripts/spawner index 6a6a58679..d8f66c476 100755 --- a/controller_manager/scripts/spawner +++ b/controller_manager/scripts/spawner @@ -63,14 +63,17 @@ def shutdown(): if c.state == "running": _running_controllers.append(c.name) + # determine if the loaded controllers are still running + _running_loaded_controllers = list(set(_running_controllers) & set(loaded)) + # create unloader and switcher unload_controller = rospy.ServiceProxy(unload_controller_service, UnloadController) switch_controller = rospy.ServiceProxy(switch_controller_service, SwitchController) - rospy.loginfo("Stopping all controllers..."); - switch_controller([], _running_controllers, SwitchControllerRequest.STRICT) + rospy.loginfo("Stopping controllers..."); + switch_controller([], _running_loaded_controllers, SwitchControllerRequest.STRICT) rospy.loginfo("Unloading all loaded controllers..."); - for name in reversed(_running_controllers): + for name in reversed(_running_loaded_controllers): rospy.logout("Trying to unload %s" % name) unload_controller(name) rospy.logout("Succeeded in unloading %s" % name) From f487d0b14edbb3554222a7284c181ef3073663c8 Mon Sep 17 00:00:00 2001 From: megantuttle Date: Fri, 19 Oct 2018 09:30:53 -0700 Subject: [PATCH 5/6] changed list intersection method --- controller_manager/scripts/spawner | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/scripts/spawner b/controller_manager/scripts/spawner index d8f66c476..32f959c41 100755 --- a/controller_manager/scripts/spawner +++ b/controller_manager/scripts/spawner @@ -64,7 +64,7 @@ def shutdown(): _running_controllers.append(c.name) # determine if the loaded controllers are still running - _running_loaded_controllers = list(set(_running_controllers) & set(loaded)) + _running_loaded_controllers = [value for value in _running_controllers if value in loaded] # create unloader and switcher unload_controller = rospy.ServiceProxy(unload_controller_service, UnloadController) From ee4cf9229e8221a379f05604f38ee5db3765297f Mon Sep 17 00:00:00 2001 From: megantuttle Date: Fri, 19 Oct 2018 11:10:39 -0700 Subject: [PATCH 6/6] switched service calls to follow convention, reverted interface to original --- controller_manager/scripts/spawner | 29 +++++++++++++------ .../controller_manager_interface.py | 18 ++++++++---- 2 files changed, 32 insertions(+), 15 deletions(-) diff --git a/controller_manager/scripts/spawner b/controller_manager/scripts/spawner index 32f959c41..71a960fd2 100755 --- a/controller_manager/scripts/spawner +++ b/controller_manager/scripts/spawner @@ -41,7 +41,6 @@ import yaml from controller_manager_msgs.srv import * from std_msgs.msg import * import argparse -import controller_manager.controller_manager_interface loaded = [] @@ -49,27 +48,34 @@ loaded = [] load_controller_service = "" switch_controller_service = "" unload_controller_service = "" +list_controllers_service = "" def shutdown(): - global loaded,unload_controller_service,load_controller_service,switch_controller_service + global loaded,unload_controller_service,load_controller_service,switch_controller_service,list_controllers_service rospy.loginfo("Shutting down spawner. Stopping and unloading controllers...") try: + # lister + list_controllers = rospy.ServiceProxy(list_controllers_service, ListControllers) + + # unloader + unload_controller = rospy.ServiceProxy(unload_controller_service, UnloadController) + + # switcher + switch_controller = rospy.ServiceProxy(switch_controller_service, SwitchController) + # get list of running controllers + rospy.loginfo("Getting list of controllers..."); + _controller_list = list_controllers() _running_controllers = [] - _loaded_controllers = controller_manager.controller_manager_interface.list_controllers() - for c in _loaded_controllers: + for c in _controller_list.controller: if c.state == "running": _running_controllers.append(c.name) # determine if the loaded controllers are still running _running_loaded_controllers = [value for value in _running_controllers if value in loaded] - # create unloader and switcher - unload_controller = rospy.ServiceProxy(unload_controller_service, UnloadController) - switch_controller = rospy.ServiceProxy(switch_controller_service, SwitchController) - rospy.loginfo("Stopping controllers..."); switch_controller([], _running_loaded_controllers, SwitchControllerRequest.STRICT) rospy.loginfo("Unloading all loaded controllers..."); @@ -97,7 +103,7 @@ def parse_args(args=None): return parser.parse_args(args=args) def main(): - global unload_controller_service,load_controller_service,switch_controller_service + global unload_controller_service,load_controller_service,switch_controller_service,list_controllers_service args = parse_args(rospy.myargv()[1:]) @@ -119,6 +125,7 @@ def main(): load_controller_service = robot_namespace+"controller_manager/load_controller" unload_controller_service = robot_namespace+"controller_manager/unload_controller" switch_controller_service = robot_namespace+"controller_manager/switch_controller" + list_controllers_service = robot_namespace+"controller_manager/list_controllers" try: # loader @@ -138,6 +145,10 @@ def main(): rospy.loginfo("Controller Spawner: Waiting for service "+unload_controller_service) rospy.wait_for_service(unload_controller_service, timeout=timeout) + # lister + rospy.loginfo("Controller Spawner: Waiting for service "+list_controllers_service) + rospy.wait_for_service(list_controllers_service, timeout=timeout) + except rospy.exceptions.ROSException: rospy.logwarn("Controller Spawner couldn't find the expected controller_manager ROS interface.") return diff --git a/controller_manager/src/controller_manager/controller_manager_interface.py b/controller_manager/src/controller_manager/controller_manager_interface.py index 5204e87a3..9cac44a0c 100644 --- a/controller_manager/src/controller_manager/controller_manager_interface.py +++ b/controller_manager/src/controller_manager/controller_manager_interface.py @@ -1,7 +1,9 @@ #! /usr/bin/env python +from __future__ import print_function import rospy from controller_manager_msgs.srv import * + def list_controller_types(): rospy.wait_for_service('controller_manager/list_controller_types') s = rospy.ServiceProxy('controller_manager/list_controller_types', ListControllerTypes) @@ -9,7 +11,8 @@ def list_controller_types(): for t in resp.types: print(t) -def reload_libraries(force_kill, restore = False): + +def reload_libraries(force_kill, restore=False): rospy.wait_for_service('controller_manager/reload_controller_libraries') s = rospy.ServiceProxy('controller_manager/reload_controller_libraries', ReloadControllerLibraries) @@ -41,7 +44,6 @@ def reload_libraries(force_kill, restore = False): def list_controllers(): - controller_list = [] rospy.wait_for_service('controller_manager/list_controllers') s = rospy.ServiceProxy('controller_manager/list_controllers', ListControllers) resp = s.call(ListControllersRequest()) @@ -49,10 +51,8 @@ def list_controllers(): print("No controllers are loaded in mechanism control") else: for c in resp.controller: - hwi = list(set(r.hardware_interface for r in c.claimed_resources)) - print('%s - %s ( %s )'%(c.name, '+'.join(hwi), c.state)) - controller_list.append(c) - return controller_list + hwi = list(set(r.hardware_interface for r in c.claimed_resources)) + print('%s - %s ( %s )' % (c.name, '+'.join(hwi), c.state)) def load_controller(name): @@ -66,6 +66,7 @@ def load_controller(name): print("Error when loading", name) return False + def unload_controller(name): rospy.wait_for_service('controller_manager/unload_controller') s = rospy.ServiceProxy('controller_manager/unload_controller', UnloadController) @@ -77,18 +78,23 @@ def unload_controller(name): print("Error when unloading", name) return False + def start_controller(name): return start_stop_controllers([name], True) + def start_controllers(names): return start_stop_controllers(names, True) + def stop_controller(name): return start_stop_controllers([name], False) + def stop_controllers(names): return start_stop_controllers(names, False) + def start_stop_controllers(names, st): rospy.wait_for_service('controller_manager/switch_controller') s = rospy.ServiceProxy('controller_manager/switch_controller', SwitchController)