From aef327490431f7aec0954ef8b8efc5b6ee8219b6 Mon Sep 17 00:00:00 2001 From: rbaynes Date: Tue, 22 Aug 2017 10:14:14 -0400 Subject: [PATCH] @goruck 's fix for #331, with a few tweaks --- nodes/arduino_handler.py | 27 +++++++++++++++++++++++++-- 1 file changed, 25 insertions(+), 2 deletions(-) diff --git a/nodes/arduino_handler.py b/nodes/arduino_handler.py index 072180b..5c09128 100755 --- a/nodes/arduino_handler.py +++ b/nodes/arduino_handler.py @@ -314,13 +314,18 @@ def process_message(line): rospy.logwarn(message) return message -def connect_serial(serial_connection=None): +def close_serial(): + if serial_connection is not None: + serial_connection.close() + +def connect_serial(): timeout_s = 2 / serial_rate_hz # serial port timeout is 2x loop rate baud_rate = rospy.get_param("~baud_rate", 115200) # Initialize the serial connection path = "/dev/serial/by-id" port = None + close_serial() while port is None: try: @@ -333,6 +338,7 @@ def connect_serial(serial_connection=None): port = ports[0] serial_connection = serial.Serial(os.path.join(path, port), baud_rate, + dsrdtr = True, # Causes an Arduino soft reset timeout=timeout_s, writeTimeout=timeout_s) return serial_connection @@ -340,6 +346,7 @@ def connect_serial(serial_connection=None): rospy.logwarn(e) rospy.sleep(0.2) #seconds + if __name__ == '__main__': if TRACE: rospy.init_node('arduino_handler', log_level=rospy.DEBUG) @@ -356,6 +363,9 @@ def connect_serial(serial_connection=None): arduino_delay_s = 0.25 / serial_rate_hz # Reserve 25% of loop period for Arduino comm delays + MAX_EMPTY_READS = 4 # Arduino error messages cause empty reads, allow 4 consecutive errors + empty_read_count = 0 # Arduino consecutive empty read counter + while not rospy.is_shutdown(): # These 2 are permanently on. actuator_state["water_aeration_pump_1"] = True @@ -405,6 +415,15 @@ def connect_serial(serial_connection=None): rospy.sleep(arduino_delay_s) # Blocks until one line is read or times out buf = serial_connection.readline() + # Count consecutive empty reads, raise exception if threshold is exceeded + if buf == "": + empty_read_count += 1 + else: + empty_read_count = 0 + trace('arduino_handler empty read count: %d', empty_read_count) + if empty_read_count > MAX_EMPTY_READS: + empty_read_count = 0 + raise Exception("arduino_handler: serial_connection.readline() broken") """ Since errors are sent first, the readline may have gotten an Arduino error message and not sensor data. Therefore the flush @@ -422,6 +441,10 @@ def connect_serial(serial_connection=None): # Exception that is raised on write timeouts rospy.logwarn(e2) serial_connection = connect_serial() + except Exception as e3: + # Exception triggered by too many consecutive empty reads + rospy.logwarn(e3) + serial_connection = connect_serial() pairs_or_error = process_message(buf) if type(pairs_or_error) is str: @@ -443,5 +466,5 @@ def connect_serial(serial_connection=None): serial_rate.sleep() # end of while loop - serial_connection.close() + close_serial() # end of main