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