SerialClient.py

Go to the documentation of this file.

#!/usr/bin/env python



import time

import rospy
from std_srvs.srv import Empty, EmptyResponse

from serial import Serial

from rosserial_python import SerialClient as _SerialClient

MINIMUM_RESET_TIME = 30

class SerialClient(_SerialClient):

    def __init__(self, *args, **kwargs):
        # The number of seconds to wait after a sync failure for a sync success before automatically performing a reset.
        # If 0, no reset is performed.
        self.auto_reset_timeout = kwargs.pop('auto_reset_timeout', 0)
        self.lastsync_reset = rospy.Time.now()
        rospy.Service('~reset_arduino', Empty, self.resetArduino)
        super(SerialClient, self).__init__(*args, **kwargs)

    def resetArduino(self, *args, **kwargs):
        """
        Forces the Arduino to perform a reset, as though its reset button was pressed.
        """
        with self.read_lock, self.write_lock:
            rospy.loginfo('Beginning Arduino reset on port %s. Closing serial port...' % self.port.portstr)
            self.port.close()
            with Serial(self.port.portstr) as arduino:
                arduino.setDTR(False)
                time.sleep(3)
                arduino.flushInput()
                arduino.setDTR(True)
                time.sleep(5)
            rospy.loginfo('Reopening serial port...')
            self.port.open()
            rospy.loginfo('Arduino reset complete.')
            self.lastsync_reset = rospy.Time.now()
        self.requestTopics()
        return EmptyResponse()

    def sendDiagnostics(self, level, msg_text):
        super(SerialClient, self).sendDiagnostics(level, msg_text)
        # Reset when we haven't received any data from the Arduino in over N seconds.
        if self.auto_reset_timeout and (rospy.Time.now() - self.last_read).secs >= self.auto_reset_timeout:
            if (rospy.Time.now() - self.lastsync_reset).secs < MINIMUM_RESET_TIME:
                rospy.loginfo('Sync has failed, but waiting for last reset to complete.')
            else:
                rospy.loginfo('Sync has failed for over %s seconds. Initiating automatic reset.' % self.auto_reset_timeout)
                self.resetArduino()