[rosserial_python] Stop old thread before recreating new SerialClient

rosserial_python/nodes/serial_node.py

@@ -105,5 +105,6 @@
             except:
                 rospy.logwarn("Unexpected Error: %s", sys.exc_info()[0])
                 client.port.close()
+                client.stopWriteThread()
                 sleep(1.0)
                 continue

rosserial_python/src/rosserial_python/SerialClient.py

@@ -335,6 +335,7 @@ def __init__(self, port=None, baud=57600, timeout=5.0, fix_pyserial_for_test=Fal
 
         self.write_lock = threading.RLock()
         self.write_queue = queue.Queue()
+        self.write_alive = False
         self.write_thread = None
 
         self.lastsync = rospy.Time(0)
@@ -449,6 +450,7 @@ def run(self):
 
         # Launch write thread.
         if self.write_thread is None:
+            self.write_alive = True
             self.write_thread = threading.Thread(target=self.processWriteQueue)
             self.write_thread.daemon = True
             self.write_thread.start()
@@ -556,6 +558,11 @@ def run(self):
                 self.requestTopics()
         self.write_thread.join()
 
+    def stopWriteThread(self):
+        # stop self.write_thread
+        self.write_alive = False
+        self.write_thread.join()
+
     def setPublishSize(self, size):
         if self.buffer_out < 0:
             self.buffer_out = size
@@ -774,7 +781,7 @@ def processWriteQueue(self):
         """
         Main loop for the thread that processes outgoing data to write to the serial port.
         """
-        while not rospy.is_shutdown():
+        while not rospy.is_shutdown() and self.write_alive:
             if self.write_queue.empty():
                 time.sleep(0.01)
             else: