make spawner kill subprocesses on exit

ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner.py

@@ -11,6 +11,7 @@
 import math
 import random
 import json
+import atexit
 
 from utils import print_error, print_info, print_ok, is_number, rinfo, rwarn, rerr
 
@@ -29,6 +30,8 @@
 VEHICLE_TYPES = ['a300', 'f450', 'f550', 't650', 'x500', 'eaglemk2', 'f330', 'brus', 'naki', 'big_dofec', 'm690']
 SPAWNING_DELAY_SECONDS = 6
 
+glob_running_processes = []
+
 class MrsDroneSpawner():
 
     # #{ __init__
@@ -80,7 +83,6 @@ def __init__(self, show_help=False, verbose=False):
         self.queued_vehicles = []
         self.got_mavlink = {}
         self.mavlink_sub = {}
-        self.running_processes= []
         self.assigned_ids = {} # dictionary {id: process_handle}
         # #}
 
@@ -173,7 +175,7 @@ def callback_action_timer(self, timer):
             roslaunch.pmon._init_signal_handlers = orig_signal_handler
 
             if process_handle is not None:
-                self.running_processes.append(process_handle)
+                glob_running_processes.append(process_handle)
             else:
                 self.process_queue_mutex.acquire()
                 self.process_queue.append((process, args))
@@ -581,8 +583,9 @@ def launch_firmware(self, ID, uav_roslaunch_args):
             launch.shutdown()
             return None
 
+        process_name = 'uav' + str(ID) + '_firmware'
         rinfo('Firmware for uav' + str(ID) + ' started!')
-        return launch
+        return launch, process_name
     # #}
 
     # #{ spawn_gazebo_model
@@ -602,7 +605,8 @@ def spawn_simulation_model(self, ID, uav_roslaunch_args):
         rinfo('Gazebo model for uav' + str(ID) + ' spawned!')
         self.queued_vehicles.remove('uav' + str(ID))
         self.active_vehicles.append('uav' + str(ID))
-        return launch
+        process_name = 'uav' + str(ID) + '_simulation_model'
+        return launch, process_name
     # #}
 
     # #{ launch_mavros
@@ -627,15 +631,39 @@ def launch_mavros(self, ID, uav_roslaunch_args):
             launch.shutdown()
             return None
 
+        process_name = 'uav' + str(ID) + '_mavros'
         rinfo('Mavros for uav' + str(ID) + ' started!')
-        return launch
+        return launch, process_name
     # #}
 
     # #{ dummy_function
     def dummy_function(self):
         pass
     # #}
 
+# #{ exit_handler
+def exit_handler():
+    print('[INFO] [MrsDroneSpawner]: Exit requested')
+
+    if len(glob_running_processes) > 0:
+        print(f'[INFO] [MrsDroneSpawner]: Shutting down {len(glob_running_processes)} subprocesses')
+
+        num_zombies = 0
+        for p, pid in glob_running_processes:
+            try:
+                p.shutdown()
+                print(f'[INFO] [MrsDroneSpawner]: Process {pid} shutdown')
+            except:
+                num_zombies += 1
+
+        if num_zombies > 0:
+            print(f'\033[91m[ERROR] [MrsDroneSpawner]: Could not stop {num_zombies} subprocesses\033[91m')
+            exit(1)
+
+    print('[INFO] [MrsDroneSpawner]: Exited gracefully')
+    exit(0)
+# #}
+
 if __name__ == '__main__':
 
     show_help = True
@@ -644,6 +672,8 @@ def dummy_function(self):
 
     verbose = 'verbose' in sys.argv
 
+    atexit.register(exit_handler)
+
     try:
         spawner = MrsDroneSpawner(show_help, verbose)