def main():
pf = ParticleFilter(numParticles=1000)
sim = Simulator(pf)
sim.start()
'''
pf.particles = util.Counter()
pf.numParticles = 8
pf.particles[(640, 500, 0)] = 1.0
pf.particles[(640, 500, -numpy.pi/2)] = 1.0
pf.particles[(640, 500, numpy.pi/2)] = 1.0
pf.particles[(640, 500, numpy.pi)] = 1.0
pf.particles[(640, 600, 0)] = 1.0
pf.particles[(640, 600, -numpy.pi/2)] = 1.0
pf.particles[(640, 600, numpy.pi/2)] = 1.0
pf.particles[(640, 600, numpy.pi)] = 1.0
'''
sim.refresh()
time.sleep(2)
# Observation is a distance and heading to an unidentified line
pf.observeLine((50, 0))
# Observation is a distance and heading to an unidentified corner
pf.observeCorner((50, 0))
sim.refresh()
time.sleep(2)
pf.elapseTime()
sim.refresh(pf.getBeliefs())
# Main logic loop
dist = 50
while True:
pf.observeCorner((dist, 0))
sim.refresh(pf.getBeliefs())
pf.elapseTime((50, 0, 0.1))
dist -=50
def main():
pf = ParticleFilter(numParticles=1000)
sim = Simulator(pf)
sim.start()
'''
pf.particles = util.Counter()
pf.numParticles = 8
pf.particles[(640, 500, 0)] = 1.0
pf.particles[(640, 500, -numpy.pi/2)] = 1.0
pf.particles[(640, 500, numpy.pi/2)] = 1.0
pf.particles[(640, 500, numpy.pi)] = 1.0
pf.particles[(640, 600, 0)] = 1.0
pf.particles[(640, 600, -numpy.pi/2)] = 1.0
pf.particles[(640, 600, numpy.pi/2)] = 1.0
pf.particles[(640, 600, numpy.pi)] = 1.0
'''
sim.refresh()
time.sleep(2)
# Observation is a distance and heading to an unidentified line
pf.observeLine((50, 0))
# Observation is a distance and heading to an unidentified corner
pf.observeCorner((50, 0))
sim.refresh()
time.sleep(2)
pf.elapseTime()
sim.refresh(pf.getBeliefs())
# Main logic loop
dist = 50
while True:
pf.observeCorner((dist, 0))
sim.refresh(pf.getBeliefs())
pf.elapseTime((50, 0, 0.1))
dist -= 50
