class LineOfSightCalculatorTest(TestCase):
def setUp(self):
self.noLineOfSightPath = Path()
self.lineOfSightPath = Path()
self.noLineOfSightPath.nodeList = [Node((100, 100)), Node((200, 100))]
self.lineOfSightPath.nodeList = [Node((0, 0)), Node((50, 50)), Node((50, 0))]
self.goodPaths = []
self.graph = MagicMock()
self.lineOfSightCalculator = LineOfSightCalculator(self.graph)
def test_whenPathHasNoLineOfSightThenPathStayTheSame(self):
self.goodPaths.append(self.noLineOfSightPath)
goodLenght = self.noLineOfSightPath.__len__()
self.lineOfSightCalculator.tryStraightLine(self.goodPaths)
obtainedLenght = self.noLineOfSightPath.__len__()
self.assertEqual(goodLenght,obtainedLenght)
def test_whenPathHasLineOfSightThenPathStayTheSame(self):
self.goodPaths.append(self.lineOfSightPath)
self.lineOfSightCalculator.tryStraightLine(self.goodPaths)
self.assertEqual(self.lineOfSightPath.contains(Node((50,50))),False)
def getPositionInFrontOfTreasure(self):
myPathFinder = Pathfinder(self)
myMapCoorDinateAjuster = MapCoordinatesAjuster(self)
myBestPath = Path()
myBestPath.totalDistance = 99999
bestInFrontPosition = (0,0)
bestOrientationForTreasure = 0
for treasurePosition in self.treasures:
if treasurePosition[1] == self.limit.getMaxCorner()[1]:
newOrientationForTreasure = 90
newInFrontPosition = (treasurePosition[0], treasurePosition[1] - self.SAFE_MARGIN_FOR_TREASURE)
elif treasurePosition[1] == self.limit.getMinCorner()[1]:
newOrientationForTreasure = 270
newInFrontPosition = (treasurePosition[0], treasurePosition[1] + self.SAFE_MARGIN_FOR_TREASURE)
else:
newOrientationForTreasure = 180
newInFrontPosition = (treasurePosition[0] + self.SAFE_MARGIN_FOR_TREASURE, treasurePosition[1])
print newInFrontPosition
myNewPath = myPathFinder.findPath(myMapCoorDinateAjuster.convertPoint((self.robot.center)), myMapCoorDinateAjuster.convertPoint(newInFrontPosition))
if myNewPath != False:
if myNewPath.totalDistance < myBestPath.totalDistance:
myBestPath = myNewPath
bestOrientationForTreasure = newOrientationForTreasure
bestInFrontPosition = newInFrontPosition
return bestInFrontPosition,bestOrientationForTreasure
class LineOfSightCalculatorTest(TestCase):
def setUp(self):
self.noLineOfSightPath = Path()
self.lineOfSightPath = Path()
self.noLineOfSightPath.nodeList = [Node((100, 100)), Node((200, 100))]
self.lineOfSightPath.nodeList = [
Node((0, 0)), Node((50, 50)),
Node((50, 0))
]
self.goodPaths = []
self.graph = MagicMock()
self.lineOfSightCalculator = LineOfSightCalculator(self.graph)
def test_whenPathHasNoLineOfSightThenPathStayTheSame(self):
self.goodPaths.append(self.noLineOfSightPath)
goodLenght = self.noLineOfSightPath.__len__()
self.lineOfSightCalculator.tryStraightLine(self.goodPaths)
obtainedLenght = self.noLineOfSightPath.__len__()
self.assertEqual(goodLenght, obtainedLenght)
def test_whenPathHasLineOfSightThenPathStayTheSame(self):
self.goodPaths.append(self.lineOfSightPath)
self.lineOfSightCalculator.tryStraightLine(self.goodPaths)
self.assertEqual(self.lineOfSightPath.contains(Node((50, 50))), False)
def setUp(self):
self.noLineOfSightPath = Path()
self.lineOfSightPath = Path()
self.noLineOfSightPath.nodeList = [Node((100, 100)), Node((200, 100))]
self.lineOfSightPath.nodeList = [Node((0, 0)), Node((50, 50)), Node((50, 0))]
self.goodPaths = []
self.graph = MagicMock()
self.lineOfSightCalculator = LineOfSightCalculator(self.graph)
def setUp(self):
self.noLineOfSightPath = Path()
self.lineOfSightPath = Path()
self.noLineOfSightPath.nodeList = [Node((100, 100)), Node((200, 100))]
self.lineOfSightPath.nodeList = [
Node((0, 0)), Node((50, 50)),
Node((50, 0))
]
self.goodPaths = []
self.graph = MagicMock()
self.lineOfSightCalculator = LineOfSightCalculator(self.graph)
def __init__(self, map):
self.mapAdaptator = MapAdaptator(map)
obstaclesList, mapSizeX, mapSizeY, minCorner = self.mapAdaptator.getMapInfo(
)
self.minCorner = minCorner
self.graphGenerator = GraphGenerator(obstaclesList, mapSizeX, mapSizeY)
self.graph = self.graphGenerator.generateGraph()
self.lineOfSightCalculator = LineOfSightCalculator(self.graph)
self.pathsList = []
self.goodPaths = []
self.indice = 0
self.theGoodPath = Path()
def __init__(self, map):
self.mapAdaptator = MapAdaptator(map)
obstaclesList, mapSizeX, mapSizeY, minCorner = self.mapAdaptator.getMapInfo()
self.minCorner = minCorner
self.graphGenerator = GraphGenerator(obstaclesList, mapSizeX, mapSizeY)
self.graph = self.graphGenerator.generateGraph()
self.lineOfSightCalculator = LineOfSightCalculator(self.graph)
self.pathsList = []
self.goodPaths = []
self.indice = 0
self.theGoodPath = Path()
def findPath(self, positionRobot, pointToMoveTo):
print("@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@")
goodPositionRobot = self.findGoodPoint(positionRobot)
startingPathNode = self.graph.findGoodSafeNodeToGo(goodPositionRobot)
endingPathNode = self.graph.findGoodSafeNodeToGo(pointToMoveTo)
print pointToMoveTo, endingPathNode.positionX, endingPathNode.positionY
self.theGoodPath = Path()
self.pathsList = []
self.goodPaths = []
path = Path()
path.append(Node(positionRobot))
path.append(startingPathNode)
self.pathsList.append(path)
self.__findAllPaths(path, endingPathNode)
goodPath = Path()
goodPath.append(Node((0,0)))
goodPath.totalDistance = 99999
for compteur in range(0, self.goodPaths.__len__()):
currentPath = self.goodPaths[compteur]
currentPath.append(Node(pointToMoveTo))
self.__polishGoodPaths()
self.lineOfSightCalculator.tryStraightLine(self.goodPaths)
for compteur in range(0, self.goodPaths.__len__()):
currentPath= self.goodPaths[compteur]
currentPath.ajustDistance()
if currentPath.totalDistance < goodPath.totalDistance:
goodPath = currentPath
self.printPath(goodPath)
if goodPath.totalDistance == 99999:
self.__displayPathfinder(goodPath, positionRobot)
return False
self.theGoodPath = goodPath
self.__displayPathfinder(goodPath, positionRobot)
return goodPath
class PathTest(TestCase):
def setUp(self):
self.path = Path()
self.path.append(Node((0, 100)))
self.path.append(Node((0, 0)))
def test_whenAdjustDistanceIsCalledThenDistanceIsUpdated(self):
self.path.ajustDistance()
goodDistance = 100
self.assertEqual(goodDistance, self.path.totalDistance)
def test_whenIsOpenIsCalledThenReturnGoodValue(self):
theValue = self.path.isOpen()
isOpen = False
self.assertEqual(theValue, isOpen)
def test_whenCloneIsCalledThenCopyIsEqual(self):
pathClone = self.path.clone()
for compteur in range(0, self.path.__len__()):
self.assertTrue(pathClone.contains(self.path.nodeList[0]))
def getPositionInFrontOfIsland(self):
myPathFinder = Pathfinder(self)
fakePath = Path()
fakePath.totalDistance = 99999
myPath = fakePath
myMapCoorDinateAjuster = MapCoordinatesAjuster(self)
myBestPosition = (0,0)
orientation = 0
targetShape = self.target
edgesList = self.target.getEdgesList()
for edge in edgesList:
xCenterOfEdge = edge[0].item(0) + (((edge[0].item(0) - edge[1].item(0)) / 2) * -1)
yCenterOfEdge = edge[0].item(1) + (((edge[0].item(1) - edge[1].item(1)) / 2) * -1)
edgePerpendicularGradient = self.getEdgeGradiant(edge)
conversionGradient = 1
if abs(edgePerpendicularGradient) > 1:
conversionGradient = 0.1
if abs(edgePerpendicularGradient) > 10:
conversionGradient = 0.01
if self.target.isOutside((xCenterOfEdge + 1 * conversionGradient, yCenterOfEdge + 1 * edgePerpendicularGradient * conversionGradient)):
positionToGo = (xCenterOfEdge + self.SAFE_MARGIN * conversionGradient, yCenterOfEdge + self.SAFE_MARGIN * edgePerpendicularGradient * conversionGradient)
hypothenuse = 0
while hypothenuse < self.SAFE_MARGIN:
positionToGo = (positionToGo[0] + 1, positionToGo[1] + edgePerpendicularGradient)
opp = abs(yCenterOfEdge - positionToGo[1])
adj = abs(xCenterOfEdge - positionToGo[0])
hypothenuse = math.sqrt((opp * opp) + (adj * adj))
else:
positionToGo = (xCenterOfEdge - self.SAFE_MARGIN * conversionGradient, yCenterOfEdge - self.SAFE_MARGIN * edgePerpendicularGradient * conversionGradient)
opp = abs(yCenterOfEdge - positionToGo[1])
adj = abs(xCenterOfEdge - positionToGo[0])
hypothenuse = math.sqrt((opp * opp) + (adj * adj))
while hypothenuse < self.SAFE_MARGIN:
positionToGo = (positionToGo[0] - 1* conversionGradient, positionToGo[1] - edgePerpendicularGradient* conversionGradient)
opp = abs(yCenterOfEdge - positionToGo[1])
adj = abs(xCenterOfEdge - positionToGo[0])
hypothenuse = math.sqrt((opp * opp) + (adj * adj))
angle = math.degrees(math.atan2(opp,adj))
if positionToGo[0] > xCenterOfEdge and positionToGo[1] < yCenterOfEdge:
angle = 180 - angle
if positionToGo[0] > xCenterOfEdge and positionToGo[1] > yCenterOfEdge:
angle = angle + 180
if positionToGo[0] < xCenterOfEdge and positionToGo[1] > yCenterOfEdge:
angle = 360 - angle
closePoint = myPathFinder.findClosePoint(myMapCoorDinateAjuster.convertPoint(positionToGo))
myNewPath = myPathFinder.findPath(myMapCoorDinateAjuster.convertPoint((self.robot.center)), closePoint)
if myNewPath != False:
if myNewPath.totalDistance < myPath.totalDistance:
myPath = myNewPath
myBestPosition = positionToGo
orientation = angle
if myBestPosition == (0, 0):
print "aucun angle parallele"
hypothenuse = 100
xCenterOfMass, yCenterOfMass = targetShape.findCenterOfMass()
for angle in range (0, 90, 10):
xValue = hypothenuse * math.cos(math.radians(angle))
yValue = hypothenuse * math.sin(math.radians(angle))
positionToGo = (xCenterOfMass - xValue, yCenterOfMass - yValue)
myNewPath = myPathFinder.findPath(myMapCoorDinateAjuster.convertPoint((self.robot.center)), myMapCoorDinateAjuster.convertPoint(positionToGo))
if not isinstance(myNewPath, bool):
myBestPosition = positionToGo
orientation = angle
myPath = myNewPath
break
if myPath.totalDistance == 99999:
for angle in range (0, 90, 10):
xValue = hypothenuse * math.sin(math.radians(angle))
yValue = hypothenuse * math.cos(math.radians(angle))
positionToGo = (xCenterOfMass + xValue, yCenterOfMass - yValue)
myNewPath = myPathFinder.findPath(myMapCoorDinateAjuster.convertPoint((self.robot.center)), myMapCoorDinateAjuster.convertPoint(positionToGo))
if not isinstance(myNewPath, bool):
myBestPosition = positionToGo
orientation = angle + 90
myPath = myNewPath
break
if myPath.totalDistance == 99999:
for angle in range (0, 91, 10):
yValue = hypothenuse * math.sin(math.radians(angle))
xValue = hypothenuse * math.cos(math.radians(angle))
positionToGo = (xCenterOfMass + xValue, yCenterOfMass + yValue)
myNewPath = myPathFinder.findPath(myMapCoorDinateAjuster.convertPoint((self.robot.center)), myMapCoorDinateAjuster.convertPoint(positionToGo))
if not isinstance(myNewPath, bool):
myBestPosition = positionToGo
orientation = angle + 180
myPath = myNewPath
break
if myPath.totalDistance == 99999:
for angle in range (0, 91, 10):
xValue = hypothenuse * math.sin(math.radians(angle))
yValue = hypothenuse * math.cos(math.radians(angle))
positionToGo = (xCenterOfMass - xValue, yCenterOfMass + yValue)
myNewPath = myPathFinder.findPath(myMapCoorDinateAjuster.convertPoint((self.robot.center)), myMapCoorDinateAjuster.convertPoint(positionToGo))
if not isinstance(myNewPath, bool):
myBestPosition = positionToGo
orientation = angle + 270
myPath = myNewPath
break
print "MEILLEUR ",myBestPosition, ", Orientation :", orientation
return myPath, orientation
class Pathfinder:
def __init__(self, map):
self.mapAdaptator = MapAdaptator(map)
obstaclesList, mapSizeX, mapSizeY, minCorner = self.mapAdaptator.getMapInfo()
self.minCorner = minCorner
self.graphGenerator = GraphGenerator(obstaclesList, mapSizeX, mapSizeY)
self.graph = self.graphGenerator.generateGraph()
self.lineOfSightCalculator = LineOfSightCalculator(self.graph)
self.pathsList = []
self.goodPaths = []
self.indice = 0
self.theGoodPath = Path()
def findGoodPoint(self, point):
return self.graph.needAGoodPointToGo(point)
def findClosePoint(self, point):
return self.graph.needAClosePoint(point)
def findPath(self, positionRobot, pointToMoveTo):
print("@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@")
goodPositionRobot = self.findGoodPoint(positionRobot)
startingPathNode = self.graph.findGoodSafeNodeToGo(goodPositionRobot)
endingPathNode = self.graph.findGoodSafeNodeToGo(pointToMoveTo)
print pointToMoveTo, endingPathNode.positionX, endingPathNode.positionY
self.theGoodPath = Path()
self.pathsList = []
self.goodPaths = []
path = Path()
path.append(Node(positionRobot))
path.append(startingPathNode)
self.pathsList.append(path)
self.__findAllPaths(path, endingPathNode)
goodPath = Path()
goodPath.append(Node((0,0)))
goodPath.totalDistance = 99999
for compteur in range(0, self.goodPaths.__len__()):
currentPath = self.goodPaths[compteur]
currentPath.append(Node(pointToMoveTo))
self.__polishGoodPaths()
self.lineOfSightCalculator.tryStraightLine(self.goodPaths)
for compteur in range(0, self.goodPaths.__len__()):
currentPath= self.goodPaths[compteur]
currentPath.ajustDistance()
if currentPath.totalDistance < goodPath.totalDistance:
goodPath = currentPath
self.printPath(goodPath)
if goodPath.totalDistance == 99999:
self.__displayPathfinder(goodPath, positionRobot)
return False
self.theGoodPath = goodPath
self.__displayPathfinder(goodPath, positionRobot)
return goodPath
def __polishGoodPaths(self):
for compteur in range(0,self.goodPaths.__len__()):
path = self.goodPaths[compteur]
nodesToBeRemoved = []
for compteur in range(1, path.__len__()):
if path[compteur].isASafeNode == True:
previousNode = path[compteur-1]
nextNode = path[compteur+1]
if previousNode.positionX != nextNode.positionX:
nodesToBeRemoved.append(path[compteur])
for compteur in range(0, nodesToBeRemoved.__len__()):
path.remove(nodesToBeRemoved[compteur])
def __findAllPaths(self, path, endingPathNode):
lastNode = path[-1]
if lastNode != endingPathNode and path.isOpen() == True:
for compteur in range(0, lastNode.connectedNodes.__len__()):
if (path.contains(lastNode.connectedNodes[compteur]) == False):
newPath = path.clone()
newPath.append(lastNode.connectedNodes[compteur])
self.pathsList.append(newPath)
self.__findAllPaths(newPath, endingPathNode)
self.pathsList.remove(path)
elif lastNode == endingPathNode:
self.goodPaths.append(path)
def drawPath(self, img):
for compteur in range (1, self.theGoodPath.__len__()):
startLine = (self.theGoodPath[compteur-1].positionX + self.minCorner[0], self.theGoodPath[compteur-1].positionY + self.minCorner[1])
endLine = (self.theGoodPath[compteur].positionX + self.minCorner[0], self.theGoodPath[compteur].positionY + self.minCorner[1])
cv2.line(img, (int(startLine[0]), int(startLine[1])), (int(endLine[0]), int(endLine[1])),
(0, 0, 255), 2, 1)
#methode pour afficher le path dans console, pas importante
def printPath(self, goodPath):
for compteur in range(0, goodPath.__len__()):
print "path:", goodPath[compteur].positionX, goodPath[compteur].positionY
print goodPath.totalDistance
#methode pour display les shits du pathfinding, pas importante non plus
def __displayPathfinder(self, goodPath, positionRobot):
img = np.zeros((600, 1000, 3), np.uint8)
for compteur in range (0, self.graphGenerator.obstaclesList.__len__()):
currentObstacle = self.graphGenerator.obstaclesList[(compteur)]
cv2.rectangle(img, (currentObstacle.positionX - self.graphGenerator.SAFE_MARGIN, currentObstacle.positionY - self.graphGenerator.SAFE_MARGIN), (currentObstacle.positionX + self.graphGenerator.SAFE_MARGIN, currentObstacle.positionY + self.graphGenerator.SAFE_MARGIN),
(0, 255, 0), -1, 1)
self.graph.nodesList.sort(key=lambda node: node.positionX)
for compteur in range (0, self.graph.nodesList.__len__()):
currentNode = self.graph.nodesList[(compteur)]
departPoint = (currentNode.positionX, currentNode.positionY)
connectedNode = currentNode.getConnectedNodesList()
#print "GRAPh", currentNode.positionX, currentNode.positionY
for compteurConnected in range(0, connectedNode.__len__()):
finalNode = connectedNode[(compteurConnected)]
finalPoint = (finalNode.positionX, finalNode.positionY)
#print " Connected", finalNode.positionX, finalNode.positionY
cv2.line(img, departPoint, finalPoint,
(255, 0, 0), 2, 1)
for compteur in range (0, goodPath.__len__()):
if compteur == 0:
startLine = positionRobot
else:
startLine = (goodPath[compteur-1].positionX,goodPath[compteur-1].positionY)
endLine = (goodPath[compteur].positionX,goodPath[compteur].positionY)
cv2.line(img, (int(startLine[0]), int(startLine[1])), (int(endLine[0]), int(endLine[1])),
(0, 0, 255), 2, 1)
for compteur in range (0, self.graph.safeZonesList.__len__()):
currentZone = self.graph.safeZonesList[compteur]
cv2.rectangle(img, currentZone.cornerTopLeft, currentZone.cornerBottomRight,
(0, 150, 150), 2, 1)
cv2.imwrite('image' + str(self.indice) + '.jpg', img)
self.indice = self.indice + 1
def setUp(self):
self.path = Path()
self.path.append(Node((0, 100)))
self.path.append(Node((0, 0)))
def findPath(self, positionRobot, pointToMoveTo):
print(
"@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@"
)
goodPositionRobot = self.findGoodPoint(positionRobot)
startingPathNode = self.graph.findGoodSafeNodeToGo(goodPositionRobot)
endingPathNode = self.graph.findGoodSafeNodeToGo(pointToMoveTo)
print pointToMoveTo, endingPathNode.positionX, endingPathNode.positionY
self.theGoodPath = Path()
self.pathsList = []
self.goodPaths = []
path = Path()
path.append(Node(positionRobot))
path.append(startingPathNode)
self.pathsList.append(path)
self.__findAllPaths(path, endingPathNode)
goodPath = Path()
goodPath.append(Node((0, 0)))
goodPath.totalDistance = 99999
for compteur in range(0, self.goodPaths.__len__()):
currentPath = self.goodPaths[compteur]
currentPath.append(Node(pointToMoveTo))
self.__polishGoodPaths()
self.lineOfSightCalculator.tryStraightLine(self.goodPaths)
for compteur in range(0, self.goodPaths.__len__()):
currentPath = self.goodPaths[compteur]
currentPath.ajustDistance()
if currentPath.totalDistance < goodPath.totalDistance:
goodPath = currentPath
self.printPath(goodPath)
if goodPath.totalDistance == 99999:
self.__displayPathfinder(goodPath, positionRobot)
return False
self.theGoodPath = goodPath
self.__displayPathfinder(goodPath, positionRobot)
return goodPath
class Pathfinder:
def __init__(self, map):
self.mapAdaptator = MapAdaptator(map)
obstaclesList, mapSizeX, mapSizeY, minCorner = self.mapAdaptator.getMapInfo(
)
self.minCorner = minCorner
self.graphGenerator = GraphGenerator(obstaclesList, mapSizeX, mapSizeY)
self.graph = self.graphGenerator.generateGraph()
self.lineOfSightCalculator = LineOfSightCalculator(self.graph)
self.pathsList = []
self.goodPaths = []
self.indice = 0
self.theGoodPath = Path()
def findGoodPoint(self, point):
return self.graph.needAGoodPointToGo(point)
def findClosePoint(self, point):
return self.graph.needAClosePoint(point)
def findPath(self, positionRobot, pointToMoveTo):
print(
"@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@"
)
goodPositionRobot = self.findGoodPoint(positionRobot)
startingPathNode = self.graph.findGoodSafeNodeToGo(goodPositionRobot)
endingPathNode = self.graph.findGoodSafeNodeToGo(pointToMoveTo)
print pointToMoveTo, endingPathNode.positionX, endingPathNode.positionY
self.theGoodPath = Path()
self.pathsList = []
self.goodPaths = []
path = Path()
path.append(Node(positionRobot))
path.append(startingPathNode)
self.pathsList.append(path)
self.__findAllPaths(path, endingPathNode)
goodPath = Path()
goodPath.append(Node((0, 0)))
goodPath.totalDistance = 99999
for compteur in range(0, self.goodPaths.__len__()):
currentPath = self.goodPaths[compteur]
currentPath.append(Node(pointToMoveTo))
self.__polishGoodPaths()
self.lineOfSightCalculator.tryStraightLine(self.goodPaths)
for compteur in range(0, self.goodPaths.__len__()):
currentPath = self.goodPaths[compteur]
currentPath.ajustDistance()
if currentPath.totalDistance < goodPath.totalDistance:
goodPath = currentPath
self.printPath(goodPath)
if goodPath.totalDistance == 99999:
self.__displayPathfinder(goodPath, positionRobot)
return False
self.theGoodPath = goodPath
self.__displayPathfinder(goodPath, positionRobot)
return goodPath
def __polishGoodPaths(self):
for compteur in range(0, self.goodPaths.__len__()):
path = self.goodPaths[compteur]
nodesToBeRemoved = []
for compteur in range(1, path.__len__()):
if path[compteur].isASafeNode == True:
previousNode = path[compteur - 1]
nextNode = path[compteur + 1]
if previousNode.positionX != nextNode.positionX:
nodesToBeRemoved.append(path[compteur])
for compteur in range(0, nodesToBeRemoved.__len__()):
path.remove(nodesToBeRemoved[compteur])
def __findAllPaths(self, path, endingPathNode):
lastNode = path[-1]
if lastNode != endingPathNode and path.isOpen() == True:
for compteur in range(0, lastNode.connectedNodes.__len__()):
if (path.contains(lastNode.connectedNodes[compteur]) == False):
newPath = path.clone()
newPath.append(lastNode.connectedNodes[compteur])
self.pathsList.append(newPath)
self.__findAllPaths(newPath, endingPathNode)
self.pathsList.remove(path)
elif lastNode == endingPathNode:
self.goodPaths.append(path)
def drawPath(self, img):
for compteur in range(1, self.theGoodPath.__len__()):
startLine = (self.theGoodPath[compteur - 1].positionX +
self.minCorner[0],
self.theGoodPath[compteur - 1].positionY +
self.minCorner[1])
endLine = (self.theGoodPath[compteur].positionX +
self.minCorner[0],
self.theGoodPath[compteur].positionY +
self.minCorner[1])
cv2.line(img, (int(startLine[0]), int(startLine[1])),
(int(endLine[0]), int(endLine[1])), (0, 0, 255), 2, 1)
#methode pour afficher le path dans console, pas importante
def printPath(self, goodPath):
for compteur in range(0, goodPath.__len__()):
print "path:", goodPath[compteur].positionX, goodPath[
compteur].positionY
print goodPath.totalDistance
#methode pour display les shits du pathfinding, pas importante non plus
def __displayPathfinder(self, goodPath, positionRobot):
img = np.zeros((600, 1000, 3), np.uint8)
for compteur in range(0, self.graphGenerator.obstaclesList.__len__()):
currentObstacle = self.graphGenerator.obstaclesList[(compteur)]
cv2.rectangle(
img,
(currentObstacle.positionX - self.graphGenerator.SAFE_MARGIN,
currentObstacle.positionY - self.graphGenerator.SAFE_MARGIN),
(currentObstacle.positionX + self.graphGenerator.SAFE_MARGIN,
currentObstacle.positionY + self.graphGenerator.SAFE_MARGIN),
(0, 255, 0), -1, 1)
self.graph.nodesList.sort(key=lambda node: node.positionX)
for compteur in range(0, self.graph.nodesList.__len__()):
currentNode = self.graph.nodesList[(compteur)]
departPoint = (currentNode.positionX, currentNode.positionY)
connectedNode = currentNode.getConnectedNodesList()
#print "GRAPh", currentNode.positionX, currentNode.positionY
for compteurConnected in range(0, connectedNode.__len__()):
finalNode = connectedNode[(compteurConnected)]
finalPoint = (finalNode.positionX, finalNode.positionY)
#print " Connected", finalNode.positionX, finalNode.positionY
cv2.line(img, departPoint, finalPoint, (255, 0, 0), 2, 1)
for compteur in range(0, goodPath.__len__()):
if compteur == 0:
startLine = positionRobot
else:
startLine = (goodPath[compteur - 1].positionX,
goodPath[compteur - 1].positionY)
endLine = (goodPath[compteur].positionX,
goodPath[compteur].positionY)
cv2.line(img, (int(startLine[0]), int(startLine[1])),
(int(endLine[0]), int(endLine[1])), (0, 0, 255), 2, 1)
for compteur in range(0, self.graph.safeZonesList.__len__()):
currentZone = self.graph.safeZonesList[compteur]
cv2.rectangle(img, currentZone.cornerTopLeft,
currentZone.cornerBottomRight, (0, 150, 150), 2, 1)
cv2.imwrite('image' + str(self.indice) + '.jpg', img)
self.indice = self.indice + 1
