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()
def __init__(self):
self._trustedNetwork = NetworkBridge()
self._untrustedNetwork = NetworkBridge()
self._localization = Localization()
self._interfaceScanner = InterfaceScanner(self)
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()
