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