def test_5x5_sense(self):
state_actual = self.env_5x5._sense_from_position(Position(0, 0))
state_actual = state_actual.asStd()
state_expected = {0: [0, -1], 1: [1, 1], 2: [1, 1], 3: [0, -1]}
self.assertEqual(state_actual, state_expected)
state_actual = self.env_5x5._sense_from_position(Position(0, 1))
state_actual = state_actual.asStd()
state_expected = {0: [0, -1], 1: [1, 1], 2: [1, 1], 3: [1, 0]}
self.assertEqual(state_actual, state_expected)
state_actual = self.env_5x5._sense_from_position(Position(0, 4))
state_actual = state_actual.asStd()
state_expected = {0: [0, -1], 1: [0, -1], 2: [1, 1], 3: [1, 1]}
self.assertEqual(state_actual, state_expected)
state_actual = self.env_5x5._sense_from_position(Position(4, 4))
state_actual = state_actual.asStd()
state_expected = {0: [1, 1], 1: [0, -1], 2: [0, -1], 3: [1, 1]}
self.assertEqual(state_actual, state_expected)
def __init__(self):
# initialize devices
self.initializeDevices()
# initial speed and steering angle values
self.speed = 0.0
self.angle = 0.0
# initial wheel length used to calculate how many m the wheels have traveled
self.leftWheelDistance = 0.0
self.rightWheelDistance = 0.0
self.updateDistanceTraveled()
# line forwared
self.lineFollower = LineFollower(self.camera)
# navigation
self.nav = PathPlanner()
self.nav.setRobotPosition(Position(4, 1))
self.nav.setGoalPosition(Position(14, 22))
self.nav.setRobotOrientation(SOUTH)
self.nav.printStatus()
# compute fastest route from robot to goal
self.turns = self.nav.getFastestRoute()
logger.debug("turns: " + str(self.turns))
# odometry
self.odometry = Odometry(self.positionSensors, self.compass,
self.nav.getRobotPosition(),
self.nav.getRobotOrientation())
def _make_action(self, rid, action):
"""
:param rid: id of the agent
:param action: action for the agent
:return: tuple of following:
state_prime: new state for the agent
reward: reward for the agent
done: agent terminated or not
info: anything else
"""
agent = self.agents[rid]
pos = agent.pos
# action: 0-north, 1-east, 2-south, 3-west
new_pos = Position(-1, -1)
if action == Direction.UP or action == Direction.UP.value:
new_pos = pos.up()
elif action == Direction.RIGHT or action == Direction.RIGHT.value:
new_pos = pos.right()
elif action == Direction.DOWN or action == Direction.DOWN.value:
new_pos = pos.down()
elif action == Direction.LEFT or action == Direction.LEFT.value:
new_pos = pos.left()
else:
print("Invalid direction: %s" % (action))
agent._set_pos(new_pos)
try:
assert new_pos >= Position(
0, 0), "out of bounds - outside map (below_min)"
assert new_pos < Position(
self.height,
self.width), "out of bounds - outside map (above_max)"
assert self.grid[
new_pos.asTuple()] != 0, "out of bounds - internal edge"
except Exception as e:
# print("position:", new_pos, "is", e)
# print("\tRemember (in y,x format) the grid size is", self.grid.shape)
self.dead = True
reward = self._get_reward(new_pos)
state_prime = agent._get_state(new_pos)
return StepResponse(state_prime, reward, True)
# return None, reward, True, None
state_prime = agent._get_state(new_pos)
reward = self._get_reward(new_pos)
done = self._get_terminate(new_pos)
resp = StepResponse(state_prime, reward, done)
return resp
def test_2x1_sense(self):
state_actual = self.env_2x1._sense_from_position(Position(0, 0))
state_actual = state_actual.asStd()
state_expected = {0: [0], 1: [1], 2: [0], 3: [0]}
self.assertEqual(state_actual, state_expected)
state_actual = self.env_2x1._sense_from_position(Position(0, 1))
state_actual = state_actual.asStd()
state_expected = {0: [0], 1: [0], 2: [0], 3: [1]}
self.assertEqual(state_actual, state_expected)
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))
def findNearestIntersection(position, radius = 1, orientation = False):
x = position.getX()
y = position.getY()
for i in range(x-radius, x+radius +1):
for j in range(y-radius, y+radius +1):
if i < HEIGHT and i > 0 and j < WIDTH and j > 0:
if MAP[i][j] == I:
return Position(i, j)
return -1
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()
def setUp(self):
self.env_2x1 = Environment(width=2,
height=1,
num_agents=1,
start=Position(0, 0),
goal=Position(0, 1),
view_range=1)
self.env_5x5 = Environment(width=5,
height=5,
num_agents=1,
start=Position(0, 0),
goal=Position(4, 4),
view_range=2)
self.env_3x3_std = Environment(width=3,
height=3,
num_agents=1,
start=(0, 0),
goal=(2, 2),
view_range=1,
std=True)
def getNearestWalkablePosition(position, orientation):
if not isWalkable(position):
logger.debug("Actual position non walkable. " + str(position) + " is unwalkable")
x = position.getX()
y = position.getY()
logger.debug("ORIENTATION: " + str(orientation))
if orientation == Orientation.NORD or orientation == Orientation.SOUTH:
p = Position(x, y - 1)
if isWalkable(p):
return p
p = Position(x, y + 1)
if isWalkable(p):
return p
elif orientation == Orientation.EAST or orientation == Orientation.WEST:
p = Position(x - 1, y)
if isWalkable(p):
return p
p = Position(x + 1, y)
if isWalkable(p):
return p
else:
return position
def getNearestWalkablePosition2(position, orientation):
if not isWalkable(position):
x = position.getX()
y = position.getY()
radius = 1
for i in range(x-radius, x+radius +1):
for j in range(y-radius, y+radius +1):
if i < HEIGHT and i > 1 and j < WIDTH and j > 1:
p = Position(x+i, y+j)
if isWalkable(p):
return p
else:
return position
def getNearestWalkablePositionEquals(position, orientation, value):
if not isWalkable(position):
logger.debug("Actual position non walkable. " + str(position) + " is unwalkable")
x = position.getX()
y = position.getY()
if orientation == Orientation.NORD or orientation == Orientation.SOUTH:
p = Position(x+1, y)
if isWalkable(p) and getValue(p) == value:
return p
p = Position(x-1, y)
if isWalkable(p) and getValue(p) == value:
return p
elif orientation == Orientation.EAST or orientation == Orientation.WEST:
p = Position(x, y+1)
if isWalkable(p) and getValue(p) == value:
return p
p = Position(x, y-1)
if isWalkable(p) and getValue(p) == value:
return p
elif getValue(position) == value:
return position
else:
return -1
def updatePath(self):
if self.isEnabled():
logger.info("Computing new path..")
p = self.positioning.getPosition()
o = self.positioning.getOrientation()
nearest = Map.getNearestWalkablePosition(p, o)
if nearest != None:
p = nearest
x = p.getX()
y = p.getY()
if o == Orientation.NORD:
Map.setNewObstacle(Position(x - 1, y))
if o == Orientation.EAST:
Map.setNewObstacle(Position(x, y + 1))
if o == Orientation.SOUTH:
Map.setNewObstacle(Position(x + 1, y))
if o == Orientation.WEST:
Map.setNewObstacle(Position(x, y - 1))
if DEBUG:
Map.printMap()
self.currentPath = self.pathPlanner.getFastestRoute()
self.actualTurn = 0
def __init__(self,
height=5,
width=5,
num_agents=1,
start=Position(0, 0),
goal=Position(9E9, 9E9),
view_range=1,
horizon=2,
goal_reward=1.0,
pos_hist_len=5,
render=False,
std=False):
self.std = std
self.width = width
self.height = height
self.num_agents = num_agents
self.start = start
self.goal = goal
self.view_range = view_range
self.dead = False
self.pos_hist_len = pos_hist_len
if self.std:
start = Position(y=start[0], x=start[1])
goal = Position(y=goal[0], x=goal[1])
if goal.x > self.width or goal.y > self.height:
goal = Position(self.height - 1, self.width - 1)
# self.grid stores state of each grid of the map
# 0-obstacle, 1-walkable, 2-goal, 3-agent
self.grid = np.ones((height, width), dtype=int)
self.grid[goal.asTuple()] = 2
self.num_agents = num_agents
self.agents = {}
for n in range(self.num_agents):
self.agents[n] = Agent(self,
pos_hist_len=self.pos_hist_len,
pos_start=self.start)
self.reward_map = RewardMap(self.goal, self.height, self.width,
horizon, goal_reward)
# self.reward_map = np.zeros((height,width))
# self.reward_map[goal.asTuple()] = 1.0
self.reward_death = -100.0
self.renderEnv = render
self.called_render = False
if self.renderEnv:
self._render()
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()) +")")
# self.im.set_data(grid_copy)
self.im.set_data(self.grid_copy)
self.fig.suptitle("Agent in grid world, plus edges")
plt.draw()
plt.show(block=block)
# if (iteration % 2 == 0 and iteration <= 8) or iteration == 40:
# fig.savefig("P=%.1f_t=%d_Iteration=%d.png" % (P, t, iteration))
plt.pause(0.001)
if __name__ == "__main__":
env = Environment(width=5,
height=5,
num_agents=1,
start=Position(0, 0),
goal=Position(4, 4),
view_range=3,
render=True)
print(env.get_state()[0].as1xnArray().shape[0])
"""
env.step returns a library of agent stepResponses,
each stepResponse.asTuple() is a tuple w/ (State Obj, reward, done, info)
each state object
"""
agents = env.step({0: Direction.RIGHT})
stepResponse = agents[0]
# print(agents[0].asTuple())
def __init__(self, imageName):
self._pos = Position(1, 1)
self._isTransposable = True
self._isMortal = False
def __init__(self):
self.map = MAP
self.robotPosition = Position(0, 0)
self.robotOrientation = NORD
self.goalPosition = Position (0, 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()) +")")
def __init__(self, positioning):
self.map = Map.MAP
self.positioning = positioning
self.robotPosition = positioning.getPosition()
self.robotOrientation = positioning.getOrientation()
self.goalPosition = Position (14, 23)
from Environment import Environment
from Utils import Agent, Direction, Position, StepResponse
import curses
import time
env = Environment(width=20,
height=20,
num_agents=1,
start=Position(0, 0),
goal=Position(19, 19),
view_range=2,
render=True)
def main(stdscr):
# do not wait for input when calling getch
stdscr.nodelay(1)
done = False
while not done:
# get keyboard input, returns -1 if none available
c = stdscr.getch()
if c >= 258 and c <= 261:
# print numeric value
if c == 258:
a = 2
elif c == 259:
a = 0
elif c == 260:
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))
