Spawner check for currently running controllers on shutdown

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 = []
 
@@ -55,16 +56,24 @@ def shutdown():
     rospy.loginfo("Shutting down spawner. Stopping and unloading controllers...")
 
     try:
-        # unloader
-        unload_controller = rospy.ServiceProxy(unload_controller_service, UnloadController)
+        # 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)
 
-        # switcher
+        # 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 all controllers...");
-        switch_controller([], loaded, SwitchControllerRequest.STRICT)
+        rospy.loginfo("Stopping controllers...");
+        switch_controller([], _running_loaded_controllers, SwitchControllerRequest.STRICT)
         rospy.loginfo("Unloading all loaded controllers...");
-        for name in reversed(loaded):
+        for name in reversed(_running_loaded_controllers):
             rospy.logout("Trying to unload %s" % name)
             unload_controller(name)
             rospy.logout("Succeeded in unloading %s" % name)
@@ -191,9 +200,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)

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)
 
-
-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)
 
@@ -44,15 +41,18 @@ 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())
     if len(resp.controller) == 0:
         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):
@@ -66,7 +66,6 @@ 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)
@@ -78,23 +77,18 @@ 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)