Go to the documentation of this file.00001 import struct
00002 import rospy
00003 from BAL.Handlers.incomingHandler import ACK_RES
00004 from BAL.Header.Response.ackResponse import ACKResponse
00005
00006 __author__ = 'tom1231'
00007 from threading import RLock
00008 HEADER_START = 0xfe
00009 HEADER_DEBUG = 0xfd
00010 KEEP_ALIVE_HEADER = 0xf9
00011
00012 class SerialWriteHandler:
00013 def __init__(self, ser, incomingDataHandler, input):
00014 self._incomingDataHandler = incomingDataHandler
00015 self._input = input
00016 self._serial = ser
00017 self._lock = RLock()
00018
00019 def write(self, data):
00020 self._lock.acquire()
00021 self._serial.write(chr(HEADER_START))
00022 self._serial.write(str(data))
00023 self._lock.release()
00024
00025 def writeAndWaitForAck(self, data, idToAck):
00026 self._lock.acquire()
00027 resend = True
00028 while resend:
00029 self.write(data)
00030 ack = self.waitForACK()
00031 if ack != None and ack.getIdToAck() == idToAck and ack.getReqLen() == len(data):
00032 resend = False
00033 self._lock.release()
00034
00035 def waitForACK(self):
00036 gotHeaderStart = False
00037 incomingLength = 0
00038 headerId = 0
00039 data = []
00040 timeoutCount = 3
00041 try:
00042 while timeoutCount > 0:
00043 if gotHeaderStart:
00044 if len(data) < 1:
00045 data.append(self._input.read())
00046 incomingLength, headerId = self._incomingDataHandler.getIncomingHeaderSizeAndId(data)
00047 elif incomingLength >= 1 and headerId == ACK_RES:
00048 for i in range(1, incomingLength):
00049 data.append(self._input.read())
00050 ack = ACKResponse()
00051 ack.buildRequest(data)
00052 if ack.checkPackage():
00053 return ack
00054 data = []
00055 timeoutCount -= 1
00056 gotHeaderStart = False
00057 else:
00058 data = []
00059 timeoutCount -= 1
00060 gotHeaderStart = False
00061 elif ord(self._input.read()) == HEADER_START:
00062 gotHeaderStart = True
00063 except TypeError:
00064 rospy.logerr('ACK have not been send ,retransmitting.......')
00065 return None