ros_M5e.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 #
00003 # Copyright (c) 2009, Georgia Tech Research Corporation
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions are met:
00008 #     * Redistributions of source code must retain the above copyright
00009 #       notice, this list of conditions and the following disclaimer.
00010 #     * Redistributions in binary form must reproduce the above copyright
00011 #       notice, this list of conditions and the following disclaimer in the
00012 #       documentation and/or other materials provided with the distribution.
00013 #     * Neither the name of the Georgia Tech Research Corporation nor the
00014 #       names of its contributors may be used to endorse or promote products
00015 #       derived from this software without specific prior written permission.
00016 #
00017 # THIS SOFTWARE IS PROVIDED BY GEORGIA TECH RESEARCH CORPORATION ''AS IS'' AND
00018 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 # DISCLAIMED. IN NO EVENT SHALL GEORGIA TECH BE LIABLE FOR ANY DIRECT, INDIRECT,
00021 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00022 # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
00023 # OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00024 # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00025 # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00026 # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 #
00028 
00029 #  \author Travis Deyle (Healthcare Robotics Lab, Georgia Tech.)
00030 
00031 
00032 
00033 # ROS imports
00034 import roslib; roslib.load_manifest('hrl_rfid')
00035 import rospy
00036 from hrl_rfid.msg import RFIDread
00037 from hrl_rfid.msg import RFIDreadArr
00038 from hrl_rfid.srv import RfidSrv
00039 import hrl_rfid.lib_M5e as M5e
00040 
00041 import time
00042 from threading import Thread
00043 
00044 
00045 # Modeled off lib_M5e.M5e_Poller
00046 class ROS_M5e( Thread ):
00047     QUERY_MODE = 'query'
00048     TRACK_MODE = 'track'
00049     
00050     def __init__(self, name = 'reader1', readPwr = 2300,
00051                  portStr = '/dev/robot/RFIDreader',
00052                  antFuncs = [], callbacks = []):
00053 
00054         Thread.__init__(self)
00055         self.should_run =  True
00056 
00057         try:
00058             rospy.init_node( 'rfid_m5e_' + name )
00059         except rospy.ROSException:
00060             pass
00061 
00062         self.mode = ''
00063         self.name = name + '_reader'
00064 
00065         rospy.logout( 'ROS_M5e: Launching RFID Reader' )
00066         rospy.logout( 'ROS_M5e: Please check out our related work @ http://www.hsi.gatech.edu/hrl/project_rfid.shtml' )
00067         rospy.logout( 'ROS_M5e: '+self.name+' Building & Connecting to reader' )
00068 
00069         def prin( x ): rospy.logout( 'ROS_M5e: lib_M5e: ' + x ) # use rospy.logout in underlying lib's output
00070 
00071         self.reader = M5e.M5e(readPwr=readPwr, portSTR = portStr, verbosity_func = prin)
00072         self.antFuncs = antFuncs
00073         self.callbacks = callbacks + [self.broadcast]
00074 
00075         rospy.logout( 'ROS_M5e: publishing RFID reader with type RFIDread to channel /rfid/'+name+'_reader' )
00076         self.channel       = rospy.Publisher('/rfid/'+name+'_reader', RFIDread)
00077         self.pub_arr = rospy.Publisher('/rfid/'+name+'_reader_arr', RFIDreadArr)
00078         self._mode_service_obj = rospy.Service('/rfid/'+name+'_mode',
00079                                                 RfidSrv, self._mode_service)
00080 
00081         rospy.logout( 'ROS_M5e: '+self.name+' Inialized and awaiting instructions' )
00082 
00083         self.start()  # Thread: calls self.run()
00084 
00085     def run( self ): 
00086         while self.should_run and not rospy.is_shutdown():
00087             if self.mode == self.QUERY_MODE:
00088                 for aF in self.antFuncs:
00089                     antennaName = aF(self.reader)    # let current antFunc make appropriate changes
00090                     results = self.reader.QueryEnvironment()
00091                     if len(results) == 0:
00092                         results = [[ '', -1 ]] # [[ tagid, rssi ], ...]
00093                         #datum = [antennaName, '', -1]
00094                         #[cF(datum) for cF in self.callbacks]
00095                     arr = []
00096                     t_now = rospy.Time.now()
00097                     for tagid, rssi in results:
00098                         rv = RFIDread( None, antennaName, tagid, rssi )
00099                         rv.header.stamp = t_now
00100                         arr.append( rv )
00101                         datum = [antennaName, tagid, rssi]
00102                         [cF(datum) for cF in self.callbacks]
00103                     rfid_arr = RFIDreadArr()
00104                     rfid_arr.header.stamp = t_now
00105                     rfid_arr.arr = arr
00106                     self.pub_arr.publish( rfid_arr )
00107             elif self.mode == self.TRACK_MODE:
00108                 for aF in self.antFuncs:
00109                     antennaName = aF(self.reader)    # let current antFunc make appropriate changes
00110                     tagid = self.tag_to_track
00111                     rssi = self.reader.TrackSingleTag(tagid, timeout=50)
00112                     t_now = rospy.Time.now()
00113                     rv = RFIDread( None, antennaName, tagid, rssi )
00114                     rv.header.stamp = t_now
00115                     rfid_arr = RFIDreadArr()
00116                     rfid_arr.header.stamp = t_now
00117                     rfid_arr.arr = [rv]
00118                     self.pub_arr.publish( rfid_arr )
00119                     #if rssi != -1:
00120                     datum = [antennaName, tagid, rssi]
00121                     [cF(datum) for cF in self.callbacks]
00122             else:
00123                 time.sleep(0.005)
00124 
00125         rospy.logout( 'ROS_M5e: '+self.name+' Shutting down reader' )
00126 
00127     def stop( self ):
00128         self.should_run = False
00129         self.join(3)
00130         if (self.isAlive()):
00131             raise RuntimeError("ROS_M5e: unable to stop thread")            
00132             
00133     def broadcast(self, data):
00134         antName, tagid, rssi = data
00135         rv = RFIDread( None, antName, tagid, rssi )
00136         rv.header.stamp = rospy.Time.now()
00137         self.channel.publish( rv )
00138     
00139     # For internal use only
00140     def _mode_service(self, data):
00141         val = data.data
00142         if len(val) == 0:
00143             rospy.logout( 'ROS_M5e: Mode Service called with invalid argument: ' + str(val) )
00144         elif len(val) == 1:
00145             if val[0] == self.QUERY_MODE:
00146                 rospy.logout( 'ROS_M5e: '+self.name+' Entering Query Mode' )
00147                 self.mode = self.QUERY_MODE
00148             else:
00149                 rospy.logout( 'ROS_M5e: '+self.name+' Stopping Reader' )
00150                 self.mode = ''
00151         elif len(val) == 2:
00152             if val[0] == self.TRACK_MODE and len(val[1]) == 12:
00153                 rospy.logout( 'ROS_M5e: '+self.name+' Entering Track Mode: ' + str(val[1]) )
00154                 self.mode = self.TRACK_MODE
00155                 self.tag_to_track = val[1]
00156             else:
00157                 rospy.logout( 'ROS_M5e: Mode Service called with invalid argument: ' + str(val) )
00158         else:
00159             rospy.logout( 'ROS_M5e: Mode Service called with invalid argument: ' + str(val) )
00160         return True
00161 
00162 
00163 
00164 # -----------------------------------------------
00165 # Likely Callbacks: (various antennas)
00166 # -----------------------------------------------
00167 
00168 def EleLeftEar(M5e):
00169     M5e.ChangeAntennaPorts(1,1)
00170     time.sleep(0.010)
00171     return 'EleLeftEar'
00172 
00173 def EleRightEar(M5e):
00174     M5e.ChangeAntennaPorts(2,2)
00175     time.sleep(0.010)
00176     return 'EleRightEar'
00177 
00178 def Hand_Right_1(M5e):
00179     # GPIO1 = 1, GPIO2 = 0
00180     M5e.TransmitCommand('\x02\x96\x01\x01')
00181     M5e.ReceiveResponse()
00182     M5e.TransmitCommand('\x02\x96\x02\x00')
00183     M5e.ReceiveResponse()
00184     return 'Hand_Right_1'
00185 
00186 def Hand_Right_2(M5e):
00187     # GPIO1 = 1, GPIO2 = 1
00188     M5e.TransmitCommand('\x02\x96\x01\x01')
00189     M5e.ReceiveResponse()
00190     M5e.TransmitCommand('\x02\x96\x02\x01')
00191     M5e.ReceiveResponse()
00192     return 'Hand_Right_2'
00193 
00194 def Hand_Left_1(M5e):
00195     # GPIO1 = 0, GPIO2 = 0
00196     M5e.TransmitCommand('\x02\x96\x01\x00')
00197     M5e.ReceiveResponse()
00198     M5e.TransmitCommand('\x02\x96\x02\x00')
00199     M5e.ReceiveResponse()
00200     return 'Hand_Left_1'
00201 
00202 def Hand_Left_2(M5e):
00203     # GPIO1 = 0, GPIO2 = 1
00204     M5e.TransmitCommand('\x02\x96\x01\x00')
00205     M5e.ReceiveResponse()
00206     M5e.TransmitCommand('\x02\x96\x02\x01')
00207     M5e.ReceiveResponse()
00208     return 'Hand_Left_2'
00209 
00210 def PrintDatum(data):
00211     ant, ids, rssi = data
00212     print data
00213 
00214 if __name__ == '__main__':
00215     import optparse
00216 
00217     p = optparse.OptionParser()
00218     p.add_option('-d', action='store', type='string', dest='device',
00219                  help='Which RFID device to initialize.')
00220     p.add_option('-p', action='store', type='int', dest='power', default=3000,
00221                  help='Which RFID device to initialize.')
00222     opt, args = p.parse_args()
00223 
00224     if opt.device == 'ears':
00225         print 'Starting Ears RFID Services'
00226         ros_rfid = ROS_M5e( name = 'ears', readPwr = opt.power,
00227                             portStr = '/dev/robot/RFIDreader',
00228                             antFuncs = [EleLeftEar, EleRightEar],
00229                             callbacks = [] )
00230         rospy.spin()
00231         ros_rfid.stop()
00232 
00233     if opt.device == 'inhand':
00234         print 'Starting Inhand RFID Services'
00235         ros_rfid = ROS_M5e( name = 'inhand', readPwr = opt.power,
00236                             portStr = '/dev/robot/inHandReader',
00237                             antFuncs = [Hand_Right_1, Hand_Right_2,
00238                                         Hand_Left_1, Hand_Left_2 ],
00239                             callbacks = [] )
00240         rospy.spin()
00241         ros_rfid.stop()
00242 
00243     if opt.device == 'head':
00244         print 'Starting PR2 Head RFID Services'
00245 
00246         def PR2_Head(M5e):
00247             M5e.ChangeAntennaPorts(1,1)
00248             time.sleep(0.010)
00249             return 'PR2_Head'
00250 
00251         ros_rfid = ROS_M5e( name = 'head', readPwr = opt.power,
00252                             portStr = '/dev/robot/RFIDreader',
00253                             antFuncs = [PR2_Head],
00254                             callbacks = [] )
00255         rospy.spin()
00256         ros_rfid.stop()
00257 
00258 


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