def setUp(self):
obstacleList = [Obstacle((100, 100)), Obstacle((300, 200))]
self.graph = Graph(obstacleList, 90)
self.A_NODE1 = Node((200, 300))
self.A_NODE2 = Node((400, 600))
self.graph.generateSafeZone((0, 100), (0, 0), (100, 0))
def test_whenRightObstacleIsTheImportantOne(self):
collisionUpperLeftCorner = Obstacle((100,0))
collisionUpperRightCorner = Obstacle((280,70))
currentObstacle = Obstacle((190,290))
borderNodeLeftTop = Node((100,100))
topLeftCorner = (100,200)
borderNodeRightTop = Node((280,100))
self.topPathGenerator.generateTopPath(collisionUpperLeftCorner, collisionUpperRightCorner, currentObstacle, borderNodeLeftTop, topLeftCorner, borderNodeRightTop)
assert self.graph.connectTwoNodes.called
def generateGraph(self):
firstObstacle = self.obstaclesList[0]
if self.collisionDetector.isCollidingWithWallFront(firstObstacle) == False:
self.__defaultStart(firstObstacle)
for compteur in range(0, self.obstaclesList.__len__()):
currentObstacle = self.obstaclesList[(compteur)]
startingNode = currentObstacle.startingNode
if compteur < self.obstaclesList.__len__() - 1:
nextObstacle = self.obstaclesList[compteur + 1]
if nextObstacle.startingNode.positionX == 0:
nextObstacle.setStartingNode(currentObstacle.startingNode)
currentOstacleTopLeftCorner = (currentObstacle.positionX - self.SAFE_MARGIN, currentObstacle.positionY - self.SAFE_MARGIN)
currentOstacleTopRightCorner = (currentObstacle.positionX + self.SAFE_MARGIN, currentObstacle.positionY - self.SAFE_MARGIN)
currentOstacleBottomRightCorner = (currentObstacle.positionX + self.SAFE_MARGIN, currentObstacle.positionY + self.SAFE_MARGIN)
currentOstacleBottomLeftCorner = (currentObstacle.positionX - self.SAFE_MARGIN, currentObstacle.positionY + self.SAFE_MARGIN)
collisionUpperLeftCorner, collisionUpperRightCorner, collisionBottomLeftCorner, collisionBottomRightCorner = self.collisionDetector.detectCloserObstacleForEachCornerXAxis(currentObstacle)
borderNodeLeftTop = Node(
(currentOstacleTopLeftCorner[0], (currentOstacleTopLeftCorner[1] + collisionUpperLeftCorner.positionY + self.SAFE_MARGIN) / 2))
borderNodeRightTop = Node(
(currentOstacleTopRightCorner[0], (currentOstacleTopRightCorner[1] + collisionUpperRightCorner.positionY + self.SAFE_MARGIN) / 2))
borderNodeLeftBottom = Node((currentOstacleBottomLeftCorner[0],
(currentOstacleBottomLeftCorner[1] + collisionBottomLeftCorner.positionY - self.SAFE_MARGIN) / 2))
borderNodeRightBottom = Node((currentOstacleBottomRightCorner[0],
(currentOstacleBottomRightCorner[1] + collisionBottomRightCorner.positionY - self.SAFE_MARGIN) / 2))
resultTop, collisionInnerTop = self.collisionDetector.hasUpperInnerCollision(currentObstacle)
resultBot, collisionInnerBot = self.collisionDetector.hasLowerInnerCollision(currentObstacle)
if not(self.collisionDetector.hasEndInnerCollision(currentObstacle) and (resultTop and collisionInnerTop[-1].positionY > currentObstacle.positionY) and (resultBot and collisionInnerBot[-1].positionY > currentObstacle.positionY)):
endNode = self.endNodeGenerator.generateEndNode(currentObstacle, currentOstacleTopRightCorner, currentOstacleBottomRightCorner,
collisionBottomRightCorner, collisionUpperRightCorner, compteur)
else:
#Might be the Problem
endNode = Node((1000,1000))
if self.collisionDetector.isCollidingWithWallFront(currentObstacle) == False:
self.__generateFrontPath(borderNodeLeftBottom, borderNodeLeftTop, currentObstacle, startingNode)
if self.collisionDetector.isCollidingWithWallUpper(currentObstacle) == False:
self.topPathGenerator.generateTopPath(collisionUpperLeftCorner, collisionUpperRightCorner,
currentObstacle, borderNodeLeftTop, currentOstacleTopLeftCorner, borderNodeRightTop)
if self.collisionDetector.isCollidingWithWallLower(currentObstacle) == False:
self.bottomPathGenerator.generateBottomPath(collisionBottomLeftCorner, collisionBottomRightCorner, currentObstacle,
borderNodeLeftBottom, currentOstacleBottomLeftCorner, borderNodeRightBottom)
if self.collisionDetector.isCollidingWithWallBack(currentObstacle) == False:
self.__generateEndPath(borderNodeRightBottom, borderNodeRightTop, currentObstacle, endNode)
return 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 test_whenRightObstacleIsTheImportantOne(self):
collisionBottomLeftCorner = Obstacle((100, 600))
collisionBottomRightCorner = Obstacle((280, 500))
currentObstacle = Obstacle((190, 290))
borderNodeLeftTop = Node((100, 400))
topLeftCorner = (100, 380)
borderNodeRightTop = Node((280, 400))
self.bottomPathGenerator.generateBottomPath(
collisionBottomLeftCorner, collisionBottomRightCorner,
currentObstacle, borderNodeLeftTop, topLeftCorner,
borderNodeRightTop)
assert self.graph.connectTwoNodes.called
class NodeTest(TestCase):
def setUp(self):
self.node2 = Node((200, 200))
self.node = Node((100, 110))
def test_initalisingGoodData(self):
self.assertEqual(self.node.positionX, 100)
self.assertEqual(self.node.positionY, 110)
def test_setConnectedNode(self):
self.node.addConnectedNode(self.node2)
self.assertTrue(self.node.connectedNodes.__contains__(self.node2))
def test_getConnectedNode(self):
self.assertEqual(self.node.connectedNodes,
self.node.getConnectedNodesList())
class NodeTest(TestCase):
def setUp(self):
self.node2 = Node((200,200))
self.node = Node((100,110))
def test_initalisingGoodData(self):
self.assertEqual(self.node.positionX, 100)
self.assertEqual(self.node.positionY, 110)
def test_setConnectedNode(self):
self.node.addConnectedNode(self.node2)
self.assertTrue(self.node.connectedNodes.__contains__(self.node2))
def test_getConnectedNode(self):
self.assertEqual(self.node.connectedNodes, self.node.getConnectedNodesList())
def __oneCollision_CollisionIsBottom_noCollisionBeforeTheSideOfMap(self, bottomRightCorner,
collisionBottomRightCorner, collisionUpperRightCorner):
endNode = (Node((collisionBottomRightCorner.positionX + self.SAFE_MARGIN,
(collisionBottomRightCorner.positionY + collisionUpperRightCorner.positionY) / 2)))
safeZoneCornerBotLeft = (bottomRightCorner[0], collisionBottomRightCorner.positionY - self.SAFE_MARGIN)
safeZoneCornerTopRight = (collisionBottomRightCorner.positionX + self.SAFE_MARGIN, 0 + self.SAFE_MARGIN)
safeZoneCornerTopLeft = (bottomRightCorner[0], 0 + self.SAFE_MARGIN)
tempNode = self.graph.generateSafeZone(safeZoneCornerBotLeft, safeZoneCornerTopLeft, safeZoneCornerTopRight)
self.graph.connectTwoNodes(endNode, tempNode)
return endNode
def __twoCollisionTop_NoObstacleBetweenCollisions(self, bottomRightCorner, collisionBottomRightCorner, collisionUpperRightCorner):
endNode = (Node((collisionUpperRightCorner.positionX + self.SAFE_MARGIN,
(collisionBottomRightCorner.positionY + collisionUpperRightCorner.positionY) / 2)))
safeZoneCornerBotLeft = (bottomRightCorner[0], collisionBottomRightCorner.positionY - self.SAFE_MARGIN)
safeZoneCornerTopRight = (collisionUpperRightCorner.positionX + self.SAFE_MARGIN,
collisionUpperRightCorner.positionY + self.SAFE_MARGIN)
safeZoneCornerTopLeft = (bottomRightCorner[0], collisionUpperRightCorner.positionY+self.SAFE_MARGIN)
tempNode = self.graph.generateSafeZone(safeZoneCornerBotLeft, safeZoneCornerTopLeft, safeZoneCornerTopRight)
self.graph.connectTwoNodes(endNode, tempNode)
return endNode
def findGoodSafeNodeToGo(self, point):
fakeNode = Node((0, 0))
nodeToBeReturned = fakeNode
for compteur in range(0, self.safeZonesList.__len__()):
currentZone = self.safeZonesList[compteur]
if point[0] >= currentZone.cornerTopLeft[0] and point[
0] <= currentZone.cornerBottomRight[0]:
if point[1] >= currentZone.cornerTopLeft[1] and point[
1] <= currentZone.cornerBottomRight[1]:
nodeToBeReturned = currentZone.centerNode
return nodeToBeReturned
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 ObstacleTest(TestCase):
A_POINT = (100, 200)
A_NODE = Node((200, 100))
def setUp(self):
self.obstacle = Obstacle(self.A_POINT)
def test_whenInitialiseThenGoodDataAreSet(self):
self.assertEqual(self.obstacle.positionX, self.A_POINT[0])
self.assertEqual(self.obstacle.positionY, self.A_POINT[1])
def test_setStartingNode(self):
self.obstacle.setStartingNode(self.A_NODE)
self.assertEqual(self.obstacle.startingNode, self.A_NODE)
def __CollisionOnTheLeftSide(self, borderNodeLeftTop,
collisionUpperLeftCorner, topLeftCorner):
tempNode = Node((collisionUpperLeftCorner.positionX + self.SAFE_MARGIN,
borderNodeLeftTop.positionY))
safeZoneCornerBotLeft = (borderNodeLeftTop.positionX, topLeftCorner[1])
safeZoneCornerTopRight = (tempNode.positionX,
collisionUpperLeftCorner.positionY +
self.SAFE_MARGIN)
safeZoneCornerTopLeft = (borderNodeLeftTop.positionX,
collisionUpperLeftCorner.positionY +
self.SAFE_MARGIN)
safeNode = self.graph.generateSafeZone(safeZoneCornerBotLeft,
safeZoneCornerTopLeft,
safeZoneCornerTopRight)
self.graph.connectTwoNodes(borderNodeLeftTop, safeNode)
self.graph.connectTwoNodes(safeNode, tempNode)
def __init__(self, position):
self.positionX = position[0]
self.positionY = position[1]
self.startingNode = Node((0, 0))
def setUp(self):
self.node2 = Node((200, 200))
self.node = Node((100, 110))
def test_findGoodSafeZone(self):
safeNode = self.graph.findGoodSafeNodeToGo((40, 50))
goodSafeNode = Node((50, 50))
self.assertEqual(safeNode.positionY, goodSafeNode.positionY)
self.assertEqual(safeNode.positionX, goodSafeNode.positionX)
def setUp(self):
self.path = Path()
self.path.append(Node((0, 100)))
self.path.append(Node((0, 0)))
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.node2 = Node((200,200))
self.node = Node((100,110))
