def on_start(self, goal, frame): self.goal = goal filtered_image = self.obstacleDetector.applyFilter(frame) planner = PathPlanner() self.path, expanded, found_goal = planner.planPath( filtered_image, self.marbleStateManager.get_position(), goal) print("Got a path of length: " + str(len(self.path)))
class PlanPath: def __init__(self, marbleStateManager): self.marbleStateManager = marbleStateManager self.obstacle_detector = SimpleBlobDetector.createObstacleDetector() self.planner = PathPlanner() def tick(self): filtered_image = self.obstacle_detector.applyFilter( WhiteBoard.RAW_FRAME) WhiteBoard.PATH, expanded, found_goal = self.planner.planPath( filtered_image, self.marbleStateManager.get_position(), GOAL) print("Got a path of length: " + str(len(WhiteBoard.PATH)) + ", found goal: " + str(found_goal)) mark_path(WhiteBoard.PATH, WhiteBoard.FRAME_TO_SHOW) show_and_wait(WhiteBoard.FRAME_TO_SHOW) if (found_goal): return SUCCESS else: return FAILURE