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 __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 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):
"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):
# 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, 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)
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])
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'})
def __init__(self):
self._trustedNetwork = NetworkBridge()
self._untrustedNetwork = NetworkBridge()
self._localization = Localization()
self._interfaceScanner = InterfaceScanner(self)
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()
