def gotFrame(self, img): # Set up parameters outImg = None # Preprocessing img = cv2.resize(img, (640, 480)) rawImg = vision.shadesOfGray(img) blurImg = cv2.GaussianBlur(rawImg, ksize=(0, 0), sigmaX=10) enhancedImg = cv2.addWeighted(rawImg, 2.5, blurImg, -1.5, 0) enhancedImg = cv2.GaussianBlur(enhancedImg, ksize=(3, 3), sigmaX=2) hsvImg = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) hsvImg = self.normalise(hsvImg) bluredimg = cv2.GaussianBlur(enhancedImg, ksize=(5, 5), sigmaX=2) bluredimg = cv2.cvtColor(bluredimg, cv2.COLOR_BGR2HSV) bluredimg = self.normalise(bluredimg) # return cv2.cvtColor(hsvImg, cv2.COLOR_HSV2BGR) # Find red image redImg = self.threshold(hsvImg, bluredimg, "RED") outImg = redImg # Find green image #greenLen, greenImg = self.threshold(img, "GREEN") # Find blue image #blueLen, blueImg = self.threshold(img, "BLUE") #outImg = redImg | greenImg | blueImg return outImg
def gotFrame(self, img): # Set up parameters outImg = None # Preprocessing img = cv2.resize(img, (640, 480)) rawImg = vision.shadesOfGray(img) blurImg = cv2.GaussianBlur(rawImg, ksize=(0, 0), sigmaX=10) enhancedImg = cv2.addWeighted(rawImg, 2.5, blurImg, -1.5, 0) hsvImg = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) hsvImg = self.normalise(hsvImg) # return cv2.cvtColor(img, cv2.COLOR_HSV2BGR) # Find red image redImg = self.threshold(hsvImg, "RED") outImg = redImg # Find green image #greenLen, greenImg = self.threshold(img, "GREEN") # Find blue image #blueLen, blueImg = self.threshold(img, "BLUE") #outImg = redImg | greenImg | blueImg return outImg
def threshold(self, img, color): self.allCentroidList = [] self.allAreaList = [] self.allRadiusList = [] #params = self.getParams(color) # Perform thresholding mask = cv2.inRange(img, self.redParams['lo3'], self.redParams['hi3']) kern = cv2.getStructuringElement(cv2.MORPH_RECT, (5,5)) # return cv2.cvtColor(mask, cv2.COLOR_GRAY2BGR) thresImg1 = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kern) # thresImg1 = cv2.dilate(mask, kern, iterations=1) mask2 = cv2.inRange(img, self.redParams['lo4'], self.redParams['hi4']) thresImg2 = cv2.dilate(mask2, kern, iterations=2) # binImg = cv2.bitwise_or(thresImg1, thresImg2) binImg = thresImg1 # return cv2.cvtColor(binImg, cv2.COLOR_GRAY2BGR) # Find contours scratchImg = binImg.copy() scratchImgCol = cv2.cvtColor(binImg, cv2.COLOR_GRAY2BGR) contours, hierachy = cv2.findContours(scratchImg, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE) contours = filter(lambda c: cv2.contourArea(c) > self.minContourArea, contours) sorted(contours, key=cv2.contourArea, reverse=True) # Sort by largest contour # If centering, just find the center of largest contour if self.comms.isCentering: if len(contours) > 0: largestContour = contours[0] mu = cv2.moments(largestContour) muArea = mu['m00'] self.comms.centroidToBump = (int(mu['m10']/muArea), int(mu['m01']/muArea)) self.comms.rectArea = muArea self.previousCentroid = self.comms.centroidToBump self.previousArea = self.comms.rectArea else: self.comms.centroidToBump = self.previousCentroid self.comms.rectArea = self.previousArea else: # Find hough circles circles = cv2.HoughCircles(binImg, cv2.cv.CV_HOUGH_GRADIENT, 1, minDist=30, param1=self.houghParams[0], param2=self.houghParams[1], minRadius = self.circleParams['minRadius'], maxRadius = self.circleParams['maxRadius']) # Check if center of circles inside contours if contours is not None: for contour in contours: mu = cv2.moments(contour) muArea = mu['m00'] centroid = (mu['m10']/muArea, mu['m01']/muArea) if circles is not None: for circle in circles[0,:,:]: circleCentroid = (circle[0], circle[1]) if abs((Utils.distBetweenPoints(centroid,circleCentroid))) < circle[2]: self.comms.foundBuoy = True # Find new centroid by averaging the centroid and circle centroid newCentroid =(int(centroid[0]+circleCentroid[0])/2, int(centroid[1]+circleCentroid[1])/2) self.allCentroidList.append(newCentroid) self.allAreaList.append(cv2.contourArea(contour)) self.allRadiusList.append(circle[2]) # Draw circles cv2.circle(scratchImgCol, newCentroid, circle[2], (255, 255, 0), 2) cv2.circle(scratchImgCol, newCentroid, 2, (255, 0, 255), 3) # Find the circle with the largest radius if not len(self.allCentroidList) == 0: maxIndex = self.allRadiusList.index(max(self.allRadiusList)) self.comms.centroidToBump = self.allCentroidList[maxIndex] self.comms.rectArea = self.allAreaList[maxIndex] self.previousCentroid = self.comms.centroidToBump self.previousArea = self.comms.rectArea self.comms.grad = self.getGradient() else: self.comms.centroidToBump = self.previousCentroid self.comms.rectArea = self.previousArea cv2.putText(scratchImgCol, "Ang: " + str(self.comms.grad), (30, 100), cv2.FONT_HERSHEY_PLAIN, 1, (204,204,204)) # Draw new centroid cv2.circle(scratchImgCol, self.comms.centroidToBump, 3, (0, 255, 255), 2) # rospy.loginfo("Area: {}".format(self.comms.rectArea)) # To put on the scratchImg cv2.putText(scratchImgCol, "Area: " + str(self.comms.rectArea), (30, 80), cv2.FONT_HERSHEY_PLAIN, 1, (204, 204, 204)) # How far centroid is off screen center self.comms.deltaX = float((self.comms.centroidToBump[0] - vision.screen['width']/2)*1.0/ vision.screen['width']) cv2.putText(scratchImgCol, "X " + str(self.comms.deltaX), (30,30), cv2.FONT_HERSHEY_PLAIN, 1, (204, 204, 204)) self.comms.deltaY = float((self.comms.centroidToBump[1] - vision.screen['height']/2)*1.0/ vision.screen['height']) cv2.putText(scratchImgCol, "Y " + str(self.comms.deltaY), (30,60), cv2.FONT_HERSHEY_PLAIN, 1, (204, 204, 204)) # Draw center rect scratchImgCol = vision.drawCenterRect(scratchImgCol) return scratchImgCol
def threshold(self, img, color): self.allCentroidList = [] self.allAreaList = [] self.allRadiusList = [] #params = self.getParams(color) # Perform thresholding mask = cv2.inRange(img, self.redParams['lo3'], self.redParams['hi3']) kern = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) # return cv2.cvtColor(mask, cv2.COLOR_GRAY2BGR) thresImg1 = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kern) # thresImg1 = cv2.dilate(mask, kern, iterations=1) mask2 = cv2.inRange(img, self.redParams['lo4'], self.redParams['hi4']) thresImg2 = cv2.dilate(mask2, kern, iterations=2) # binImg = cv2.bitwise_or(thresImg1, thresImg2) binImg = thresImg1 # return cv2.cvtColor(binImg, cv2.COLOR_GRAY2BGR) # Find contours scratchImg = binImg.copy() scratchImgCol = cv2.cvtColor(binImg, cv2.COLOR_GRAY2BGR) contours, hierachy = cv2.findContours(scratchImg, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE) contours = filter(lambda c: cv2.contourArea(c) > self.minContourArea, contours) sorted(contours, key=cv2.contourArea, reverse=True) # Sort by largest contour # If centering, just find the center of largest contour if self.comms.isCentering: if len(contours) > 0: largestContour = contours[0] mu = cv2.moments(largestContour) muArea = mu['m00'] self.comms.centroidToBump = (int(mu['m10'] / muArea), int(mu['m01'] / muArea)) self.comms.rectArea = muArea self.previousCentroid = self.comms.centroidToBump self.previousArea = self.comms.rectArea else: self.comms.centroidToBump = self.previousCentroid self.comms.rectArea = self.previousArea else: # Find hough circles circles = cv2.HoughCircles( binImg, cv2.cv.CV_HOUGH_GRADIENT, 1, minDist=30, param1=self.houghParams[0], param2=self.houghParams[1], minRadius=self.circleParams['minRadius'], maxRadius=self.circleParams['maxRadius']) # Check if center of circles inside contours if contours is not None: for contour in contours: mu = cv2.moments(contour) muArea = mu['m00'] centroid = (mu['m10'] / muArea, mu['m01'] / muArea) if circles is not None: for circle in circles[0, :, :]: circleCentroid = (circle[0], circle[1]) if abs((Utils.distBetweenPoints( centroid, circleCentroid))) < circle[2]: self.comms.foundBuoy = True # Find new centroid by averaging the centroid and circle centroid newCentroid = ( int(centroid[0] + circleCentroid[0]) / 2, int(centroid[1] + circleCentroid[1]) / 2) self.allCentroidList.append(newCentroid) self.allAreaList.append( cv2.contourArea(contour)) self.allRadiusList.append(circle[2]) # Draw circles cv2.circle(scratchImgCol, newCentroid, circle[2], (255, 255, 0), 2) cv2.circle(scratchImgCol, newCentroid, 2, (255, 0, 255), 3) # Find the circle with the largest radius if not len(self.allCentroidList) == 0: maxIndex = self.allRadiusList.index(max(self.allRadiusList)) self.comms.centroidToBump = self.allCentroidList[maxIndex] self.comms.rectArea = self.allAreaList[maxIndex] self.previousCentroid = self.comms.centroidToBump self.previousArea = self.comms.rectArea self.comms.grad = self.getGradient() else: self.comms.centroidToBump = self.previousCentroid self.comms.rectArea = self.previousArea cv2.putText(scratchImgCol, "Ang: " + str(self.comms.grad), (30, 100), cv2.FONT_HERSHEY_PLAIN, 1, (204, 204, 204)) # Draw new centroid cv2.circle(scratchImgCol, self.comms.centroidToBump, 3, (0, 255, 255), 2) # rospy.loginfo("Area: {}".format(self.comms.rectArea)) # To put on the scratchImg cv2.putText(scratchImgCol, "Area: " + str(self.comms.rectArea), (30, 80), cv2.FONT_HERSHEY_PLAIN, 1, (204, 204, 204)) # How far centroid is off screen center self.comms.deltaX = float( (self.comms.centroidToBump[0] - vision.screen['width'] / 2) * 1.0 / vision.screen['width']) cv2.putText(scratchImgCol, "X " + str(self.comms.deltaX), (30, 30), cv2.FONT_HERSHEY_PLAIN, 1, (204, 204, 204)) self.comms.deltaY = float( (self.comms.centroidToBump[1] - vision.screen['height'] / 2) * 1.0 / vision.screen['height']) cv2.putText(scratchImgCol, "Y " + str(self.comms.deltaY), (30, 60), cv2.FONT_HERSHEY_PLAIN, 1, (204, 204, 204)) # Draw center rect scratchImgCol = vision.drawCenterRect(scratchImgCol) return scratchImgCol
def gotFrame(self, img): allCentroidList = [] allRadiusList = [] img = cv2.resize(img, (640, 480)) # img = self.illumMask(img) # img = self.whiteBal(img) # img = self.illuminanceRemoval(img) # hsvImg = self.toHSV(img) # enhancedImg = self.enhance(hsvImg) # return cv2.cvtColor(enhancedImg, cv2.COLOR_HSV2BGR) labImg = cv2.cvtColor(img, cv2.COLOR_BGR2LAB) channels = cv2.split(labImg) # return cv2.cvtColor(channels[2], cv2.COLOR_GRAY2BGR) # Threshold out something binImg = self.morphology( cv2.inRange(labImg, self.greenParams['lo'], self.greenParams['hi'])) kern = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) binImg = cv2.morphologyEx(binImg, cv2.MORPH_OPEN, kern) # return cv2.cvtColor(binImg, cv2.COLOR_GRAY2BGR) # binImg = self.morphology(cv2.inRange(enhancedImg, # self.greenParams['lo'], self.greenParams['hi'])) # erodeKern = cv2.getStructuringElement(cv2.MORPH_RECT, (5,5)) # binImg = cv2.erode(binImg, erodeKern, iterations=2) dilateKern = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3)) binImg = cv2.dilate(binImg, dilateKern, iterations=3) # Find contours and fill them for i in range(4): binImgCopy = binImg.copy() contours, hierarchy = cv2.findContours(binImgCopy, cv2.RETR_CCOMP, cv2.CHAIN_APPROX_NONE) cv2.drawContours(image=binImg, contours=contours, contourIdx=-1, color=(255, 255, 255), thickness=-1) ''' Detect the board first ''' # Detecting the board # Find the largest contours and make sure its a square scratchImgCol = cv2.cvtColor(binImg, cv2.COLOR_GRAY2BGR) binImgCopy = binImg.copy() contours, hierarchy = cv2.findContours(binImgCopy, cv2.RETR_CCOMP, cv2.CHAIN_APPROX_NONE) contours = filter(lambda c: cv2.contourArea(c) > self.minContourArea, contours) if len(contours) == 0: return scratchImgCol self.comms.foundSomething = True sorted(contours, key=cv2.contourArea, reverse=True) largestContour = contours[0] rect = cv2.minAreaRect(largestContour) ((center_x, center_y), (width, height), angle) = cv2.minAreaRect(largestContour) box = cv2.cv.BoxPoints(rect) box = np.int0(box) cv2.drawContours(scratchImgCol, [box], 0, (0, 0, 255), 2) # Find centroid of rect returned mu = cv2.moments(largestContour) muArea = mu['m00'] tempBoardCentroid = (int(mu['m10'] / muArea), int(mu['m01'] / muArea)) tempBoardArea = muArea # self.comms.skew = mu['m30']/(pow(mu['02'],1.5)) self.comms.boardCentroid = tempBoardCentroid self.comms.boardArea = tempBoardArea # Dist where centroid of board is off self.comms.boardDeltaX = float( (self.comms.boardCentroid[0] - vision.screen['width'] / 2) * 1.0 / vision.screen['width']) self.comms.boardDeltaY = float( (self.comms.boardCentroid[1] - vision.screen['height'] / 2) * 1.0 / vision.screen['height']) cv2.putText(scratchImgCol, "Board Area: " + str(self.comms.boardArea), (410, 30), cv2.FONT_HERSHEY_PLAIN, 1, (204, 204, 204)) cv2.circle(scratchImgCol, self.comms.boardCentroid, 2, (0, 0, 255), 2) cv2.putText(scratchImgCol, "Board X: " + str(self.comms.boardDeltaX), (410, 60), cv2.FONT_HERSHEY_PLAIN, 1, (204, 204, 204)) cv2.putText(scratchImgCol, "Board Y: " + str(self.comms.boardDeltaY), (410, 80), cv2.FONT_HERSHEY_PLAIN, 1, (204, 204, 204)) binImgCopy = binImg.copy() contours, hierarchy = cv2.findContours(binImgCopy, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) if len(contours) > 0: self.comms.foundCircles = True if not self.comms.isCenteringState: contours = filter(lambda c: cv2.contourArea(c) < 10000, contours) for contour in contours: # if cv2.contourArea(contour) < 10000 and cv2.contourArea(contour) > self.minContourArea: if cv2.contourArea(contour) > self.minContourArea: (cx, cy), radius = cv2.minEnclosingCircle(contour) cv2.drawContours(scratchImgCol, [contour], 0, (255, 0, 255), 2) cv2.circle(scratchImgCol, (int(cx), int(cy)), int(radius), (255, 255, 0), 2) cv2.circle(scratchImgCol, (int(cx), int(cy)), 1, (255, 0, 255), 2) allCentroidList.append((cx, cy, radius)) if contours is None: self.comms.foundCount = self.comms.foundCount + 1 return scratchImgCol sorted(allCentroidList, key=lambda centroid: centroid[2], reverse=True) ''' Detect the circles ''' ''' # Find Hough circles - used to be edges scratchImg = binImg.copy() circles = cv2.HoughCircles(scratchImg, cv2.cv.CV_HOUGH_GRADIENT, 1, minDist=10, param1=self.houghParams[0], param2=self.houghParams[1], minRadius = self.circleParams['minRadius'], maxRadius = self.circleParams['maxRadius']) if circles is None: self.comms.foundCount = self.comms.foundCount + 1 return scratchImgCol self.comms.foundCircles = True circlesSorted = np.array(sorted(circles[0], key=lambda x:x[2], reverse=True)) for circle in circlesSorted: circleCentroid = (circle[0], circle[1]) # Only find circles within the bounding rect if self.isInRect(circleCentroid, (center_x, center_y), (width, height)): allCentroidList.append(circleCentroid) allRadiusList.append(circle[2]) # Draw Circles cv2.circle(scratchImgCol, circleCentroid, circle[2], (255, 255, 0), 2) cv2.circle(scratchImgCol, circleCentroid, 2, (255, 0, 255), 3) ''' # Centroid resetted if self.comms.centroidToShoot is None: ''' # Pick the largest circle if self.comms.numShoot == 0 or len(circles) < 2: self.comms.centroidToShoot = (circlesSorted[0][0], circlesSorted[0][1]) self.comms.radius = circlesSorted[0][2] elif self.comms.numShoot == 1: self.comms.centroidToShoot = (circlesSorted[1][0], circlesSorted[1][1]) self.comms.radius = circlesSorted[1][2] ''' if self.comms.numShoot == 0: self.comms.centroidToShoot = (int(allCentroidList[0][0]), int(allCentroidList[0][1])) self.comms.radius = allCentroidList[0][2] elif self.comms.numShoot == 1: self.comms.centroidToShoot = (int(allCentroidList[1][0]), int(allCentroidList[1][1])) self.comms.radius = allCentroidList[1][2] else: # Find the centroid closest to the previous if len(allCentroidList) != 0: for centroid in allCentroidList: distDiff = [] distDiff.append( Utils.distBetweenPoints(self.previousCentroid, (centroid[0], centroid[1]))) minIndex = distDiff.index(min(distDiff)) self.comms.centroidToShoot = ( int(allCentroidList[minIndex][0]), int(allCentroidList[minIndex][1])) self.comms.radius = allCentroidList[minIndex][2] # self.comms.radius = allRadiusList[minIndex] else: # If not then just use the previous one self.comms.centroidToShoot = self.previousCentroid self.comms.radius = self.previousRadius # Draw centroid to shoot self.previousCentroid = self.comms.centroidToShoot self.previousRadius = self.comms.radius cv2.circle(scratchImgCol, self.comms.centroidToShoot, 3, (0, 255, 255), 2) # How far the centroid is off the screen center self.comms.deltaX = float( (self.comms.centroidToShoot[0] - vision.screen['width'] / 2) * 1.0 / vision.screen['width']) # Draw everything cv2.putText(scratchImgCol, "X " + str(self.comms.deltaX), (30, 30), cv2.FONT_HERSHEY_PLAIN, 1, (0, 255, 255)) self.comms.deltaY = float( (self.comms.centroidToShoot[1] - vision.screen['height'] / 2) * 1.0 / vision.screen['height']) cv2.putText(scratchImgCol, "Y " + str(self.comms.deltaY), (30, 60), cv2.FONT_HERSHEY_PLAIN, 1, (0, 255, 255)) cv2.putText(scratchImgCol, "Rad " + str(self.comms.radius), (30, 85), cv2.FONT_HERSHEY_PLAIN, 1, (0, 255, 255)) cv2.putText(scratchImgCol, "SHOOT TO KILL", (30, 430), cv2.FONT_HERSHEY_PLAIN, 2, (255, 200, 255)) # Draw center of screen scratchImgCol = vision.drawCenterRect(scratchImgCol) return scratchImgCol
def gotFrame(self, img): #Set up parameters allCentroidList = [] allAreaList = [] self.comms.foundSomething = False outImg = None #Preprocessing img = cv2.resize(img, (640, 480)) # White balance img = vision.whiteBal(img) hsvImg = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) hsvImg = np.array(hsvImg, dtype=np.uint8) # Blur image gauss = cv2.GaussianBlur(hsvImg, ksize=(5, 5), sigmaX=9) sum = cv2.addWeighted(hsvImg, 1.5, gauss, -0.6, 0) enhancedImg = cv2.medianBlur(sum, 3) return cv2.cvtColor(enhancedImg, cv2.COLOR_HSV2BGR) # Threshold red params = self.redParams # Perform thresholding # binImg1 = cv2.inRange(hsvImg, params['lo1'], params['hi1']) # binImg2 = cv2.inRange(hsvImg, params['lo2'], params['hi2']) # binImg = cv2.bitwise_or(binImg1, binImg2) # binImg = self.erodeAndDilateImg(binImg, params) # Perform thresholding kern = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) mask = cv2.inRange(enhancedImg, self.redParams['lo3'], self.redParams['hi3']) mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSe, kern) kern2 = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) threshImg = cv2.dilate(mask, kern2, iterations=3) return cv2.cvtColor(mask, cv2.COLOR_GRAY2BGR) # Find contours scratchImgCol = cv2.cvtColor( binImg, cv2.COLOR_GRAY2BGR) # To overlay with centroids scratchImg = binImg.copy() # For contours to mess up contours, hierachy = cv2.findContours(scratchImg, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE) if len(contours) == 0: return scratchImgCol contours = filter(lambda c: cv2.contourArea(c) > self.minContourArea, contours) sorted(contours, key=cv2.contourArea, reverse=True) # Sort by largest contour if self.comms.centering: self.comms.foundSomething = True # Find largest contour largestContour = contours[0] mu = cv2.moments(largestContour) muArea = mu['m00'] self.comms.centroidToPick = (int(mu['m10'] / muArea), int(mu['m01'] / muArea)) self.comms.areaRect = cv2.minAreaRect(largestContour) rospy.loginfo("Area of centroid:{}".format(self.comms.areaRect)) # Draw new centroid cv2.circle(scratchImgCol, self.comms.centroidToPick, 3, (0, 255, 255), 2) # How far the centroid is off the screen center self.comms.deltaX = float( (vision.screen['width'] / 2 - self.comms.centroidToPick[0]) * 1.0 / vision.screen['width']) cv2.putText(scratchImgCol, str(self.comms.deltaX), (30, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255)) self.comms.deltaY = float( (self.comms.centroidToPick[1] - vision.screen['height'] / 2) * 1.0 / vision.screen['height']) cv2.putText(scratchImgCol, "Y " + str(self.comms.deltaY), (30, 60), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255)) cv2.putText(scratchImgCol, "Area " + str(self.comms.areaRect), (30, 85), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255)) return scratchImgCol # When not centering find a circle # Find Hough circles circles = cv2.HoughCircles(binImg, cv2.cv.CV_HOUGH_GRADIENT, 1, minDist=1, param1=self.houghParams[0], param2=self.houghParams[1], minRadius=self.circleParams['minRadius'], maxRadius=self.circleParams['maxRadius']) # Check if centroid of contour is inside a circle for contour in contours: mu = cv2.moments(contour) muArea = mu['m00'] centroid = (mu['m10'] / muArea, mu['m01'] / muArea) if circles is None: self.comms.foundSomething = False return scratchImgCol for circle in circles[0, :, :]: circleCentroid = (circle[0], circle[1]) if abs((Utils.distBetweenPoints(centroid, circleCentroid))) < circle[2]: self.comms.foundSomething = True # Find new centroid by averaging the centroid and the circle centroid newCentroid = (int(centroid[0] + circleCentroid[0]) / 2, int(centroid[1] + circleCentroid[1]) / 2) allCentroidList.append(newCentroid) allAreaList.append(cv2.contourArea(contour)) # Draw Circles cv2.circle(scratchImgCol, newCentroid, circle[2], (255, 255, 0), 2) cv2.circle(scratchImgCol, newCentroid, 2, (255, 0, 255), 3) break # Centroid resetted if self.comms.centroidToPick == None: if not len(allCentroidList) == 0: # Pick the nth centroid to hit self.comms.centroidToPick = allCentroidList[self.comms.count] self.comms.areaRect = allAreaList[self.comms.count] self.prevCentroid = self.comms.centroidToPick self.prevArea = self.comms.areaRect else: if not len(allCentroidList) == 0: # Compare with the previous centroid and pick the one nearest for centroid in allCentroidList: distDiff = [] distDiff.append( Utils.distBetweenPoints(self.previousCentroid, centroid)) minIndex = distDiff.index(min(distDiff)) self.comms.centroidToPick = allCentroidList[minIndex] self.comms.areaRect = allAreaList[minIndex] self.prevCentroid = self.comms.centroidToPick self.prevArea = self.comms.areaRect else: # Use back the previous self.comms.centroidToPick = self.previousCentroid self.comms.areaRect = self.previousArea rospy.loginfo("Area: {}".format(self.comms.areaRect)) # Draw new centroid cv2.circle(scratchImgCol, self.comms.centroidToPick, 3, (0, 255, 255), 2) # How far the centroid is off the screen center self.comms.deltaX = float( (vision.screen['width'] / 2 - self.comms.centroidToPick[0]) * 1.0 / vision.screen['width']) cv2.putText(scratchImgCol, str(self.comms.deltaX), (30, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255)) return scratchImgCol