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()) +")")