from cozmo_fsm import *

class Lab7b(StateMachineProgram):
    def __init__(self):
        self.pf = SLAMParticleFilter(robot)
        super().__init__(particle_filter=self.pf, particle_viewer=True)

    def start(self):
        super().start()
        # Force one evaluation to load any initially visible landmarks
        pf = self.pf
        pf.sensor_model.evaluate(pf.particles, force=True)
        # Make landmarks visible to the particle_viewer
        pf.sensor_model.landmarks = pf.particles[0].landmarks