def createBaseCartographySystem():
rowNumber, columnNumber = 117, 56
cartographySystem = CartographySystem(rowNumber, columnNumber)
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), (105, 44), True),
(RectangularObject(1, 1), (71, 44), True),
(RectangularObject(1, 1), (105, 11), True),
(RectangularObject(1, 1), (71, 11), True),
]
cornerCommandPanel, cornerCommandPanelPosition = RectangularObject(1, 8), (0, 14)
clockwiseCommandPanel, clockwiseCommandPanelPosition = RectangularObject(1, 8), (0, 32)
orangePanel, orangePanelPosition = RectangularObject(7, 1), (103, columnNumber - 1)
robot, robotPosition = PaddedRectangularObject(11, 11, 4), (81, 16)
robotOrientation = 45
for mapObject, coordinates, walkable in borders + startingZoneCorners:
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 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