Ejemplo n.º 1
0
    def __init__(self, pitchnum, stdout, sourcefile, resetPitchSize, resetThresholds, displayBlur, normalizeAtStartup, noDribbling):
               
        self.running = True
        self.connected = False
   
        self.stdout = stdout 

        if sourcefile is None:  
            self.cap = Camera()
        else:
            filetype = 'video'
            if sourcefile.endswith(('jpg', 'png')):
                filetype = 'image'

            self.cap = VirtualCamera(sourcefile, filetype)
        
        calibrationPath = os.path.join('calibration', 'pitch{0}'.format(pitchnum))
        self.cap.loadCalibration(os.path.join(sys.path[0], calibrationPath))

        self.preprocessor = Preprocessor(pitchnum, resetPitchSize)
        if self.preprocessor.hasPitchSize:
            self.gui = Gui(self.preprocessor.pitch_size)
        else:
            self.gui = Gui()
        self.threshold = Threshold(pitchnum, resetThresholds, displayBlur, normalizeAtStartup)
        self.thresholdGui = ThresholdGui(self.threshold, self.gui)
        self.features = Features(self.gui, self.threshold)
        self.filter = Filter(noDribbling)
        
        eventHandler = self.gui.getEventHandler()
        eventHandler.addListener('q', self.quit)

        while self.running:
            try:
                if not self.stdout:
                    self.connect()
                else:
                    self.connected = True

                if self.preprocessor.hasPitchSize:
                    self.outputPitchSize()
                    self.gui.setShowMouse(False)
                else:
                    eventHandler.setClickListener(self.setNextPitchCorner)

                while self.running:
                    self.doStuff()

            except socket.error:
                self.connected = False
                # If the rest of the system is not up yet/gets quit,
                # just wait for it to come available.
                time.sleep(1)

                # Strange things seem to happen to X sometimes if the
                # display isn't updated for a while
                self.doStuff()

        if not self.stdout:
            self.socket.close()
Ejemplo n.º 2
0
    def __init__(self, pitchnum, stdout, sourcefile, resetPitchSize, noGui, debug_window,  pipe):

        self.noGui = noGui
        self.lastFrameTime = self.begin_time = time.time()
        self.processed_frames = 0

        self.running = True
        self.stdout = stdout

        self.pipe = pipe
        

        
        if sourcefile is None:
            self.camera = Camera()
        else:
            self.filetype = 'video'
            if sourcefile.endswith(('jpg', 'png')):
                self.filetype = 'image'

        self.gui = Gui(self.noGui)
        self.threshold = Threshold(pitchnum)
        self.thresholdGui = ThresholdGui(self.threshold, self.gui)
        self.preprocessor = Preprocessor(resetPitchSize)
        self.features = Features(self.gui, self.threshold)
        # if self.debug_window:
        #     self.debug_window = DebugWindow()
        # else:
        #     self.debug_window = None

        calibrationPath = os.path.join('calibration', 'pitch{0}'.format(pitchnum))
        self.camera.loadCalibration(os.path.join(sys.path[0], calibrationPath))

        eventHandler = self.gui.getEventHandler()
        eventHandler.addListener('q', self.quit)

        # Ugly stuff for smoothing coordinates - should probably move it
        self._pastSize = 5
        self._pastCoordinates = {
                            'yellow': [(0, 0)] * self._pastSize,
                            'blue': [(0, 0)] * self._pastSize,
                            'ball': [(0, 0)] * self._pastSize
                            }
        self._pastAngles = {
                            'yellow': [1.0] * self._pastSize,
                            'blue': [1.0] * self._pastSize
                           }

        while self.running:
            if self.preprocessor.hasPitchSize:
                self.outputPitchSize()
                self.gui.setShowMouse(False)
            else:
                eventHandler.setClickListener(self.setNextPitchCorner)

            while self.running:
                self.doStuff()
