def __init__(self):
self.camera = CameraAccessor()
self.drawingExtractor = DrawingExtractor()
self.contourExtractor = ContourExtractor()
self.cornerDetector = CornerDetector()
self.sideDetector = SideDetector()
self. positionner = Positionner()
class Camera:
def __init__(self):
self.camera = CameraAccessor()
self.drawingExtractor = DrawingExtractor()
self.contourExtractor = ContourExtractor()
self.cornerDetector = CornerDetector()
self.sideDetector = SideDetector()
self. positionner = Positionner()
def getDrawingContour(self):
try:
image = self.camera.getFrame(False)
drawingImage = self.drawingExtractor.ExtractShape(image)
contourPoints = self.contourExtractor.findContours(drawingImage)
size = cv.GetSize(drawingImage)
squareSize = size[0]
newSquareSize = size[0]-(2*19)
for point in contourPoints:
pointCopy = (point[0], squareSize - point[1])
cv.Circle(drawingImage, pointCopy, 5, (0,0,0), 2)
#cv.SaveImage("segmentationResult.jpg", drawingImage)
return contourPoints, newSquareSize
except:
raise ValueError("Couldn't find drawing in image")
def getCurrentPose(self):
try:
image = self.camera.getFrame(False)
pointBlue, pointOrange, side = self.getVisibleCorners(image)
print "blue: ", len(pointBlue)
print "orange: ", len(pointOrange)
self.drawPointsOnImage(image, pointBlue)
self.drawPointsOnImage(image, pointOrange)
if len(pointBlue) > 0 and side == SideDetector.EAST_SIDE:
print "Blue East Corner"
x, y, theta = self.positionner.getCurrentPose(pointBlue[0], pointBlue[1], CornerDetector.EAST_BLUE_CORNER)
elif len(pointBlue) > 0 and side == SideDetector.WEST_SIDE:
print "Blue West Corner"
x, y, theta = self.positionner.getCurrentPose(pointBlue[0], pointBlue[1], CornerDetector.WEST_BLUE_CORNER)
elif len(pointOrange) > 0 and side == SideDetector.EAST_SIDE:
print "Orange East Corner"
x, y, theta = self.positionner.getCurrentPose(pointOrange[0], pointOrange[1], CornerDetector.EAST_ORANGE_CORNER)
elif len(pointOrange) > 0 and side == SideDetector.WEST_SIDE:
print "Orange West Corner"
x, y, theta = self.positionner.getCurrentPose(pointOrange[0], pointOrange[1], CornerDetector.WEST_ORANGE_CORNER)
else:
print "No corners detected"
raise ValueError("No corners detected for positionning")
if theta < 0:
theta = 360 + theta
return x, y, theta
except:
raise ValueError("Problem while getting robot pose")
def getCurrentPose2(self):
xTotal = 0
yTotal = 0
thetaTotal = 0
nbSuccess = 0
attemps = 0
try:
while attemps < 10 and nbSuccess <= 3:
attemps += 1
print "Camera: Getting current pose. Attemp ", attemps
x = 0
y = 0
theta = 0
image = self.camera.getFrame(False)
pointBlue, pointOrange, side = self.getVisibleCorners(image)
#self.drawPointsOnImage(image, pointBlue)
#self.drawPointsOnImage(image, pointOrange)
if len(pointBlue) > 0 and side == SideDetector.EAST_SIDE:
x, y, theta = self.positionner.getCurrentPose(pointBlue[0], pointBlue[1], CornerDetector.EAST_BLUE_CORNER)
print "Camera Getting current pose. Success! Blue East corner found."
elif len(pointBlue) > 0 and side == SideDetector.WEST_SIDE:
x, y, theta = self.positionner.getCurrentPose(pointBlue[0], pointBlue[1], CornerDetector.WEST_BLUE_CORNER)
print "Camera Getting current pose. Success! Blue West corner found."
elif len(pointOrange) > 0 and side == SideDetector.EAST_SIDE:
x, y, theta = self.positionner.getCurrentPose(pointOrange[0], pointOrange[1], CornerDetector.EAST_ORANGE_CORNER)
print "Camera Getting current pose. Success! Orange East corner found."
elif len(pointOrange) > 0 and side == SideDetector.WEST_SIDE:
x, y, theta = self.positionner.getCurrentPose(pointOrange[0], pointOrange[1], CornerDetector.WEST_ORANGE_CORNER)
print "Camera Getting current pose. Success! Orange West corner found."
if x > 130:
nbSuccess += 1
xTotal += x
yTotal += y
thetaTotal += theta
if nbSuccess > 0:
xAverage = xTotal/nbSuccess
yAverage = yTotal/nbSuccess
thetaAverage = thetaTotal/nbSuccess
print "Camera: Robot pose found. X = ", xAverage, ", Y = ", yAverage, ", Theta = ", thetaAverage
return xAverage, yAverage, thetaAverage
else:
raise ValueError("Couldn't find corner")
except:
raise ValueError("Problem while getting robot pose")
def getCurrentPoseBackup(self):
image = self.camera.getFrame(True)
imageSize = cv.GetSize(image)
pointBlue, pointOrange, side = self.getVisibleCorners(image)
if side == SideDetector.EAST_SIDE and len(pointBlue) > 0:
lenght = (pointBlue[1][0] - pointBlue[0][0])/size[0]
distance = 1699.1*lenght*lenght - 1051.6*lenght + 229.33
return True, distance
else:
return False, 0
def drawPointsOnImage(self, image, points):
#cv.SaveImage("cornerDetectionResult.jpg", image)
if len(points) > 0:
for point in points:
#pointCopy = (point[0], squareSize - point[1])
cv.Circle(image, point, 5, (0,0,0), 2)
#cv.SaveImage("cornerDetectionResult.jpg", image)
def getVisibleCorners(self, image):
contourBlue, contourOrange = self.cornerDetector.detectCorners(image)
side = self.sideDetector.detectVisibleSide(image)
return contourBlue, contourOrange, side
def __doubleImage__(self, contourPoints, size):
doubleImage = cv.CreateImage((1000,1000),8,1)
for scale in range(1,3):
x = size[0]*scale
y = size[1]*scale
for i in range(len(contourPoints)):
if i == len(contourPoints)-1:
x1 = contourPoints[i][0]*scale
y1 = contourPoints[i][1]*scale
x2 = contourPoints[0][0]*scale
y2 = contourPoints[0][1]*scale
else:
x1 = contourPoints[i][0]*scale
y1 = contourPoints[i][1]*scale
x2 = contourPoints[i+1][0]*scale
y2 = contourPoints[i+1][1]*scale
point1 = (x1, y1)
point2 = (x2, y2)
cv.Line(doubleImage,point1, point2,255, 2)
#cv.SaveImage("scale.jpg", doubleImage)
