ui.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 import roslib; roslib.load_manifest('hrl_lib'); import rospy
00004 # from hrl_lib.msg import String
00005 # from hrl_lib.msg import RFID_Interface
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 #     def publish_tag_ids(self, tagIDs):
00033 #         ''' tagIDs is a list of strings '''
00034 #         for tag in tagIDs:
00035 #             rt = String()
00036 #             rt.data = tag
00037 #             self.tag_pub.publish( rt )
00038 #             time.sleep(0.2)
00039 #         rt.data = '-1'
00040 #         self.tag_pub.publish( rt )
00041 #         time.sleep(0.002)
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 #     def selection_cb(self, pc_msg):
00052 #         self.selected_tag = pc_msg.data
00053 #         print 'Received tag selection from UI: ', self.selected_tag
00054 #         return self.selected_tag
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         #print 'Received selection from UI: ', self.selection
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 #     def receive_response(self):
00070 #         rt = String()
00071 #         rt = self.listener.read()
00072 #         return rt.data
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         #self.tag_pub = rospy.Publisher('/hrl/ele/UI_PC',String)
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         #rospy.Subscriber('/hrl/ele/UI_Robot', String, self.selection_cb, None, 1)
00088         rospy.Subscriber('/hrl/ele/UI_Robot', RFID_Interface, self.process_robot_request, None, 1)
00089 #         self.listener = ru.GenericListener('RFID_UI_Robot', String,
00090 #                                            '/hrl/ele/UI_Robot', 20)
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         # Get data out of msg to prevent overwriting!
00101         # From the robot, msg: tagIDs = ['id1','id2',...], humanread = [], actions = []
00102         tagIDs = msg.tagIDs
00103         humanread = []
00104         actions = []
00105 
00106         # Remote interface is responsible for populating other categories
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 #             else:
00117 #                 humanread.append( tag )
00118 #                 actions.append( ['fetch'] )
00119 
00120         # Get the selection (returned as RFID_Interface message)
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         # Publish the message
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             #pygame.display.set_mode(tag_images[i].get_size())
00199             #tag_surfaces.append(pygame.display.get_surface())
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             #pygame.display.set_mode(tag_images[i].get_size())
00214             #tag_surfaces.append(pygame.display.get_surface())
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             # Clear the screen
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             #print 'going'
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: # Esc
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                         # left button
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 # pc = ROS_UI_PC(init = False, graphical = True)
00293 # pc.get_selection_graphical(['LightSwitch1','LightSwitch1'],
00294 #                            ['lightswitch','LightSwitch1'],
00295 #                            [['on','off'],['on','off']])
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 #     while True:
00318 #         print 'Waiting for robot action(s)...\n'
00319 #         pc.publish_selected_id()
00320 #         pc.ids = []
00321 #         pc.ids_done = False
00322 
00323 
00324 # On the Robot's side:
00325 # ro = ROS_UI_Robot()
00326 # ro.publish_tag_ids(['one','two','hello'])
00327 # ro.receive_response()


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