class Game(object):
def __init__(self):
self._robot = Robot((XMAX/2+100,YMAX/2-25),0)
#self._robot = particleGeneratorGenerator(0)()
self._keydown = defaultdict(bool)
self.reset_pf()
def reset_pf(self):
self._pf = ParticleFilter(particleGeneratorGenerator(5),N=500,regenProb=0.2)
def draw(self,surf):
#self._robot.sense(surf)
#self._robot.draw(surf,(255,255,0))
for i in range(len(self._pf._particles)):
p = self._pf._particles[i]
p.draw(surf,color=(255,0,0))
def evolve(self):
pause = True
ds = 0
dh = 0
if self._keydown[pygame.K_x]:
pause = False
if self._keydown[pygame.K_r]:
self.reset_pf()
pause = False
if self._keydown[pygame.K_UP]:
pause = False
ds = 2
elif self._keydown[pygame.K_DOWN]:
pause = False
ds = -2
if self._keydown[pygame.K_RIGHT]:
pause = False
dh = pi/32
elif self._keydown[pygame.K_LEFT]:
pause = False
dh = -pi/32
if not pause:
action = (ds,dh)
self._robot.evolve(action)
ranges = self._robot.sense()
def likelihood(p):
ranges_prime = p.sense()
ranges_diff = np.abs(ranges_prime - ranges)
rv = 1.0
for i in range(len(ranges_diff)):
#rv *= exp(-(ranges_diff[i]/SIGMA_RANGE)**2)*0.5 + 0.5
if (ranges_diff[i] / SIGMA_RANGE) > 2:
rv *= 0.3
elif (ranges_diff[i] / SIGMA_RANGE) > 2:
rv *= 0.5
elif (ranges_diff[i] / SIGMA_RANGE) > 1:
rv *= 0.9
return rv
if not self._keydown[pygame.K_h]:
#self._pf.iterate(action,likelihood)
self._pf._evolve(action)
if self._keydown[pygame.K_x]:
self._pf._update(likelihood)
def handle_event(self,event):
if event.type == pygame.KEYDOWN:
self._keydown[event.key] = True
elif event.type == pygame.KEYUP:
self._keydown[event.key] = False
