class Simulation(object): # Constructor # @param duration Duration (in seconds) of the Simulation # @param dt Time step # @param height Height of the window # @param width Width of the window # @param env_file Path to the file containing Environment data def __init__(self, duration, dt, height, width, env_file): self._duration = duration self._dt = dt # Create the Canvas onto which everything will be rendered self._root = Tk() self._canvas = Canvas( self._root, width=width, height=height) self._canvas.grid(column=0, row=0, sticky=(N, W, E, S)) # Set the title, and make the Window un-sizable self._parent = self._root self._parent.title("Awesome Robot Simulation") self._parent.resizable(0, 0) # Determine the window position (centered nicely) self.center_window(height, width) # Read in the environment data self._env = Environment() self._env.set_dimensions(height, width) if env_file: self._env.read_env(env_file) # Center the window on the screen # @param wh Height of the window to center # @param ww Width of the window to center def center_window(self, wh, ww): sw = self._parent.winfo_screenwidth() sh = self._parent.winfo_screenheight() x, y = (sw - ww) / 2, (sh - wh) / 2 self._parent.geometry('%dx%d+%d+%d' % (ww, wh, x ,y)) # Start the event loop def start_event_loop(self): self.timer_triggered() self._parent.mainloop() # When the timer goes off, update and redraw everything and reset the timer def timer_triggered(self): self.update() self.redraw() self._canvas.after(self._dt, self.timer_triggered) # Update the simulation for the latest time step def update(self): sleep(0.05) # sleep so the animation doesn't finish too quickly self._duration -= self._dt; if not self._duration > 0: raw_input("Press <Enter> to stop the simulation...") # Print the results print for robot in self._env.robots(): M, N = robot.slam().M(), robot.slam().N() for i in range(N): computed_landmark_pos = np.matrix([0.0, 0.0]).T for k in range(M): computed_landmark_pos += robot.slam().particles()[k].features()[i][0] computed_landmark_pos /= M computed_landmark_x = computed_landmark_pos.item(0) computed_landmark_y = computed_landmark_pos.item(1) (actual_landmark_x, actual_landmark_y) = self._env.landmarks()[i].pos() print "Landmark %d => actual: (%f, %f), computed: (%f, %f)" % \ (i, actual_landmark_x, actual_landmark_y, computed_landmark_x, computed_landmark_y) print # TODO --- maybe just take the particle with the highest weight (maybe top 2 or 3)? [same for landmarks] actual_robot_x, actual_robot_y, actual_robot_theta = robot.pose() computed_robot_x, computed_robot_y, computed_robot_theta = (0.0, 0.0, 0.0) for k in range(M): (p_x, p_y, p_theta) = robot.slam().particles()[k].pose() computed_robot_x += p_x computed_robot_y += p_y computed_robot_theta += p_theta print "Robot pose => actual: (%f, %f, %f), computed: (%f, %f, %f)" % \ (actual_robot_x, actual_robot_y, actual_robot_theta, computed_robot_x / M, computed_robot_y / M, computed_robot_theta / M) exit() # Update all of the robots updated_robots = [] for robot in self._env.robots(): updated_robots.append(robot.update(self._dt)) self._env._robots = updated_robots # Redraw the canvas def redraw(self): self._canvas.delete(ALL) for wall in self._env.walls(): wall.draw(self._canvas) for landmark in self._env.landmarks(): landmark.draw(self._canvas) for robot in self._env.robots(): robot.draw(self._canvas) # Start the simulation def start(self): self.start_event_loop()