Ejemplo n.º 3
0
class Vision:

    def __init__(self, pitchnum, stdout, sourcefile, resetPitchSize, noGui, debug_window,  pipe):

        self.noGui = noGui
        self.lastFrameTime = self.begin_time = time.time()
        self.processed_frames = 0

        self.running = True
        self.stdout = stdout

        self.pipe = pipe
        

        
        if sourcefile is None:
            self.camera = Camera()
        else:
            self.filetype = 'video'
            if sourcefile.endswith(('jpg', 'png')):
                self.filetype = 'image'

        self.gui = Gui(self.noGui)
        self.threshold = Threshold(pitchnum)
        self.thresholdGui = ThresholdGui(self.threshold, self.gui)
        self.preprocessor = Preprocessor(resetPitchSize)
        self.features = Features(self.gui, self.threshold)
        # if self.debug_window:
        #     self.debug_window = DebugWindow()
        # else:
        #     self.debug_window = None

        calibrationPath = os.path.join('calibration', 'pitch{0}'.format(pitchnum))
        self.camera.loadCalibration(os.path.join(sys.path[0], calibrationPath))

        eventHandler = self.gui.getEventHandler()
        eventHandler.addListener('q', self.quit)

        # Ugly stuff for smoothing coordinates - should probably move it
        self._pastSize = 5
        self._pastCoordinates = {
                            'yellow': [(0, 0)] * self._pastSize,
                            'blue': [(0, 0)] * self._pastSize,
                            'ball': [(0, 0)] * self._pastSize
                            }
        self._pastAngles = {
                            'yellow': [1.0] * self._pastSize,
                            'blue': [1.0] * self._pastSize
                           }

        while self.running:
            if self.preprocessor.hasPitchSize:
                self.outputPitchSize()
                self.gui.setShowMouse(False)
            else:
                eventHandler.setClickListener(self.setNextPitchCorner)

            while self.running:
                self.doStuff()

    def quit(self):
        self.running = False
        self.pipe.send('q')

    def print_fps(self):
        thisFrameTime = time.time()
        time_diff = thisFrameTime - self.lastFrameTime
        fps = 1.0 / time_diff
        self.processed_frames = self.processed_frames + 1
        avg_fps = self.processed_frames * 1.0 / (thisFrameTime - self.begin_time)
        self.lastFrameTime = thisFrameTime

        if self.stdout:
            print("Instantaneous fps = %f Average fps = %f" % (fps, avg_fps))

    def doStuff(self):

        frame = self.camera.getImageUndistort()

        # Uncomment to see changes in barrell distortion matrix
        # calibrationPath = os.path.join('calibration', 'pitch{0}'.format(0))
        # self.camera.loadCalibration(os.path.join(sys.path[0], calibrationPath))

        frame = self.preprocessor.preprocess(frame)

        self.gui.updateLayer('raw', frame)

        ents = self.features.extractFeatures(frame)
        self.outputEnts(ents)

        self.print_fps()

        self.gui.loop()

    def setNextPitchCorner(self, where):
        self.preprocessor.setNextPitchCorner(where)

        if self.preprocessor.hasPitchSize:
            self.outputPitchSize()
            self.gui.setShowMouse(False)
            self.gui.updateLayer('corner', None)
        else:
            self.gui.drawCrosshair(where, 'corner')

    def outputPitchSize(self):
        if self.stdout:
            print ("Pitch size:\t %i\t %i\n" % tuple(self.preprocessor.pitch_size))
        # if self.debug_window:
        #     self.debug_window.insert_text("Pitch size:\t %i\t %i\n" % tuple(self.preprocessor.pitch_size))
        self.pipe.send(InitSignal(self.preprocessor.pitch_size[0], self.preprocessor.pitch_size[1]))

    def addCoordinates(self, entity, coordinates):
	self._pastCoordinates[entity].pop(0)
        self._pastCoordinates[entity].append(coordinates)
	
        #(x, y) = coordinates;
        
        # if the frame is bad(-1) then add the most recent coordinate instead
        #if (x != -1):
        #    self._pastCoordinates[entity].append(coordinates)
        #else:
        #    self._pastCoordinates[entity].append(self._pastCoordinates[entity][-1])

    def smoothCoordinates(self, entity):
        x = sum(map(lambda (x, _): x, self._pastCoordinates[entity])) / self._pastSize
        y = sum(map(lambda (_, y): y, self._pastCoordinates[entity])) / self._pastSize
        return (x, y)

    def addAngle(self, entity, angle):
        self._pastAngles[entity].pop(0)
        self._pastAngles[entity].append(angle)
        # if the frame is bad(-1) then add the most recent angle instead
        # good angle is always in (0,2pi), bad angle is -1, careful with real number
        #if (angle > -0.5):
        #    self._pastAngles[entity].append(angle)
        #else:
        #    self._pastAngles[entity].append(self._pastAngles[entity][-1])


    def smoothAngle(self, entity):
        # angle is periodic (of 2pi) and a bit tricky to smooth
        temp = sorted (self._pastAngles[entity])
        
        # if max_angle > min_angle > pi, those angles are crossing 0
        # we must add a period to the small ones
        if (temp[-1] - temp[0] > math.pi):
            temp = map(lambda angle: angle + 2*math.pi if angle < math.pi else angle, temp)
        
        return sum(temp) / self._pastSize
        
    # add/substract period (2pi) so angle is always in (0,2pi)
    # assume they are off by at most a period
    def standardize_angle(self, angle):
        if (angle > 2*math.pi):
            return angle - 2*math.pi
        if (angle < 0):
            return angle + 2*math.pi
        return angle
            
    def outputEnts(self, ents):
        # Messyyy
        if not self.preprocessor.hasPitchSize:
            return

        msg_data = []

        for name in ['yellow', 'blue', 'ball']:
            entity = ents[name]
            coordinates = entity.coordinates()
            
            # This is currently not needed
            # if the frame is not recognized, skip a maximum of self.max_skip times
            #if (coordinates[0] != -1):
            #    self.addCoordinates(name, coordinates)
            #    self.skip = 0
            #else:
            #    if (self.skip < self.max_skip):
            #        self.skip = self.skip + 1;
            #    else:
            #        self.addCoordinates(name, coordinates)
                    
            self.addCoordinates(name, coordinates)
            x, y = self.smoothCoordinates(name)

            # TODO: The system needs (0, 0) at top left!
            if y != -1:
                y = self.preprocessor.pitch_size[1] - y

            if name == 'ball':
                # self.send('{0} {1} '.format(x, y))
                msg_data += [int(x), int(y)]
                #print (self._pastCoordinates[name])
                #print(coordinates)
            else:
                # angle is currently clockwise, this makes it anti-clockwise
                angle = self.standardize_angle( 2*math.pi - entity.angle() )
                
                self.addAngle(name, angle)
                angle = self.standardize_angle ( self.smoothAngle(name) );
                
                msg_data += [int(x), int(y), angle]
                


        msg_data.append(int(time.time() * 1000))
        data = FrameData(*msg_data)

        if self.stdout:
            print ("Yellow:\t %i\t %i\t Angle:\t %s\nBlue:\t %i\t %i\t Angle:\t %s\nBall:\t %i\t %i\t\nTime:\t %i\n" % tuple(msg_data))
        # if debug_window:
        #     debug_window.insert_text("Yellow:\t %i\t %i\t Angle:\t %s\nBlue:\t %i\t %i\t Angle:\t %s\nBall:\t %i\t %i\t\nTime:\t %i\n" % tuple(msg_data))

        self.pipe.send(data)
