00001
00002
00003 import roslib; roslib.load_manifest('hrl_lib'); import rospy
00004
00005
00006 import hrl_lib.util as ut
00007 import hrl_lib.rutils as ru
00008 import time
00009 import pygame
00010 import pygame.display
00011 import pygame.locals
00012 import pygame.transform
00013 import numpy as np, math
00014
00015
00016 class ROS_UI_Robot():
00017 def __init__(self, init=True):
00018 if init:
00019 try:
00020 print 'Initializing RFID UI on Robot'
00021 rospy.init_node('RFID_UI_Robot',anonymous=True)
00022 except rospy.ROSException:
00023 pass
00024 self.tag_pub = rospy.Publisher('/hrl/ele/UI_Robot',RFID_Interface)
00025 print 'RFID UI on Robot Running'
00026
00027 print 'Connecting to RFID UI on PC'
00028 self.selected_tag = ''
00029 self.selection = []
00030 rospy.Subscriber('/hrl/ele/UI_PC_Selection', RFID_Interface, self.selection_cb, None, 1)
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042 def publish_tag_ids(self, tagIDs):
00043 pub_msg = RFID_Interface()
00044 pub_msg.tagIDs = tagIDs
00045 pub_msg.humanread = []
00046 pub_msg.actions = []
00047
00048 self.tag_pub.publish( pub_msg )
00049 time.sleep(0.002)
00050
00051
00052
00053
00054
00055 def selection_cb(self, rfid_msg):
00056 tagID = rfid_msg.tagIDs[0]
00057 action = rfid_msg.actions[0]
00058 humanread = rfid_msg.humanread[0]
00059 self.selection = [tagID, humanread, action]
00060
00061
00062 def receive_response(self):
00063 while self.selection == []:
00064 time.sleep(0.02)
00065 rv = self.selection
00066 self.selection = []
00067 return rv
00068
00069
00070
00071
00072
00073
00074 class ROS_UI_PC():
00075 def __init__(self, init=True, graphical=False):
00076 if init:
00077 try:
00078 print 'Initializing RFID UI on PC'
00079 rospy.init_node('RFID_UI_PC',anonymous=True)
00080 except rospy.ROSException:
00081 pass
00082
00083 self.ui_selection = rospy.Publisher('/hrl/ele/UI_PC_Selection',RFID_Interface)
00084 print 'RFID UI on PC Running'
00085
00086 print 'Connecting to RFID UI on Robot'
00087
00088 rospy.Subscriber('/hrl/ele/UI_Robot', RFID_Interface, self.process_robot_request, None, 1)
00089
00090
00091 self.ids = []
00092 self.ids_done = False
00093 self.graphical = graphical
00094 self.images_db = '/home/travis/svn/robot1/src/projects/08_03_dog_commands/images_db/'
00095 self.pps_db = ut.load_pickle('/home/travis/svn/robot1/src/projects/08_03_dog_commands/ele_rfid.pickle')
00096
00097 def process_robot_request(self, rfid_interface_msg):
00098 msg = rfid_interface_msg
00099
00100
00101
00102 tagIDs = msg.tagIDs
00103 humanread = []
00104 actions = []
00105
00106
00107
00108
00109 ids_in_pps = []
00110 for i, tag in enumerate( tagIDs ):
00111 if self.pps_db.has_key( tag ):
00112 ids_in_pps.append( tag )
00113 humanread.append( self.pps_db[tag]['properties']['name'] )
00114 acts = self.pps_db[tag]['actions'].keys()
00115 actions.append( acts )
00116
00117
00118
00119
00120
00121 if self.graphical:
00122 selection = self.get_selection_graphical( ids_in_pps, humanread, actions )
00123 else:
00124 selection = self.get_selection_text( ids_in_pps, humanread, actions )
00125
00126
00127 self.ui_selection.publish( selection )
00128 print '\n\n Waiting for next request... \n\n'
00129
00130
00131 def get_selection_text( self, tagIDs, humanread, actions ):
00132 print '\n\nSelect a tag:'
00133 if len(tagIDs) == 1:
00134 print '\tOnly one option available: ', humanread[0]
00135 tag_ind = 0
00136 else:
00137 for i, tag in enumerate( tagIDs ):
00138 print '\t(%d) %s' % (i, humanread[i])
00139 tag_ind = int(raw_input())
00140
00141 print 'Select an action for that tag:'
00142 if len( actions[tag_ind] ) == 1:
00143 print '\tOnly one option available: ', actions[tag_ind][0]
00144 act_ind = 0
00145 else:
00146 for i, act in enumerate( actions[tag_ind] ):
00147 print '\t(%d) %s' % (i, actions[tag_ind][i])
00148 act_ind = int(raw_input())
00149
00150 retmsg = RFID_Interface()
00151 retmsg.tagIDs = [ tagIDs[tag_ind] ]
00152 retmsg.humanread = [ humanread[tag_ind] ]
00153 retmsg.actions = [ actions[tag_ind][act_ind] ]
00154 return retmsg
00155
00156 def smart_scale(self, image):
00157 ims = np.array(image.get_size(),dtype='float')
00158 scale = self.imheight / np.max(ims)
00159 return pygame.transform.scale(image, tuple(ims*scale))
00160
00161 def calc_blit_loc(self, image, center_pos):
00162 ims = np.array(image.get_size(), dtype='float')
00163 horiz = center_pos[0] - self.imheight/2 + (self.imheight - ims[0]) / 2.
00164 vert = center_pos[1] - self.imheight/2 + (self.imheight - ims[1]) / 2.
00165 return (horiz, vert)
00166
00167 def get_selection_graphical(self, tagIDs, humanread, actions):
00168 pygame.init()
00169 self.s_width = 600
00170 self.s_height = 700
00171 srf = pygame.display.set_mode((self.s_width,self.s_height))
00172 fps = 100
00173 loopFlag = True
00174 clk = pygame.time.Clock()
00175 obj = [srf, fps, clk]
00176 self.imheight = 175.
00177
00178 w = self.s_width * 1.0
00179 h = self.s_height * 1.0
00180 blit_pos = [[ w/3-w/6, h/3-h/6],
00181 [ w/3-w/6+w/3, h/3-h/6],
00182 [ w/3-w/6+2*w/3, h/3-h/6],
00183 [ w/3-w/6, h/3-h/6+h/3],
00184 [ w/3-w/6+w/3, h/3-h/6+h/3],
00185 [ w/3-w/6+2*w/3, h/3-h/6+h/3],
00186 [ w/3-w/6, h/3-h/6+2*h/3],
00187 [ w/3-w/6+w/3, h/3-h/6+2*h/3],
00188 [ w/3-w/6+2*w/3, h/3-h/6+2*h/3]]
00189 tag_images = []
00190 tag_surfaces = []
00191 blit_loc = []
00192 for i, tag in enumerate(tagIDs):
00193 print 'Loading image for tag ', tag
00194 tag_image = pygame.image.load(self.images_db + tag + '.jpg').convert()
00195 tag_image = self.smart_scale(tag_image)
00196 tag_images.append( tag_image )
00197 blit_loc.append( blit_pos[i] )
00198
00199
00200 srf.blit(tag_image, self.calc_blit_loc(tag_image,blit_loc[i]))
00201 tag_ind = self.get_selection( obj, tag_images, humanread, blit_loc )
00202 print 'Selected tag ', tag_ind, ': ', humanread[tag_ind]
00203
00204 act_images = []
00205 act_surfaces = []
00206 blit_loc = []
00207 for i, act in enumerate(actions[tag_ind]):
00208 print 'Loading image for act ', act
00209 act_image = pygame.image.load(self.images_db + tag + act + '.jpg').convert()
00210 act_image = self.smart_scale(act_image)
00211 act_images.append( act_image )
00212 blit_loc.append( blit_pos[i] )
00213
00214
00215 srf.blit(act_image, self.calc_blit_loc(tag_image,blit_loc[i]))
00216 act_ind = self.get_selection( obj, act_images, actions[tag_ind], blit_loc )
00217 print 'Selected action ', act_ind, ': ', actions[tag_ind][act_ind]
00218
00219 retmsg = RFID_Interface()
00220 retmsg.tagIDs = [ tagIDs[tag_ind] ]
00221 retmsg.humanread = [ humanread[tag_ind] ]
00222 retmsg.actions = [ actions[tag_ind][act_ind] ]
00223 return retmsg
00224
00225 def put_bottom_text( self, srf, text ):
00226 font = pygame.font.Font(None, 25)
00227 box = font.render(text, 1,(10, 10, 10, 0))
00228 ts = box.get_size()
00229 horiz = self.s_width / 2.0 - ts[0]/2.0
00230 vt = self.s_height - 50.0 - ts[1]/2.0
00231 srf.blit(box, (horiz, vt))
00232 return True
00233
00234 def draw_rect(self, srf, blit_loc):
00235 width = self.imheight * 1.10
00236 height = self.imheight *1.10
00237 horiz = blit_loc[0] - width / 2.0
00238 vert = blit_loc[1] - height / 2.0
00239 pygame.draw.rect(srf, (255, 0, 0), (horiz, vert, width, height))
00240 width = self.imheight * 1.01
00241 height = self.imheight *1.01
00242 horiz = blit_loc[0] - width / 2.0
00243 vert = blit_loc[1] - height / 2.0
00244 pygame.draw.rect(srf, (255, 255, 255), (horiz, vert, width, height))
00245 return True
00246
00247 def get_selection( self, obj, images, humanread, blit_loc ):
00248 [srf, fps, clk] = obj
00249 loopFlag = True
00250 ind = 0
00251 pos = (0,0)
00252 while loopFlag:
00253
00254 srf.fill((255,255,255))
00255
00256 diffs = np.array(blit_loc) - np.array(pos)
00257 ind = np.argmin( ut.norm( diffs.T ))
00258 self.put_bottom_text( srf, humanread[ind] )
00259
00260 self.draw_rect(srf, blit_loc[ind])
00261
00262 for i, image in enumerate(images):
00263 srf.blit(image, self.calc_blit_loc(image,blit_loc[i]))
00264
00265
00266 pygame.display.flip()
00267
00268 events = pygame.event.get()
00269 for e in events:
00270 if e.type==pygame.locals.QUIT:
00271 loopFlag=False
00272 if e.type==pygame.locals.KEYDOWN:
00273 if e.key == 27:
00274 loopFlag=False
00275 if e.type == pygame.locals.MOUSEMOTION:
00276 pos = e.pos
00277 if e.type==pygame.locals.MOUSEBUTTONDOWN:
00278 if e.button == 1:
00279
00280 pos = e.pos
00281 diffs = np.array(blit_loc) - np.array(pos)
00282 ind = np.argmin( ut.norm( diffs.T ))
00283 loopFlag = False
00284
00285 clk.tick(fps)
00286 srf.fill((255,255,255))
00287 pygame.display.flip()
00288 clk.tick(fps)
00289 return ind
00290
00291
00292
00293
00294
00295
00296
00297
00298 if __name__ == '__main__':
00299 import optparse
00300
00301 p = optparse.OptionParser()
00302 p.add_option('-d', action='store_true', dest='graphical',
00303 help='Use a graphical display.')
00304 p.add_option('-g', action='store_true', dest='client',
00305 help='Build Client?', default=False)
00306 opt, args = p.parse_args()
00307
00308 if opt.client:
00309 pc = ROS_UI_PC(graphical = opt.graphical)
00310 pc.get_selection_graphical( ['person '
00311 rospy.spin()
00312 else:
00313 ro = ROS_UI_Robot()
00314 ro.publish_tag_ids([ 'one', 'two', 'hello' ])
00315 ro.receive_response()
00316
00317
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327