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