Ejemplo n.º 4
0
class Vision:
    
    def __init__(self, pitchnum, stdout, sourcefile, resetPitchSize):
               
        self.running = True
        self.connected = False
        
        self.stdout = stdout 

        if sourcefile is None:  
            self.cap = Camera()
        else:
            filetype = 'video'
            if sourcefile.endswith(('jpg', 'png')):
                filetype = 'image'

            self.cap = VirtualCamera(sourcefile, filetype)
        
        calibrationPath = os.path.join('calibration', 'pitch{0}'.format(pitchnum))
        self.cap.loadCalibration(os.path.join(sys.path[0], calibrationPath))

        self.gui = Gui()
        self.threshold = Threshold(pitchnum)
        self.thresholdGui = ThresholdGui(self.threshold, self.gui)
        self.preprocessor = Preprocessor(resetPitchSize)
        self.features = Features(self.gui, self.threshold)
        
        eventHandler = self.gui.getEventHandler()
        eventHandler.addListener('q', self.quit)

        while self.running:
            try:
                if not self.stdout:
                    self.connect()
                else:
                    self.connected = True

                if self.preprocessor.hasPitchSize:
                    self.outputPitchSize()
                    self.gui.setShowMouse(False)
                else:
                    eventHandler.setClickListener(self.setNextPitchCorner)

                while self.running:
                    self.doStuff()

            except socket.error:
                self.connected = False
                # If the rest of the system is not up yet/gets quit,
                # just wait for it to come available.
                time.sleep(1)

                # Strange things seem to happen to X sometimes if the
                # display isn't updated for a while
                self.doStuff()

        if not self.stdout:
            self.socket.close()
        
    def connect(self):
        print("Attempting to connect...")
        self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.socket.connect( (HOST, PORT) )
        self.connected = True

    def quit(self):
        self.running = False
        
    def doStuff(self):
        if self.cap.getCameraMatrix is None:
            frame = self.cap.getImage()
        else:
            frame = self.cap.getImageUndistort()

        frame = self.preprocessor.preprocess(frame)
        
        self.gui.updateLayer('raw', frame)

        ents = self.features.extractFeatures(frame)
        self.outputEnts(ents)

        self.gui.loop()

    def setNextPitchCorner(self, where):
        self.preprocessor.setNextPitchCorner(where)
        
        if self.preprocessor.hasPitchSize:
            print("Pitch size: {0!r}".format(self.preprocessor.pitch_size))
            self.outputPitchSize()
            self.gui.setShowMouse(False)
            self.gui.updateLayer('corner', None)
        else:
            self.gui.drawCrosshair(where, 'corner')
    
    def outputPitchSize(self):
        print(self.preprocessor.pitch_size)
        self.send('{0} {1} {2} \n'.format(
                PITCH_SIZE_BIT, self.preprocessor.pitch_size[0], self.preprocessor.pitch_size[1]))

    def outputEnts(self, ents):

        # Messyyy
        if not self.connected or not self.preprocessor.hasPitchSize:
            return

        self.send("{0} ".format(ENTITY_BIT))

        for name in ['yellow', 'blue', 'ball']:
            entity = ents[name]
            x, y = entity.coordinates()

            # The rest of the system needs (0, 0) at the bottom left
            if y != -1:
                y = self.preprocessor.pitch_size[1] - y

            if name == 'ball':
                self.send('{0} {1} '.format(x, y))
            else:
                angle = 360 - (((entity.angle() * (180/math.pi)) - 360) % 360)
                self.send('{0} {1} {2} '.format(x, y, angle))

        self.send(str(int(time.time() * 1000)) + " \n")
        
    def send(self, string):
        if self.stdout:
            sys.stdout.write(string)
        else:
            self.socket.send(string)