From 82b2bbb54679b31e835e51d73af08f5a1d783e55 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sun, 26 Jun 2016 18:43:21 -0700 Subject: [PATCH] Improved initial connection loop in arduino_driver.py --- .../src/ros_arduino_python/arduino_driver.py | 28 +++++++++++++++---- 1 file changed, 22 insertions(+), 6 deletions(-) diff --git a/ros_arduino_python/src/ros_arduino_python/arduino_driver.py b/ros_arduino_python/src/ros_arduino_python/arduino_driver.py index e84c70e..2f1d90c 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_driver.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_driver.py @@ -77,15 +77,31 @@ class Arduino: # When an Arduino is first plugged in, it can take time for the serial port to wake up max_attempts = 10 - attempts = 0 + attempts = 1 + timeout = self.timeout # Wake up the serial port - self.serial_port.write('\r') - while attempts < max_attempts and self.serial_port.read() == '': - rospy.loginfo("Waking up serial port...") + while attempts < max_attempts: self.serial_port.write('\r') + time.sleep(timeout) + test = self.serial_port.read() + if test != '': + break + + rospy.loginfo("Waking up serial port attempt " + str(attempts) + " of " + str(max_attempts)) + # Increase timeout by 10% + timeout *= 1.1 + self.serial_port.timeout = timeout + self.serial_port.writeTimeout = timeout + self.serial_port.flushInput() + self.serial_port.flushOutput() attempts += 1 - time.sleep(1) + + if test == '': + raise SerialException + + if timeout != self.timeout: + rospy.loginfo("Found best timeout to be " + str(timeout) + " seconds") # Test the connection by reading the baudrate attepmpts = 0 @@ -97,7 +113,7 @@ class Arduino: try: self.serial_port.inWaiting() rospy.loginfo("Connected at " + str(self.baudrate)) - rospy.loginfo("Arduino is ready.") + rospy.loginfo("Arduino is ready!") except IOError: raise SerialException