lib_M5e.py
Go to the documentation of this file.
00001 #
00002 # Copyright (c) 2009, Georgia Tech Research Corporation
00003 # All rights reserved.
00004 #
00005 # Redistribution and use in source and binary forms, with or without
00006 # modification, are permitted provided that the following conditions are met:
00007 #     * Redistributions of source code must retain the above copyright
00008 #       notice, this list of conditions and the following disclaimer.
00009 #     * Redistributions in binary form must reproduce the above copyright
00010 #       notice, this list of conditions and the following disclaimer in the
00011 #       documentation and/or other materials provided with the distribution.
00012 #     * Neither the name of the Georgia Tech Research Corporation nor the
00013 #       names of its contributors may be used to endorse or promote products
00014 #       derived from this software without specific prior written permission.
00015 #
00016 # THIS SOFTWARE IS PROVIDED BY GEORGIA TECH RESEARCH CORPORATION ''AS IS'' AND
00017 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 # DISCLAIMED. IN NO EVENT SHALL GEORGIA TECH BE LIABLE FOR ANY DIRECT, INDIRECT,
00020 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00021 # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
00022 # OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00023 # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00024 # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00025 # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 #
00027 
00028 #  \author Travis Deyle (Healthcare Robotics Lab, Georgia Tech.)
00029 
00030 
00031 ##  Thing Magic M5e and M5e-Compact Python Interface
00032 ##  Written by Travis Deyle please remember me if you become rich and/or famous, and please  
00033 ##    refer to our related work(s) when writing papers.  Thanks.
00034 
00035 import sys, serial, time, string
00036 from threading import Thread
00037 
00038 # A nice functional print function (Norvig)
00039 def prin(x): print x
00040 
00041 
00042 # This class is more general in that it allows custum antenna functions (if
00043 # using custom hardware) in addition to callbacks.
00044 
00045 class M5e_Poller(Thread):
00046     QUERY_MODE = 'query'
00047     TRACK_MODE = 'track'
00048 
00049     def __init__(self, M5e, antfuncs=[], callbacks=[]):
00050         Thread.__init__(self)
00051         self.M5e = M5e
00052         self.antfuncs = antfuncs
00053         self.callbacks = callbacks
00054         self.mode = ''
00055         self.should_run = True
00056         
00057         print 'Creating M5e Polling Thread'
00058         self.start()
00059 
00060     def pause_poller(self):
00061         self.mode = ''
00062 
00063     def query_mode(self):
00064         self.mode = self.QUERY_MODE
00065 
00066     def track_mode(self, tag_id):
00067         self.mode         = self.TRACK_MODE
00068         self.tag_to_track = tag_id
00069     
00070     def run(self):
00071         while self.should_run:
00072             if self.mode == self.QUERY_MODE:
00073                 for aF in self.antfuncs:
00074                     antennaName = aF(self.M5e)    # let current antFunc make appropriate changes
00075                     results = self.M5e.QueryEnvironment()
00076                     for tagid, rssi in results:
00077                         datum = [antennaName, tagid, rssi]
00078                         [cF(datum) for cF in self.callbacks]
00079             elif self.mode == self.TRACK_MODE:
00080                 for aF in self.antfuncs:
00081                     antennaName = aF(self.M5e)    # let current antFunc make appropriate changes
00082                     tagid = self.tag_to_track
00083                     rssi = self.M5e.TrackSingleTag(tagid)
00084                     #if rssi != -1:
00085                     datum = [antennaName, tagid, rssi]
00086                     [cF(datum) for cF in self.callbacks]
00087             else:
00088                 time.sleep(0.005)
00089         
00090     def stop(self):
00091         self.should_run = False
00092         self.join(3)
00093         if (self.isAlive()): 
00094             raise RuntimeError("RFID M5e Poller: unable to stop thread")
00095     
00096 class M5e:
00097     "Interface to Mercury M5e and M5e-Compact"
00098     def __init__(self, portSTR='/dev/robot/RFIDreader', baudrate=9600, 
00099         TXport=1, RXport=1, readPwr=2300, protocol='GEN2', compact=True, verbosity_func=prin):
00100 
00101         # verbosity_func should alawys take string as an input.  Lets us choose a selective 
00102         #   print function (like rospy.logout) instead of built-in print.
00103 
00104         try:
00105             self.port = string.atoi( portSTR ) # stores the serial port as 0-based integer for Windows
00106         except:
00107             self.port = portSTR # stores it as a /dev-mapped string for Linux / Mac
00108         
00109         self.baudrate = baudrate    # should be 9600 for M5e by default.  May want to up the baud for faster reading (after bootup)
00110         self.TXport = TXport        # Initialized transmit antenna
00111         self.RXport = RXport        # Initialized receive antenna
00112         self.readPwr = readPwr      # Initialized read TX power in centi-dBm
00113         self.protocol = protocol    # Initialized protocol
00114         self.compact = compact
00115         self.prin = verbosity_func
00116         self.ser = None
00117         
00118         self.prin( 'Initializing M5e (or M5e-Compact)' )
00119         
00120         # Setup Serial port  (kinda Hacky, but it works and I don't want to waste time on it)
00121         #    Read/Write timeouts should be appropriate for data sizes being dealt with.  Keep this in mind
00122         
00123         failed = False
00124         try:
00125             self.prin( '\tAttempting 230400 bps' )
00126             self.ser = serial.Serial(self.port, 230400, timeout=2, writeTimeout=2)
00127             # Check if BootLoader is running by issuing "Get BootLoader/Firmware Version"        
00128             self.TransmitCommand('\x00\x03')
00129             self.ReceiveResponse()      # automatically makes sure returned status = 0x0000
00130             self.prin( '\tSuccessful at 230400 bps' )
00131         except:
00132             self.ser = None
00133             failed = True
00134             self.prin( '\tFailed @ 230400 bps' )
00135             pass
00136         
00137         if failed:
00138             self.prin( '\tAttempting 9600 bps' )
00139             try:
00140                 self.ser = serial.Serial(self.port, 9600, timeout=2, writeTimeout=2)
00141                 self.TransmitCommand('\x00\x03')
00142                 self.ReceiveResponse()      # automatically makes sure returned status = 0x0000
00143                 self.prin( '\tSuccessful @ 9600 bps' )
00144             except:
00145                 self.prin( '\tFailed 9600 bps' )
00146                 self.ser = None
00147                 raise M5e_SerialPortError('Could not open serial port %s at baudrate %d or %d.' % (str(self.port),230400,9600))
00148             
00149             # Change baud to 230400 bps (instead of 9600 default)
00150             self.prin( '\tSwitching to 230400 bps' )
00151             self.TransmitCommand('\x04\x06\x00\x03\x84\x00')
00152             self.ReceiveResponse()    
00153             self.ser = None
00154             try:
00155                 self.prin( '\tAttempting to reconnect @ 230400 bps' )
00156                 self.ser = serial.Serial(self.port, 230400, timeout=2, writeTimeout=2)
00157                 # Check if BootLoader is running by issuing "Get BootLoader/Firmware Version"        
00158                 self.TransmitCommand('\x00\x03')
00159                 self.ReceiveResponse()      # automatically makes sure returned status = 0x0000
00160                 self.prin( '\tSuccessfully reconnected at 230400 bps' )
00161             except:
00162                 self.ser = None
00163                 self.prin( '\tFailed @ 230400 bps' )
00164                 pass
00165         
00166         if self.ser == None:
00167             raise M5e_SerialPortError('Could not open serial port %s at baudrate %d or %d.' % (str(self.port),230400,9600))
00168         
00169         # Check if BootLoader is running by issuing "Get BootLoader/Firmware Version"        
00170         self.TransmitCommand('\x00\x03')
00171         self.ReceiveResponse()      # automatically makes sure returned status = 0x0000
00172         
00173         # Boot into Firmware
00174         self.TransmitCommand('\x00\x04')
00175         try:
00176             self.ReceiveResponse()
00177         except M5e_CommandStatusError, inst:
00178             # Non-Zero Response will be received if the reader has already booted into firmware
00179             #   This occurs when you've already powered-up & previously configured the reader.  
00180             #   Can safely ignore this problem and continue initialization
00181                 if inst[1] == '\x01\x01':       # This actually means "invalid opcode"
00182                     pass
00183                 else:
00184                     raise
00185         
00186         self.ChangeAntennaPorts(self.TXport, self.RXport)
00187         self.ChangeTXReadPower(self.readPwr)
00188                 
00189         # Set Current Tag Protocol to GEN2
00190         if self.protocol != 'GEN2':
00191             raise M5e_error('Sorry, GEN2 is only protocol supported at this time')
00192         self.TransmitCommand('\x02\x93\x00\x05')
00193         self.ReceiveResponse()
00194         
00195         # Set Region (we're only going to deal with North America)
00196         self.TransmitCommand('\x01\x97\x01')
00197         self.ReceiveResponse()
00198         
00199         # Set Power Mode (we'll just use default of "full power mode").
00200         # Use remaining defaults
00201         
00202         
00203         
00204     def ChangeAntennaPorts(self, TXport, RXport):
00205         "Changes TX and RX ports"
00206         self.TXport = TXport
00207         self.RXport = RXport
00208         self.TransmitCommand('\x02\x91' + chr(self.TXport) + chr(self.RXport))
00209         self.ReceiveResponse()
00210         
00211     def ChangeTXReadPower(self, readPwr):
00212         "Sets the Read TX Power based on current value of readPwr (in centi-dBm)"
00213         self.readPwr = readPwr
00214         readTXPwrHighByte = chr((self.readPwr & 0xFFFF) >> 8)
00215         readTXPwrLowByte = chr(self.readPwr & 0x00FF)
00216         self.TransmitCommand('\x02\x92'+ readTXPwrHighByte + readTXPwrLowByte)
00217         self.ReceiveResponse()
00218         
00219     def CalculateCRC(self, msg):
00220         "Implements CCITT CRC-16 defined in Mercury Embedded Module Dev Guide."
00221         crcResult = 0xFFFF
00222         for x in range(len(msg)):
00223             currChar = ord(msg[x])
00224             v = 0x80
00225             for y in range(8):
00226                 xor_flag = 0
00227                 if (crcResult & 0x8000):
00228                     xor_flag = 1
00229                 crcResult = crcResult << 1
00230                 if (currChar & v):
00231                     crcResult = crcResult + 1
00232                 if (xor_flag):
00233                     crcResult = crcResult ^ 0x1021
00234                 v = v >> 1
00235                 crcResult = crcResult & 0xFFFF
00236             #print hex(currChar)
00237         return chr((crcResult >> 8) & 0xFF) + chr(crcResult & 0xFF)
00238         # return the 16-bit result in the form of 2 characters
00239 
00240     def ReturnHexString(self, hexStr):
00241         "Helper function to visualize a hex string (such as a hexCommand)"
00242         result = ''
00243         for i in range(len(hexStr)):
00244             result = result + hex(ord(hexStr[i])) + ' '
00245         return result
00246 
00247     def ConstructCommand(self, hexCommand):
00248         "Helper function to attach start byte and CRC to a command string"
00249         return '\xFF' + hexCommand + self.CalculateCRC(hexCommand)
00250 
00251     def TransmitCommand(self, command):
00252         "Transmits a command.  Should call ReceiveResponse before calling again."
00253         try:
00254             self.ser.write(self.ConstructCommand(command))
00255         except:
00256             raise M5e_TransmitTimeoutExceeded('Something happened (probably power failure) to disable serial transmission')
00257         
00258     def ReceiveResponse(self):
00259         "Receives a single command's response"
00260         # Get Start byte, disregard anything else...
00261         timeoutsToWait = 5
00262         timeoutsWaited = 0
00263 
00264         while timeoutsWaited < timeoutsToWait:
00265             start = self.ser.read()         # this is non-blocking.  Will wait for timeout duration, then return
00266             if start == '\xFF':
00267                 break
00268             timeoutsWaited += 1
00269             
00270         if start != '\xFF':
00271             time.sleep(5)
00272             self.ser.flushInput()
00273             raise M5e_ReceiveError('Error in receive stream (start byte).  Waited 5 seconds then flushed input.  Try reissueing command and receive response.')
00274             
00275         length = self.ser.read()        # non blocking, returns after timeout
00276         if len(length) != 1:
00277             time.sleep(5)
00278             self.ser.flushInput()
00279             raise M5e_ReceiveError('Error in receive stream (length byte).  Waited 5 seconds then flushed input.  Try reissueing command and receive response.')
00280 
00281         command = self.ser.read()
00282         if len(command) != 1:
00283             time.sleep(5)
00284             self.ser.flushInput()
00285             raise M5e_ReceiveError('Error in receive stream (command byte).  Waited 5 seconds then flushed input.  Try reissueing command and receive response.')
00286 
00287         status = self.ser.read(2)       # non-blocking
00288         if len(status) != 2:
00289             time.sleep(5)
00290             self.ser.flushInput()
00291             raise M5e_ReceiveError('Error in receive stream (status bytes).  Waited 5 seconds then flushed input.  Try reissueing command and receive response.')
00292 
00293         data = self.ser.read(ord(length))
00294         if len(data) != ord(length):
00295             time.sleep(5)
00296             self.ser.flushInput()
00297             raise M5e_ReceiveError('Error in receive stream (data bytes).  Waited 5 seconds then flushed input.  Try reissueing command and receive response.')
00298 
00299         CRC = self.ser.read(2)
00300         if len(CRC) != 2:
00301             time.sleep(5)
00302             self.ser.flushInput()
00303             raise M5e_ReceiveError('Error in receive stream (CRC bytes).  Waited 5 seconds then flushed input.  Try reissueing command and receive response.')
00304         
00305         # Validate the CRC
00306         validateCRC = length + command + status + data
00307         if self.CalculateCRC(validateCRC) != CRC:
00308             raise M5e_CRCError('Received response CRC failed')
00309         
00310         # Check if return status was OK (0x0000 is success)
00311         if status != '\x00\x00':
00312             raise M5e_CommandStatusError('Received response returned non-zero status',status)
00313             
00314         return (start, length, command, status, data, CRC)
00315         
00316     def QueryEnvironment(self, timeout=50):
00317         # Read Tag ID Multiple
00318         timeoutHighByte = chr((timeout & 0xFFFF) >> 8)
00319         timeoutLowByte = chr(timeout & 0x00FF)
00320         try:
00321             self.TransmitCommand('\x04\x22\x00\x00'+timeoutHighByte+timeoutLowByte)
00322             self.ReceiveResponse()
00323         except M5e_CommandStatusError, inst:
00324             flag = False
00325             # Read & Get returns non-zero status when no tags found.  This is 
00326             #   an expected event... so ignore the exception.
00327             if inst[1] == '\x04\x00':   
00328                 return []
00329             else:
00330                 raise
00331 
00332         # Get Tag Buffer
00333         #   Get # Tags remaining
00334         self.TransmitCommand('\x00\x29')
00335         (start, length, command, status, data, CRC) = self.ReceiveResponse()
00336         
00337         readIndex = (ord(data[0]) << 8) + ord(data[1])
00338         writeIndex = (ord(data[2]) << 8) + ord(data[3])
00339         numTags = writeIndex - readIndex
00340         
00341 #         tagEPCs = []
00342 #         while numTags > 0:
00343 #             numFetch = min([numTags, 13])
00344 #             numFetchHighByte = chr((numFetch & 0xFFFF) >> 8)
00345 #             numFetchLowByte = chr(numFetch & 0x00FF)
00346 #             self.TransmitCommand('\x02\x29' + numFetchHighByte + numFetchLowByte)
00347 #             (start, length, command, status, data, CRC) = self.ReceiveResponse()
00348             
00349 #             # each tag occupies 18 bytes in the response, regardless of tag size
00350 #             for i in range(numFetch): # NOTE: first 4 bytes in GEN2 are size & protocol.  Last 2 are CRC
00351 #                 tagEPCs.append(data[i*18+4:(i+1)*18-2])
00352 #                 #tagEPCs.append(data[i*18:(i+1)*18])
00353             
00354 #             numTags = numTags - numFetch
00355 
00356         results = []   # stored as (ID, RSSI)
00357         while numTags > 0:
00358             self.TransmitCommand('\x03\x29\x00\x02\x00')
00359             (start, length, command, status, data, CRC) = self.ReceiveResponse()
00360 
00361             tagsRetrieved = ord(data[3])
00362             for i in xrange(tagsRetrieved):
00363                 rssi = ord(data[4 + i*19])
00364                 tagID = data[4 + i*19 + 5 : 4 + i*19 + 5 + 12]
00365                 results.append( (tagID, rssi) )
00366             
00367             numTags = numTags - tagsRetrieved
00368             
00369         # Reset/Clear the Tag ID Buffer for next Read Tag ID Multiple
00370         self.TransmitCommand('\x00\x2A')
00371         self.ReceiveResponse()
00372             
00373         return results
00374     
00375     def TrackSingleTag(self, tagID, timeout=50):
00376         # Make sure TagID is 60-bits
00377         if len(tagID) != 12:
00378             raise M5e_Error('Only 96-bit tags supported by TrackSingleTag')
00379         
00380         timeoutHighByte = chr((timeout & 0xFFFF) >> 8)
00381         timeoutLowByte = chr(timeout & 0x00FF)
00382         try:
00383             self.TransmitCommand('\x12\x21'+timeoutHighByte+timeoutLowByte+'\x11\x00\x02\x60'+tagID)
00384             (start, length, command, status, data, CRC) = self.ReceiveResponse()
00385             # data is in form OptionsFlags (1 byte) MetaFlags (2 bytes) RSSI (1 byte), EPC (12 bytes data[-14:-2]) TAG CRC (2 bytes)
00386         except M5e_CommandStatusError, inst:
00387             flag = False
00388             # Read & Get returns non-zero status when no tags found.  This is 
00389             #   an expected event... so ignore the exception.
00390             if inst[1] == '\x04\x00':   
00391                 return -1   # Requested tag not found
00392             else:
00393                 raise
00394             
00395         return ord(data[3])
00396 
00397     def ChangeTagID(self, newTagID, timeout=50):
00398         # Note: This will rewrite the EPC on the first tag detected!
00399         
00400         # Make sure TagID is 60-bits
00401         if len(newTagID) != 12:
00402             raise M5e_Error('Only 96-bit tags supported by TrackSingleTag')
00403         
00404         timeoutHighByte = chr((timeout & 0xFFFF) >> 8)
00405         timeoutLowByte = chr(timeout & 0x00FF)
00406         try:
00407             cmdLen = chr(len(newTagID)+4)
00408             self.TransmitCommand(cmdLen+'\x23'+timeoutHighByte+timeoutLowByte+'\x00\x00'+newTagID)
00409             (start, length, command, status, data, CRC) = self.ReceiveResponse()
00410         except M5e_CommandStatusError, inst:
00411             return False
00412             
00413         return True
00414 
00415 
00416 class M5e_error(Exception):
00417     "General Exception"
00418     pass
00419 
00420 class M5e_SerialPortError(M5e_error):
00421     "If pyserial throws error"
00422     pass
00423     
00424 class M5e_CRCError(M5e_error):
00425     "If CRC check from Mercury packet is incorrect (data corruption)"
00426     pass
00427     
00428 class M5e_CommandStatusError(M5e_error):
00429     "If return response from Mercury is non-zero (error status)"
00430     pass
00431 
00432 class M5e_TransmitTimeoutExceeded(M5e_error):
00433     "Something happened (probably power failure) to disable serial transmission"
00434     pass
00435 
00436 class M5e_ReceiveError(M5e_error):
00437     "Serial input out of synch.  Try waiting a few seconds, flush input stream, and reissue command"
00438     pass
00439 
00440 
00441 ## THIS IS THE "DIRECT" METHOD...
00442 ##if __name__ == '__main__':
00443 ##    import time
00444 ##    import numpy as np
00445 ##    
00446 ##    read = M5e(portSTR='/dev/ttyUSB0', baudrate=230400)   
00447 ##
00448 ##    # Demonstrate Querying the environment
00449 ##    for i in range(5):
00450 ##        print read.QueryEnvironment()
00451 ##        time.sleep(1)
00452 ##        
00453 ##    # To track tags' RSSI
00454 ##    tags = read.QueryEnvironment()
00455 ##    for tag in tags:
00456 ##        print [tag], read.TrackSingleTag(tag)
00457 
00458 
00459 ## METHOD USING THREADS (more generally useful)
00460 if __name__ == '__main__':
00461     import time
00462     
00463     def P1(M5e):
00464         M5e.ChangeAntennaPorts(1,1)
00465         return 'AntPort1'
00466 
00467     def P2(M5e):
00468         M5e.ChangeAntennaPorts(2,2)
00469         return 'AntPort2'
00470 
00471     def PrintDatum(data):
00472         ant, ids, rssi = data
00473         print data
00474 
00475     print 'Starting with read power 2300 centi-dBm.  Change to 3000 for M5e-full'
00476     r = M5e(readPwr=2300)
00477     q = M5e_Poller(r, antfuncs=[P1, P2], callbacks=[PrintDatum])
00478 
00479     q.query_mode()
00480     
00481     t0 = time.time()
00482     while time.time() - t0 < 3.0:
00483         blah = 0
00484 
00485     q.track_mode('RedBottle   ')
00486 
00487     t0 = time.time()
00488     while time.time() - t0 < 3.0:
00489         blah = 0
00490 
00491     q.stop()


hrl_rfid
Author(s): Travis Deyle, Advisor: Prof. Charlie Kemp (Healthcare Robotics Lab at Georgia Tech) and Prof. Matt Reynolds (Duke University)
autogenerated on Wed Nov 27 2013 11:32:45