fix for #331

nodes/arduino_handler.py

@@ -323,7 +323,6 @@ def connect_serial(serial_connection=None):
     port = None
     while port is None:
         try:
-
             if not os.path.exists(path):
               raise Exception("No serial device found on system in {}".format(path))
 
@@ -333,8 +332,9 @@ def connect_serial(serial_connection=None):
             port = ports[0]
             serial_connection = serial.Serial(os.path.join(path, port),
                                               baud_rate,
-                                              timeout=timeout_s,
-                                              writeTimeout=timeout_s)
+                                              dsrdtr = True, # Causes an Arduino soft reset
+                                              timeout = timeout_s,
+                                              writeTimeout = timeout_s)
             return serial_connection
         except Exception as e:
             rospy.logwarn(e)
@@ -356,6 +356,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
@@ -389,6 +392,7 @@ def connect_serial(serial_connection=None):
         # Fix issue #328, sometimes serial_connection is None because of a 
         # serial port path / or error opening issue.
         if serial_connection is None:
+            serial_connection.close() # Forces DTS on reconnect to reset Arduino
             serial_connection = connect_serial()
 
         try:
@@ -404,7 +408,16 @@ def connect_serial(serial_connection=None):
             # (rospy.Rate will still try to keep the loop at serial_rate_hz)
             rospy.sleep(arduino_delay_s)
             # Blocks until one line is read or times out
-            buf = serial_connection.readline() 
+            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
@@ -417,10 +430,17 @@ def connect_serial(serial_connection=None):
         except serial.serialutil.SerialException as e1:
             # This usually happens when the serial port gets closed or switches
             rospy.logwarn(e1)
+            serial_connection.close()
             serial_connection = connect_serial()
         except serial.serialutil.SerialTimeoutException as e2:
             # Exception that is raised on write timeouts
             rospy.logwarn(e2)
+            serial_connection.close()
+            serial_connection = connect_serial()
+        except Exception as e3:
+            # Exception triggered by too many consecutive empty reads
+            rospy.logwarn(e3)
+            serial_connection.close()
             serial_connection = connect_serial()
 
         pairs_or_error = process_message(buf)