# the_map = TrueMap()
viz = Visualization(the_map, start_x=initx, start_y=inity)
sensor = Sensor(the_map)
odom = Odometry(the_map, start_x=initx, start_y=inity)
part_filt = ParticleFilter(the_map, sensor, odom, numParticles=100)
part_filt.initializeParticles()
scale = 7 # number of pixels to move each time a movement is given
# assuming a move command represents a 1 pixel move
moveCommandDeltas = {'a':(0, -scale), 'w':(-scale, 0), 's':(scale, 0),'d':(0, scale), 'aw':(-scale, -scale), 'wa':(-scale,-scale),
'wd':(-scale, scale) , 'dw':(-scale, scale) , 'sd':(scale,scale), 'ds':(scale,scale),
'as':(scale, -scale), 'sa':(scale, -scale)}
# Do some stuff ***
viz.replot_particles(part_filt.getParticleLocations(), numtoplot=100)
viz.display_plot()
step_count = 0
while True:
direction = raw_input("Move command: ")
direction.lower()
step_count += 1
# startTime = time.time() # determine amount of time for particle update
# process the movement as a change in position
if direction in moveCommandDeltas:
odomMeasure = odom.updatePosition(moveCommandDeltas[direction])
if odomMeasure is not None:
part_filt.moveParticles(odomMeasure)
