driver/mavcan.py: gracefully exit io thread when close is called

dronecan/driver/mavcan.py

@@ -28,13 +28,13 @@
 TX_QUEUE_SIZE = 1000
 
 logger = getLogger(__name__)
-
+kill_process = False
 class ControlMessage(object):
     def __init__(self, command, data):
         self.command = command
         self.data = data
 
-def io_process(url, bus, target_system, baudrate, tx_queue, rx_queue):
+def io_process(url, bus, target_system, baudrate, tx_queue, rx_queue, exit_queue):
     os.environ['MAVLINK20'] = '1'
 
     target_component = 0
@@ -43,7 +43,7 @@ def io_process(url, bus, target_system, baudrate, tx_queue, rx_queue):
     filter_list = None
     signing_key = None
     last_loss_print_t = time.time()
-
+    exit_proc = False
     def connect():
         nonlocal conn, baudrate
         conn = mavutil.mavlink_connection(url, baud=baudrate, source_system=250,
@@ -56,7 +56,11 @@ def connect():
             conn.setup_signing(signing_key, sign_outgoing=True)
 
     def reconnect():
-        while True:
+        nonlocal exit_proc
+        while True and not exit_proc:
+            if (not exit_queue.empty() and exit_queue.get() == "QUIT"):
+                exit_proc = True
+                return
             try:
                 time.sleep(1)
                 logger.info('reconnecting to %s' % url)
@@ -114,7 +118,13 @@ def handle_control_message(m):
     enable_can_forward()
 
     while True:
+        if (not exit_queue.empty() and exit_queue.get() == "QUIT") or exit_proc:
+            conn.close()
+            return
         while not tx_queue.empty():
+            if (not exit_queue.empty() and exit_queue.get() == "QUIT") or exit_proc:
+                conn.close()
+                return
             frame = tx_queue.get()
             if isinstance(frame, ControlMessage):
                 handle_control_message(frame)
@@ -186,10 +196,11 @@ def __init__(self, url, **kwargs):
 
         self.rx_queue = multiprocessing.Queue(maxsize=RX_QUEUE_SIZE)
         self.tx_queue = multiprocessing.Queue(maxsize=TX_QUEUE_SIZE)
+        self.exit_queue = multiprocessing.Queue(maxsize=1)
 
         self.proc = multiprocessing.Process(target=io_process, name='mavcan_io_process',
                                             args=(url, self.bus, self.target_system, baudrate,
-                                            self.tx_queue, self.rx_queue))
+                                            self.tx_queue, self.rx_queue, self.exit_queue))
         self.proc.daemon = True
         self.proc.start()
 
@@ -199,7 +210,9 @@ def __init__(self, url, **kwargs):
             self.set_signing_passphrase(pass_phrase)
 
     def close(self):
-        pass
+        if self.proc is not None:
+            self.exit_queue.put_nowait("QUIT")
+            self.proc.join()
 
     def __del__(self):
         self.close()
@@ -270,7 +283,3 @@ def set_signing_passphrase(self, passphrase):
         '''set MAVLink2 signing passphrase'''
         signing_key = self.passphrase_to_key(passphrase)
         self.tx_queue.put_nowait(ControlMessage('SigningKey', signing_key))
-
-
-
-