def set_video_widget(self, video_widget, conf={}): self.video_widget = video_widget self.video_thread = VideoThread(video_widget, conf) if self.form: self.video_thread.set_separation_threshold( self.form.threshold.value())
def main(): # Initialize frame rate calculation frame_rate_calc = 1 freq = cv2.getTickFrequency() font = cv2.FONT_HERSHEY_SIMPLEX objectifier = Model() # Start serial connection to arduino ser = serial.Serial('/dev/ttyACM0', 9600, timeout=2) time.sleep(1) # Initialize queues pic_q = queue.LifoQueue(5) command_q = queue.Queue() # Inizialize grid anf gridmovement grid = Grid(8,8) movement = GridMovement(grid, ser) # Initialize VideoThread vt = VideoThread(pic_q, objectifier) vt.start() # Setup GPIO GPIO.setmode(GPIO.BOARD) GPIO.setup(BUTTONPIN, GPIO.IN, pull_up_down=GPIO.PUD_UP) GPIO.setup(CONTACT_PIN, GPIO.IN, pull_up_down=GPIO.PUD_UP) GPIO.setup(LEDPIN, GPIO.OUT, initial=GPIO.LOW) # Keep track of movements after approach is called approach_movement_list = queue.LifoQueue() wait_for_button(GPIO) time.sleep(2) # # RUN ROUND # print("Starting round") begin_round(movement, pic_q) # map the targets from json file map_JSON(mar1.json,movement) # now set the maximum amount of obstacles based on amount of targets grid.set_obstacles_max() begin_round(movement, pic_q) print("I will try and map the mothership") map_mothership(movement, pic_q) print("Mothership is located in the following tiles: ", grid.mothership) # We can save these values in movement.access_point so other functions with access to movement # can use these mothership_angle, dist, side_angle = approach_mothership_side(movement, pic_q, ser, GPIO) movement.set_mothership_angle(mothership_angle) movement.set_side_angle(side_angle) movement.set_access_dist(dist) print("Mothership angle: {}, Distance: {}, Side_angle: {}".format(mothership_angle, dist, side_angle)) print("Going home") go_home(movement, pic_q) while grid.targets: # find closest target and set it as the goal movement.current_target = closest_point(grid.targets, movement.current) movement.set_goal(movement.current_target) follow_path(movement, pic_q) # Once we reach the target we attempt to pick it up approach(movement, pic_q) success, target_id = check_pick_up(movement, pic_q) print("Success: {}, Target Id: {}".format(success, target_id)) if not success: go_home(movement, pic_q) else: grid.targets.remove(movement.current_target) # We would correct alignment here if errors were fixed # Move to mothership and drop target print("Access point is: ",movement.get_access_point()) movement.set_goal(movement.get_access_point()) follow_path(movement, pic_q, True) movement.face(movement.get_side_point()) print("Going to drop it") mothership_drop(dist, mothership_angle, side_angle, target_id, movement, serial, pic_q) go_home(movement, pic_q) # Once all targets are delivered, go home and turn on finishing light go_home(movement, pic_q) wait_for_button(GPIO) # close video thread vt.join()