def _findObstaclePositionFromContour(self, contour):
contourCenterPoint = findCenterPoint(contour)
contourLeftmostPoint = findLeftmostPoint(contour)
contourRightmostPoint = findRightmostPoint(contour)
firstPoint = findPointBetweenPoints(contourCenterPoint, contourLeftmostPoint, 1./2)
secondPoint = findPointBetweenPoints(contourCenterPoint, contourRightmostPoint, 1./2)
firstPosition = self.pointConvertor.convertPoint(firstPoint)
secondPosition = self.pointConvertor.convertPoint(secondPoint)
obstaclePosition = self._findObstacleCenter(firstPosition, secondPosition)
return obstaclePosition
def _findBestContour(self):
bestContours = []
sideColors = self.ANGLE_OFFSET_BY_COLOR.keys()
image = self.rgbImage.copy()
image = cv2.GaussianBlur(image, self.BLUR_VALUE, self.BLUR_SIGMA_X)
image = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
for color in sideColors:
contours = self._findContoursByColor(image, color)
correctContours = []
for contour in contours:
centerPoint = findCenterPoint(contour)
try:
x, y = self.pointConvertor.convertPoint(centerPoint)
except InvalidDistanceException:
continue
if -2 <= x <= 120 and -2 <= y <= 242:
correctContours.append(contour)
if correctContours:
bestContour = max(correctContours, key=cv2.contourArea)
centerPoint = findCenterPoint(bestContour)
x, y = centerPoint
bestContourColor = color
bestContours.append((bestContour, bestContourColor))
try:
bestContour = max(bestContours, key=lambda contourTuple: cv2.contourArea(contourTuple[0]))
except ValueError:
raise NoContourFoundException
return bestContour
def findPositionAndOrientation(self):
contour, color = self._findBestContour()
centerPoint = findCenterPoint(contour)
leftmostPoint = findLeftmostPoint(contour)
rightmostPoint = findRightmostPoint(contour)
# A factor of 1/2 will give the point halfway between the center and the leftmost/rightmost point
leftsidePoint = findPointBetweenPoints(centerPoint, leftmostPoint, 1./2)
rightsidePoint = findPointBetweenPoints(centerPoint, rightmostPoint, 1./2)
leftsidePosition = self.pointConvertor.convertPoint(leftsidePoint)
rightsidePosition = self.pointConvertor.convertPoint(rightsidePoint)
robotPosition, robotOrientation = self._findRobotCenterAndOrientation(leftsidePosition, rightsidePosition, color)
return robotPosition, robotOrientation
