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
