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))