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 Positioning: #initialize positioning service def __init__(self, positionSensors, compass, lineFollower, actuators): self.positionSensors = positionSensors self.frontLeft = positionSensors.frontLeft self.frontRight = positionSensors.frontRight self.compass = compass self.lineFollower = lineFollower self.actuators = actuators self.leftWheelDistance = 0.0 self.rightWheelDistance = 0.0 self.reference = self.getActualDistance() self.lineAlreadyLost = False self.position = Position(SPX,SPY) self.orientation = UNKNOWN self.inaccurateOrientation = UNKNOWN self.updateOrientation() # set robot position in the map def setPosition(self, position): x = position.getX() y = position.getY() if x > 0 and x < Map.HEIGHT - 1: self.position.setX(position.x) if y > 0 and y < Map.WIDTH - 1: self.position.setY(position.y) #logger.warning("Invalid Position") # set robot orientation def setOrientation(self, orientation): self.orientation = orientation # get current stimated robot position def getPosition(self): return self.position # get current stimated robot orientation def getOrientation(self): return self.orientation # print status of positioning def printStatus(self): logger.debug("Orientation: " + str(self.orientation) + " - Positioning: " + str(self.position)) # update positioning service def update(self): self.printStatus() self.updateWheelTraveledDistance() self.updateOrientation() self.updatePosition() self.computePositionBasedOnLandmark() # update distance traveled by wheels using encoders def updateWheelTraveledDistance(self): # get radiants from wheel radFLW = self.frontLeft.getValue() radFRW = self.frontRight.getValue() # compute distance traveled self.leftWheelDistance = radFLW * WHEEL_RADIUS self.rightWheelDistance = radFRW * WHEEL_RADIUS # return current stimated traveled distance def getActualDistance(self): return (self.leftWheelDistance + self.rightWheelDistance) / 2.0 # update orientation using inaccurate compass orientation def updateOrientation(self): self.orientation = self.compass.getOrientation() self.inaccurateOrientation = self.compass.getInaccurateOrientation() # update positioning using map landmarks def computePositionBasedOnLandmark(self): # if the line get lost provably the robot is near an intersecion isLineLost = self.lineFollower.isLineLost() logger.debug("isLineLost: " + str(isLineLost) + " isLineAlreadyLost: " + str(self.lineAlreadyLost)) if isLineLost and not self.lineAlreadyLost: self.lineAlreadyLost = True nearestIntersecion = Map.findNearestIntersection(self.position) offset = 0.25 if nearestIntersecion != -1: x = nearestIntersecion.getX() y = nearestIntersecion.getY() if self.orientation == Orientation.NORD: nearestIntersecion.setX(x + offset) if self.orientation == Orientation.EAST: nearestIntersecion.setY(y - offset) if self.orientation == Orientation.SOUTH: nearestIntersecion.setX(x - offset) if self.orientation == Orientation.WEST: nearestIntersecion.setY(y + offset) self.position = nearestIntersecion elif not isLineLost: self.lineAlreadyLost = False # update position based on dead reckoning def updatePosition(self): speed = self.actuators.getSpeed() if speed != 0: # get actual float map position x = self.position.x y = self.position.y # 72 = 0.50 m/s * speed/MAX_SPEED [1.8] / 40 step/s / 0.5 m # linearMove = ((0.50 * (speed/MAX_SPEED)) / 40) * 2 linearMove = speed/72 # compass decimal digits precision = 2 turnCoeficent = 1 # if turning you need to do less meters in order to change position in the map steeringAngle = self.actuators.getAngle() if abs(steeringAngle) == 0.57: turnCoeficent = 1.2 # update position newX = x - round(self.compass.getXComponent(), precision) * linearMove * turnCoeficent newY = y + round(self.compass.getYComponent(), precision) * linearMove * turnCoeficent self.setPosition(Position(newX,newY))
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()) +")")