Exemplo n.º 1
0
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()) +")")
Exemplo n.º 2
0
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))
Exemplo n.º 3
0
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()) +")")