def __init__(self, pitch, color, our_side, video_port=0, comm_port='/dev/ttyUSB0', comms=1): """ Entry point for the SDP system. Params: [int] video_port port number for the camera [string] comm_port port number for the arduino [int] pitch 0 - main pitch, 1 - secondary pitch [string] our_side the side we're on - 'left' or 'right' *[int] port The camera port to take the feed from *[Robot_Controller] attacker Robot controller object - Attacker Robot has a RED power wire *[Robot_Controller] defender Robot controller object - Defender Robot has a YELLOW power wire """ assert pitch in [0, 1] assert color in ['yellow', 'blue'] assert our_side in ['left', 'right'] self.pitch = pitch # Set up the Arduino communications self.arduino = Arduino(comm_port, 115200, 1, comms) # Set up camera for frames self.camera = Camera(port=video_port, pitch=self.pitch) frame = self.camera.get_frame() center_point = self.camera.get_adjusted_center(frame) # Set up vision self.calibration = tools.get_colors(pitch) self.vision = Vision( pitch=pitch, color=color, our_side=our_side, frame_shape=frame.shape, frame_center=center_point, calibration=self.calibration) # Set up postprocessing for vision self.postprocessing = Postprocessing() # Set up main planner self.planner = Planner(our_side=our_side, pitch_num=self.pitch) # Set up GUI self.GUI = GUI(calibration=self.calibration, arduino=self.arduino, pitch=self.pitch) self.color = color self.side = our_side self.preprocessing = Preprocessing() self.attacker = Attacker_Controller() self.defender = Defender_Controller()
def __init__(self, pitch, color, our_side, video_port=0, comm_port="/dev/ttyUSB0", comms=1): """ Entry point for the SDP system. Params: [int] video_port port number for the camera [string] comm_port port number for the arduino [int] pitch 0 - main pitch, 1 - secondary pitch [string] our_side the side we're on - 'left' or 'right' *[int] port The camera port to take the feed from *[Robot_Controller] attacker Robot controller object - Attacker Robot has a RED power wire *[Robot_Controller] defender Robot controller object - Defender Robot has a YELLOW power wire """ assert pitch in [0, 1] assert color in ["yellow", "blue"] assert our_side in ["left", "right"] self.pitch = pitch # Set up the Arduino communications self.arduino = Arduino(comm_port, 115200, 1, comms) # Set up camera for frames self.camera = Camera(port=video_port, pitch=self.pitch) frame = self.camera.get_frame() center_point = self.camera.get_adjusted_center(frame) # Set up vision self.calibration = tools.get_colors(pitch) self.vision = Vision( pitch=pitch, color=color, our_side=our_side, frame_shape=frame.shape, frame_center=center_point, calibration=self.calibration, ) # Set up postprocessing for vision self.postprocessing = Postprocessing() # Set up main planner self.planner = Planner(our_side=our_side, pitch_num=self.pitch) # Set up GUI self.GUI = GUI(calibration=self.calibration, arduino=self.arduino, pitch=self.pitch) self.color = color self.side = our_side self.preprocessing = Preprocessing() self.attacker = Attacker_Controller() self.defender = Defender_Controller()
def __init__(self, pitch, color, our_side, video_port=0): """ Entry point for the SDP system. Params: [int] pitch 0 - main pitch, 1 - secondary pitch [string] colour The colour of our teams plates [string] our_side the side we're on - 'left' or 'right' [int] video_port port number for the camera Fields pitch camera calibration vision postporcessing color side preprocessing model_positions regular_positions """ assert pitch in [0, 1] assert color in ['yellow', 'blue'] assert our_side in ['left', 'right'] self.pitch = pitch # Set up camera for frames self.camera = Camera(port=video_port, pitch=pitch) self.frame = self.camera.get_frame() center_point = self.camera.get_adjusted_center(self.frame) # Set up vision self.calibration = tools.get_colors(pitch) self.vision = Vision( pitch=pitch, color=color, our_side=our_side, frame_shape=self.frame.shape, frame_center=center_point, calibration=self.calibration) # Set up preprocessing and postprocessing self.postprocessing = Postprocessing() self.preprocessing = Preprocessing() self.color = color self.side = our_side self.frameQueue = []
b.show(frame) """ scalibration = tools.get_colors(0) # print scalibration vision = _Vision(pitch=0, color='blue', our_side='left', frame_shape=frame.shape, frame_center=cam.get_adjusted_center(frame), calibration=scalibration) # Set up postprocessing for oldvision postprocessing = Postprocessing() GUI = GUI(calibration=scalibration, pitch=0) preprocessing = Preprocessing() frame = cam.get_frame() pre_options = preprocessing.options # Apply preprocessing methods toggled in the UI preprocessed = preprocessing.run(frame, pre_options) frame = preprocessed['frame'] if 'background_sub' in preprocessed: cv2.imshow('bg sub', preprocessed['background_sub']) cv2.waitKey() height, width, channels = frame.shape
class Controller: """ Primary source of robot control. Ties vision and planning together. """ def __init__(self, pitch, color, our_side, video_port=0, comm_port='/dev/ttyUSB0', comms=1): """ Entry point for the SDP system. Params: [int] video_port port number for the camera [string] comm_port port number for the arduino [int] pitch 0 - main pitch, 1 - secondary pitch [string] our_side the side we're on - 'left' or 'right' *[int] port The camera port to take the feed from *[Robot_Controller] attacker Robot controller object - Attacker Robot has a RED power wire *[Robot_Controller] defender Robot controller object - Defender Robot has a YELLOW power wire """ assert pitch in [0, 1] assert color in ['yellow', 'blue'] assert our_side in ['left', 'right'] self.pitch = pitch # Set up the Arduino communications self.arduino = Arduino(comm_port, 115200, 1, comms) # Set up camera for frames self.camera = Camera(port=video_port, pitch=self.pitch) frame = self.camera.get_frame() center_point = self.camera.get_adjusted_center(frame) # Set up vision self.calibration = tools.get_colors(pitch) self.vision = Vision( pitch=pitch, color=color, our_side=our_side, frame_shape=frame.shape, frame_center=center_point, calibration=self.calibration) # Set up postprocessing for vision self.postprocessing = Postprocessing() # Set up main planner self.planner = Planner(our_side=our_side, pitch_num=self.pitch) # Set up GUI self.GUI = GUI(calibration=self.calibration, arduino=self.arduino, pitch=self.pitch) self.color = color self.side = our_side self.preprocessing = Preprocessing() self.attacker = Attacker_Controller() self.defender = Defender_Controller() def wow(self): """ Ready your sword, here be dragons. """ counter = 1L timer = time.clock() try: c = True while c != 27: # the ESC key frame = self.camera.get_frame() pre_options = self.preprocessing.options # Apply preprocessing methods toggled in the UI preprocessed = self.preprocessing.run(frame, pre_options) frame = preprocessed['frame'] if 'background_sub' in preprocessed: cv2.imshow('bg sub', preprocessed['background_sub']) # Find object positions # model_positions have their y coordinate inverted model_positions, regular_positions = self.vision.locate(frame) model_positions = self.postprocessing.analyze(model_positions) # Find appropriate action self.planner.update_world(model_positions) attacker_actions = self.planner.plan('attacker') defender_actions = self.planner.plan('defender') if self.attacker is not None: self.attacker.execute(self.arduino, attacker_actions) if self.defender is not None: self.defender.execute(self.arduino, defender_actions) # Information about the grabbers from the world grabbers = { 'our_defender': self.planner._world.our_defender.catcher_area, 'our_attacker': self.planner._world.our_attacker.catcher_area } # Information about states attackerState = (self.planner.attacker_state, self.planner.attacker_strat_state) defenderState = (self.planner.defender_state, self.planner.defender_strat_state) # Use 'y', 'b', 'r' to change color. c = waitKey(2) & 0xFF actions = [] fps = float(counter) / (time.clock() - timer) # Draw vision content and actions self.GUI.draw( frame, model_positions, actions, regular_positions, fps, attackerState, defenderState, attacker_actions, defender_actions, grabbers, our_color=self.color, our_side=self.side, key=c, preprocess=pre_options) counter += 1 except: if self.defender is not None: self.defender.shutdown(self.arduino) if self.attacker is not None: self.attacker.shutdown(self.arduino) raise finally: # Write the new calibrations to a file. tools.save_colors(self.pitch, self.calibration) if self.attacker is not None: self.attacker.shutdown(self.arduino) if self.defender is not None: self.defender.shutdown(self.arduino)
metavar='Parameter Set Name', type=str, help='Name of parameter set') args = parser.parse_args() batch_name = args.batch_name param_set_name = args.param_set_name # load config module config_module = importlib.import_module('postprocessing.pp_config_' + param_set_name) Config = config_module.Config # create Config and Postprocessing config = Config(batch_name) post = Postprocessing(config) submission = Submission(config) path_to_test_preds = config.POST_PREDICTIONS_IN_DIR path_to_test_imgs = config.TEST_IMAGES_IN_DIR out_dir = config.OUTPUT_DIR if not os.path.exists(out_dir): os.makedirs(out_dir) # aux function def get_id_from_filename(file_name): int_list = list(filter(str.isdigit, f)) result = '' for el in int_list: result += str(el)
def __init__(self, pitch, color, our_side, video_port=0, comm_port='/dev/ttyACM0', comms=1): """ Entry point for the SDP system. Params: [int] video_port port number for the camera [string] comm_port port number for the arduino [int] pitch 0 - main pitch, 1 - secondary pitch [string] our_side the side we're on - 'left' or 'right' *[int] port The camera port to take the feed from *[Robot_Controller] attacker Robot controller object - Attacker Robot has a RED power wire *[Robot_Controller] defender Robot controller object - Defender Robot has a YELLOW power wire """ assert pitch in [0, 1] assert color in ['yellow', 'blue'] assert our_side in ['left', 'right'] self.pitch = pitch # Set up communications if thre are any try: self.robotComs = RobotCommunications(debug=False) except: print("arduino unplugged moving on to vision") # Set up robot communications to bet sent to planner. if self.USE_REAL_ROBOT: try: self.robotCom = RobotCommunications(debug=False) except: self.robotCom = TestCommunications(debug=True) print 'Not connected to the radio, using TestCommunications instead.' else: self.robotCom = TestCommunications(debug=True) # Set up main planner if(self.robotCom is not None): # currently we are assuming we are the defender self.planner = Planner(our_side=our_side, pitch_num=self.pitch, robotCom=self.robotCom, robotType='defender') # Set up camera for frames self.camera = Camera(port=video_port, pitch=self.pitch) frame = self.camera.get_frame() center_point = self.camera.get_adjusted_center(frame) # Set up vision self.calibration = tools.get_colors(pitch) print self.calibration self.vision = Vision( pitch=pitch, color=color, our_side=our_side, frame_shape=frame.shape, frame_center=center_point, calibration=self.calibration) # Set up postprocessing for vision self.postprocessing = Postprocessing() # Set up GUI self.GUI = GUI(calibration=self.calibration, pitch=self.pitch) self.color = color self.side = our_side self.preprocessing = Preprocessing()
class Controller: """ This class aims to be the bridge in between vision and strategy/logic """ robotCom = None # Set to True if we want to use the real robot. # Set to False if we want to print out commands to console only. USE_REAL_ROBOT = True def __init__(self, pitch, color, our_side, video_port=0, comm_port='/dev/ttyACM0', comms=1): """ Entry point for the SDP system. Params: [int] video_port port number for the camera [string] comm_port port number for the arduino [int] pitch 0 - main pitch, 1 - secondary pitch [string] our_side the side we're on - 'left' or 'right' *[int] port The camera port to take the feed from *[Robot_Controller] attacker Robot controller object - Attacker Robot has a RED power wire *[Robot_Controller] defender Robot controller object - Defender Robot has a YELLOW power wire """ assert pitch in [0, 1] assert color in ['yellow', 'blue'] assert our_side in ['left', 'right'] self.pitch = pitch # Set up communications if thre are any try: self.robotComs = RobotCommunications(debug=False) except: print("arduino unplugged moving on to vision") # Set up robot communications to bet sent to planner. if self.USE_REAL_ROBOT: try: self.robotCom = RobotCommunications(debug=False) except: self.robotCom = TestCommunications(debug=True) print 'Not connected to the radio, using TestCommunications instead.' else: self.robotCom = TestCommunications(debug=True) # Set up main planner if(self.robotCom is not None): # currently we are assuming we are the defender self.planner = Planner(our_side=our_side, pitch_num=self.pitch, robotCom=self.robotCom, robotType='defender') # Set up camera for frames self.camera = Camera(port=video_port, pitch=self.pitch) frame = self.camera.get_frame() center_point = self.camera.get_adjusted_center(frame) # Set up vision self.calibration = tools.get_colors(pitch) print self.calibration self.vision = Vision( pitch=pitch, color=color, our_side=our_side, frame_shape=frame.shape, frame_center=center_point, calibration=self.calibration) # Set up postprocessing for vision self.postprocessing = Postprocessing() # Set up GUI self.GUI = GUI(calibration=self.calibration, pitch=self.pitch) self.color = color self.side = our_side self.preprocessing = Preprocessing() def main(self): """ This main method brings in to action the controller class which so far does nothing but set up the vision and its ouput """ counter = 1L timer = time.clock() try: c = True while c != 27: # the ESC key frame = self.camera.get_frame() pre_options = self.preprocessing.options # Apply preprocessing methods toggled in the UI preprocessed = self.preprocessing.run(frame, pre_options) frame = preprocessed['frame'] if 'background_sub' in preprocessed: cv2.imshow('bg sub', preprocessed['background_sub']) # Find object positions # model_positions have their y coordinate inverted # IMPORTANT model_positions, regular_positions = self.vision.locate(frame) model_positions = self.postprocessing.analyze(model_positions) # Update planner world beliefs self.planner.update_world(model_positions) self.planner.plan() # Use 'y', 'b', 'r' to change color. c = waitKey(2) & 0xFF actions = [] fps = float(counter) / (time.clock() - timer) # Draw vision content and actions self.GUI.draw( frame, model_positions, actions, regular_positions, fps, None, None, None, None, False, our_color=self.color, our_side=self.side, key=c, preprocess=pre_options) counter += 1 except: # This exception is stupid TODO: refactor. print("TODO SOMETHING CLEVER HERE") raise finally: tools.save_colors(self.pitch, self.calibration)
class VisionWrapper: """ Class that handles vision """ def __init__(self, pitch, color, our_side, video_port=0): """ Entry point for the SDP system. Params: [int] pitch 0 - main pitch, 1 - secondary pitch [string] colour The colour of our teams plates [string] our_side the side we're on - 'left' or 'right' [int] video_port port number for the camera Fields pitch camera calibration vision postporcessing color side preprocessing model_positions regular_positions """ assert pitch in [0, 1] assert color in ['yellow', 'blue'] assert our_side in ['left', 'right'] self.pitch = pitch # Set up camera for frames self.camera = Camera(port=video_port, pitch=pitch) self.frame = self.camera.get_frame() center_point = self.camera.get_adjusted_center(self.frame) # Set up vision self.calibration = tools.get_colors(pitch) self.vision = Vision( pitch=pitch, color=color, our_side=our_side, frame_shape=self.frame.shape, frame_center=center_point, calibration=self.calibration) # Set up preprocessing and postprocessing self.postprocessing = Postprocessing() self.preprocessing = Preprocessing() self.color = color self.side = our_side self.frameQueue = [] def update(self): """ Gets this frame's positions from the vision system. """ self.frame = self.camera.get_frame() # Apply preprocessing methods toggled in the UI self.preprocessed = self.preprocessing.run(self.frame, self.preprocessing.options) self.frame = self.preprocessed['frame'] if 'background_sub' in self.preprocessed: cv2.imshow('bg sub', self.preprocessed['background_sub']) # Find object positions # model_positions have their y coordinate inverted self.model_positions, self.regular_positions = self.vision.locate(self.frame) self.model_positions = self.postprocessing.analyze(self.model_positions) #self.model_positions = self.averagePositions(3, self.model_positions) def averagePositions(self, frames, positions_in): """ :param frames: number of frames to average :param positions_in: positions for the current frame :return: averaged positions """ validFrames = self.frameQueue.__len__() + 1 positions_out = deepcopy(positions_in) # Check that the incoming positions have legal values for obj in positions_out.items(): if (positions_out[obj[0]].velocity is None): positions_out[obj[0]].velocity = 0 if positions_out[obj[0]].x is None: positions_out[obj[0]].x = 0 if positions_out[obj[0]].y is None: positions_out[obj[0]].y = 0 positions_out[obj[0]].angle = positions_in[obj[0]].angle # Loop over queue for positions in self.frameQueue: # Loop over each object in the position dictionary isFrameValid = True for obj in positions.items(): # Check if the current object's positions have legal values if (obj[1].x is None) or (obj[1].y is None) or (obj[1].angle is None) or (obj[1].velocity is None): isFrameValid = False else: positions_out[obj[0]].x += obj[1].x positions_out[obj[0]].y += obj[1].y positions_out[obj[0]].velocity += obj[1].velocity if not isFrameValid and validFrames > 1: #validFrames -= 1 pass # Loop over each object in the position dictionary and average the values for obj in positions_out.items(): positions_out[obj[0]].velocity /= validFrames positions_out[obj[0]].x /= validFrames positions_out[obj[0]].y /= validFrames # If frameQueue is already full then pop the top entry off if self.frameQueue.__len__() >= frames: self.frameQueue.pop(0) # Add our new positions to the end self.frameQueue.append(positions_in) return positions_out def saveCalibrations(self): tools.save_colors(self.vision.pitch, self.calibration)
def __init__(self, pitch, color, our_side, video_port=0, comm_port='/dev/ttyACM0', comms=1): """ Entry point for the SDP system. Params: [int] video_port port number for the camera [string] comm_port port number for the arduino [int] pitch 0 - main pitch, 1 - secondary pitch [string] our_side the side we're on - 'left' or 'right' *[int] port The camera port to take the feed from *[Robot_Controller] attacker Robot controller object - Attacker Robot has a RED power wire *[Robot_Controller] defender Robot controller object - Defender Robot has a YELLOW power wire """ assert pitch in [0, 1] assert color in ['yellow', 'blue'] assert our_side in ['left', 'right'] self.pitch = pitch # Set up the Arduino communications self.arduino = arduinoComm.Communication("/dev/ttyACM0", 9600) time.sleep(0.5) self.arduino.grabberUp() self.arduino.grab() # Set up camera for frames self.camera = Camera(port=video_port, pitch=self.pitch) frame = self.camera.get_frame() center_point = self.camera.get_adjusted_center(frame) # Set up vision self.calibration = tools.get_colors(pitch) self.vision = Vision( pitch=pitch, color=color, our_side=our_side, frame_shape=frame.shape, frame_center=center_point, calibration=self.calibration) # Set up postprocessing for vision self.postprocessing = Postprocessing() # Set up GUI self.GUI = GUI(calibration=self.calibration, arduino=self.arduino, pitch=self.pitch) # Set up main planner self.planner = Planner(our_side=our_side, pitch_num=self.pitch, our_color=color, gui=self.GUI, comm = self.arduino) self.color = color self.side = our_side self.timeOfNextAction = 0 self.preprocessing = Preprocessing() #it doesn't matter whether it is an Attacker or a Defender Controller self.controller = Attacker_Controller(self.planner._world, self.GUI) self.robot_action_list = []
class Controller: """ Primary source of robot control. Ties vision and planning together. """ def __init__(self, pitch, color, our_side, video_port=0, comm_port='/dev/ttyACM0', comms=1): """ Entry point for the SDP system. Params: [int] video_port port number for the camera [string] comm_port port number for the arduino [int] pitch 0 - main pitch, 1 - secondary pitch [string] our_side the side we're on - 'left' or 'right' *[int] port The camera port to take the feed from *[Robot_Controller] attacker Robot controller object - Attacker Robot has a RED power wire *[Robot_Controller] defender Robot controller object - Defender Robot has a YELLOW power wire """ assert pitch in [0, 1] assert color in ['yellow', 'blue'] assert our_side in ['left', 'right'] self.pitch = pitch # Set up the Arduino communications self.arduino = arduinoComm.Communication("/dev/ttyACM0", 9600) time.sleep(0.5) self.arduino.grabberUp() self.arduino.grab() # Set up camera for frames self.camera = Camera(port=video_port, pitch=self.pitch) frame = self.camera.get_frame() center_point = self.camera.get_adjusted_center(frame) # Set up vision self.calibration = tools.get_colors(pitch) self.vision = Vision( pitch=pitch, color=color, our_side=our_side, frame_shape=frame.shape, frame_center=center_point, calibration=self.calibration) # Set up postprocessing for vision self.postprocessing = Postprocessing() # Set up GUI self.GUI = GUI(calibration=self.calibration, arduino=self.arduino, pitch=self.pitch) # Set up main planner self.planner = Planner(our_side=our_side, pitch_num=self.pitch, our_color=color, gui=self.GUI, comm = self.arduino) self.color = color self.side = our_side self.timeOfNextAction = 0 self.preprocessing = Preprocessing() #it doesn't matter whether it is an Attacker or a Defender Controller self.controller = Attacker_Controller(self.planner._world, self.GUI) self.robot_action_list = [] def wow(self): """ Ready your sword, here be dragons. """ counter = 1L timer = time.clock() try: c = True while c != 27: # the ESC key frame = self.camera.get_frame() pre_options = self.preprocessing.options # Apply preprocessing methods toggled in the UI preprocessed = self.preprocessing.run(frame, pre_options) frame = preprocessed['frame'] if 'background_sub' in preprocessed: cv2.imshow('bg sub', preprocessed['background_sub']) # Find object positions # model_positions have their y coordinate inverted model_positions, regular_positions = self.vision.locate(frame) model_positions = self.postprocessing.analyze(model_positions) # Find appropriate action self.planner.update_world(model_positions) if time.time() >= self.timeOfNextAction: if self.robot_action_list == []: plan = self.planner.plan() if isinstance(plan, list): self.robot_action_list = plan else: self.robot_action_list = [(plan, 0)] if self.controller is not None: self.controller.execute(self.arduino, self) # Information about the grabbers from the world grabbers = { 'our_defender': self.planner._world.our_defender.catcher_area, 'our_attacker': self.planner._world.our_attacker.catcher_area } # Information about states robotState = 'test' # Use 'y', 'b', 'r' to change color. c = waitKey(2) & 0xFF actions = [] fps = float(counter) / (time.clock() - timer) # Draw vision content and actions self.GUI.draw( frame, model_positions, actions, regular_positions, fps, robotState, "we dont need it", '', "we dont need it", grabbers, our_color='blue', our_side=self.side, key=c, preprocess=pre_options) counter += 1 if c == ord('a'): self.arduino.grabberUp() self.arduino.grab() except: if self.controller is not None: self.controller.shutdown(self.arduino) raise finally: # Write the new calibrations to a file. tools.save_colors(self.pitch, self.calibration) if self.controller is not None: self.controller.shutdown(self.arduino)
class Controller: """ Primary source of robot control. Ties vision and planning together. """ def __init__(self, pitch, color, our_side, video_port=0, comm_port="/dev/ttyUSB0", comms=1): """ Entry point for the SDP system. Params: [int] video_port port number for the camera [string] comm_port port number for the arduino [int] pitch 0 - main pitch, 1 - secondary pitch [string] our_side the side we're on - 'left' or 'right' *[int] port The camera port to take the feed from *[Robot_Controller] attacker Robot controller object - Attacker Robot has a RED power wire *[Robot_Controller] defender Robot controller object - Defender Robot has a YELLOW power wire """ assert pitch in [0, 1] assert color in ["yellow", "blue"] assert our_side in ["left", "right"] self.pitch = pitch # Set up the Arduino communications self.arduino = Arduino(comm_port, 115200, 1, comms) # Set up camera for frames self.camera = Camera(port=video_port, pitch=self.pitch) frame = self.camera.get_frame() center_point = self.camera.get_adjusted_center(frame) # Set up vision self.calibration = tools.get_colors(pitch) self.vision = Vision( pitch=pitch, color=color, our_side=our_side, frame_shape=frame.shape, frame_center=center_point, calibration=self.calibration, ) # Set up postprocessing for vision self.postprocessing = Postprocessing() # Set up main planner self.planner = Planner(our_side=our_side, pitch_num=self.pitch) # Set up GUI self.GUI = GUI(calibration=self.calibration, arduino=self.arduino, pitch=self.pitch) self.color = color self.side = our_side self.preprocessing = Preprocessing() self.attacker = Attacker_Controller() self.defender = Defender_Controller() def wow(self): """ Ready your sword, here be dragons. """ counter = 1L timer = time.clock() try: c = True while c != 27: # the ESC key frame = self.camera.get_frame() pre_options = self.preprocessing.options # Apply preprocessing methods toggled in the UI preprocessed = self.preprocessing.run(frame, pre_options) frame = preprocessed["frame"] if "background_sub" in preprocessed: cv2.imshow("bg sub", preprocessed["background_sub"]) # Find object positions # model_positions have their y coordinate inverted model_positions, regular_positions = self.vision.locate(frame) model_positions = self.postprocessing.analyze(model_positions) # Find appropriate action self.planner.update_world(model_positions) attacker_actions = self.planner.plan("attacker") defender_actions = self.planner.plan("defender") if self.attacker is not None: self.attacker.execute(self.arduino, attacker_actions) if self.defender is not None: self.defender.execute(self.arduino, defender_actions) # Information about the grabbers from the world grabbers = { "our_defender": self.planner._world.our_defender.catcher_area, "our_attacker": self.planner._world.our_attacker.catcher_area, } # Information about states attackerState = (self.planner.attacker_state, self.planner.attacker_strat_state) defenderState = (self.planner.defender_state, self.planner.defender_strat_state) # Use 'y', 'b', 'r' to change color. c = waitKey(2) & 0xFF actions = [] fps = float(counter) / (time.clock() - timer) # Draw vision content and actions self.GUI.draw( frame, model_positions, actions, regular_positions, fps, attackerState, defenderState, attacker_actions, defender_actions, grabbers, our_color=self.color, our_side=self.side, key=c, preprocess=pre_options, ) counter += 1 except: if self.defender is not None: self.defender.shutdown(self.arduino) if self.attacker is not None: self.attacker.shutdown(self.arduino) raise finally: # Write the new calibrations to a file. tools.save_colors(self.pitch, self.calibration) if self.attacker is not None: self.attacker.shutdown(self.arduino) if self.defender is not None: self.defender.shutdown(self.arduino)