def __init__(self): "Set up the Exit menu" Menu.__init__(self, "MoleFusion Exit Menu", "sprites/back1.jpg") self.parser = make_parser() self.curHandler = Localization() self.parser.setContentHandler(self.curHandler) self.parser.parse( open("languages/ExitMenu_" + Constants.LANGUAGE + ".xml")) self.title = GW_Label(self.curHandler.getText("title"), (0.5, 0.1), (27, 22, 24)) self.game_by = GW_Label(self.curHandler.getText("game"), (0.5, 0.3), (240, 255, 220)) self.music_by = GW_Label(self.curHandler.getText("music"), (0.5, 0.5), (240, 255, 220)) self.web = GW_Label(self.curHandler.getText("web"), (0.5, 0.7), (255, 255, 255)) self.widget_man = GuiWidgetManager( [self.title, self.game_by, self.music_by, self.web]) self.time_speed = pygame.time.Clock() self.exit = False self.on_enter()
def __init__(self, map, display): # environment inits self.landmarks = map["landmarks"] self.goals = map["goals"] self.currentGoal = 0 self.currentPos = map["initial_position"] self.currentAngle = map["initial_angle"] self.num_particles = map["num_particles"] self.dist_per_frame = map["distance_per_frame"] self.rot_per_frame = math.radians(map["rotation_per_frame"]) # noise stds self.move_std = map["noise"]["move_std"] self.rotate_std = map["noise"]["rotate_std"] # display init self.display = display self.cmd_msg = "Waiting for commands..." # localization self.localization = Localization(self.num_particles, map["dimensions"]["width"], map["dimensions"]["height"], map["noise"]) #estimated positions self.estimatedPos, self.estimatedAngle = self.localization.estimate_position( )
class ExitMenu(Menu): "Exit Menu" def __init__(self): "Set up the Exit menu" Menu.__init__(self, "MoleFusion Exit Menu", "sprites/back1.jpg") self.parser = make_parser() self.curHandler = Localization() self.parser.setContentHandler(self.curHandler) self.parser.parse( open("languages/ExitMenu_" + Constants.LANGUAGE + ".xml")) self.title = GW_Label(self.curHandler.getText("title"), (0.5, 0.1), (27, 22, 24)) self.game_by = GW_Label(self.curHandler.getText("game"), (0.5, 0.3), (240, 255, 220)) self.music_by = GW_Label(self.curHandler.getText("music"), (0.5, 0.5), (240, 255, 220)) self.web = GW_Label(self.curHandler.getText("web"), (0.5, 0.7), (255, 255, 255)) self.widget_man = GuiWidgetManager( [self.title, self.game_by, self.music_by, self.web]) self.time_speed = pygame.time.Clock() self.exit = False self.on_enter() def on_enter(self): pygame.event.clear() self.screen.blit(self.background, (0, 0)) pygame.display.flip() self.exit = False self.widget_man.set_draw(True) def on_exit(self): pygame.event.clear() f = FadeTransition(2000, Constants.FADECOLOR, "to") del f self.exit = True self.widget_man.set_draw(False) def run(self): while 1 and self.exit == False: self.time_speed.tick(Constants.FPS) for event in pygame.event.get(): if event.type == QUIT: self.on_exit() elif event.type == KEYDOWN or event.type == MOUSEBUTTONDOWN: self.on_exit() else: pygame.event.post( event ) #Reinject the event into the queue for maybe latter process self.widget_man.run()
class ExitMenu(Menu): "Exit Menu" def __init__(self): "Set up the Exit menu" Menu.__init__(self,"MoleFusion Exit Menu","sprites/back1.jpg") self.parser = make_parser() self.curHandler = Localization() self.parser.setContentHandler(self.curHandler) self.parser.parse(open("languages/ExitMenu_" + Constants.LANGUAGE + ".xml")) self.title = GW_Label(self.curHandler.getText("title"),(0.5,0.1),(27,22,24)) self.game_by = GW_Label(self.curHandler.getText("game"),(0.5,0.3),(240,255,220)) self.music_by = GW_Label(self.curHandler.getText("music"),(0.5,0.5),(240,255,220)) self.web = GW_Label(self.curHandler.getText("web"),(0.5,0.7),(255,255,255)) self.widget_man = GuiWidgetManager([self.title,self.game_by,self.music_by,self.web]) self.time_speed=pygame.time.Clock() self.exit=False self.on_enter() def on_enter(self): pygame.event.clear() self.screen.blit(self.background, (0, 0)) pygame.display.flip() self.exit=False self.widget_man.set_draw(True) def on_exit(self): pygame.event.clear() f = FadeTransition(2000,Constants.FADECOLOR,"to") del f self.exit=True self.widget_man.set_draw(False) def run(self): while 1 and self.exit==False: self.time_speed.tick(Constants.FPS) for event in pygame.event.get(): if event.type == QUIT: self.on_exit() elif event.type == KEYDOWN or event.type == MOUSEBUTTONDOWN: self.on_exit() else: pygame.event.post(event) #Reinject the event into the queue for maybe latter process self.widget_man.run()
def __init__(self): "Sets up the menu" Menu.__init__(self, "MoleFusion Main Menu", "sprites/back1.jpg") self.parser = make_parser() self.curHandler = Localization() self.parser.setContentHandler(self.curHandler) self.parser.parse(open("languages/MainMenu_" + Constants.LANGUAGE + ".xml")) self.title = GW_Label(self.curHandler.getText("title"), (0.5, 0.1), (27, 22, 24)) self.start = GW_Button(self.curHandler.getText("start"), (0.5, 0.3), (255, 255, 255)) self.start.add_eventhandler("onmouseclick", self.start_onmouseclick) self.options = GW_Button(self.curHandler.getText("options"), (0.5, 0.45), (255, 255, 255)) self.options.add_eventhandler("onmouseclick", self.options_onmouseclick) self.highscores = GW_Button(self.curHandler.getText("highscores"), (0.5, 0.6), (255, 255, 255)) self.highscores.add_eventhandler("onmouseclick", self.highscores_onmouseclick) self.help = GW_Button(self.curHandler.getText("help"), (0.5, 0.75), (255, 255, 255)) self.help.add_eventhandler("onmouseclick", self.help_onmouseclick) self.quit = GW_Button(self.curHandler.getText("quit"), (0.5, 0.9), (255, 255, 255)) self.quit.add_eventhandler("onmouseclick", self.quit_onmouseclick) self.widget_man = GuiWidgetManager( [self.title, self.start, self.options, self.highscores, self.help, self.quit] ) self.time_speed = pygame.time.Clock() self.on_enter()
def __init__(self,background): "Set up the HighScores menu" Menu.__init__(self,"MoleFusion HighScore Menu",background) self.parser = make_parser() self.curHandler = Localization() self.parser.setContentHandler(self.curHandler) self.parser.parse(open("languages/HighScoresMenu_" + Constants.LANGUAGE + ".xml")) self.title = GW_Label(self.curHandler.getText("title"),(0.5,0.1),(27,22,24)) self.name_column = GW_Label(self.curHandler.getText("name"),(0.25,0.25),(212,224,130)) self.points_column = GW_Label(self.curHandler.getText("points"),(0.75,0.25),(212,224,130)) self.returnMain = GW_Button(self.curHandler.getText("returnMain"),(0.5,0.9),(255,255,255)) self.returnMain.add_eventhandler("onmouseclick",self.returnMain_onmouseclick) h = HighScores() highscorelist = h.get_HighScores() self.widgetlist = [self.title,self.name_column,self.points_column,self.returnMain] for val,i in enumerate(highscorelist[0:5]): self.widgetlist.append(GW_Label(i.get_name(),(0.25,0.35+val/10.0),(250,254,210))) self.widgetlist.append(GW_Label(str(i.get_points()),(0.75,0.35+val/10.0),(250,254,210))) self.widget_man = GuiWidgetManager(self.widgetlist) self.time_speed=pygame.time.Clock() self.exit=False self.on_enter()
def __init__(self): "Sets up the options menu" Menu.__init__(self, "MoleFusion Options Menu", "sprites/back1.jpg") self.parser = make_parser() self.curHandler = Localization() self.parser.setContentHandler(self.curHandler) self.parser.parse(open("languages/OptionsMenu_" + Constants.LANGUAGE + ".xml")) self.title = GW_Label(self.curHandler.getText("title"), (0.5, 0.1), (27, 22, 24)) self.name = GW_Label(self.curHandler.getText("name"), (0.5, 0.2), (255, 255, 255)) self.inputname = GW_TextInput((0.5, 0.3), 24, 0.4, Constants.PLAYERNAME) self.language = GW_Button(self.curHandler.getText("language"), (0.5, 0.5), (255, 255, 255)) self.language.add_eventhandler("onmouseclick", self.language_onmouseclick) self.returnMain = GW_Button(self.curHandler.getText("returnMain"), (0.5, 0.8), (255, 255, 255)) self.returnMain.add_eventhandler("onmouseclick", self.returnMain_onmouseclick) self.widget_man = GuiWidgetManager([self.title, self.name, self.inputname, self.language, self.returnMain]) self.time_speed = pygame.time.Clock() self.exit = False self.on_enter()
def __init__(self): "Sets up the menu" Menu.__init__(self, "MoleFusion Main Menu", "sprites/back1.jpg") self.parser = make_parser() self.curHandler = Localization() self.parser.setContentHandler(self.curHandler) self.parser.parse( open("languages/MainMenu_" + Constants.LANGUAGE + ".xml")) self.title = GW_Label(self.curHandler.getText("title"), (0.5, 0.1), (27, 22, 24)) self.start = GW_Button(self.curHandler.getText("start"), (0.5, 0.3), (255, 255, 255)) self.start.add_eventhandler("onmouseclick", self.start_onmouseclick) self.options = GW_Button(self.curHandler.getText("options"), (0.5, 0.45), (255, 255, 255)) self.options.add_eventhandler("onmouseclick", self.options_onmouseclick) self.highscores = GW_Button(self.curHandler.getText("highscores"), (0.5, 0.6), (255, 255, 255)) self.highscores.add_eventhandler("onmouseclick", self.highscores_onmouseclick) self.help = GW_Button(self.curHandler.getText("help"), (0.5, 0.75), (255, 255, 255)) self.help.add_eventhandler("onmouseclick", self.help_onmouseclick) self.quit = GW_Button(self.curHandler.getText("quit"), (0.5, 0.9), (255, 255, 255)) self.quit.add_eventhandler("onmouseclick", self.quit_onmouseclick) self.widget_man = GuiWidgetManager([ self.title, self.start, self.options, self.highscores, self.help, self.quit ]) self.time_speed = pygame.time.Clock() self.on_enter()
def __init__(self, background): "Set up the HighScores menu" Menu.__init__(self, "MoleFusion HighScore Menu", background) self.parser = make_parser() self.curHandler = Localization() self.parser.setContentHandler(self.curHandler) self.parser.parse( open("languages/HighScoresMenu_" + Constants.LANGUAGE + ".xml")) self.title = GW_Label(self.curHandler.getText("title"), (0.5, 0.1), (27, 22, 24)) self.name_column = GW_Label(self.curHandler.getText("name"), (0.25, 0.25), (212, 224, 130)) self.points_column = GW_Label(self.curHandler.getText("points"), (0.75, 0.25), (212, 224, 130)) self.returnMain = GW_Button(self.curHandler.getText("returnMain"), (0.5, 0.9), (255, 255, 255)) self.returnMain.add_eventhandler("onmouseclick", self.returnMain_onmouseclick) h = HighScores() highscorelist = h.get_HighScores() self.widgetlist = [ self.title, self.name_column, self.points_column, self.returnMain ] for val, i in enumerate(highscorelist[0:5]): self.widgetlist.append( GW_Label(i.get_name(), (0.25, 0.35 + val / 10.0), (250, 254, 210))) self.widgetlist.append( GW_Label(str(i.get_points()), (0.75, 0.35 + val / 10.0), (250, 254, 210))) self.widget_man = GuiWidgetManager(self.widgetlist) self.time_speed = pygame.time.Clock() self.exit = False self.on_enter()
class LeddarPositionInterface: def __init__(self, leddar1_com_port, leddar1_address, leddar2_com_port, leddar2_address, top_post_position, bottom_post_position): self.leddar1 = LeddarInterface(leddar1_com_port, leddar1_address) self.leddar2 = LeddarInterface(leddar2_com_port, leddar2_address) self.localizer = Localization(top_post_position, bottom_post_position) def get_position(self): dist1 = self.leddar1.get_distance() dist2 = self.leddar2.get_distance() position = self.localizer.find_position(dist1, dist2) return position;
def __init__(self): # GLOBAL VARIABLES self.P = np.eye(4) # Pose self.k = np.array( [[640.0, 0, 640.0], [0, 480.0, 480.0], [0, 0, 1.0]], dtype=np.float32, ) # ahmed dataset # self.k = np.array( # [[827.0, 0.0, 638.0], [0.0, 826.0, 347], [0.0000, 0.0000, 1.0000]], # dtype=np.float32, # ) self.poses = [[0.0, 0.0, np.pi / 2]] # initial pose self.trajectory = [np.array([0, 0, 0])] # 3d trajectory self.tMats = [(np.zeros((3, 3)), np.zeros( (3, 1)))] # Transformation Matrices (rmat, tvec) self.env_map = Map(62, 62) self._create_map(self.env_map) self.particle_filter = Localization(self.env_map)
def __init__(self): "Sets up the Help menu" Menu.__init__(self,"MoleFusion Help Menu","sprites/back1.jpg") self.parser = make_parser() self.curHandler = Localization() self.parser.setContentHandler(self.curHandler) self.parser.parse(open("languages/HelpMenu_" + Constants.LANGUAGE + ".xml")) self.title = GW_Label(self.curHandler.getText("title"),(0.5,0.1),(27,22,24)) self.help = GW_Image(self.curHandler.getText("help"),(0.5,0.3)) self.returnMain = GW_Button(self.curHandler.getText("returnMain"),(0.5,0.5),(255,255,255)) self.returnMain.add_eventhandler("onmouseclick",self.returnMain_onmouseclick) self.widget_man = GuiWidgetManager([self.title,self.help,self.returnMain]) self.time_speed=pygame.time.Clock() self.exit=False self.on_enter()
def __init__(self,gamecontainer,time): "Initialises resources " pygame.sprite.Sprite.__init__(self) self.parser = make_parser() self.curHandler = Localization() self.parser.setContentHandler(self.curHandler) self.parser.parse(open("languages/MessageBox_" + Constants.LANGUAGE + ".xml")) self.gamecontainer=gamecontainer self.time_start=self.gamecontainer.time self.time=0.0 self.endtime = time self.dirty = 1 self.screen_size=Constants.SCREEN.get_size() self.image=pygame.image.load("sprites/back_letters.png") self.image.convert_alpha() self.rect = self.image.get_rect() self.rect.centerx = 0.5*self.screen_size[0] self.rect.centery = 0.5*self.screen_size[1] self.state = "grow" self.scale = 0.0 self.font = pygame.font.Font(os.path.join("fonts","verdana.ttf"), 24) if self.gamecontainer.start == True: self.fontimg = self.font.render( self.curHandler.getText("level") + " " + str(self.gamecontainer.level), True , (230,240,255,255) ) rect = self.fontimg.get_rect() self.image.blit(self.fontimg,(self.rect.width/2 - rect.width/2,100)) elif self.gamecontainer.completed == True: self.fontimg = self.font.render( self.curHandler.getText("finish"), True , (230,240,255,255) ) rect = self.fontimg.get_rect() self.image.blit(self.fontimg,(self.rect.width/2 - rect.width/2,80)) self.fontimg = self.font.render( self.curHandler.getText("points"), True , (230,240,255,255) ) rect = self.fontimg.get_rect() self.image.blit(self.fontimg,(self.rect.width/2 - rect.width/2,110)) self.fontimg = self.font.render( str(self.gamecontainer.timeclock.get_time()*15), True , (255,255,235,255) ) rect = self.fontimg.get_rect() self.image.blit(self.fontimg,(self.rect.width/2 - rect.width/2,140)) self.gamecontainer.score.change(self.gamecontainer.timeclock.get_time()*15) elif self.gamecontainer.gameover == True: self.fontimg = self.font.render( self.curHandler.getText("over"), True , (230,240,255,255) ) rect = self.fontimg.get_rect() self.image.blit(self.fontimg,(self.rect.width/2 - rect.width/2,100)) self.original=self.image.copy()
def localization_transformations(self): localization_file = open(self.path_file_localization.encode('utf-8'), 'rU') read = list(csv.reader(localization_file, delimiter=";")) for raw in read[1:]: localization = Localization() localization.room = raw[0] localization.name = raw[1] localization.placement = raw[2] localization.beacon = raw[3] self.localizations.append(localization)
def __init__(self): "Set up the Exit menu" Menu.__init__(self,"MoleFusion Exit Menu","sprites/back1.jpg") self.parser = make_parser() self.curHandler = Localization() self.parser.setContentHandler(self.curHandler) self.parser.parse(open("languages/ExitMenu_" + Constants.LANGUAGE + ".xml")) self.title = GW_Label(self.curHandler.getText("title"),(0.5,0.1),(27,22,24)) self.game_by = GW_Label(self.curHandler.getText("game"),(0.5,0.3),(240,255,220)) self.music_by = GW_Label(self.curHandler.getText("music"),(0.5,0.5),(240,255,220)) self.web = GW_Label(self.curHandler.getText("web"),(0.5,0.7),(255,255,255)) self.widget_man = GuiWidgetManager([self.title,self.game_by,self.music_by,self.web]) self.time_speed=pygame.time.Clock() self.exit=False self.on_enter()
class MessageBox(pygame.sprite.Sprite): "MessageBox class" def __init__(self, gamecontainer, time): "Initialises resources " pygame.sprite.Sprite.__init__(self) self.parser = make_parser() self.curHandler = Localization() self.parser.setContentHandler(self.curHandler) self.parser.parse( open("languages/MessageBox_" + Constants.LANGUAGE + ".xml")) self.gamecontainer = gamecontainer self.time_start = self.gamecontainer.time self.time = 0.0 self.endtime = time self.dirty = 1 self.screen_size = Constants.SCREEN.get_size() self.image = pygame.image.load("sprites/back_letters.png") self.image.convert_alpha() self.rect = self.image.get_rect() self.rect.centerx = 0.5 * self.screen_size[0] self.rect.centery = 0.5 * self.screen_size[1] self.state = "grow" self.scale = 0.0 self.font = pygame.font.Font(os.path.join("fonts", "verdana.ttf"), 24) if self.gamecontainer.start == True: self.fontimg = self.font.render( self.curHandler.getText("level") + " " + str(self.gamecontainer.level), True, (230, 240, 255, 255)) rect = self.fontimg.get_rect() self.image.blit(self.fontimg, (self.rect.width / 2 - rect.width / 2, 100)) elif self.gamecontainer.completed == True: self.fontimg = self.font.render(self.curHandler.getText("finish"), True, (230, 240, 255, 255)) rect = self.fontimg.get_rect() self.image.blit(self.fontimg, (self.rect.width / 2 - rect.width / 2, 80)) self.fontimg = self.font.render(self.curHandler.getText("points"), True, (230, 240, 255, 255)) rect = self.fontimg.get_rect() self.image.blit(self.fontimg, (self.rect.width / 2 - rect.width / 2, 110)) self.fontimg = self.font.render( str(self.gamecontainer.timeclock.get_time() * 15), True, (255, 255, 235, 255)) rect = self.fontimg.get_rect() self.image.blit(self.fontimg, (self.rect.width / 2 - rect.width / 2, 140)) self.gamecontainer.score.change( self.gamecontainer.timeclock.get_time() * 15) elif self.gamecontainer.gameover == True: self.fontimg = self.font.render(self.curHandler.getText("over"), True, (230, 240, 255, 255)) rect = self.fontimg.get_rect() self.image.blit(self.fontimg, (self.rect.width / 2 - rect.width / 2, 100)) self.original = self.image.copy() def update(self): self.time = self.gamecontainer.time - self.time_start if self.time > self.endtime: if self.gamecontainer.start == True: self.gamecontainer.pause = False else: self.gamecontainer.quit = True elif self.time > self.endtime * 0.1 and self.time < self.endtime * 0.9: self.state = "stop" self.dirty = 0 elif self.time > self.endtime * 0.9: self.state = "shrink" self.dirty = 1 self.scale = cosine_Interpolate( (1.0, 0.0), (self.time - (self.endtime * 0.9)) / (self.endtime * 0.1)) else: self.scale = cosine_Interpolate((0.0, 1.0), self.time / (self.endtime * 0.1)) if not self.dirty: return self.image = pygame.transform.rotozoom(self.original, 0.0, self.scale) self.rect = self.image.get_rect(center=(self.rect.centerx, self.rect.centery)) self.dirty = 1
class HelpMenu(Menu): "Help Menu" def __init__(self): "Sets up the Help menu" Menu.__init__(self,"MoleFusion Help Menu","sprites/back1.jpg") self.parser = make_parser() self.curHandler = Localization() self.parser.setContentHandler(self.curHandler) self.parser.parse(open("languages/HelpMenu_" + Constants.LANGUAGE + ".xml")) self.title = GW_Label(self.curHandler.getText("title"),(0.5,0.1),(27,22,24)) self.help = GW_Image(self.curHandler.getText("help"),(0.5,0.3)) self.returnMain = GW_Button(self.curHandler.getText("returnMain"),(0.5,0.5),(255,255,255)) self.returnMain.add_eventhandler("onmouseclick",self.returnMain_onmouseclick) self.widget_man = GuiWidgetManager([self.title,self.help,self.returnMain]) self.time_speed=pygame.time.Clock() self.exit=False self.on_enter() def on_enter(self): pygame.event.clear() self.screen.blit(self.background, (0, 0)) pygame.display.flip() self.exit=False self.widget_man.set_draw(True) def on_exit(self): f = FadeTransition(500,Constants.FADECOLOR,"to") del f self.exit=True self.widget_man.set_draw(False) pygame.event.clear() def returnMain_onmouseclick(self,event): self.returnMain.onmouseclick(event) self.on_exit() def reload_titles(self): self.parser.parse(open("languages/OptionsMenu_" + Constants.LANGUAGE + ".xml")) self.title.set_title(self.curHandler.getText("title")) self.help.set_title(self.curHandler.getText("help")) self.returnMain.set_title(self.curHandler.getText("returnMain")) def run(self): while 1 and self.exit==False: self.time_speed.tick(Constants.FPS) for event in pygame.event.get(): if event.type == QUIT: os._exit(0) elif event.type == KEYDOWN: if (not self.widget_man.has_input_focus()): if(event.key == K_ESCAPE): return #else: # pygame.event.post(event) #Reinject the event into the queue for maybe latter process else: pygame.event.post(event) #Reinject the event into the queue for maybe latter process self.widget_man.run()
def __init__(self, leddar1_com_port, leddar1_address, leddar2_com_port, leddar2_address, top_post_position, bottom_post_position): self.leddar1 = LeddarInterface(leddar1_com_port, leddar1_address) self.leddar2 = LeddarInterface(leddar2_com_port, leddar2_address) self.localizer = Localization(top_post_position, bottom_post_position)
def __init__(self, gamecontainer, time): "Initialises resources " pygame.sprite.Sprite.__init__(self) self.parser = make_parser() self.curHandler = Localization() self.parser.setContentHandler(self.curHandler) self.parser.parse( open("languages/MessageBox_" + Constants.LANGUAGE + ".xml")) self.gamecontainer = gamecontainer self.time_start = self.gamecontainer.time self.time = 0.0 self.endtime = time self.dirty = 1 self.screen_size = Constants.SCREEN.get_size() self.image = pygame.image.load("sprites/back_letters.png") self.image.convert_alpha() self.rect = self.image.get_rect() self.rect.centerx = 0.5 * self.screen_size[0] self.rect.centery = 0.5 * self.screen_size[1] self.state = "grow" self.scale = 0.0 self.font = pygame.font.Font(os.path.join("fonts", "verdana.ttf"), 24) if self.gamecontainer.start == True: self.fontimg = self.font.render( self.curHandler.getText("level") + " " + str(self.gamecontainer.level), True, (230, 240, 255, 255)) rect = self.fontimg.get_rect() self.image.blit(self.fontimg, (self.rect.width / 2 - rect.width / 2, 100)) elif self.gamecontainer.completed == True: self.fontimg = self.font.render(self.curHandler.getText("finish"), True, (230, 240, 255, 255)) rect = self.fontimg.get_rect() self.image.blit(self.fontimg, (self.rect.width / 2 - rect.width / 2, 80)) self.fontimg = self.font.render(self.curHandler.getText("points"), True, (230, 240, 255, 255)) rect = self.fontimg.get_rect() self.image.blit(self.fontimg, (self.rect.width / 2 - rect.width / 2, 110)) self.fontimg = self.font.render( str(self.gamecontainer.timeclock.get_time() * 15), True, (255, 255, 235, 255)) rect = self.fontimg.get_rect() self.image.blit(self.fontimg, (self.rect.width / 2 - rect.width / 2, 140)) self.gamecontainer.score.change( self.gamecontainer.timeclock.get_time() * 15) elif self.gamecontainer.gameover == True: self.fontimg = self.font.render(self.curHandler.getText("over"), True, (230, 240, 255, 255)) rect = self.fontimg.get_rect() self.image.blit(self.fontimg, (self.rect.width / 2 - rect.width / 2, 100)) self.original = self.image.copy()
class NetworkServer(InterfaceCallbacks, NetworkCallbacks): def __init__(self): self._trustedNetwork = NetworkBridge() self._untrustedNetwork = NetworkBridge() self._localization = Localization() self._interfaceScanner = InterfaceScanner(self) def initialize(self): if not self._trustedNetwork.initialize() or \ not self._untrustedNetwork.initialize() or \ not self._localization.initialize(): return False return True def deinitialize(self): self._localization.deinitialize() self._untrustedNetwork.deinitialize() self._trustedNetwork.deinitialize() def scanInterfaces(self): self._interfaceScanner.runScan() def interfaceAdded(self, iface): # Handle WiFi interfaces if iface.type() == InterfaceType.WIFI: rospy.loginfo('Registering wireless [%s]' % iface.name()) self._localization.setInterface(iface) # Handle bridge interfaces elif iface.type() == InterfaceType.BRIDGE: rospy.loginfo('Registering bridge [%s]' % iface.name()) if iface.isTrusted(): self._trustedNetwork.addInterface(iface) else: self._untrustedNetwork.addInterface(iface) # Handle tap interfaces elif iface.type() == InterfaceType.TAP: rospy.loginfo('Registering tap [%s]' % iface.name()) self._trustedNetwork.addInterface(iface) # Handle ethernet interfaces elif iface.type() == InterfaceType.ETHERNET: gateway = iface.getGateway() if not gateway: # Interface might not have IP address yet, try to get one now if DHCP.getLease(iface.name()): gateway = iface.getGateway() if gateway: rospy.loginfo('Skipping interface [%s] with gateway %s' % (iface.name(), gateway)) else: rospy.loginfo('Registering ethernet [%s]' % iface.name()) self._untrustedNetwork.addInterface(iface) def interfaceRemoved(self, iface): rospy.loginfo('Unregistering [%s]' % iface.name()) # Handle WiFi interfaces if iface.type() == InterfaceType.WIFI: self._localization.unsetInterface(iface) # Handle bridge interfaces elif iface.type() == InterfaceType.BRIDGE: if iface.isTrusted(): self._trustedNetwork.removeInterface(iface) else: self._untrustedNetwork.removeInterface(iface) # Handle tap interfaces elif iface.type() == InterfaceType.TAP: self._trustedNetwork.removeInterface(iface) # Handle ethernet interfaces elif iface.type() == InterfaceType.ETHERNET: self._untrustedNetwork.removeInterface(iface) def enableAccessPoint(self): params = {'name': 'Deliverator', 'channel': 11} # TODO rospy.loginfo('Enabling wireless AP "%s" (channel %d)' % (params['name'], params['channel'])) if self._localization.enableAccessPointMode(params): iface = self._localization.getInterface() iface.enableAccessPoint(self) self._trustedNetwork.addInterface(iface) def disableAccessPoint(self): rospy.loginfo('Disabling wireless AP') iface = self._localization.getInterface() self._trustedNetwork.removeInterface(iface) iface.disableAccessPoint() self._localization.disableAccesPointMode() def wifiClientConnected(self, client): self._localization.enableTargetMode(client) def wifiClientDisconnected(self, client): self._localization.disableTargetMode(client) def wifiConnect(self, params): rospy.loginfo('Connecting to WiFi network "%s" (channel %d)' % (params['name'], params['channel'])) if self._localization.targetNetwork(params): self._untrustedNetwork.addInterface( self._localization.getInterface()) def wifiDisconnect(self): rospy.loginfo('Disconnecting from WiFi network') self._untrustedNetwork.removeInterface( self._localization.getInterface()) self._localization.untargetNetwork()
class HighScoresMenu(Menu): "HighScores Menu" def __init__(self, background): "Set up the HighScores menu" Menu.__init__(self, "MoleFusion HighScore Menu", background) self.parser = make_parser() self.curHandler = Localization() self.parser.setContentHandler(self.curHandler) self.parser.parse( open("languages/HighScoresMenu_" + Constants.LANGUAGE + ".xml")) self.title = GW_Label(self.curHandler.getText("title"), (0.5, 0.1), (27, 22, 24)) self.name_column = GW_Label(self.curHandler.getText("name"), (0.25, 0.25), (212, 224, 130)) self.points_column = GW_Label(self.curHandler.getText("points"), (0.75, 0.25), (212, 224, 130)) self.returnMain = GW_Button(self.curHandler.getText("returnMain"), (0.5, 0.9), (255, 255, 255)) self.returnMain.add_eventhandler("onmouseclick", self.returnMain_onmouseclick) h = HighScores() highscorelist = h.get_HighScores() self.widgetlist = [ self.title, self.name_column, self.points_column, self.returnMain ] for val, i in enumerate(highscorelist[0:5]): self.widgetlist.append( GW_Label(i.get_name(), (0.25, 0.35 + val / 10.0), (250, 254, 210))) self.widgetlist.append( GW_Label(str(i.get_points()), (0.75, 0.35 + val / 10.0), (250, 254, 210))) self.widget_man = GuiWidgetManager(self.widgetlist) self.time_speed = pygame.time.Clock() self.exit = False self.on_enter() def on_enter(self): pygame.event.clear() self.screen.blit(self.background, (0, 0)) pygame.display.flip() self.exit = False self.widget_man.set_draw(True) def on_exit(self): pygame.event.clear() f = FadeTransition(500, Constants.FADECOLOR, "to") del f self.exit = True self.widget_man.set_draw(False) def returnMain_onmouseclick(self, event): self.returnMain.onmouseclick(event) self.on_exit() def run(self): while 1 and self.exit == False: self.time_speed.tick(Constants.FPS) for event in pygame.event.get(): if event.type == QUIT: os._exit(0) elif event.type == KEYDOWN: if (not self.widget_man.has_input_focus()): if (event.key == K_ESCAPE): return #else: # pygame.event.post(event) #Reinject the event into the queue for maybe latter process else: pygame.event.post( event ) #Reinject the event into the queue for maybe latter process self.widget_man.run()
class MainMenu(Menu): "Main Menu" def __init__(self): "Sets up the menu" Menu.__init__(self, "MoleFusion Main Menu", "sprites/back1.jpg") self.parser = make_parser() self.curHandler = Localization() self.parser.setContentHandler(self.curHandler) self.parser.parse( open("languages/MainMenu_" + Constants.LANGUAGE + ".xml")) self.title = GW_Label(self.curHandler.getText("title"), (0.5, 0.1), (27, 22, 24)) self.start = GW_Button(self.curHandler.getText("start"), (0.5, 0.3), (255, 255, 255)) self.start.add_eventhandler("onmouseclick", self.start_onmouseclick) self.options = GW_Button(self.curHandler.getText("options"), (0.5, 0.45), (255, 255, 255)) self.options.add_eventhandler("onmouseclick", self.options_onmouseclick) self.highscores = GW_Button(self.curHandler.getText("highscores"), (0.5, 0.6), (255, 255, 255)) self.highscores.add_eventhandler("onmouseclick", self.highscores_onmouseclick) self.help = GW_Button(self.curHandler.getText("help"), (0.5, 0.75), (255, 255, 255)) self.help.add_eventhandler("onmouseclick", self.help_onmouseclick) self.quit = GW_Button(self.curHandler.getText("quit"), (0.5, 0.9), (255, 255, 255)) self.quit.add_eventhandler("onmouseclick", self.quit_onmouseclick) self.widget_man = GuiWidgetManager([ self.title, self.start, self.options, self.highscores, self.help, self.quit ]) self.time_speed = pygame.time.Clock() self.on_enter() def on_enter(self): pygame.event.clear() self.screen.blit(self.background, (0, 0)) pygame.display.flip() self.widget_man.set_draw(True) def on_exit(self): pygame.event.clear() f = FadeTransition(500, Constants.FADECOLOR, "to") del f self.widget_man.set_draw(False) def start_onmouseclick(self, event): self.start.onmouseclick(event) self.on_exit() game = Game() game.run() self.start.onmouseclickup(event) self.on_enter() def options_onmouseclick(self, event): self.options.onmouseclick(event) self.on_exit() options = OptionsMenu() options.run() self.reload_titles() self.options.onmouseclickup(event) self.on_enter() def highscores_onmouseclick(self, event): self.highscores.onmouseclick(event) self.on_exit() highscores = HighScoresMenu("sprites/back1.jpg") highscores.run() self.reload_titles() self.highscores.onmouseclickup(event) self.on_enter() def help_onmouseclick(self, event): self.help.onmouseclick(event) self.on_exit() help = HelpMenu() help.run() self.help.onmouseclickup(event) self.reload_titles() self.on_enter() def quit_onmouseclick(self, event): self.quit.onmouseclick(event) self.on_exit() exit = ExitMenu() exit.run() pygame.event.post(pygame.event.Event(QUIT, {})) def reload_titles(self): self.parser.parse( open("languages/MainMenu_" + Constants.LANGUAGE + ".xml")) self.title.set_title(self.curHandler.getText("title")) self.start.set_title(self.curHandler.getText("start")) self.options.set_title(self.curHandler.getText("options")) self.highscores.set_title(self.curHandler.getText("highscores")) self.help.set_title(self.curHandler.getText("help")) self.quit.set_title(self.curHandler.getText("quit")) def run(self): while 1: self.time_speed.tick(Constants.FPS) for event in pygame.event.get(): if event.type == QUIT: os._exit(0) elif event.type == KEYDOWN and event.key == K_ESCAPE: self.quit_onmouseclick(event) else: pygame.event.post( event ) #Reinject the event into the queue for maybe latter process self.widget_man.run()
class OptionsMenu(Menu): "Options Menu" def __init__(self): "Sets up the options menu" Menu.__init__(self, "MoleFusion Options Menu", "sprites/back1.jpg") self.parser = make_parser() self.curHandler = Localization() self.parser.setContentHandler(self.curHandler) self.parser.parse(open("languages/OptionsMenu_" + Constants.LANGUAGE + ".xml")) self.title = GW_Label(self.curHandler.getText("title"), (0.5, 0.1), (27, 22, 24)) self.name = GW_Label(self.curHandler.getText("name"), (0.5, 0.2), (255, 255, 255)) self.inputname = GW_TextInput((0.5, 0.3), 24, 0.4, Constants.PLAYERNAME) self.language = GW_Button(self.curHandler.getText("language"), (0.5, 0.5), (255, 255, 255)) self.language.add_eventhandler("onmouseclick", self.language_onmouseclick) self.returnMain = GW_Button(self.curHandler.getText("returnMain"), (0.5, 0.8), (255, 255, 255)) self.returnMain.add_eventhandler("onmouseclick", self.returnMain_onmouseclick) self.widget_man = GuiWidgetManager([self.title, self.name, self.inputname, self.language, self.returnMain]) self.time_speed = pygame.time.Clock() self.exit = False self.on_enter() def on_enter(self): pygame.event.clear() self.screen.blit(self.background, (0, 0)) pygame.display.flip() self.exit = False self.widget_man.set_draw(True) def on_exit(self): f = FadeTransition(500, Constants.FADECOLOR, "to") del f Constants.PLAYTIME = pygame.mixer.music.get_pos() self.exit = True self.widget_man.set_draw(False) Constants.PLAYERNAME = self.inputname.get_Text() pygame.event.clear() def language_onmouseclick(self, event): self.language.onmouseclick(event) self.on_exit() l = LanguageMenu() l.run() self.language.onmouseclickup(event) self.reload_titles() self.on_enter() pygame.mixer.music.play() def returnMain_onmouseclick(self, event): self.returnMain.onmouseclick(event) self.on_exit() def reload_titles(self): self.parser.parse(open("languages/OptionsMenu_" + Constants.LANGUAGE + ".xml")) self.name.set_title(self.curHandler.getText("name")) self.title.set_title(self.curHandler.getText("title")) self.language.set_title(self.curHandler.getText("language")) self.returnMain.set_title(self.curHandler.getText("returnMain")) def run(self): while 1 and self.exit == False: self.time_speed.tick(Constants.FPS) for event in pygame.event.get(): if event.type == QUIT: os._exit(0) elif event.type == KEYDOWN: if not self.widget_man.has_input_focus(): if event.key == K_ESCAPE: return else: pygame.event.post(event) # Reinject the event into the queue for maybe latter process else: pygame.event.post(event) # Reinject the event into the queue for maybe latter process self.widget_man.run()
class MainMenu(Menu): "Main Menu" def __init__(self): "Sets up the menu" Menu.__init__(self, "MoleFusion Main Menu", "sprites/back1.jpg") self.parser = make_parser() self.curHandler = Localization() self.parser.setContentHandler(self.curHandler) self.parser.parse(open("languages/MainMenu_" + Constants.LANGUAGE + ".xml")) self.title = GW_Label(self.curHandler.getText("title"), (0.5, 0.1), (27, 22, 24)) self.start = GW_Button(self.curHandler.getText("start"), (0.5, 0.3), (255, 255, 255)) self.start.add_eventhandler("onmouseclick", self.start_onmouseclick) self.options = GW_Button(self.curHandler.getText("options"), (0.5, 0.45), (255, 255, 255)) self.options.add_eventhandler("onmouseclick", self.options_onmouseclick) self.highscores = GW_Button(self.curHandler.getText("highscores"), (0.5, 0.6), (255, 255, 255)) self.highscores.add_eventhandler("onmouseclick", self.highscores_onmouseclick) self.help = GW_Button(self.curHandler.getText("help"), (0.5, 0.75), (255, 255, 255)) self.help.add_eventhandler("onmouseclick", self.help_onmouseclick) self.quit = GW_Button(self.curHandler.getText("quit"), (0.5, 0.9), (255, 255, 255)) self.quit.add_eventhandler("onmouseclick", self.quit_onmouseclick) self.widget_man = GuiWidgetManager( [self.title, self.start, self.options, self.highscores, self.help, self.quit] ) self.time_speed = pygame.time.Clock() self.on_enter() def on_enter(self): pygame.event.clear() self.screen.blit(self.background, (0, 0)) pygame.display.flip() self.widget_man.set_draw(True) def on_exit(self): pygame.event.clear() f = FadeTransition(500, Constants.FADECOLOR, "to") del f self.widget_man.set_draw(False) def start_onmouseclick(self, event): self.start.onmouseclick(event) self.on_exit() game = Game() game.run() self.start.onmouseclickup(event) self.on_enter() def options_onmouseclick(self, event): self.options.onmouseclick(event) self.on_exit() options = OptionsMenu() options.run() self.reload_titles() self.options.onmouseclickup(event) self.on_enter() def highscores_onmouseclick(self, event): self.highscores.onmouseclick(event) self.on_exit() highscores = HighScoresMenu("sprites/back1.jpg") highscores.run() self.reload_titles() self.highscores.onmouseclickup(event) self.on_enter() def help_onmouseclick(self, event): self.help.onmouseclick(event) self.on_exit() help = HelpMenu() help.run() self.help.onmouseclickup(event) self.reload_titles() self.on_enter() def quit_onmouseclick(self, event): self.quit.onmouseclick(event) self.on_exit() exit = ExitMenu() exit.run() pygame.event.post(pygame.event.Event(QUIT, {})) def reload_titles(self): self.parser.parse(open("languages/MainMenu_" + Constants.LANGUAGE + ".xml")) self.title.set_title(self.curHandler.getText("title")) self.start.set_title(self.curHandler.getText("start")) self.options.set_title(self.curHandler.getText("options")) self.highscores.set_title(self.curHandler.getText("highscores")) self.help.set_title(self.curHandler.getText("help")) self.quit.set_title(self.curHandler.getText("quit")) def run(self): while 1: self.time_speed.tick(Constants.FPS) for event in pygame.event.get(): if event.type == QUIT: os._exit(0) elif event.type == KEYDOWN and event.key == K_ESCAPE: self.quit_onmouseclick(event) else: pygame.event.post(event) # Reinject the event into the queue for maybe latter process self.widget_man.run()
elif ((featureMap[i][arrayCount][0] == 0) and (featureMap[i][arrayCount][1] == 0)) and ((featureMap[i][arrayCount-1][0] != 0) or (featureMap[i][arrayCount-1][1] != 0)): count=arrayCount if(flag==0): featureMap[i+1][count][0] = prevFeatures[featureIndex][0] featureMap[i+1][count][1] = prevFeatures[featureIndex][1] featureMap[i][count][0] = features[featureIndex][0] featureMap[i][count][1] = features[featureIndex][1] count += 1 if ((param.IS_INVERSE_DEPTH_PARAMETRIZATION == 0) and (param.IS_LOCALIZATION_FROM_GT == 1)): # Only XYZ representation available in the first iteration l = Localization("") #tx, ty, tz, qx, qy, qz, qw, fBlock, sBlock = l.staticLocalizationFrmGT(param.GROUND_TRUTH_FILE,'',0) _tx, _ty, _tz, _qx, _qy, _qz, _qw, size = l.getDataBetweenImagesGT(imageName1, imageName2, param.GROUND_TRUTH_FILE) _w.append(_w) if (param.DEBUG==1): print "Size of interim quaternions {}" .format(size) for q in range(0,size): #if (param.DEBUG==1): #print "GT tuple : {},{},{},{},{},{}, {}" .format(_tx[q], _ty[q], _tz[q], _qx[q], _qy[q], _qz[q], _qw[q]) if (q==0) and (i==0): cameraState[3] = float(_qw[q])
def __init__(self): self._trustedNetwork = NetworkBridge() self._untrustedNetwork = NetworkBridge() self._localization = Localization() self._interfaceScanner = InterfaceScanner(self)
class SLAM: def __init__(self): # GLOBAL VARIABLES self.P = np.eye(4) # Pose self.k = np.array( [[640.0, 0, 640.0], [0, 480.0, 480.0], [0, 0, 1.0]], dtype=np.float32, ) # ahmed dataset # self.k = np.array( # [[827.0, 0.0, 638.0], [0.0, 826.0, 347], [0.0000, 0.0000, 1.0000]], # dtype=np.float32, # ) self.poses = [[0.0, 0.0, np.pi / 2]] # initial pose self.trajectory = [np.array([0, 0, 0])] # 3d trajectory self.tMats = [(np.zeros((3, 3)), np.zeros( (3, 1)))] # Transformation Matrices (rmat, tvec) self.env_map = Map(62, 62) self._create_map(self.env_map) self.particle_filter = Localization(self.env_map) def _create_map(self, env_map): # store landmarks landmark_imgs = glob.glob("./train/*.jpg") crd = [(0, 62), (62, 62)] # coordinates for idx, img in enumerate(landmark_imgs): img = cv.imread(img) croppedImg, _ = applyTranformations(img) # gray = cv.cvtColor(croppedImg, cv.COLOR_BGR2GRAY) # print(croppedImg.shape) kp, des = orb_extractor(croppedImg) env_map.add_landmark( Landmark(crd[idx][0], crd[idx][1], kp, des, 12)) def process(self, images, depths, iterator): imgs_grey = [ cv.cvtColor(images[0], cv.COLOR_BGR2GRAY), cv.cvtColor(images[1], cv.COLOR_BGR2GRAY), ] # Check if this frame should be dropped (blur/same) if drop_frame(imgs_grey): #print("Dropping the frame") print("Sharpening the frame") fm = cv.Laplacian(imgs_grey[1], cv.CV_64F).var() if fm < 40: kernel = np.array([[0, -1, 0], [-1, 5, -1], [0, -1, 0]]) im = cv.filter2D(imgs_grey[1], -1, kernel) # sharp imgs_grey[1] = im # Part I. Features Extraction kp_list, des_list = extract_features(imgs_grey) # Part II. Feature Matching matches = match_features(des_list) is_main_filtered_m = True # Filter matches if is_main_filtered_m: filtered_matches = filter_matches(matches) matches = filtered_matches # Removing Same frames smatches = sorted(matches, key=lambda x: x.distance) sdiff = sum([x.distance for x in smatches[:500]]) if sdiff < 1000: print(f"\t->->Frame Filtered because isSame: {sdiff}") return # Part III. Trajectory Estimation # Essential Matrix or PNP # pnp_estimation || essential_matrix_estimation self.P, rmat, tvec = estimate_trajectory(matches, kp_list, self.k, self.P, depths[1]) # No motion estimation if np.isscalar(rmat): return # Compare same frame # prevRmat, prevTvec = self.tMats[-1] # if not np.allclose(rmat, prevRmat) and not np.allclose(tvec, prevTvec): self.tMats.append((rmat, tvec)) new_trajectory = self.P[:3, 3] self.trajectory.append(new_trajectory * 2.95) self.poses.append(calc_robot_pose(self.P[:3, :3], self.P[:, 3] * 2.95)) # else: # print(f"\t->Frame Filtered because same TMat") # return # Part IV. Localize last_pose = self.poses[-1] second_last_pose = self.poses[-2] print(f"Odometry:\n\t{[second_last_pose, last_pose]}") self.particle_filter.motion_update([second_last_pose, last_pose]) if iterator % 5 == 0: print(">>>>> Updating Measurement") self.particle_filter.measurement_update(images[1], kp_list[1], des_list[1], iterator) self.particle_filter.sample_particles() # Part V. Save Visualization plot visualize_data(self.env_map.plot_map, showPlot=False) visualize_data( self.particle_filter.plot_particles, clean_start=False, showPlot=False, figName=f"frame{iterator}", ) # plt.cla() # npTraj = np.array(self.trajectory).T # visualize_trajectory(npTraj) # plt.savefig(f'dataviz/frame{iterator}.png') def get_trajectory(self): return np.array(self.trajectory).T def get_robot_poses(self): return self.poses
class NetworkServer(InterfaceCallbacks, NetworkCallbacks): def __init__(self): self._trustedNetwork = NetworkBridge() self._untrustedNetwork = NetworkBridge() self._localization = Localization() self._interfaceScanner = InterfaceScanner(self) def initialize(self): if not self._trustedNetwork.initialize() or \ not self._untrustedNetwork.initialize() or \ not self._localization.initialize(): return False return True def deinitialize(self): self._localization.deinitialize() self._untrustedNetwork.deinitialize() self._trustedNetwork.deinitialize() def scanInterfaces(self): self._interfaceScanner.runScan() def interfaceAdded(self, iface): # Handle WiFi interfaces if iface.type() == InterfaceType.WIFI: rospy.loginfo('Registering wireless [%s]' % iface.name()) self._localization.setInterface(iface) # Handle bridge interfaces elif iface.type() == InterfaceType.BRIDGE: rospy.loginfo('Registering bridge [%s]' % iface.name()) if iface.isTrusted(): self._trustedNetwork.addInterface(iface) else: self._untrustedNetwork.addInterface(iface) # Handle tap interfaces elif iface.type() == InterfaceType.TAP: rospy.loginfo('Registering tap [%s]' % iface.name()) self._trustedNetwork.addInterface(iface) # Handle ethernet interfaces elif iface.type() == InterfaceType.ETHERNET: gateway = iface.getGateway() if not gateway: # Interface might not have IP address yet, try to get one now if DHCP.getLease(iface.name()): gateway = iface.getGateway() if gateway: rospy.loginfo('Skipping interface [%s] with gateway %s' % (iface.name(), gateway)) else: rospy.loginfo('Registering ethernet [%s]' % iface.name()) self._untrustedNetwork.addInterface(iface) def interfaceRemoved(self, iface): rospy.loginfo('Unregistering [%s]' % iface.name()) # Handle WiFi interfaces if iface.type() == InterfaceType.WIFI: self._localization.unsetInterface(iface) # Handle bridge interfaces elif iface.type() == InterfaceType.BRIDGE: if iface.isTrusted(): self._trustedNetwork.removeInterface(iface) else: self._untrustedNetwork.removeInterface(iface) # Handle tap interfaces elif iface.type() == InterfaceType.TAP: self._trustedNetwork.removeInterface(iface) # Handle ethernet interfaces elif iface.type() == InterfaceType.ETHERNET: self._untrustedNetwork.removeInterface(iface) def enableAccessPoint(self): params = { 'name': 'Deliverator', 'channel': 11 } # TODO rospy.loginfo('Enabling wireless AP "%s" (channel %d)' % (params['name'], params['channel'])) if self._localization.enableAccessPointMode(params): iface = self._localization.getInterface() iface.enableAccessPoint(self) self._trustedNetwork.addInterface(iface) def disableAccessPoint(self): rospy.loginfo('Disabling wireless AP') iface = self._localization.getInterface() self._trustedNetwork.removeInterface(iface) iface.disableAccessPoint() self._localization.disableAccesPointMode() def wifiClientConnected(self, client): self._localization.enableTargetMode(client) def wifiClientDisconnected(self, client): self._localization.disableTargetMode(client) def wifiConnect(self, params): rospy.loginfo('Connecting to WiFi network "%s" (channel %d)' % (params['name'], params['channel'])) if self._localization.targetNetwork(params): self._untrustedNetwork.addInterface(self._localization.getInterface()) def wifiDisconnect(self): rospy.loginfo('Disconnecting from WiFi network') self._untrustedNetwork.removeInterface(self._localization.getInterface()) self._localization.untargetNetwork()
class HighScoresMenu(Menu): "HighScores Menu" def __init__(self,background): "Set up the HighScores menu" Menu.__init__(self,"MoleFusion HighScore Menu",background) self.parser = make_parser() self.curHandler = Localization() self.parser.setContentHandler(self.curHandler) self.parser.parse(open("languages/HighScoresMenu_" + Constants.LANGUAGE + ".xml")) self.title = GW_Label(self.curHandler.getText("title"),(0.5,0.1),(27,22,24)) self.name_column = GW_Label(self.curHandler.getText("name"),(0.25,0.25),(212,224,130)) self.points_column = GW_Label(self.curHandler.getText("points"),(0.75,0.25),(212,224,130)) self.returnMain = GW_Button(self.curHandler.getText("returnMain"),(0.5,0.9),(255,255,255)) self.returnMain.add_eventhandler("onmouseclick",self.returnMain_onmouseclick) h = HighScores() highscorelist = h.get_HighScores() self.widgetlist = [self.title,self.name_column,self.points_column,self.returnMain] for val,i in enumerate(highscorelist[0:5]): self.widgetlist.append(GW_Label(i.get_name(),(0.25,0.35+val/10.0),(250,254,210))) self.widgetlist.append(GW_Label(str(i.get_points()),(0.75,0.35+val/10.0),(250,254,210))) self.widget_man = GuiWidgetManager(self.widgetlist) self.time_speed=pygame.time.Clock() self.exit=False self.on_enter() def on_enter(self): pygame.event.clear() self.screen.blit(self.background, (0, 0)) pygame.display.flip() self.exit=False self.widget_man.set_draw(True) def on_exit(self): pygame.event.clear() f = FadeTransition(500,Constants.FADECOLOR,"to") del f self.exit=True self.widget_man.set_draw(False) def returnMain_onmouseclick(self,event): self.returnMain.onmouseclick(event) self.on_exit() def run(self): while 1 and self.exit==False: self.time_speed.tick(Constants.FPS) for event in pygame.event.get(): if event.type == QUIT: os._exit(0) elif event.type == KEYDOWN: if (not self.widget_man.has_input_focus()): if(event.key == K_ESCAPE): return #else: # pygame.event.post(event) #Reinject the event into the queue for maybe latter process else: pygame.event.post(event) #Reinject the event into the queue for maybe latter process self.widget_man.run()
from Undock import Undock from Localization import Localization from TakePicture import TakePicture # from CancelGoal import CancelGoal if __name__ == "__main__": rospy.init_node('state_machine_controller') sm = StateMachine(outcomes=['success','ready_to_dock','docking_complete','docking_failed', 'cancel_outcome']) # sm = StateMachine(outcomes=['done']) with sm: StateMachine.add('UNDOCKING', Undock(), transitions={'success': 'LOCALIZATION'}) StateMachine.add('LOCALIZATION', Localization(), transitions={'success': 'NAVIGATION'}) StateMachine.add('NAVIGATION', Navigation(), transitions={'done':'PLAY_SOUND'}) StateMachine.add('TO_DOCKING', ToDocking(), transitions={'ready_to_dock': 'DOCKING'}) StateMachine.add('DOCKING', Docking(), transitions={'docking_complete': None}) StateMachine.add('TAKE_PICTURE', TakePicture(), transitions={'done': 'PLAY_SOUND'}) StateMachine.add('PLAY_SOUND', NavSound(), transitions={'done': None}) # StateMachine.add('CANCEL_TASK', CancelGoal(), # transitions={'cancel_outcome': 'TO_DOCKING'})
class MessageBox(pygame.sprite.Sprite): "MessageBox class" def __init__(self,gamecontainer,time): "Initialises resources " pygame.sprite.Sprite.__init__(self) self.parser = make_parser() self.curHandler = Localization() self.parser.setContentHandler(self.curHandler) self.parser.parse(open("languages/MessageBox_" + Constants.LANGUAGE + ".xml")) self.gamecontainer=gamecontainer self.time_start=self.gamecontainer.time self.time=0.0 self.endtime = time self.dirty = 1 self.screen_size=Constants.SCREEN.get_size() self.image=pygame.image.load("sprites/back_letters.png") self.image.convert_alpha() self.rect = self.image.get_rect() self.rect.centerx = 0.5*self.screen_size[0] self.rect.centery = 0.5*self.screen_size[1] self.state = "grow" self.scale = 0.0 self.font = pygame.font.Font(os.path.join("fonts","verdana.ttf"), 24) if self.gamecontainer.start == True: self.fontimg = self.font.render( self.curHandler.getText("level") + " " + str(self.gamecontainer.level), True , (230,240,255,255) ) rect = self.fontimg.get_rect() self.image.blit(self.fontimg,(self.rect.width/2 - rect.width/2,100)) elif self.gamecontainer.completed == True: self.fontimg = self.font.render( self.curHandler.getText("finish"), True , (230,240,255,255) ) rect = self.fontimg.get_rect() self.image.blit(self.fontimg,(self.rect.width/2 - rect.width/2,80)) self.fontimg = self.font.render( self.curHandler.getText("points"), True , (230,240,255,255) ) rect = self.fontimg.get_rect() self.image.blit(self.fontimg,(self.rect.width/2 - rect.width/2,110)) self.fontimg = self.font.render( str(self.gamecontainer.timeclock.get_time()*15), True , (255,255,235,255) ) rect = self.fontimg.get_rect() self.image.blit(self.fontimg,(self.rect.width/2 - rect.width/2,140)) self.gamecontainer.score.change(self.gamecontainer.timeclock.get_time()*15) elif self.gamecontainer.gameover == True: self.fontimg = self.font.render( self.curHandler.getText("over"), True , (230,240,255,255) ) rect = self.fontimg.get_rect() self.image.blit(self.fontimg,(self.rect.width/2 - rect.width/2,100)) self.original=self.image.copy() def update(self): self.time= self.gamecontainer.time - self.time_start if self.time > self.endtime: if self.gamecontainer.start == True: self.gamecontainer.pause = False else: self.gamecontainer.quit = True elif self.time > self.endtime*0.1 and self.time < self.endtime*0.9: self.state = "stop" self.dirty = 0 elif self.time > self.endtime*0.9: self.state = "shrink" self.dirty = 1 self.scale = cosine_Interpolate((1.0,0.0),(self.time-(self.endtime*0.9))/(self.endtime*0.1)) else: self.scale = cosine_Interpolate((0.0,1.0),self.time/(self.endtime*0.1)) if not self.dirty: return self.image = pygame.transform.rotozoom(self.original,0.0,self.scale) self.rect = self.image.get_rect(center=(self.rect.centerx,self.rect.centery)) self.dirty=1
class Robot: def __init__(self, map, display): # environment inits self.landmarks = map["landmarks"] self.goals = map["goals"] self.currentGoal = 0 self.currentPos = map["initial_position"] self.currentAngle = map["initial_angle"] self.num_particles = map["num_particles"] self.dist_per_frame = map["distance_per_frame"] self.rot_per_frame = math.radians(map["rotation_per_frame"]) # noise stds self.move_std = map["noise"]["move_std"] self.rotate_std = map["noise"]["rotate_std"] # display init self.display = display self.cmd_msg = "Waiting for commands..." # localization self.localization = Localization(self.num_particles, map["dimensions"]["width"], map["dimensions"]["height"], map["noise"]) #estimated positions self.estimatedPos, self.estimatedAngle = self.localization.estimate_position( ) def move(self, distance): frames = int(distance / self.dist_per_frame) # movement to the calculated point for i in range(frames): # error is added at each frame dist_with_noise = self.dist_per_frame + np.random.randn( ) * self.move_std incx = math.cos(self.currentAngle) * dist_with_noise incy = math.sin(self.currentAngle) * dist_with_noise # motion update (robot and particles) self.currentPos = [ self.currentPos[0] + incx, self.currentPos[1] + incy ] self.localization.motion_update(self.dist_per_frame, 0) self.estimatedPos, self.estimatedAngle = self.localization.estimate_position( ) self.display.drawFrame(self) # sense and update self.localization.sensor_update(self.landmarks, self.currentPos, self.currentAngle) self.localization.resample() # estimate position self.estimatedPos, self.estimatedAngle = self.localization.estimate_position( ) def rotate(self, angle): # calculate difference and direction of rotation if angle > self.estimatedAngle: if angle - self.estimatedAngle <= math.pi: dif_angle = angle - self.estimatedAngle dir_rot = 1 else: dif_angle = 2 * math.pi - (angle - self.estimatedAngle) dir_rot = -1 else: if self.estimatedAngle - angle <= math.pi: dif_angle = self.estimatedAngle - angle dir_rot = -1 else: dif_angle = 2 * math.pi - (self.estimatedAngle - angle) dir_rot = 1 frames = int(dif_angle / self.rot_per_frame) # rotation to calculated angle for i in range(frames): # add noise rot_with_noise = self.rot_per_frame + np.random.randn( ) * self.rotate_std # motion update self.currentAngle = self.currentAngle + rot_with_noise * dir_rot if self.currentAngle < 0: self.currentAngle = self.currentAngle + math.pi * 2 if self.currentAngle >= math.pi * 2: self.currentAngle = self.currentAngle - math.pi * 2 self.localization.motion_update(0, self.rot_per_frame * dir_rot) self.estimatedPos, self.estimatedAngle = self.localization.estimate_position( ) self.display.drawFrame(self) # sense and update self.localization.sensor_update(self.landmarks, self.currentPos, self.currentAngle) self.localization.resample() # estimate position self.estimatedPos, self.estimatedAngle = self.localization.estimate_position( ) def random_moves(self): self.cmd_msg = "Taking random moves..." rand_rot = np.random.uniform(0, 2 * math.pi) rand_dist = np.random.uniform(0, 100) self.rotate(rand_rot) self.move(rand_dist) # main execution loop def begin(self): while len(self.goals) > self.currentGoal: self.display.drawFrame(self) # check position difx = self.goals[self.currentGoal][0] - self.estimatedPos[0] dify = self.goals[self.currentGoal][1] - self.estimatedPos[1] if abs(difx) < 10 and abs(dify) < 10: print("Position reached, rotate to plant") orientated = False # rotate to plant while not orientated: # get angle of orientation to plant difx = self.landmarks[ self.currentGoal][0] - self.estimatedPos[0] dify = self.landmarks[ self.currentGoal][1] - self.estimatedPos[1] if difx != 0: angle = math.atan(dify / difx) else: if dify < 0: angle = -math.pi / 2 else: angle = -math.pi / 2 # normalize angle between 0 and 2*pi if difx < 0: angle = math.pi + angle if angle < 0: angle = angle + math.pi * 2 self.cmd_msg = "Rotating to plant..." self.rotate(angle) if abs(self.estimatedAngle - angle) < math.pi / 50: orientated = True print("Goal reached! Take photo.") self.cmd_msg = "Taking photo..." self.currentGoal += 1 else: print("Move to goal") # rotate to position if difx != 0: angle = math.atan(dify / difx) else: if dify < 0: angle = -math.pi / 2 else: angle = -math.pi / 2 # normalize angle between 0 and 2*pi if difx < 0: angle = math.pi + angle if angle < 0: angle = angle + math.pi * 2 self.cmd_msg = "Rotating to goal..." self.rotate(angle) # move to position distance = math.sqrt(difx**2 + dify**2) # cut long distances to avoid big decompensation with the estimated position if distance > 100: distance = 100 self.cmd_msg = "Moving to goal..." self.move(distance) def finish(self): self.cmd_msg = "All goals achieved!" # wait 50 frames before closing window for i in range(50): self.display.drawFrame(self) self.display.finish()