from aim_fsm import *

class Lab6a(StateMachineProgram):
    def __init__(self):
        super().__init__(launch_path_viewer = True,
                         launch_particle_viewer = True,
                         speech = False)

    class PathToOrange(StateNode):
        def start(self, event=None):
            super().start(event)
            ob = self.robot.world_map.objects['OrangeBarrel.a']
            self.robot.path_planner.plan_path_this_process(self.robot, ob)
            self.robot.rrt.draw_path = self.robot.rrt.path
            print(self.robot.rrt.path)

    $setup{
        TagDetection(enabled=True) =N=> loop

        loop: Print("\n\nType 'tm' to proceed") =TM=> self.PathToOrange() =N=> loop
    }