00001
00002
00003 import roslib
00004 roslib.load_manifest('rfid_people_following')
00005 import rospy
00006
00007 import rfid_people_following.db_rfid as db_rfid
00008 from rfid_people_following.srv import rfid_gui as gui_srv
00009
00010 import time
00011 import string
00012 import pygame
00013 import pygame.display
00014 import pygame.locals
00015 import pygame.transform
00016 import numpy as np, math
00017
00018
00019 class ROS_UI_Robot():
00020 def __init__(self, init=True):
00021 if init:
00022 try:
00023 print 'Initializing RFID UI on Robot'
00024 rospy.init_node('RFID_UI_Robot',anonymous=True)
00025 except rospy.ROSException:
00026 pass
00027 self.tag_pub = rospy.Publisher('/hrl/ele/UI_Robot',RFID_Interface)
00028 print 'RFID UI on Robot Running'
00029
00030 print 'Connecting to RFID UI on PC'
00031 self.selected_tag = ''
00032 self.selection = []
00033 rospy.Subscriber('/hrl/ele/UI_PC_Selection', RFID_Interface, self.selection_cb, None, 1)
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045 def publish_tag_ids(self, tagIDs):
00046 pub_msg = RFID_Interface()
00047 pub_msg.tagIDs = tagIDs
00048 pub_msg.humanread = []
00049 pub_msg.actions = []
00050
00051 self.tag_pub.publish( pub_msg )
00052 time.sleep(0.002)
00053
00054
00055
00056
00057
00058 def selection_cb(self, rfid_msg):
00059 tagID = rfid_msg.tagIDs[0]
00060 action = rfid_msg.actions[0]
00061 humanread = rfid_msg.humanread[0]
00062 self.selection = [tagID, humanread, action]
00063
00064
00065 def receive_response(self):
00066 while self.selection == []:
00067 time.sleep(0.02)
00068 rv = self.selection
00069 self.selection = []
00070 return rv
00071
00072
00073
00074
00075
00076
00077 class RFID_GUI():
00078 def __init__(self, graphical=False, db = None):
00079 try:
00080 rospy.init_node('rfid_gui',anonymous=True)
00081 except rospy.ROSException:
00082 pass
00083
00084 rospy.logout( 'rfid_gui: Initializing' )
00085
00086 self.graphical = graphical
00087 self.images_path = 'db_images/'
00088 self.db = db
00089
00090 self._srv = rospy.Service( '/rfid_gui/select', gui_srv, self.gui_response )
00091
00092 rospy.logout( 'rfid_gui: Waiting for service calls.' )
00093
00094 def gui_response(self, request):
00095 keys = request.keys
00096
00097 keys = [ k for k in keys if self.db.has_key( k ) ]
00098 humanread = [ self.db[ k ]['humanread'] for k in keys ]
00099 imgs = [ self.db[ k ]['img'] for k in keys ]
00100
00101 if len( keys ) == 0:
00102 rospy.logout( 'rfid_gui: No items to select from. Fail.' )
00103 return False, ''
00104
00105
00106
00107
00108
00109 if self.graphical:
00110 success, selection = self.get_selection_graphical( keys, humanread, imgs )
00111 else:
00112 success, selection = self.get_selection_text( keys, humanread, imgs )
00113
00114 return int(success), selection
00115
00116
00117 def get_selection_text( self, keys, humanread, imgs ):
00118
00119 ind = -1
00120 while ind.__class__ != (1).__class__ or ind < 0 or ind > len( keys ) - 1:
00121 print 'Make a selection:'
00122 for i, hr in enumerate( humanread ):
00123 print '\t(%d) %s' % (i, hr)
00124 resp = raw_input()
00125 try:
00126 ind = string.atoi( resp )
00127 except:
00128 ind = ''
00129 pass
00130 rospy.logout( 'rfid_gui: Returning selection: %s' % humanread[ ind ] )
00131 return True, keys[ ind ]
00132
00133 def smart_scale(self, image):
00134 ims = np.array(image.get_size(),dtype='float')
00135 scale = self.imheight / np.max(ims)
00136 return pygame.transform.scale(image, tuple(ims*scale))
00137
00138 def calc_blit_loc(self, image, center_pos):
00139 ims = np.array(image.get_size(), dtype='float')
00140 horiz = center_pos[0] - self.imheight/2 + (self.imheight - ims[0]) / 2.
00141 vert = center_pos[1] - self.imheight/2 + (self.imheight - ims[1]) / 2.
00142 return (horiz, vert)
00143
00144 def get_selection_graphical( self, keys, humanread, imgs ):
00145
00146 pygame.init()
00147 self.s_width = 600
00148 self.s_height = 700
00149 srf = pygame.display.set_mode((self.s_width,self.s_height))
00150 fps = 100
00151 loopFlag = True
00152 clk = pygame.time.Clock()
00153 obj = [srf, fps, clk]
00154 self.imheight = 175.
00155
00156 w = self.s_width * 1.0
00157 h = self.s_height * 1.0
00158 blit_pos = [[ w/3-w/6, h/3-h/6],
00159 [ w/3-w/6+w/3, h/3-h/6],
00160 [ w/3-w/6+2*w/3, h/3-h/6],
00161 [ w/3-w/6, h/3-h/6+h/3],
00162 [ w/3-w/6+w/3, h/3-h/6+h/3],
00163 [ w/3-w/6+2*w/3, h/3-h/6+h/3],
00164 [ w/3-w/6, h/3-h/6+2*h/3],
00165 [ w/3-w/6+w/3, h/3-h/6+2*h/3],
00166 [ w/3-w/6+2*w/3, h/3-h/6+2*h/3]]
00167 images = []
00168 surfaces = []
00169 blit_loc = []
00170 for i, k in enumerate(keys):
00171 rospy.logout( 'rfid_gui: Loading image: %s' % self.images_path + imgs[i] )
00172 k_image = pygame.image.load(self.images_path + imgs[i]).convert()
00173 k_image = self.smart_scale(k_image)
00174 images.append( k_image )
00175 blit_loc.append( blit_pos[i] )
00176
00177
00178 srf.blit(k_image, self.calc_blit_loc(k_image,blit_loc[i]))
00179
00180 ind = self.get_selection( obj, images, humanread, blit_loc )
00181
00182 rospy.logout( 'rfid_gui: Returning selection: %s' % humanread[ ind ] )
00183 return True, keys[ ind ]
00184
00185 def put_bottom_text( self, srf, text ):
00186 font = pygame.font.Font(None, 25)
00187 box = font.render(text, 1,(10, 10, 10, 0))
00188 ts = box.get_size()
00189 horiz = self.s_width / 2.0 - ts[0]/2.0
00190 vt = self.s_height - 50.0 - ts[1]/2.0
00191 srf.blit(box, (horiz, vt))
00192 return True
00193
00194 def draw_rect(self, srf, blit_loc):
00195 width = self.imheight * 1.10
00196 height = self.imheight *1.10
00197 horiz = blit_loc[0] - width / 2.0
00198 vert = blit_loc[1] - height / 2.0
00199 pygame.draw.rect(srf, (255, 0, 0), (horiz, vert, width, height))
00200 width = self.imheight * 1.01
00201 height = self.imheight *1.01
00202 horiz = blit_loc[0] - width / 2.0
00203 vert = blit_loc[1] - height / 2.0
00204 pygame.draw.rect(srf, (255, 255, 255), (horiz, vert, width, height))
00205 return True
00206
00207 def get_selection( self, obj, images, humanread, blit_loc ):
00208 [srf, fps, clk] = obj
00209 loopFlag = True
00210 ind = 0
00211 pos = (0,0)
00212 while loopFlag:
00213
00214 srf.fill((255,255,255))
00215
00216 diffs = np.array(blit_loc) - np.array(pos)
00217 ind = np.argmin( np.sum( np.power( diffs, 2.0 ), axis = 1 ) )
00218
00219 self.put_bottom_text( srf, humanread[ind] )
00220 self.draw_rect(srf, blit_loc[ind])
00221
00222 for i, image in enumerate(images):
00223 srf.blit(image, self.calc_blit_loc(image,blit_loc[i]))
00224
00225
00226 pygame.display.flip()
00227
00228 events = pygame.event.get()
00229 for e in events:
00230 if e.type==pygame.locals.QUIT:
00231 loopFlag=False
00232 if e.type==pygame.locals.KEYDOWN:
00233 if e.key == 27:
00234 loopFlag=False
00235 if e.type == pygame.locals.MOUSEMOTION:
00236 pos = e.pos
00237 if e.type==pygame.locals.MOUSEBUTTONDOWN:
00238 if e.button == 1:
00239
00240 pos = e.pos
00241 diffs = np.array(blit_loc) - np.array(pos)
00242 ind = np.argmin( np.sum( np.power( diffs, 2.0 ), axis = 1 ) )
00243 loopFlag = False
00244
00245 clk.tick(fps)
00246 srf.fill((255,255,255))
00247 pygame.display.flip()
00248 clk.tick(fps)
00249 return ind
00250
00251
00252
00253
00254
00255
00256
00257
00258 if __name__ == '__main__':
00259 import optparse
00260
00261 p = optparse.OptionParser()
00262 p.add_option('-d', action='store_true', dest='graphical',
00263 help='Use a graphical display.', default=False)
00264 opt, args = p.parse_args()
00265
00266 gui = RFID_GUI( graphical = opt.graphical, db = db_rfid.db )
00267 rospy.spin()
00268
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280