From 66a92392eeb9982315bcee5101d4decbaf63476e Mon Sep 17 00:00:00 2001
From: Naoya Yamaguchi <708yamaguchi@gmail.com>
Date: Tue, 19 Jul 2022 03:03:17 +0900
Subject: [PATCH] [rosserial_python] Stop old thread before recreating new
 SerialClient

---
 rosserial_python/nodes/serial_node.py                 | 1 +
 rosserial_python/src/rosserial_python/SerialClient.py | 9 ++++++++-
 2 files changed, 9 insertions(+), 1 deletion(-)

diff --git a/rosserial_python/nodes/serial_node.py b/rosserial_python/nodes/serial_node.py
index 483c6a21a..0921a1cae 100755
--- a/rosserial_python/nodes/serial_node.py
+++ b/rosserial_python/nodes/serial_node.py
@@ -98,6 +98,7 @@
             except:
                 rospy.logwarn("Unexpected Error.%s", sys.exc_info()[0])
                 client.port.close()
+                client.stopWriteThread()
                 sleep(1.0)
                 continue
 
diff --git a/rosserial_python/src/rosserial_python/SerialClient.py b/rosserial_python/src/rosserial_python/SerialClient.py
index 4a6306522..f272261e6 100644
--- a/rosserial_python/src/rosserial_python/SerialClient.py
+++ b/rosserial_python/src/rosserial_python/SerialClient.py
@@ -328,6 +328,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()
+        self.write_alive = False
         self.write_thread = None
 
         self.lastsync = rospy.Time(0)
@@ -448,6 +449,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.port.flushOutput()
                 self.requestTopics()
 
+    def stopWriteThread(self):
+        # stop self.write_thread
+        self.write_alive = False
+        self.write_thread.join()
+
     def setPublishSize(self, bytes):
         if self.buffer_out < 0:
             self.buffer_out = bytes
@@ -771,7 +778,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: