def testWhenPathIsFoundThenTheReturnedPathDoesntTouchObstacles(self):
processedMap = self.pathfinder._processMap(self.simpleMap)
path = self.pathfinder.findPath(self.simpleMap, self.AN_INITIAL_POSITION, self.A_FINAL_POSITION)
for x, y in path:
i, j = convertCoordinatesToMapPoint((x, y), self.ROW_NUMBER, self.SQUARE_SIZE_IN_CENTIMETERS)
self.assertNotEqual(processedMap[i, j], self.pathfinder.INFINITY)
def findPath(self, map, initialCoordinates, finalCoordinates, dilatation=LARGE_DILATATION, prnt=False):
map = self._processMap(map, dilatation=dilatation).copy()
rowNumber, _ = map.shape
initialI, initialJ = convertCoordinatesToMapPoint(initialCoordinates, rowNumber, self.squareSizeInCentimeters)
finalI, finalJ = convertCoordinatesToMapPoint(finalCoordinates, rowNumber, self.squareSizeInCentimeters)
map[finalI, finalJ] = 1
if initialI == finalI and initialJ == finalJ:
return []
path = Planner.findPath(map, (initialI, initialJ), (finalI, finalJ))
if not path:
raise NoPathFoundException
return [initialCoordinates] + [convertMapPointToCoordinates(point, rowNumber, self.squareSizeInCentimeters) for point in path]
def addObstaclesToMap(self):
self.obstaclePositions = self.askForObstaclePositions()
obstacleRadius = 7
print "Nombre d'obstacles: {0}".format(len(self.obstaclePositions))
for position in self.obstaclePositions:
obstacle = CircularObject(obstacleRadius*2)
i, j = convertCoordinatesToMapPoint(position, self.cartographySystem.rowNumber, self.SQUARE_SIZE_IN_CENTIMETERS)
i, j = i - obstacleRadius, j - obstacleRadius
try:
self.cartographySystem.createObject(obstacle, i, j, walkable=False)
except ValueError:
print "Erreur obstacle"
continue
def refreshMap(self):
try:
self.cartographySystem = self.mapUpdater.updateMap()
except ValueError:
print "Erreur position robot"
while not obstacles.empty():
self.obstaclePositions = obstacles.get_nowait()
rowNumber = 117
obstacleDiameterInCentimeters = 14
obstacleRadiusInCentimeters = obstacleDiameterInCentimeters/2
for x, y in self.obstaclePositions:
try:
i, j = convertCoordinatesToMapPoint((x, y), rowNumber, self.MAP_SQUARE_SIZE_IN_CENTIMETERS)
obstacle = CircularObject(obstacleDiameterInCentimeters)
self.cartographySystem.createObject(obstacle, i-obstacleRadiusInCentimeters, j-obstacleRadiusInCentimeters, walkable=False)#upperleft point for placement
except e:
print "Exception pour placer les obstacles"
continue
self.map_handle.set_data(self.cartographySystem.map)
while len(self.subplot.lines) > 0 :
self.subplot.lines.pop()
while not plannedPath.empty():
self.path = plannedPath.get_nowait()
for z in range(len(self.path)):
x, y = self.path[z]
i,j = convertCoordinatesToMapPoint((x, y), rowNumber, self.MAP_SQUARE_SIZE_IN_CENTIMETERS)
i, j = int(i), int(j)
self.path[z] = (i, j)
self.drawPath()
self.map_canvas.get_tk_widget().after(self.REFRESHRATE, self.refreshMap)
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
