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 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
