def __init__(self, pitch, ourTeamColor='yellow', otherTeamColor='blue', ballColor='red', record = False, extras = False): self.pitch = pitch self.camera = Camera(pitch=pitch) self.calibration = tools.get_colors(pitch) self.tracker = MyTracker(self.calibration, self.pitch, extras=extras) #self.tracker = MyTracker([ourTeamColor, otherTeamColor, ballColor, 'pink', 'bright_green'], self.calibration) self.points = {} # point dictionary for tracked colors self.GUI_name = "Killer Robot App" self.calibration_gui= CameraCalibrationGUI(calibration=self.calibration , name=self.GUI_name) self.record = record
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 = []
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)
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 VisionWrapper(object): def __init__(self, pitch, ourTeamColor='yellow', otherTeamColor='blue', ballColor='red', record = False, extras = False): self.pitch = pitch self.camera = Camera(pitch=pitch) self.calibration = tools.get_colors(pitch) self.tracker = MyTracker(self.calibration, self.pitch, extras=extras) #self.tracker = MyTracker([ourTeamColor, otherTeamColor, ballColor, 'pink', 'bright_green'], self.calibration) self.points = {} # point dictionary for tracked colors self.GUI_name = "Killer Robot App" self.calibration_gui= CameraCalibrationGUI(calibration=self.calibration , name=self.GUI_name) self.record = record def run(self): subprocess.call("vision/xawtv.sh") if (self.record == True): fourcc = cv2.VideoWriter_fourcc('M','J','P','G') output_name = 'output_' + str( tools.current_milli_time() ) + '.avi' out = cv2.VideoWriter(output_name, fourcc, 24, (640,480)) images = [] while(True): time = tools.current_milli_time() # Update trackbars self.calibration_gui.update() # get updated camera settings settings = self.calibration_gui.getSettings() # Update camera settings self.camera.reset_camera_settings(settings=settings) # capture frame by frame frame = self.camera.get_frame() if (self.record == True): out.write(frame) #images.append(frame) #if len(images) > 2: # images = images[1:] image = frame; #if len(images) == 2: # image = cv2.addWeighted(images[0], 0.5 ,images[1], 0.5, 0) # temp2 = cv2.addWeighted(images[2], 0.5 ,images[3], 0.5, 0) # image = cv2.addWeighted(temp1, 0.5 ,temp2, 0.5, 0) # Get tracked object points self.points, modified_frame = self.tracker.get_world_state(image) # TODO: think about tracked points sometimes jerking arround # maybe get a threshold of how far of a distance change should # be accounted # TODO: implement functions to find robots using points # dictionary # TODO: some rudamentary logic can be implemented here # along with connection to the robot # Better to have two classes, one for robot logic # Another for communication handling World.set_points(self.points) w = World.get_world() # Separate field in 6x4 for strategy debugging for l1 in range(0,640,107): cv2.line(image, (l1,0), (l1,480), (255,255,255), thickness=1) for l2 in range(0,480,120): cv2.line(image, (0,l2), (640,l2), (255,255,255), thickness=1) for i3 , robot in enumerate(w.robots): if robot != None: #print "Angle:" + str(robot.rot) rads = radians(robot.rot) x1, y1 = (robot.x_px, robot.y_px) x2 = int(x1 + cos(rads) * 50); y2 = int(y1 + sin(rads) * 50); cv2.arrowedLine(image, (int(x1), int(y1)), (x2,y2), BGR_COMMON[ World.our_team_colour if i3 < 2 else World.oposite_team_colour], 2) corners = [(int(x1 + cos(rads + radians(45) + 90*pi/180*i)*20) , (int(y1 + sin(rads + radians(45)+ 90*pi/180*i)*20))) for i in range(4)] cv2.putText (img=image, text=str(i3), org=(int(x1-10), int(y1+10)), fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=1, color=(255,255,255)) for i2 in range(4): col = BGR_COMMON[ World.our_primary if i3 % 2 == 0 else World.other_primary] cv2.line(image, corners[i2] , corners[(i2+1)%4] , col , 2) if w.ball != None : ball = World.cm_to_px(w.ball.x , w.ball.y) ball = (int(ball[0]) , int(ball[1]) ) cv2.circle(image , (ball[0] , ball[1]) , 10 , (255,0,0)) cv2.arrowedLine(image , (ball[0] , ball[1]) ,(int(ball[0] + w.ball.dir[0] ), int(ball[1] + w.ball.dir[1])) ,(255, 0 ,0) , 2) #cv2.imshow('test', frame) #cv2.waitKey(0) if cv2.waitKey(1) & 0xFF == ord('q'): break # display FPS time = ((tools.current_milli_time()-time))/1000.0 fps = 1.0 / time cv2.putText(image, "FPS: "+str(round(fps)), (10,20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, 255) cv2.imshow(self.GUI_name, image) # Display the resulting frame #cv2.imshow('Killer Robot App', image) # When everything is done, release the capture out.release() cv2.destroyAllWindows()
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)