diff --git a/rosserial_python/nodes/serial_node.py b/rosserial_python/nodes/serial_node.py index ddbcf8328..13ff10205 100755 --- a/rosserial_python/nodes/serial_node.py +++ b/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 diff --git a/rosserial_python/src/rosserial_python/SerialClient.py b/rosserial_python/src/rosserial_python/SerialClient.py index 6b1b87574..f7b10b081 100644 --- a/rosserial_python/src/rosserial_python/SerialClient.py +++ b/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: