00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
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
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 )
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()
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)
00090 results = self.reader.QueryEnvironment()
00091 if len(results) == 0:
00092 results = [[ '', -1 ]]
00093
00094
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)
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
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
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
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
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
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
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
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