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()
