def getRobotPositionAndOrientation(self):
print 'getRobotPositionAndOrientation'
for _ in range(self.NUMBER_OF_RETRIES):
try:
rgbImage, depthImage = self.imageTaker.getImages()
pointConvertor = KinectPointConvertor(rgbImage, depthImage, self.transformMatrix)
robotFinder = RobotFinder(rgbImage, pointConvertor)
position, theta = robotFinder.findPositionAndOrientation()
x, y = position
return Position(x, y, theta)
except (NoContourFoundException, InvalidDistanceException) as exception:
print "getRobotPositionAndOrientation: ", exception
else:
return Position(-1, -1, -1)
def updateMap(self):
for _ in range(self.NUMBER_OF_RETRIES):
try:
rgbImage, depthImage = self.imageTaker.getImages()
pointConvertor = KinectPointConvertor(rgbImage, depthImage, self.transformMatrix)
robotFinder = RobotFinder(rgbImage, pointConvertor)
self.lastRobotPosition, self.lastRobotOrientation= robotFinder.findPositionAndOrientation()
except (InvalidDistanceException, NoContourFoundException) as e:
print "updateMap: ", str(e)
x, y = convertCoordinatesToMapPoint(self.lastRobotPosition, self.ROW_NUMBER, self.SQUARE_SIZE_IN_CENTIMETERS)
x-=5 #We want upperleft point
y-=5
robotPosition=(int(x),int(y))
robotOrientation = self.lastRobotOrientation * 180 / numpy.pi
rowNumber, columnNumber = 117, 56
borders = [(RectangularObject(1, columnNumber), (0, 0), False),
(RectangularObject(1, columnNumber), (rowNumber - 1, 0), False),
(RectangularObject(rowNumber, 1), (0, 0), False),
(RectangularObject(rowNumber, 1), (0, columnNumber - 1), False)]
startingZoneCorners = [(RectangularObject(1, 1), (106, 44), True),
(RectangularObject(1, 1), (73, 44), True),
(RectangularObject(1, 1), (106, 11), True),
(RectangularObject(1, 1), (73, 11), True)]
puckZones = [(RectangularObject(40, 3), (1, 1), False),
(RectangularObject(3, 54), (1, 1), False),
(RectangularObject(40, 3), (1, 52), False)]
cornerCommandPanel, cornerCommandPanelPosition = RectangularObject(1, 8), (0, 14)
clockwiseCommandPanel, clockwiseCommandPanelPosition = RectangularObject(1, 8), (0, 32)
orangePanel, orangePanelPosition = RectangularObject(7, 1), (103, columnNumber-1)
robot= PaddedRectangularObject(11, 11, 4)
cartographySystem = CartographySystem(rowNumber, columnNumber)
for mapObject, coordinates, walkable in borders + startingZoneCorners + puckZones:
cartographySystem.createObject(mapObject, *coordinates, walkable=walkable)
cartographySystem.createObject(cornerCommandPanel, *cornerCommandPanelPosition, walkable=False)
cartographySystem.createObject(clockwiseCommandPanel, *clockwiseCommandPanelPosition, walkable=False)
cartographySystem.createObject(orangePanel, *orangePanelPosition, walkable=False)
cartographySystem.createObject(robot, *robotPosition, walkable=True)
cartographySystem.rotateObject(robot, robotOrientation)
return cartographySystem
def printRobotPositionAndOrientation(table, rgbImage, depthImage):
pointConvertor = KinectPointConvertor(rgbImage, depthImage, tablesTransformMatrix[table - 1])
robotFinder = RobotFinder(rgbImage, pointConvertor)
position, orientation = robotFinder.findPositionAndOrientation()
print position, orientation * 180 / numpy.pi
