class Navigation: def __init__(self): self.map = MAP self.robotPosition = Position(0, 0) self.robotOrientation = NORD self.goalPosition = Position (0, 0) # return array containing a turn for each intersection in the path between robot position and goal def getFastestRoute(self): # get fastest route from AStar giving map, start position and goal position route = AStar.findPath(self.map, self.robotPosition.getPositionArray(), self.goalPosition.getPositionArray()) # get only intersection nodes from AStar route intersections = self.getIntersectionNodesFromRoute(route) # get cardinal points directions based on intersection nodes directions = self.getDirectionsFromIntersections(intersections) # get turns based on robot directions and robot orientation turns = self.getTurnsFromDirections(directions) # remove curve turns turns = self.removeCurves(turns, intersections) # return the turns return turns # return first, last and intersection nodes from AStar route def getIntersectionNodesFromRoute(self, route): intersections = [] #if self.map[route[0][0]][route[0][1]] == I: # get first node intersections.append(route[0]) # get intersection nodes for node in route[1:-1]: if self.map[node[0]][node[1]] == I or self.map[node[0]][node[1]] == C: intersections.append(node) # get last node intersections.append(route[-1]) # return intersections return intersections # return cardinal points direction from one intersection to another def getDirectionsFromIntersections(self, intersections): directions = [] # for each cople of nodes compute cardinal points direction between them prevNode = intersections[0] for currentNode in intersections[1:]: # check if X has changed if currentNode[0] > prevNode[0]: directions.append(SOUTH) elif currentNode[0] < prevNode[0]: directions.append(NORD) # check if Y has changed elif currentNode[1] > prevNode[1]: directions.append(EAST) elif currentNode[1] < prevNode[1]: directions.append(WEST) else: logger.error("Invalid intersetions") # go to next couple of node prevNode = currentNode return directions # contains a list of turns (RIGHT, LEFT, FORWARD, U_TURN) for each intersection based on robot orientation and next direction def getTurnsFromDirections(self, directions): turns = [] # get actual robot orientation actualDirection = self.robotOrientation # for each cardinal point direction compute turn based on robot current and future orientation for direction in directions: # FORWARD case if actualDirection == direction: turns.append(FORWARD) # EST cases elif actualDirection == EAST and direction == SOUTH: turns.append(RIGHT) elif actualDirection == EAST and direction == NORD: turns.append(LEFT) elif actualDirection == EAST and direction == WEST: turns.append(U_TURN) # WEST cases elif actualDirection == WEST and direction == SOUTH: turns.append(LEFT) elif actualDirection == WEST and direction == NORD: turns.append(RIGHT) elif actualDirection == WEST and direction == EAST: turns.append(U_TURN) # NORD cases elif actualDirection == NORD and direction == EAST: turns.append(RIGHT) elif actualDirection == NORD and direction == WEST: turns.append(LEFT) elif actualDirection == NORD and direction == SOUTH: turns.append(U_TURN) # SOUTH cases elif actualDirection == SOUTH and direction == EAST: turns.append(LEFT) elif actualDirection == SOUTH and direction == WEST: turns.append(RIGHT) elif actualDirection == SOUTH and direction == NORD: turns.append(U_TURN) # change actual direction actualDirection = direction return turns # remove curves from turns def removeCurves(self, turns, intersections): newTurns = [turns[0]] for i in range(1, len(intersections) - 1): node = intersections[i] if self.map[node[0]][node[1]] != C: newTurns.append(turns[i]) return newTurns # set robot position in the map def setRobotPosition(self, position): x = position.getX() y = position.getY() if x > 0 and x < HEIGHT - 1: self.robotPosition.setX(x) if y > 0 and y < WIDTH - 1: self.robotPosition.setY(y) def getRobotPosition(self): return self.robotPosition # set robot orientation def setRobotOrientation(self, orientation): if orientation >= 0 and orientation <= 4: self.robotOrientation = orientation def getRobotOrientation(self): return self.robotOrientation #set goal position in the map def setGoalPosition(self, position): x = position.getX() y = position.getY() if x > 0 and x < HEIGHT - 1: self.goalPosition.setX(x) if y > 0 and y < WIDTH - 1: self.goalPosition.setY(y) # print actual status def printStatus(self): robotPosition = self.robotPosition goalPosition = self.goalPosition robotOrientation = "UNKNOWN" if self.robotOrientation == NORD: robotOrientation = "NORD" if self.robotOrientation == EAST: robotOrientation = "EST" if self.robotOrientation == SOUTH: robotOrientation = "SOUTH" if self.robotOrientation == WEST: robotOrientation = "WEST" print("Navigation Status: ") print("Robot Position: " + "(X: " + str(robotPosition.getX()) + ", Y: " + str(robotPosition.getY()) +")") print("Robot Orientation: " + str(robotOrientation)) print("Goal Position: " + "(X: " + str(goalPosition.getX()) + ", Y: " + str(goalPosition.getY()) +")")
class PathPlanner: # initialize path planning service def __init__(self, positioning): self.map = Map.MAP self.positioning = positioning self.robotPosition = positioning.getPosition() self.robotOrientation = positioning.getOrientation() self.goalPosition = Position (14, 23) # update path planning service def update(self): self.robotPosition = self.positioning.getPosition() self.robotOrientation = self.positioning.getOrientation() # update map to improve path planning when obstacle are found def updateMap(self): self.map = Map.MAP # return array containing a turn for each intersection in the path between robot position and goal def getFastestRoute(self): # update map status, this ensure new obstacles are detected self.updateMap() logger.debug("Path from: " + str(self.robotPosition) + " to " + str(self.goalPosition) + " Initial Orientation: " + str(self.robotOrientation)) # get fastest route from AStar giving map, start position and goal position route = AStar.findPath(self.map, self.robotPosition.getPositionArray(), self.goalPosition.getPositionArray()) # if no route was found, return UNKNOWN path if route == None: return UNKNOWN # get only intersection nodes from AStar route intersections = self.getIntersectionNodesFromRoute(route) # get cardinal points directions based on intersection nodes directions = self.getDirectionsFromIntersections(intersections) logger.debug(directions) # get turns based on robot directions and robot orientation turns = self.getTurnsFromDirections(directions) logger.debug(turns) # remove curve turns turns = self.removeCurves(turns, intersections) # return the turns return turns #set goal position in the map def setGoalPosition(self, position): x = position.getX() y = position.getY() if x > 0 and x < Map.HEIGHT - 1: if y > 0 and y < Map.WIDTH - 1: self.goalPosition.setY(y) self.goalPosition.setX(x) return self.goalPosition = UNKNOWN def getGoalPosition(self): return self.goalPosition # return first, last and intersection nodes from AStar route def getIntersectionNodesFromRoute(self, route): intersections = [] #if self.map[route[0][0]][route[0][1]] == I: # get first node if route != None: intersections.append(route[0]) # get intersection nodes for node in route[1:-1]: if self.map[node[0]][node[1]] == Map.I or self.map[node[0]][node[1]] == Map.C: intersections.append(node) # get last node intersections.append(route[-1]) # return intersections return intersections # return cardinal points direction from one intersection to another def getDirectionsFromIntersections(self, intersections): directions = [] # for each cople of nodes compute cardinal points direction between them prevNode = intersections[0] for currentNode in intersections[1:]: # check if X has changed if currentNode[0] > prevNode[0]: directions.append(Orientation.SOUTH) elif currentNode[0] < prevNode[0]: directions.append(Orientation.NORD) # check if Y has changed elif currentNode[1] > prevNode[1]: directions.append(Orientation.EAST) elif currentNode[1] < prevNode[1]: directions.append(Orientation.WEST) else: logger.error("Invalid intersetions") # go to next couple of node prevNode = currentNode return directions # contains a list of turns (RIGHT, LEFT, FORWARD, U_TURN) for each intersection based on robot orientation and next direction def getTurnsFromDirections(self, directions): turns = [] # get actual robot orientation actualDirection = self.robotOrientation # for each cardinal point direction compute turn based on robot current and future orientation for direction in directions: # FORWARD case if actualDirection == direction: turns.append(FORWARD) # EST cases elif actualDirection == Orientation.EAST and direction == Orientation.SOUTH: turns.append(RIGHT) elif actualDirection == Orientation.EAST and direction == Orientation.NORD: turns.append(LEFT) elif actualDirection == Orientation.EAST and direction == Orientation.WEST: turns.append(U_TURN) # WEST cases elif actualDirection == Orientation.WEST and direction == Orientation.SOUTH: turns.append(LEFT) elif actualDirection == Orientation.WEST and direction == Orientation.NORD: turns.append(RIGHT) elif actualDirection == Orientation.WEST and direction == Orientation.EAST: turns.append(U_TURN) # NORD cases elif actualDirection == Orientation.NORD and direction == Orientation.EAST: turns.append(RIGHT) elif actualDirection == Orientation.NORD and direction == Orientation.WEST: turns.append(LEFT) elif actualDirection == Orientation.NORD and direction == Orientation.SOUTH: turns.append(U_TURN) # SOUTH cases elif actualDirection == Orientation.SOUTH and direction == Orientation.EAST: turns.append(LEFT) elif actualDirection == Orientation.SOUTH and direction == Orientation.WEST: turns.append(RIGHT) elif actualDirection == Orientation.SOUTH and direction == Orientation.NORD: turns.append(U_TURN) # change actual direction actualDirection = direction return turns # remove curves from turns def removeCurves(self, turns, intersections): newTurns = [turns[0]] for i in range(1, len(intersections) - 1): node = intersections[i] if self.map[node[0]][node[1]] != Map.C: newTurns.append(turns[i]) return newTurns # print actual status def printStatus(self): robotPosition = self.robotPosition goalPosition = self.goalPosition robotOrientation = "UNKNOWN" if self.robotOrientation == Orientation.NORD: robotOrientation = "Orientation.NORD" if self.robotOrientation == Orientation.EAST: robotOrientation = "EST" if self.robotOrientation == Orientation.SOUTH: robotOrientation = "Orientation.SOUTH" if self.robotOrientation == Orientation.WEST: robotOrientation = "Orientation.WEST" print("Navigation Status: ") print("Robot Position: " + "(X: " + str(robotPosition.getX()) + ", Y: " + str(robotPosition.getY()) +")") print("Robot Orientation: " + str(robotOrientation)) print("Goal Position: " + "(X: " + str(goalPosition.getX()) + ", Y: " + str(goalPosition.getY()) +")")