def __init__(self,
exploredMap,
start,
goal,
direction,
waypoint=None,
calibrateLim=5,
sim=True):
self.exploredMap = exploredMap
self.graph = []
self.start = start
self.goal = goal
self.waypoint = waypoint
self.index = 1
self.direction = direction
self.path = []
self.movement = []
self.calibrateLim = calibrateLim
self.sim = sim
if sim:
from Simulator import Robot
self.robot = Robot(self.exploredMap, direction, start, None)
else:
from Real import Robot
self.robot = Robot(self.exploredMap, direction, start)
def __init__(self, realMap=None, timeLimit=None, calibrateLim=6, sim=True):
"""Constructor to initialise an instance of the Exploration class
Args:
realMap (string): File name for real map during simulation stage
timeLimit (int): Maximum time allowed for exploration
sim (bool, optional): To specify is the exploration mode is simulation or real
"""
self.timeLimit = timeLimit
self.exploredArea = 0
self.currentMap = np.zeros([20, 15])
# Mark start as free
self.currentMap[17:20, 0:3] = 1
self.currentMap[0:3, 12:15] = 1
# call class according to running simulator or real run
if sim:
from Simulator import Robot
self.robot = Robot(self.currentMap, EAST, START, realMap)
self.sensors = self.robot.getSensors(
) # Returns a numpy array of numpy arrays of the sensor values from all sensors
else:
from Real import Robot
self.robot = Robot(self.currentMap, EAST, START)
self.exploredNeighbours = dict()
self.sim = sim
# self.calibrateLim = calibrateLim
# initialize as boundary of the arena
# Set the limits of unexplored area on the map
self.virtualWall = [0, 0, MAX_ROWS, MAX_COLS]
self.alignRightCount = 0
self.justCheckedRight = False
def __init__(self,
exploredMap,
start,
goal,
direction,
waypoint=None,
calibrateLim=5,
sim=True):
"""Constructor to initialize an instance of the FastestPath class
Args:
exploredMap (Numpy array): The map after the exploration stage
start (list): Coordinate of the initial cell
goal (list): Coordinate of the goal cell
direction (int): The starting direction of the robot
waypoint (list, optional): The coordinate of the way-point if specified
sim (bool, optional): If specify if run is simulation or real
"""
self.exploredMap = exploredMap
self.graph = []
self.start = start
self.goal = goal
self.waypoint = waypoint
self.index = 1
self.direction = direction
self.path = []
self.movement = []
self.calibrateLim = calibrateLim
self.sim = sim
if sim:
from Simulator import Robot
self.robot = Robot(self.exploredMap, direction, start, None)
else:
from Real import Robot
self.robot = Robot(self.exploredMap, direction, start)
def __init__(self, realMap=None, timeLimit=None, calibrateLim=6, sim=True):
self.timeLimit = timeLimit
self.exploredArea = 0
self.currentMap = np.zeros([20, 15])
if sim:
from Simulator import Robot
self.robot = Robot(self.currentMap, EAST, START, realMap)
self.sensors = self.robot.getSensors()
else:
from Real import Robot
self.robot = Robot(self.currentMap, EAST, START)
self.exploredNeighbours = dict()
self.sim = sim
self.calibrateLim = calibrateLim
self.virtualWall = [0, 0, MAX_ROWS, MAX_COLS]
def __init__(self, realMap=None, timeLimit=None, calibrateLim=6, sim=True):
"""Constructor to initialise an instance of the Exploration class
Args:
realMap (string): File name for real map during simulation stage
timeLimit (int): Maximum time allowed for exploration
sim (bool, optional): To specify is the exploration mode is simulation or real
"""
self.timeLimit = timeLimit
self.exploredArea = 0
self.currentMap = np.zeros([20, 15])
if sim:
from Simulator import Robot
self.robot = Robot(self.currentMap, EAST, START, realMap)
self.sensors = self.robot.getSensors()
else:
from Real import Robot
self.robot = Robot(self.currentMap, EAST, START)
self.exploredNeighbours = dict()
self.sim = sim
self.calibrateLim = calibrateLim
self.virtualWall = [0, 0, MAX_ROWS, MAX_COLS]
class Exploration:
"""Implementation of the Right-Wall hugging algorithm for a maze solving
robot.
The implementation assumes that the robot starts at the bottom-left corner of the map,
i.e. (MAX_ROWS - 2, 1). And the robot is facing North
Attributes:
currentMap (Numpy array): To store the current state of the exploration map
exploredArea (float): Percentage of arena explored
robot (Robot): Instance of the Robot class
sensors (list of Numpy arrays): Readings from all sensors
timeLimit (int): Maximum time allowed for exploration
"""
def __init__(self, realMap=None, timeLimit=None, calibrateLim=6, sim=True):
"""Constructor to initialise an instance of the Exploration class
Args:
realMap (string): File name for real map during simulation stage
timeLimit (int): Maximum time allowed for exploration
sim (bool, optional): To specify is the exploration mode is simulation or real
"""
self.timeLimit = timeLimit
self.exploredArea = 0
self.currentMap = np.zeros([20, 15])
# Mark start as free
self.currentMap[17:20, 0:3] = 1
self.currentMap[0:3, 12:15] = 1
# call class according to running simulator or real run
if sim:
from Simulator import Robot
self.robot = Robot(self.currentMap, EAST, START, realMap)
self.sensors = self.robot.getSensors(
) # Returns a numpy array of numpy arrays of the sensor values from all sensors
else:
from Real import Robot
self.robot = Robot(self.currentMap, EAST, START)
self.exploredNeighbours = dict()
self.sim = sim
# self.calibrateLim = calibrateLim
# initialize as boundary of the arena
# Set the limits of unexplored area on the map
self.virtualWall = [0, 0, MAX_ROWS, MAX_COLS]
self.alignRightCount = 0
self.justCheckedRight = False
def __validInds(self, inds):
"""To check if the passed indices are valid or not
To be valid the following conditions should be met:
* A 3x3 neighbourhood around the center should lie within the arena
* A 3x3 neighbourhood around the center should have no obstacle
Args:
inds (list of list): List of coordinates to be checked
Returns:
list of list: All indices that were valid
"""
valid = []
for i in inds:
r, c = i
x, y = np.meshgrid([-1, 0, 1], [-1, 0, 1])
# np.meshgrid([-1, 0, 1], [-1, 0, 1]) outputs
# [array([[-1, 0, 1],
# [-1, 0, 1],
# [-1, 0, 1]]),
# array([[-1, -1, -1],
# [ 0, 0, 0],
# [ 1, 1, 1]])]
# x is row coordinate while y is column coordinate
x, y = x + r, y + c
# If any of the x coordinate is less than zero
# or if any of the y coordinate is less than
# or if any of the x coordinate is greater than or equal to the maximum number of rows
# or if any of the y coordinates is greater than or equal to the maximum number of columns
if (np.any(x < 0) or np.any(y < 0) or np.any(x >= MAX_ROWS)
or np.any(y >= MAX_COLS)):
# Indicate that the coordinate is invalid
valid.append(False)
# If the value of any of the coordinate is not 1 (meaning that it is free)
# We assume that there is an obstacle in the 3 by 3 area
elif (np.any(self.currentMap[x[0, 0]:x[0, 2] + 1,
y[0, 0]:y[2, 0] + 1] != 1)):
# Indicate that the coordinate is invalid
valid.append(False)
# If coordinates within arena and there are no obstacles
else:
# Indicate that the coordinate is valid
valid.append(True)
# Return all valid coordinates
return [tuple(inds[i]) for i in range(len(inds)) if valid[i]]
def getExploredArea(self):
"""Updates the total number of cells explored at the current state
"""
# returns percent of map explored
self.exploredArea = (np.sum(self.currentMap != 0) / 300.0) * 100
def nextMove(self):
# Decides which direction is free and commands the robot the next action
move = []
if (not (self.sim) and self.robot.is_corner() == True):
move = [ALIGNFRONT, LEFT]
self.robot.moveBot(LEFT)
else:
if not (self.sim):
# Check if there is a wall in front for robot to calibrate
calibrate_front = self.robot.can_calibrate_front()
# Check if there is a wall to the right for the robot to calibrate
calibrate_right = self.robot.can_calibrate_right()
# If the robot is at a corner
if self.robot.is_corner():
move.append(ALIGNFRONT)
move.append(ALIGNRIGHT)
# If robot is not at a corner but there is a wall to the right for calibration
elif (calibrate_right[0]):
self.alignRightCount += 1
if (self.alignRightCount % 3) == 0:
move.append(RIGHT)
move.append(ALIGNFRONT)
move.append(LEFT)
else:
# Append command from can_calibrate_right function
move.append(calibrate_right[1])
# If robot is not at a corner and there is no wall to the right of the robot
# If there is a wall to the front for calibration
elif (calibrate_front[0]):
# Append command from can_calibrate_front function
move.append(calibrate_front[1])
# multi step
# Number of spaces in front of the robot which is free and where there are obstacles on the right
# and all spaces detectable by the left middle, right top and right bottom sensors at these spaces have been explored
front = self.frontFree()
# If right of robot is free
if (self.justCheckedRight == True):
self.justCheckedRight = False
if (front):
# Move robot forward
for i in range(front):
self.robot.moveBot(FORWARD)
move.extend([FORWARD] * front)
elif (self.checkFree([1, 2, 3, 0], self.robot.center)):
# Move robot to the right
self.robot.moveBot(RIGHT)
move.append(RIGHT)
front = self.frontFree()
# Move robot forward according to frontFree function
for i in range(front):
self.robot.moveBot(FORWARD)
move.extend([FORWARD] * front)
# Else if the robot's left is free
elif (self.checkFree([3, 0, 1, 2], self.robot.center)):
# Move robot to the left
self.robot.moveBot(LEFT)
move.append(LEFT)
front = self.frontFree()
for i in range(front):
self.robot.moveBot(FORWARD)
move.extend([FORWARD] * front)
elif (self.checkLeftUnexplored()):
self.robot.moveBot(LEFT)
move.append(LEFT)
# Else, turn the robot around
else:
self.robot.moveBot(RIGHT)
self.robot.moveBot(RIGHT)
move.append(RIGHT)
move.append(RIGHT)
else:
if (self.checkFree([1, 2, 3, 0], self.robot.center)):
# Move robot to the right
self.robot.moveBot(RIGHT)
move.append(RIGHT)
front = self.frontFree()
# Move robot forward according to frontFree function
for i in range(front):
self.robot.moveBot(FORWARD)
move.extend([FORWARD] * front)
elif (self.checkRightUnexplored()):
self.justCheckedRight = True
self.robot.moveBot(RIGHT)
move.append(RIGHT)
# If front > 0
elif (front):
# Move robot forward
for i in range(front):
self.robot.moveBot(FORWARD)
move.extend([FORWARD] * front)
# Else if the robot's left is free
elif (self.checkFree([3, 0, 1, 2], self.robot.center)):
# Move robot to the left
self.robot.moveBot(LEFT)
move.append(LEFT)
front = self.frontFree()
for i in range(front):
self.robot.moveBot(FORWARD)
move.extend([FORWARD] * front)
elif (self.checkLeftUnexplored()):
self.robot.moveBot(LEFT)
move.append(LEFT)
# Else, turn the robot around
else:
self.robot.moveBot(RIGHT)
self.robot.moveBot(RIGHT)
move.append(RIGHT)
move.append(RIGHT)
# Return list of moves
return move
def checkFree(self, order, center):
"""Checks if a specific direction is free to move to
Args:
order (list): Ordering for the directionFree list based on the
the next move (Right, Left, Forward)
Returns:
bool: If the queried direction is free
"""
# Creates an array of boolean values
directionFree = np.asarray([
self.northFree(center),
self.eastFree(center),
self.southFree(center),
self.westFree(center)
])
directionFree = directionFree[order]
# If directionFree = np.asarray(["North", "East", "South", "West"])
# directionFree[[1, 2, 3, 0]] = ['East' 'South' 'West' 'North']
# directionFree[[3, 0, 1, 2]] = ['West' 'North' 'East' 'South']
if self.robot.direction == NORTH:
return directionFree[0]
elif self.robot.direction == EAST:
return directionFree[1]
elif self.robot.direction == SOUTH:
return directionFree[2]
else:
return directionFree[3]
def checkLeftUnexplored(self):
r, c = self.robot.center
if self.robot.direction == NORTH:
if (c - 2 < 0):
return False
return (self.currentMap[r][c - 2] == 0
or self.currentMap[r + 1][c - 2] == 0)
elif self.robot.direction == EAST:
if (r - 2 < 0):
return False
return (self.currentMap[r - 2][c] == 0
or self.currentMap[r - 2][c - 1] == 0)
elif self.robot.direction == SOUTH:
if (c + 2 >= MAX_COLS):
return False
return (self.currentMap[r][c + 2] == 0
or self.currentMap[r - 1][c + 2] == 0)
else:
if (r + 2 >= MAX_ROWS):
return False
return (self.currentMap[r + 2][c] == 0
or self.currentMap[r + 2][c + 1] == 0)
def checkRightUnexplored(self):
r, c = self.robot.center
if self.robot.direction == NORTH:
if (c + 2 >= MAX_COLS):
return False
return (self.currentMap[r][c + 2] == 0)
elif self.robot.direction == EAST:
if (r + 2 >= MAX_ROWS):
return False
return (self.currentMap[r + 2][c] == 0)
elif self.robot.direction == SOUTH:
if (c - 2 < 0):
return False
return (self.currentMap[r][c - 2] == 0)
else:
if (r - 2 < 0):
return False
return (self.currentMap[r - 2][c] == 0)
def validMove(self, inds):
"""Checks if all the three cells on one side of the robot are free
Args:
inds (list of list): List of cell indices to be checked
Returns:
bool: If all indices are free (no obstacle)
"""
for (r, c) in inds:
# self.virtualWall indicates the boundaries of unexplored area
# If the indices are not within the unexplored area
if not ((0 <= r < MAX_ROWS) and (0 <= c < MAX_COLS)):
# Indicate that the move is not valid
return False
# Check if all three indices have a value of 1 (Explored and is free)
return (self.currentMap[inds[0][0], inds[0][1]] == 1
and self.currentMap[inds[1][0], inds[1][1]] == 1
and self.currentMap[inds[2][0], inds[2][1]] == 1)
def northFree(self, center):
"""Checks if the north direction is free to move
Returns:
bool: if north is free
"""
r, c = center
# Checks if the three spaces immediately to the north of the robot is free and within the unexplored area
inds = [[r - 2, c], [r - 2, c - 1], [r - 2, c + 1]]
return self.validMove(inds)
def eastFree(self, center):
"""Checks if the east direction is free to move
Returns:
bool: if east is free
"""
r, c = center
# Checks if the three spaces immediately to the east of the robot is free and within the unexplored area
inds = [[r, c + 2], [r - 1, c + 2], [r + 1, c + 2]]
return self.validMove(inds)
def southFree(self, center):
"""Checks if the south direction is free to move
Returns:
bool: if south is free
"""
r, c = center
# Checks if the three spaces immediately to the south of the robot is free and within the unexplored area
inds = [[r + 2, c], [r + 2, c - 1], [r + 2, c + 1]]
return self.validMove(inds)
def westFree(self, center):
"""Checks if the west direction is free to move
Returns:
bool: if west is free
"""
r, c = center
# Checks if the three spaces immediately to the west of the robot is free and within the unexplored area
inds = [[r, c - 2], [r - 1, c - 2], [r + 1, c - 2]]
return self.validMove(inds)
def frontFree(self):
"""
returns number of spaces in front of the robot which is free and where there are obstacles on the right
and all spaces detectable by the left middle, right top and right bottom sensors at these spaces have been explored
"""
r, c = self.robot.center
counter = 0 # Keeps track of number of moves the robot can make that are directly in front
# If robot is facing north and three spaces immediately to the north of robot is within unexplored boundaries
# and has no obstacles
if self.robot.direction == NORTH and self.validMove(
[[r - 2, c], [r - 2, c - 1], [r - 2, c + 1]]):
# Increase counter by 1
counter = 1
# while(True):
# # If all three spaces immediately to the north of the robot is free
# # and the three spaces immediately to the east of the robot is not free
# # and all coordinates reachable by sensors are explored at position with coordinate [r-counter, c]
# # increase counter by 1
# if (self.validMove([[r-2-counter, c], [r-2-counter, c-1], [r-2-counter, c+1]])) and\
# not self.checkFree([1, 2, 3, 0], [r-(counter), c]) and\
# self.checkExplored([r-(counter), c]):
# counter += 1
# else:
# break
elif self.robot.direction == EAST and self.validMove(
[[r, c + 2], [r - 1, c + 2], [r + 1, c + 2]]):
counter = 1
# while(True):
# if (self.validMove([[r, c+2+counter], [r-1, c+2+counter], [r+1, c+2+counter]])) and\
# not self.checkFree([1, 2, 3, 0], [r, c+(counter)]) and\
# self.checkExplored([r, c+(counter)]):
# counter += 1
# else:
# break
elif self.robot.direction == WEST and self.validMove(
[[r, c - 2], [r - 1, c - 2], [r + 1, c - 2]]):
counter = 1
# while(True):
# if (self.validMove([[r, c-2-counter], [r-1, c-2-counter], [r+1, c-2-counter]])) and\
# not self.checkFree([1, 2, 3, 0], [r, c-(counter)]) and\
# self.checkExplored([r, c-(counter)]):
# counter += 1
# else:
# break
elif self.robot.direction == SOUTH and self.validMove(
[[r + 2, c], [r + 2, c - 1], [r + 2, c + 1]]):
counter = 1
# while(True):
# if (self.validMove([[r+2+counter, c], [r+2+counter, c-1], [r+2+counter, c+1]])) and\
# not self.checkFree([1, 2, 3, 0], [r+(counter), c]) and\
# self.checkExplored([r+(counter), c]):
# counter += 1
# else:
# break
return counter
# ***Update according to sensor placement
def checkExplored(self, center):
r, c = center
flag = True
inds = []
distanceShort = 3
distanceLong = 5
if self.robot.direction == NORTH:
# If r = 5, c = 8, distanceShort = 3 and distanceLong = 5
# zip([r-1]*distanceShort, range(c+2, c+distanceShort+2))
# gives [(4, 10), (4, 11), (4, 12)]
inds.append(
zip([r - 1] * distanceShort, range(c + 2,
c + distanceShort + 2)))
# zip([r+1]*distanceLong, range(c+2, c+distanceLong+2))
# gives [(6, 10), (6, 11), (6, 12), (6, 13), (6, 14)]
inds.append(
zip([r + 1] * distanceLong, range(c + 2,
c + distanceLong + 2)))
# zip([r]*distanceLong, range(c-distanceLong-1, c-1))[::-1]
# gives [(5, 6), (5, 5), (5, 4), (5, 3), (5, 2)]
inds.append(
zip([r] * distanceLong, range(c - distanceLong - 1,
c - 1))[::-1])
elif self.robot.direction == EAST:
# If r = 5, c = 8, distanceShort = 3 and distanceLong = 5
# zip(range(r+2, r+distanceShort+2), [c+1]*distanceShort)
# [(7, 9), (8, 9), (9, 9)]
inds.append(
zip(range(r + 2, r + distanceShort + 2),
[c + 1] * distanceShort))
# zip(range(r+2, r+distanceLong+2), [c-1]*distanceLong)
# gives [(7, 7), (8, 7), (9, 7), (10, 7), (11, 7)]
inds.append(
zip(range(r + 2, r + distanceLong + 2),
[c - 1] * distanceLong))
# zip(range(r-distanceLong-1, r-1), [c]*distanceLong)[::-1]
# gives [(3, 8), (2, 8), (1, 8), (0, 8), (-1, 8)]
inds.append(
zip(range(r - distanceLong - 1, r - 1),
[c] * distanceLong)[::-1])
elif self.robot.direction == WEST:
# zip(range(r-distanceShort-1, r-1), [c-1]*distanceShort)[::-1]
# gives [(3, 7), (2, 7), (1, 7)]
inds.append(
zip(range(r - distanceShort - 1, r - 1),
[c - 1] * distanceShort)[::-1])
# zip(range(r-distanceLong-1, r-1), [c+1]*distanceLong)[::-1]
# gives [(3, 9), (2, 9), (1, 9), (0, 9), (-1, 9)]
inds.append(
zip(range(r - distanceLong - 1, r - 1),
[c + 1] * distanceLong)[::-1])
# zip(range(r+2, r+distanceLong+2), [c]*distanceLong)
# gives [(7, 8), (8, 8), (9, 8), (10, 8), (11, 8)]
inds.append(
zip(range(r + 2, r + distanceLong + 2), [c] * distanceLong))
else: # self.direction == SOUTH
# zip([r+1]*distanceShort, range(c-distanceShort-1, c-1))[::-1]
# gives [(6, 6), (6, 5), (6, 4)]
inds.append(
zip([r + 1] * distanceShort, range(c - distanceShort - 1,
c - 1))[::-1])
# zip([r-1]*distanceLong, range(c-distanceLong-1, c-1))[::-1]
# gives [(4, 6), (4, 5), (4, 4), (4, 3), (4, 2)]
inds.append(
zip([r - 1] * distanceLong, range(c - distanceLong - 1,
c - 1))[::-1])
# zip([r]*distanceLong, range(c+2, c+distanceLong+2))
# [(5, 10), (5, 11), (5, 12), (5, 13), (5, 14)]
inds.append(
zip([r] * distanceLong, range(c + 2, c + distanceLong + 2)))
# For each list of coordinates
for sensor in inds:
if flag:
# For each coordinate
for (x, y) in sensor:
# If x coordinate is outside the unexplored boundaries
# or x coordinate is equal to upper limit of unexplored boundaries in terms of row
# or if y is outside the unexplored boundaries
# or if y is equal to upper limit of unexplored boundaries in terms of column
# of if the coordinate is an obstacle
# break out of the for loop, where flag will still be true
if (x < self.virtualWall[0] or x == self.virtualWall[2]
or y < self.virtualWall[1]
or y == self.virtualWall[3]
or self.currentMap[x, y] == 2):
break
# If the coordinate has a value of 0, indicating that it is unexplored
# Set flag to be False and break out of the loop
elif (self.currentMap[x, y] == 0):
flag = False
break
# return final value of flag
return flag
def moveStep(self, sensor_vals=None):
"""Moves the robot one step for exploration
Returns:
bool: True is the map is fully explored
"""
# If sensor values are given, update self.exploredMap
if (sensor_vals):
self.robot.getSensors(sensor_vals)
else:
self.robot.getSensors()
move = self.nextMove()
self.getExploredArea()
# If full coverage
if (self.exploredArea == 100):
return move, True
# if self.robot.center[0]==START[0] and self.robot.center[1]==START[1] and self.exploredArea>50:
# return move,True
else:
return move, False
def explore(self):
"""Runs the exploration till the map is fully explored of time runs out
"""
print("Starting exploration ...")
startTime = time.time()
endTime = startTime + self.timeLimit
while (time.time() <= endTime):
if (self.moveStep()):
print("Exploration completed !")
return
print("Time over !")
return
def getCloseExploredNeighbour(self):
locs = np.where(self.currentMap == 0)
if (len(locs[0]) == 0):
return None
self.virtualWall = [
np.min(locs[0]),
np.min(locs[1]),
np.max(locs[0]) + 1,
np.max(locs[1]) + 1
]
if ((self.virtualWall[2] - self.virtualWall[0] < 3)
and self.virtualWall[2] < MAX_ROWS - 3):
self.virtualWall[2] += 3
locs = list(np.asarray(zip(locs[0], locs[1])))
if (0 <= self.robot.center[0] < 10):
if (0 <= self.robot.center[1] < 8):
locs = [
loc for loc in locs if 0 <= loc[0] < 10 and 0 <= loc[1] < 8
]
else:
locs = [
loc for loc in locs
if 0 <= loc[0] < 10 and 8 <= loc[1] < 15
]
else:
if (10 <= self.robot.center[0] < 20):
locs = [
loc for loc in locs
if 10 <= loc[0] < 20 and 0 <= loc[1] < 8
]
else:
locs = [
loc for loc in locs
if 10 <= loc[0] < 20 and 8 <= loc[1] < 15
]
if (len(locs) == 0):
return None
locs = np.asarray(locs)
cost = np.abs(locs[:, 0] -
self.robot.center[0]) + np.abs(locs[:, 1] -
self.robot.center[1])
cost = cost.tolist()
# for each coordinate the cost is (x- center_x + y- center_y)
# the distance of the neighbour away from the center of robot
locs = locs.tolist()
while (cost):
position = np.argmin(cost) # position gives index of minimum cost
coord = locs.pop(position)
cost.pop(position)
neighbours = np.asarray(
[[-2, 0], [-2, -1], [-2, 1], [2, 0], [2, -1], [2, 1], [0, -2],
[1, -2], [-1, -2], [0, 2], [1, 2], [-1, 2]]) + coord
neighbours = self.__validInds(
neighbours
) # check if the coordinates in the list are valid or not
for neighbour in neighbours:
if (0 <= self.robot.center[0] < 10):
if (0 <= self.robot.center[1] < 8):
if (neighbour not in self.exploredNeighbours
and 0 <= neighbour[0] < 10
and 0 <= neighbour[1] < 8):
self.exploredNeighbours[neighbour] = True
return neighbour
else:
if (neighbour not in self.exploredNeighbours
and 0 <= neighbour[0] < 10
and 8 <= neighbour[1] < 15):
self.exploredNeighbours[neighbour] = True
return neighbour
else:
if (10 <= self.robot.center[0] < 20):
if (neighbour not in self.exploredNeighbours
and 10 <= neighbour[0] < 20
and 0 <= neighbour[1] < 8):
self.exploredNeighbours[neighbour] = True
return neighbour
else:
if (neighbour not in self.exploredNeighbours
and 10 <= neighbour[0] < 20
and 8 <= neighbour[1] < 15):
self.exploredNeighbours[neighbour] = True
return neighbour
return None
# Get valid and explored neighbours of unexplored spaces
def getExploredNeighbour(self):
# initial virtual wall = [0, 0, MAX_ROWS, MAX_COLS]
# returns all location where current map is unexplored, with x coordinate in one array and y coordinate in different array
locs = np.where(self.currentMap == 0)
if (len(locs[0]) == 0):
return None
# forms a virtual arena of unexplored area as rectangle
self.virtualWall = [
np.min(locs[0]),
np.min(locs[1]),
np.max(locs[0]) + 1,
np.max(locs[1]) + 1
]
if ((self.virtualWall[2] - self.virtualWall[0] < 3)
and self.virtualWall[2] < MAX_ROWS - 3):
self.virtualWall[2] += 3
# if there is more than 3 blocks between this new wall and the actual arean wall, we move the virtual wall 3 blocks away from the top of areana to
# accomodaate the size of the robot
# new virtual arena is [ unexplored rows +3 x unexplored column]
locs = np.asarray(zip(locs[0], locs[1]))
# converts locs back into unexplored coordinates
cost = np.abs(locs[:, 0] -
self.robot.center[0]) + np.abs(locs[:, 1] -
self.robot.center[1])
cost = cost.tolist()
# for each coordinate the cost is (x- center_x + y- center_y)
# the distance of the neighbour away from the center of robot
locs = locs.tolist()
while (cost):
position = np.argmin(cost) # position gives index of minimum cost
coord = locs.pop(position)
cost.pop(position)
neighbours = np.asarray(
[[-2, 0], [-2, -1], [-2, 1], [2, 0], [2, -1], [2, 1], [0, -2],
[1, -2], [-1, -2], [0, 2], [1, 2], [-1, 2]]) + coord
neighbours = self.__validInds(
neighbours
) # check if the coordinates in the list are valid or not
for neighbour in neighbours:
# added a new item to the exploredNeighbours dictionary
if (neighbour not in self.exploredNeighbours):
self.exploredNeighbours[neighbour] = True
return neighbour
return neighbour
return None
class Exploration:
"""Implementation of the Right-Wall hugging algorithm for a maze solving
robot.
The implementation assumes that the robot starts at the bottom-left corner of the map,
i.e. (Rows - 2, 1). And the robot is facing North
Attributes:
currentMap (Numpy array): To store the current state of the exploration map
exploredArea (int): Count of the number of cells explored
robot (Robot): Instance of the Robot class
sensors (list of Numpy arrays): Readings from all sensors
timeLimit (int): Maximum time allowed for exploration
"""
def __init__(self, realMap=None, timeLimit=None, calibrateLim=6, sim=True):
"""Constructor to initialise an instance of the Exploration class
Args:
realMap (string): File name for real map during simulation stage
timeLimit (int): Maximum time allowed for exploration
sim (bool, optional): To specify is the exploration mode is simulation or real
"""
self.timeLimit = timeLimit
self.exploredArea = 0
self.currentMap = np.zeros([20, 15])
if sim:
from Simulator import Robot
self.robot = Robot(self.currentMap, EAST, START, realMap)
self.sensors = self.robot.getSensors()
else:
from Real import Robot
self.robot = Robot(self.currentMap, EAST, START)
self.exploredNeighbours = dict()
self.sim = sim
self.calibrateLim = calibrateLim
self.virtualWall = [0, 0, MAX_ROWS, MAX_COLS]
def __validInds(self, inds):
"""To check if the passed indices are valid or not
To be valid the following conditions should be met:
* A 3x3 neighbourhood around the center should lie within the arena
* A 3x3 neighbourhood around the center should have no obstacle
Args:
inds (list of list): List of coordinates to be checked
Returns:
list of list: All indices that were valid
"""
valid = []
for i in inds:
r, c = i
x, y = np.meshgrid([-1, 0, 1], [-1, 0, 1])
x, y = x + r, y + c
if (np.any(x < 0) or np.any(y < 0) or np.any(x >= MAX_ROWS)
or np.any(y >= MAX_COLS)):
valid.append(False)
elif (np.any(self.currentMap[x[0, 0]:x[0, 2] + 1,
y[0, 0]:y[2, 0] + 1] != 1)):
valid.append(False)
else:
valid.append(True)
return [tuple(inds[i]) for i in range(len(inds)) if valid[i]]
def getExploredArea(self):
"""Updates the total number of cells explored at the current state
"""
self.exploredArea = (np.sum(self.currentMap != 0) / 300.0) * 100
def nextMove(self):
"""Decides which direction is free and commands the robot the next action
"""
move = []
# multi step
front = self.frontFree()
if (self.checkFree([1, 2, 3, 0], self.robot.center)):
self.robot.moveBot(RIGHT)
move.append(RIGHT)
front = self.frontFree()
for i in range(front):
self.robot.moveBot(FORWARD)
move.extend([FORWARD] * front)
elif (front):
for i in range(front):
self.robot.moveBot(FORWARD)
move.extend([FORWARD] * front)
elif (self.checkFree([3, 0, 1, 2], self.robot.center)):
self.robot.moveBot(LEFT)
move.append(LEFT)
front = self.frontFree()
for i in range(front):
self.robot.moveBot(FORWARD)
move.extend([FORWARD] * front)
else:
self.robot.moveBot(RIGHT)
self.robot.moveBot(RIGHT)
move.extend(('O'))
# single step
# if (self.checkFree([1, 2, 3, 0], self.robot.center)):
# self.robot.moveBot(RIGHT)
# move.append(RIGHT)
# if (self.checkFree([0, 1, 2, 3], self.robot.center)):
# self.robot.moveBot(FORWARD)
# move.append(FORWARD)
# elif (self.checkFree([0, 1, 2, 3], self.robot.center)):
# self.robot.moveBot(FORWARD)
# move.append(FORWARD)
# elif (self.checkFree([3, 0, 1, 2], self.robot.center)):
# self.robot.moveBot(LEFT)
# move.append(LEFT)
# if (self.checkFree([0, 1, 2, 3], self.robot.center)):
# self.robot.moveBot(FORWARD)
# move.append(FORWARD)
# else:
# self.robot.moveBot(RIGHT)
# self.robot.moveBot(RIGHT)
# move.extend(('O'))
if not (self.sim):
calibrate_front = self.robot.can_calibrate_front()
calibrate_right = self.robot.can_calibrate_right()
if self.robot.is_corner():
move.append('L')
elif (calibrate_right[0]):
move.append(calibrate_right[1])
elif (calibrate_front[0]):
move.append(calibrate_front[1])
return move
def checkFree(self, order, center):
"""Checks if a specific direction is free to move to
Args:
order (list): Ordering for the directionFree list based on the
the next move (Right, Left, Forward)
Returns:
bool: If the queried direction is free
"""
directionFree = np.asarray([
self.northFree(center),
self.eastFree(center),
self.southFree(center),
self.westFree(center)
])
directionFree = directionFree[order]
if self.robot.direction == NORTH:
return directionFree[0]
elif self.robot.direction == EAST:
return directionFree[1]
elif self.robot.direction == SOUTH:
return directionFree[2]
else:
return directionFree[3]
def validMove(self, inds):
"""Checks if all the three cells on one side of the robot are free
Args:
inds (list of list): List of cell indices to be checked
Returns:
bool: If all indices are free (no obstacle)
"""
for (r, c) in inds:
if not ((self.virtualWall[0] <= r < self.virtualWall[2]) and
(self.virtualWall[1] <= c < self.virtualWall[3])):
return False
return (self.currentMap[inds[0][0], inds[0][1]] == 1
and self.currentMap[inds[1][0], inds[1][1]] == 1
and self.currentMap[inds[2][0], inds[2][1]] == 1)
def northFree(self, center):
"""Checks if the north direction is free to move
Returns:
bool: if north is free
"""
r, c = center
inds = [[r - 2, c], [r - 2, c - 1], [r - 2, c + 1]]
return self.validMove(inds)
def eastFree(self, center):
"""Checks if the east direction is free to move
Returns:
bool: if east is free
"""
r, c = center
inds = [[r, c + 2], [r - 1, c + 2], [r + 1, c + 2]]
return self.validMove(inds)
def southFree(self, center):
"""Checks if the south direction is free to move
Returns:
bool: if south is free
"""
r, c = center
inds = [[r + 2, c], [r + 2, c - 1], [r + 2, c + 1]]
return self.validMove(inds)
def westFree(self, center):
"""Checks if the west direction is free to move
Returns:
bool: if west is free
"""
r, c = center
inds = [[r, c - 2], [r - 1, c - 2], [r + 1, c - 2]]
return self.validMove(inds)
def frontFree(self):
r, c = self.robot.center
counter = 0
if self.robot.direction == NORTH and self.validMove(
[[r - 2, c], [r - 2, c - 1], [r - 2, c + 1]]):
counter = 1
while (True):
if (self.validMove([[r-2-counter, c], [r-2-counter, c-1], [r-2-counter, c+1]])) and\
not self.checkFree([1, 2, 3, 0], [r-(counter), c]) and\
self.checkExplored([r-(counter), c]):
counter += 1
else:
break
elif self.robot.direction == EAST and self.validMove(
[[r, c + 2], [r - 1, c + 2], [r + 1, c + 2]]):
counter = 1
while (True):
if (self.validMove([[r, c+2+counter], [r-1, c+2+counter], [r+1, c+2+counter]])) and\
not self.checkFree([1, 2, 3, 0], [r, c+(counter)]) and\
self.checkExplored([r, c+(counter)]):
counter += 1
else:
break
elif self.robot.direction == WEST and self.validMove(
[[r, c - 2], [r - 1, c - 2], [r + 1, c - 2]]):
counter = 1
while (True):
if (self.validMove([[r, c-2-counter], [r-1, c-2-counter], [r+1, c-2-counter]])) and\
not self.checkFree([1, 2, 3, 0], [r, c-(counter)]) and\
self.checkExplored([r, c-(counter)]):
counter += 1
else:
break
elif self.robot.direction == SOUTH and self.validMove(
[[r + 2, c], [r + 2, c - 1], [r + 2, c + 1]]):
counter = 1
while (True):
if (self.validMove([[r+2+counter, c], [r+2+counter, c-1], [r+2+counter, c+1]])) and\
not self.checkFree([1, 2, 3, 0], [r+(counter), c]) and\
self.checkExplored([r+(counter), c]):
counter += 1
else:
break
return counter
def checkExplored(self, center):
r, c = center
flag = True
inds = []
distanceShort = 3
distanceLong = 5
if self.robot.direction == NORTH:
inds.append(
zip([r - 1] * distanceShort, range(c + 2,
c + distanceShort + 2)))
inds.append(
zip([r + 1] * distanceLong, range(c + 2,
c + distanceLong + 2)))
inds.append(
zip([r - 1] * distanceLong, range(c - distanceLong - 1,
c - 1))[::-1])
elif self.robot.direction == EAST:
inds.append(
zip(range(r + 2, r + distanceShort + 2),
[c + 1] * distanceShort))
inds.append(
zip(range(r + 2, r + distanceLong + 2),
[c - 1] * distanceLong))
inds.append(
zip(range(r - distanceLong - 1, r - 1),
[c + 1] * distanceLong)[::-1])
elif self.robot.direction == WEST:
inds.append(
zip(range(r - distanceShort - 1, r - 1),
[c - 1] * distanceShort)[::-1])
inds.append(
zip(range(r - distanceLong - 1, r - 1),
[c + 1] * distanceLong)[::-1])
inds.append(
zip(range(r + 2, r + distanceLong + 2),
[c - 1] * distanceLong))
else:
inds.append(
zip([r + 1] * distanceShort, range(c - distanceShort - 1,
c - 1))[::-1])
inds.append(
zip([r - 1] * distanceLong, range(c - distanceLong - 1,
c - 1))[::-1])
inds.append(
zip([r + 1] * distanceLong, range(c + 2,
c + distanceLong + 2)))
for sensor in inds:
if flag:
for (x, y) in sensor:
if (x < self.virtualWall[0] or x == self.virtualWall[2]
or y < self.virtualWall[1]
or y == self.virtualWall[3]
or self.currentMap[x, y] == 2):
break
elif (self.currentMap[x, y] == 0):
flag = False
break
return flag
def moveStep(self, sensor_vals=None):
"""Moves the robot one step for exploration
Returns:
bool: True is the map is fully explored
"""
if (sensor_vals):
self.robot.getSensors(sensor_vals)
else:
self.robot.getSensors()
move = self.nextMove()
self.getExploredArea()
if (self.exploredArea == 100):
return move, True
else:
return move, False
def explore(self):
"""Runs the exploration till the map is fully explored of time runs out
"""
print "Starting exploration ..."
startTime = time.time()
endTime = startTime + self.timeLimit
while (time.time() <= endTime):
if (self.moveStep()):
print "Exploration completed !"
return
print "Time over !"
return
def getExploredNeighbour(self):
locs = np.where(self.currentMap == 0)
self.virtualWall = [
np.min(locs[0]),
np.min(locs[1]),
np.max(locs[0]) + 1,
np.max(locs[1]) + 1
]
if ((self.virtualWall[2] - self.virtualWall[0] < 3)
and self.virtualWall[2] < MAX_ROWS - 3):
self.virtualWall[2] += 3
locs = np.asarray(zip(locs[0], locs[1]))
cost = np.abs(locs[:, 0] -
self.robot.center[0]) + np.abs(locs[:, 1] -
self.robot.center[1])
cost = cost.tolist()
locs = locs.tolist()
while (cost):
position = np.argmin(cost)
coord = locs.pop(position)
cost.pop(position)
neighbours = np.asarray([[-2, 0], [2, 0], [0, -2], [0, 2]]) + coord
neighbours = self.__validInds(neighbours)
for neighbour in neighbours:
if (neighbour not in self.exploredNeighbours):
self.exploredNeighbours[neighbour] = True
return neighbour
return None
class FastestPath:
"""Implementation of the A* algorithm for getting the fastest path in a 2D grid maze
Attributes:
exploredMap (Numpy array): The map after the exploration stage
goal (list): Coordinate of the goal cell
graph (list): To form the fastest path graph
index (int): Counter
path (list): The fastest path
robot (Robot): Instance of the Robot class
start (list): Coordinate of the initial cell
waypoint (list): Coordinate of the way-point if specified
"""
def __init__(self,
exploredMap,
start,
goal,
direction,
waypoint=None,
calibrateLim=5,
sim=True):
"""Constructor to initialize an instance of the FastestPath class
Args:
exploredMap (Numpy array): The map after the exploration stage
start (list): Coordinate of the initial cell
goal (list): Coordinate of the goal cell
direction (int): The starting direction of the robot
waypoint (list, optional): The coordinate of the way-point if specified
sim (bool, optional): If specify if run is simulation or real
"""
self.exploredMap = exploredMap
self.graph = []
self.start = start
self.goal = goal
self.waypoint = waypoint
self.index = 1
self.direction = direction
self.path = []
self.movement = []
self.calibrateLim = calibrateLim
self.sim = sim
if sim:
from Simulator import Robot
self.robot = Robot(self.exploredMap, direction, start, None)
else:
from Real import Robot
self.robot = Robot(self.exploredMap, direction, start)
def __getHeuristicCosts(self, goal):
"""Calculates the Manhattan distance between each cell and the goal cell
Args:
goal (list): Coordinates of the goal cell
Returns:
Numpy array: 2D grid with each cell storing the distance to the goal cell
"""
# this will create a grid of coordinates
cols, rows = np.meshgrid(range(0, 15), range(0, MAX_ROWS))
cost = np.zeros([MAX_ROWS, 15])
cost = np.sqrt(np.square(rows - goal[0]) + np.square(cols - goal[1]))
cost /= np.max(cost)
return cost
# e.g. If goal = np.asarray([1, 13])
# cost will be
# [[0.58722022 0.54232614 0.49745804 0.45262363 0.40783404 0.36310583
# 0.31846488 0.27395385 0.22964829 0.18569534 0.14242182 0.10070744
# 0.06369298 0.04503773 0.06369298]
# [0.58549055 0.54045282 0.49541508 0.45037735 0.40533961 0.36030188
# 0.31526414 0.27022641 0.22518867 0.18015094 0.1351132 0.09007547
# 0.04503773 0. 0.04503773]
# [0.58722022
# 0.54232614
# 0.49745804
# 0.45262363
# 0.40783404
# 0.36310583
# 0.31846488
# 0.27395385
# 0.22964829
# 0.18569534
# 0.14242182
# 0.10070744
# 0.06369298
# 0.04503773
# 0.06369298]
# [0.59237891 0.54790769 0.50353718 0.45929658 0.4152274 0.37139068
# 0.32787966 0.28484365 0.24253563 0.20141487 0.16238586 0.12738595
# 0.10070744 0.09007547 0.10070744]
# [0.60087833
# 0.55708601
# 0.51350919
# 0.47020776
# 0.42726547
# 0.38480258
# 0.34299717
# 0.30212231
# 0.26261287
# 0.22518867
# 0.19107893
# 0.16238586
# 0.14242182
# 0.1351132
# 0.14242182]
# [0.61257942 0.56968729 0.52715317 0.48507125 0.44357025 0.40282975
# 0.36310583 0.32477173 0.28838221 0.2547719 0.22518867 0.20141487
# 0.18569534 0.18015094 0.18569534]
# [0.62730306
# 0.58549055
# 0.54419302
# 0.50353718
# 0.46369186
# 0.42488514
# 0.38742924
# 0.35175595
# 0.31846488
# 0.28838221
# 0.26261287
# 0.24253563
# 0.22964829
# 0.22518867
# 0.22964829]
# [0.64484223 0.60424462 0.5643212 0.52522573 0.48715759 0.45037735
# 0.4152274 0.38215785 0.35175595 0.32477173 0.30212231 0.28484365
# 0.27395385 0.27022641 0.27395385]
# [0.66497419
# 0.62568421
# 0.58722022
# 0.54975562
# 0.51350919
# 0.47875769
# 0.44585083
# 0.4152274
# 0.38742924
# 0.36310583
# 0.34299717
# 0.32787966
# 0.31846488
# 0.31526414
# 0.31846488]
# [0.68747119 0.64954345 0.61257942 0.57676442 0.54232614 0.5095438
# 0.47875769 0.45037735 0.42488514 0.40282975 0.38480258 0.37139068
# 0.36310583 0.36030188 0.36310583]
# [0.71210911
# 0.67556602
# 0.64010648
# 0.60592075
# 0.57323678
# 0.54232614
# 0.51350919
# 0.48715759
# 0.46369186
# 0.44357025
# 0.42726547
# 0.4152274
# 0.40783404
# 0.40533961
# 0.40783404]
# [0.73867377 0.70351191 0.66953406 0.63692976 0.60592075 0.57676442
# 0.54975562 0.52522573 0.50353718 0.48507125 0.47020776 0.45929658
# 0.45262363 0.45037735 0.45262363]
# [0.76696499
# 0.73316121
# 0.70062273
# 0.66953406
# 0.64010648
# 0.61257942
# 0.58722022
# 0.5643212
# 0.54419302
# 0.52715317
# 0.51350919
# 0.50353718
# 0.49745804
# 0.49541508
# 0.49745804]
# [0.79679887 0.76431571 0.73316121 0.70351191 0.67556602 0.64954345
# 0.62568421 0.60424462 0.58549055 0.56968729 0.55708601 0.54790769
# 0.54232614 0.54045282 0.54232614]
# [0.82800868
# 0.79679887
# 0.76696499
# 0.73867377
# 0.71210911
# 0.68747119
# 0.66497419
# 0.64484223
# 0.62730306
# 0.61257942
# 0.60087833
# 0.59237891
# 0.58722022
# 0.58549055
# 0.58722022]
# [0.86044472 0.8304548 0.80187407 0.77485849 0.7495773 0.72621165
# 0.70495206 0.68599434 0.66953406 0.65575932 0.64484223 0.63692976
# 0.63213473 0.63052829 0.63213473]
# [0.89397351
# 0.86514664
# 0.8377503
# 0.81192931
# 0.7878386
# 0.76564149
# 0.74550716
# 0.72760688
# 0.71210911
# 0.69917366
# 0.68894487
# 0.6815446
# 0.67706562
# 0.67556602
# 0.67706562]
# [0.92847669 0.9007547 0.87447463 0.84977028 0.82678291 0.80565949
# 0.78655023 0.76960515 0.75497001 0.74278135 0.73316121 0.72621165
# 0.72200982 0.72060376 0.72200982]
# [0.96384962
# 0.93717455
# 0.91194463
# 0.88828298
# 0.86631813
# 0.84618221
# 0.82800868
# 0.81192931
# 0.7980707
# 0.78655023
# 0.77747185
# 0.77092184
# 0.76696499
# 0.76564149
# 0.76696499]
# [1. 0.97431518 0.95007206 0.92738372 0.90636693 0.88714049
# 0.86982314 0.85453094 0.84137432 0.8304548 0.82186154 0.81566807
# 0.81192931 0.81067923 0.81192931]]
def __validInds(self, inds):
"""To check if the passed indices are valid or not
To be valid the following conditions should be met:
* A 3x3 neighbourhood around the center should lie within the arena
* A 3x3 neighbourhood around the center should have no obstacle
Args:
inds (list of list): List of coordinates to be checked
Returns:
list of list: All indices that were valid
"""
valid = []
for i in inds:
r, c = i
x, y = np.meshgrid([-1, 0, 1], [-1, 0, 1])
# x: [[-1 0 1]
# [-1 0 1]
# [-1 0 1]]
# y: [[-1 -1 -1]
# [ 0 0 0]
# [ 1 1 1]]
x, y = x + r, y + c
# e.g. If r = 5 and c = 12
# x: [[4 5 6]
# [4 5 6]
# [4 5 6]]
# y: [[11 11 11]
# [12 12 12]
# [13 13 13]]
if np.any(x < 0) or np.any(y < 0) or np.any(
x >= MAX_ROWS) or np.any(y >= MAX_COLS):
valid.append(False)
elif (np.any(self.exploredMap[x[0, 0]:x[0, 2] + 1,
y[0, 0]:y[2, 0] + 1] != 1)):
# If any of the space within the 3 by 3 space is not a free space, append false
valid.append(False)
else:
valid.append(True)
return [inds[i] for i in range(len(inds))
if valid[i]] # Returns only valid indices
def __getNeighbours(self, loc):
"""Returns the coordinates of a 3x3 neighbourhood around the passed location
Only those neighbours are returned that have been explored and are not obstacles
Args:
loc (list): Coordinates of a cell
Returns:
list of list: Coordinates of all valid neighbours of the cell
"""
r, c = loc.coord
inds = [(r - 1, c), (r + 1, c), (r, c - 1), (r, c + 1)]
inds = self.__validInds(inds)
neighbours = [self.graph[n[0]][n[1]] for n in inds]
# if its explored and not an obstacle
return [n for n in neighbours if n.value == 1]
def __getCost(self, current_pos, next_pos):
# Cost is one if the robot needs to turn if it wants to move from current position to the next position
if self.direction in [NORTH, SOUTH]:
if current_pos[1] == next_pos[1]:
return 0
else:
return 1
else:
if current_pos[0] == next_pos[0]:
return 0
else:
return 1
return 1
# Function to set current direction of robot based on previous and current position
def __setDirection(self, prev_pos, current_pos, backwards=False):
# If row index increases, the robot has moved south
if prev_pos[0] < current_pos[0]:
if (backwards == False):
self.direction = SOUTH
else:
self.direction = NORTH
# If the column index increases, the robot has moved east
elif prev_pos[1] < current_pos[1]:
if (backwards == False):
self.direction = EAST
else:
self.direction = WEST
# If the column index has decreased, the robot has moved west
elif prev_pos[1] > current_pos[1]:
if (backwards == False):
self.direction = WEST
else:
self.direction = EAST
# If row index decreases, the robot has moved north
else:
if (backwards == False):
self.direction = NORTH
else:
self.direction = SOUTH
def __astar(self, start, goal, backwards=False):
"""Implementation of the a* algorithm for finding the shortest path in a 2d grid maze
Args:
start (list): Coordinates of the starting cell
goal (list): Coordinates of the goal cell
Returns:
list of list: A list of coordinates specifying the path from start to goal
Raises:
ValueError: If no path is found between the given start and goal
"""
goal = self.graph[goal[0]][goal[1]]
# set of nodes already evaluated
closedSet = set()
# set of discovered nodes that have not been evaluated yet
openSet = set()
current = self.graph[start[0]][start[1]]
current.G = 0
# add start node to openSet
openSet.add(current)
prev = None
# While there are still nodes to be evaluated
while (openSet):
# Get the node with the minimum sum of cost from start to that position and from that position to the goal
current = min(openSet, key=lambda o: o.G + o.H)
# If the node has a previous node (not start), set the new direction of robot
if prev:
if (backwards == False):
self.__setDirection(prev.coord, current.coord)
else:
self.__setDirection(prev.coord,
current.coord,
backwards=True)
# if goal is reached trace back the path
if (current == goal):
path = []
while (current.parent):
path.append(current.coord)
current = current.parent
# Append the coordinate which does not have a parent (which is the start point)
path.append(current.coord)
# Return path backwards (from start to goal)
return path[::-1]
# If goal is not yet reached
else:
# Mark current node to be evaluated
openSet.remove(current)
closedSet.add(current)
# Get indices of 3 by 3 area around the current coordinate which are free and explored
for node in self.__getNeighbours(current):
# For each node returned,
# Do nothing if the node has already been evaluated
if node in closedSet:
continue
# If the node has yet to be evaluated
if node in openSet:
# calculate new G cost
new_g = current.G + self.__getCost(
current.coord, node.coord)
# If previous cost from goal to the coordinate is larger than the new cost
if node.G > new_g:
# Update to new, lower cost
node.G = new_g
# Update node's parent to be the current node
node.parent = current
else:
# if neither in openSet nor in closedSet
# cost of moving between neighbours is 1
node.G = current.G + self.__getCost(
current.coord, node.coord)
node.parent = current
# Add new node to be evaluated
openSet.add(node)
prev = copy.deepcopy(current)
# exception is no path is found
raise ValueError('No Path Found')
def __initGraph(self, h_n):
"""To create a grid of graph nodes from the exploration map
Args:
h_n (Numpy array): The heuristic cost of each node to goal node
"""
self.graph = []
for row in xrange(MAX_ROWS):
self.graph.append([])
for col in xrange(MAX_COLS):
self.graph[row].append(
Node(self.exploredMap[row][col], (row, col),
h_n[row][col]))
def getFastestPath(self, backwards=False):
"""To call the fastest path algorithm and handle the case if there is a way-point or not
"""
path = []
start = copy.copy(self.start)
# If there is a waypoint
if (self.waypoint):
h_n = self.__getHeuristicCosts(self.waypoint)
self.__initGraph(h_n)
# Set waypoint as the goal first and find shortest path from start to goal
if (backwards == False):
fsp = self.__astar(start, self.waypoint)
else:
fsp = self.__astar(start, self.waypoint, backwards=True)
# Set start to be way point
start = copy.copy(self.waypoint)
# Leaves out the last element as it will be the starting node for the next fastest path
# (Otherwise the way-point will be added twice)
path.extend(fsp[:-1]) # Up to but not including the last element
h_n = self.__getHeuristicCosts(self.goal)
self.__initGraph(h_n)
# start will be waypoint at this point if there is a waypoint
if (backwards == False):
fsp = self.__astar(start, self.goal)
else:
fsp = self.__astar(start, self.goal, backwards=True)
path.extend(fsp)
self.path = path
self.markMap()
def markMap(self):
"""To mark the path on the exploration map
"""
# Related to the GUI (Refer to the color coding in the javascript code for the GUI)
for ind in self.path:
self.exploredMap[tuple(ind)] = 6
# Adds more movements to self.movement so that robot moves one space if possible
def moveStep(self, backwards=False):
"""To take the fastest path, robot location and determine the next
move for the robot to follow the path
"""
movement = []
if (backwards == False):
move_command = FORWARD
else:
move_command = BACKWARDS
# self.index is first initialised to 1 when the FastestPath class is first initialised
# Moving one step will increase self.index by one, thereby making self.path[self.index]) go to the next space allong the path
# If the next space in the path is not equal to robot's centre and hence, robot should move to the next space in the path
if (self.robot.center.tolist() != self.path[self.index]):
# diff will be an array where first element is vertical distance from robot's centre to next space in path
# Second element is horizontal distance from robot's centre to next space in path
diff = self.robot.center - np.asarray(self.path[self.index])
# If difference between row index of robot's centre and the next space is -1, ot means that the robot should move south
if (diff[0] == -1 and diff[1] == 0): # Going south
if self.robot.direction == NORTH:
# If robot is facing north, it needs to turn right twice and move forward once before it can move south by one space
movement.extend((RIGHT, RIGHT, move_command))
elif self.robot.direction == EAST:
movement.extend((RIGHT, move_command))
elif self.robot.direction == SOUTH:
movement.append(move_command)
else:
movement.extend((LEFT, move_command))
elif (diff[0] == 0 and diff[1] == 1): # Going west
if self.robot.direction == NORTH:
movement.extend((LEFT, move_command))
elif self.robot.direction == EAST:
movement.extend((RIGHT, RIGHT, move_command))
elif self.robot.direction == SOUTH:
movement.extend((RIGHT, move_command))
else:
movement.append(move_command)
elif (diff[0] == 0 and diff[1] == -1): # Going east
if self.robot.direction == NORTH:
movement.extend((RIGHT, move_command))
elif self.robot.direction == EAST:
movement.append(move_command)
elif self.robot.direction == SOUTH:
movement.extend((LEFT, move_command))
else:
movement.extend((RIGHT, RIGHT, move_command))
else: # Going north
if self.robot.direction == NORTH:
movement.append(move_command)
elif self.robot.direction == EAST:
movement.extend((LEFT, move_command))
elif self.robot.direction == SOUTH:
movement.extend((RIGHT, RIGHT, move_command))
else:
movement.extend((RIGHT, move_command))
for move in movement:
self.robot.moveBot(move)
# Add movements to self.movement
self.movement.extend(movement)
# if not (self.sim):
# if (self.robot.stepCounter + len(movement) > self.calibrateLim):
# calibrate = self.robot.can_calibrate()
# if (calibrate[0]):
# movement.append(calibrate[1])
# self.robot.stepCounter = 0
# Increase self.index by one so that the next space in self.path can be evaluated
self.index += 1
class FastestPath:
def __init__(self,
exploredMap,
start,
goal,
direction,
waypoint=None,
calibrateLim=5,
sim=True):
self.exploredMap = exploredMap
self.graph = []
self.start = start
self.goal = goal
self.waypoint = waypoint
self.index = 1
self.direction = direction
self.path = []
self.movement = []
self.calibrateLim = calibrateLim
self.sim = sim
if sim:
from Simulator import Robot
self.robot = Robot(self.exploredMap, direction, start, None)
else:
from Real import Robot
self.robot = Robot(self.exploredMap, direction, start)
def __getHeuristicCosts(self, goal):
cols, rows = np.meshgrid(range(0, 15), range(0, 20))
cost = np.zeros([20, 15])
cost = np.sqrt(np.square(rows - goal[0]) + np.square(cols - goal[1]))
cost /= np.max(cost)
return cost
def __validInds(self, inds):
valid = []
for i in inds:
r, c = i
x, y = np.meshgrid([-1, 0, 1], [-1, 0, 1])
x, y = x + r, y + c
if (np.any(x < 0) or np.any(y < 0) or np.any(x >= MAX_ROWS)
or np.any(y >= MAX_COLS)):
valid.append(False)
elif (np.any(self.exploredMap[x[0, 0]:x[0, 2] + 1,
y[0, 0]:y[2, 0] + 1] != 1)):
valid.append(False)
else:
valid.append(True)
return [inds[i] for i in range(len(inds)) if valid[i]]
def __getNeighbours(self, loc):
r, c = loc.coord
inds = [(r - 1, c), (r + 1, c), (r, c - 1), (r, c + 1)]
inds = self.__validInds(inds)
neighbours = [self.graph[n[0]][n[1]] for n in inds]
return [n for n in neighbours if n.value == 1]
def __getCost(self, current_pos, next_pos):
if self.direction in [NORTH, SOUTH]:
if current_pos[1] == next_pos[1]:
return 0
else:
return 1
else:
if current_pos[0] == next_pos[0]:
return 0
else:
return 1
return 1
def __setDirection(self, prev_pos, current_pos):
if prev_pos[0] < current_pos[0]:
self.direction = SOUTH
elif prev_pos[1] < current_pos[1]:
self.direction = EAST
elif prev_pos[1] > current_pos[1]:
self.direction = WEST
else:
self.direction = NORTH
def __astar(self, start, goal):
goal = self.graph[goal[0]][goal[1]]
closedSet = set()
openSet = set()
current = self.graph[start[0]][start[1]]
current.G = 0
openSet.add(current)
prev = None
while (openSet):
current = min(openSet, key=lambda o: o.G + o.H)
if prev:
self.__setDirection(prev.coord, current.coord)
if (current == goal):
path = []
while (current.parent):
path.append(current.coord)
current = current.parent
path.append(current.coord)
return path[::-1]
else:
openSet.remove(current)
closedSet.add(current)
for node in self.__getNeighbours(current):
if node in closedSet:
continue
if node in openSet:
new_g = current.G + self.__getCost(
current.coord, node.coord)
if node.G > new_g:
node.G = new_g
node.parent = current
else:
node.G = current.G + self.__getCost(
current.coord, node.coord)
node.parent = current
openSet.add(node)
prev = copy.deepcopy(current)
raise ValueError('No Path Found')
def __initGraph(self, h_n):
self.graph = []
for row in xrange(MAX_ROWS):
self.graph.append([])
for col in xrange(MAX_COLS):
self.graph[row].append(
Node(self.exploredMap[row][col], (row, col),
h_n[row][col]))
def getFastestPath(self):
path = []
start = copy.copy(self.start)
if (self.waypoint):
h_n = self.__getHeuristicCosts(self.waypoint)
self.__initGraph(h_n)
fsp = self.__astar(start, self.waypoint)
start = copy.copy(self.waypoint)
path.extend(fsp[:-1])
h_n = self.__getHeuristicCosts(self.goal)
self.__initGraph(h_n)
fsp = self.__astar(start, self.goal)
path.extend(fsp)
self.path = path
self.markMap()
def markMap(self):
for ind in self.path:
self.exploredMap[tuple(ind)] = 6
def moveStep(self):
movement = []
if (self.robot.center.tolist() != self.path[self.index]):
diff = self.robot.center - np.asarray(self.path[self.index])
if (diff[0] == -1 and diff[1] == 0): # Going south
if self.robot.direction == NORTH:
movement.extend((RIGHT, RIGHT, FORWARD))
elif self.robot.direction == EAST:
movement.extend((RIGHT, FORWARD))
elif self.robot.direction == SOUTH:
movement.append(FORWARD)
else:
movement.extend((LEFT, FORWARD))
elif (diff[0] == 0 and diff[1] == 1): # Going west
if self.robot.direction == NORTH:
movement.extend((LEFT, FORWARD))
elif self.robot.direction == EAST:
movement.extend((RIGHT, RIGHT, FORWARD))
elif self.robot.direction == SOUTH:
movement.extend((RIGHT, FORWARD))
else:
movement.append(FORWARD)
elif (diff[0] == 0 and diff[1] == -1): # Going east
if self.robot.direction == NORTH:
movement.extend((RIGHT, FORWARD))
elif self.robot.direction == EAST:
movement.append(FORWARD)
elif self.robot.direction == SOUTH:
movement.extend((LEFT, FORWARD))
else:
movement.extend((RIGHT, RIGHT, FORWARD))
else: # Going north
if self.robot.direction == NORTH:
movement.append(FORWARD)
elif self.robot.direction == EAST:
movement.extend((LEFT, FORWARD))
elif self.robot.direction == SOUTH:
movement.extend((RIGHT, RIGHT, FORWARD))
else:
movement.extend((RIGHT, FORWARD))
for move in movement:
self.robot.moveBot(move)
self.movement.extend(movement)
self.index += 1
class Exploration:
def __init__(self, realMap=None, timeLimit=None, calibrateLim=6, sim=True):
self.timeLimit = timeLimit
self.exploredArea = 0
self.currentMap = np.zeros([20, 15])
if sim:
from Simulator import Robot
self.robot = Robot(self.currentMap, EAST, START, realMap)
self.sensors = self.robot.getSensors()
else:
from Real import Robot
self.robot = Robot(self.currentMap, EAST, START)
self.exploredNeighbours = dict()
self.sim = sim
self.calibrateLim = calibrateLim
self.virtualWall = [0, 0, MAX_ROWS, MAX_COLS]
def __validInds(self, inds):
front = self.frontFree()
print(front)
if (self.checkFree([1, 2, 3, 0], self.robot.center)):
self.robot.moveBot(RIGHT)
move.append(RIGHT)
front = self.frontFree()
for i in range(front):
self.robot.moveBot(FORWARD)
move.extend([FORWARD] * front)
print("try right")
elif (front):
for i in range(front):
self.robot.moveBot(FORWARD)
move.extend([FORWARD] * front)
print("try front")
elif (self.checkFree([3, 0, 1, 2], self.robot.center)):
self.robot.moveBot(LEFT)
move.append(LEFT)
front = self.frontFree()
for i in range(front):
self.robot.moveBot(FORWARD)
move.extend([FORWARD] * front)
print("try Left")
else:
self.robot.moveBot(RIGHT)
self.robot.moveBot(RIGHT)
move.append(BACK)
if not (self.sim):
calibrate_front = self.robot.can_calibrate_front()
calibrate_right = self.robot.can_calibrate_right()
if self.robot.is_corner():
move.append('L')
elif (calibrate_right[0]):
global cal_count
cal_count = cal_count + 1
if (cal_count % 2 == 0):
move.append(calibrate_right[1])
elif (calibrate_front[0]):
move.append(calibrate_front[1])
return move
def checkFree(self, order, center):
directionFree = np.asarray([
self.northFree(center),
self.eastFree(center),
self.southFree(center),
self.westFree(center)
])
directionFree = directionFree[order]
if self.robot.direction == NORTH:
return directionFree[0]
elif self.robot.direction == EAST:
return directionFree[1]
elif self.robot.direction == SOUTH:
return directionFree[2]
else:
return directionFree[3]
def validMove(self, inds):
for (r, c) in inds:
if not ((self.virtualWall[0] <= r < self.virtualWall[2]) and
(self.virtualWall[1] <= c < self.virtualWall[3])):
return False
return (self.currentMap[inds[0][0], inds[0][1]] == 1
and self.currentMap[inds[1][0], inds[1][1]] == 1
and self.currentMap[inds[2][0], inds[2][1]] == 1)
def northFree(self, center):
r, c = center
inds = [[r - 2, c], [r - 2, c - 1], [r - 2, c + 1]]
return self.validMove(inds)
def eastFree(self, center):
r, c = center
inds = [[r, c + 2], [r - 1, c + 2], [r + 1, c + 2]]
return self.validMove(inds)
def southFree(self, center):
r, c = center
inds = [[r + 2, c], [r + 2, c - 1], [r + 2, c + 1]]
return self.validMove(inds)
def westFree(self, center):
r, c = center
inds = [[r, c - 2], [r - 1, c - 2], [r + 1, c - 2]]
return self.validMove(inds)
def frontFree(self):
r, c = self.robot.center
counter = 0
if self.robot.direction == NORTH and self.validMove(
[[r - 2, c], [r - 2, c - 1], [r - 2, c + 1]]):
counter = 1
while (True):
if (self.validMove([[r-2-counter, c], [r-2-counter, c-1], [r-2-counter, c+1]])) and\
not self.checkFree([1, 2, 3, 0], [r-(counter), c]) and\
self.checkExplored([r-(counter), c]):
counter += 1
else:
break
elif self.robot.direction == EAST and self.validMove(
[[r, c + 2], [r - 1, c + 2], [r + 1, c + 2]]):
counter = 1
while (True):
if (self.validMove([[r, c+2+counter], [r-1, c+2+counter], [r+1, c+2+counter]])) and\
not self.checkFree([1, 2, 3, 0], [r, c+(counter)]) and\
self.checkExplored([r, c+(counter)]):
counter += 1
else:
break
elif self.robot.direction == WEST and self.validMove(
[[r, c - 2], [r - 1, c - 2], [r + 1, c - 2]]):
counter = 1
while (True):
if (self.validMove([[r, c-2-counter], [r-1, c-2-counter], [r+1, c-2-counter]])) and\
not self.checkFree([1, 2, 3, 0], [r, c-(counter)]) and\
self.checkExplored([r, c-(counter)]):
counter += 1
else:
break
elif self.robot.direction == SOUTH and self.validMove(
[[r + 2, c], [r + 2, c - 1], [r + 2, c + 1]]):
counter = 1
while (True):
if (self.validMove([[r+2+counter, c], [r+2+counter, c-1], [r+2+counter, c+1]])) and\
not self.checkFree([1, 2, 3, 0], [r+(counter), c]) and\
self.checkExplored([r+(counter), c]):
counter += 1
else:
break
return counter
def checkExplored(self, center):
r, c = center
flag = True
inds = []
distanceShort = 3
distanceLong = 5
if self.robot.direction == NORTH:
inds.append(
zip([r - 1] * distanceShort, range(c + 2,
c + distanceShort + 2)))
inds.append(
zip([r + 1] * distanceLong, range(c + 2,
c + distanceLong + 2)))
inds.append(
zip([r - 1] * distanceLong, range(c - distanceLong - 1,
c - 1))[::-1])
elif self.robot.direction == EAST:
inds.append(
zip(range(r + 2, r + distanceShort + 2),
[c + 1] * distanceShort))
inds.append(
zip(range(r + 2, r + distanceLong + 2),
[c - 1] * distanceLong))
inds.append(
zip(range(r - distanceLong - 1, r - 1),
[c + 1] * distanceLong)[::-1])
elif self.robot.direction == WEST:
inds.append(
zip(range(r - distanceShort - 1, r - 1),
[c - 1] * distanceShort)[::-1])
inds.append(
zip(range(r - distanceLong - 1, r - 1),
[c + 1] * distanceLong)[::-1])
inds.append(
zip(range(r + 2, r + distanceLong + 2),
[c - 1] * distanceLong))
else:
inds.append(
zip([r + 1] * distanceShort, range(c - distanceShort - 1,
c - 1))[::-1])
inds.append(
zip([r - 1] * distanceLong, range(c - distanceLong - 1,
c - 1))[::-1])
inds.append(
zip([r + 1] * distanceLong, range(c + 2,
c + distanceLong + 2)))
for sensor in inds:
if flag:
for (x, y) in sensor:
if (x < self.virtualWall[0] or x == self.virtualWall[2]
or y < self.virtualWall[1]
or y == self.virtualWall[3]
or self.currentMap[x, y] == 2):
break
elif (self.currentMap[x, y] == 0):
flag = False
break
return flag
def moveStep(self, sensor_vals=None):
if (sensor_vals):
self.robot.getSensors(sensor_vals)
else:
self.robot.getSensors()
move = self.nextMove()
self.getExploredArea()
if (self.exploredArea == 100):
return move, True
else:
return move, False
def explore(self):
print "Starting exploration ..."
startTime = time.time()
endTime = startTime + self.timeLimit
while (time.time() <= endTime):
if (self.moveStep()):
print "Exploration completed !"
return
print "Time over !"
return
def getExploredNeighbour(self):
locs = np.where(self.currentMap == 0)
self.virtualWall = [
np.min(locs[0]),
np.min(locs[1]),
np.max(locs[0]) + 4,
np.max(locs[1]) + 4
]
if ((self.virtualWall[2] - self.virtualWall[0] < 3)
and self.virtualWall[2] < MAX_ROWS - 3):
self.virtualWall[2] += 3
locs = np.asarray(zip(locs[0], locs[1]))
cost = np.abs(locs[:, 0] -
self.robot.center[0]) + np.abs(locs[:, 1] -
self.robot.center[1])
cost = cost.tolist()
locs = locs.tolist()
while (cost):
position = np.argmin(cost)
coord = locs.pop(position)
cost.pop(position)
neighbours = np.asarray([[-2, 0], [2, 0], [0, -2], [0, 2]]) + coord
neighbours = self.__validInds(neighbours)
for neighbour in neighbours:
if (neighbour not in self.exploredNeighbours):
self.exploredNeighbours[neighbour] = True
return neighbour
return None
class FastestPath:
"""Implementation of the A* algorithm for getting the fastest path in a 2D grid maze
Attributes:
exploredMap (Numpy array): The map after the exploration stage
goal (list): Coordinate of the goal cell
graph (list): To form the fastest path graph
index (int): Counter
path (list): The fastest path
robot (Robot): Instance of the Robot class
start (list): Coordinate of the initial cell
waypoint (list): Coordinate of the way-point if specified
"""
def __init__(self,
exploredMap,
start,
goal,
direction,
waypoint=None,
calibrateLim=5,
sim=True):
"""Constructor to initialize an instance of the FastestPath class
Args:
exploredMap (Numpy array): The map after the exploration stage
start (list): Coordinate of the initial cell
goal (list): Coordinate of the goal cell
direction (int): The starting direction of the robot
waypoint (list, optional): The coordinate of the way-point if specified
sim (bool, optional): If specify if run is simulation or real
"""
self.exploredMap = exploredMap
self.graph = []
self.start = start
self.goal = goal
self.waypoint = waypoint
self.index = 1
self.direction = direction
self.path = []
self.movement = []
self.calibrateLim = calibrateLim
self.sim = sim
if sim:
from Simulator import Robot
self.robot = Robot(self.exploredMap, direction, start, None)
else:
from Real import Robot
self.robot = Robot(self.exploredMap, direction, start)
def __getHeuristicCosts(self, goal):
"""Calculates the Manhattan distance between each cell and the goal cell
Args:
goal (list): Coordinates of the goal cell
Returns:
Numpy array: 2D grid with each cell storing the distance to the goal cell
"""
# this will create a grid of coordinates
cols, rows = np.meshgrid(range(0, 15), range(0, 20))
cost = np.zeros([20, 15])
cost = np.sqrt(np.square(rows - goal[0]) + np.square(cols - goal[1]))
cost /= np.max(cost)
return cost
def __validInds(self, inds):
"""To check if the passed indices are valid or not
To be valid the following conditions should be met:
* A 3x3 neighbourhood around the center should lie within the arena
* A 3x3 neighbourhood around the center should have no obstacle
Args:
inds (list of list): List of coordinates to be checked
Returns:
list of list: All indices that were valid
"""
valid = []
for i in inds:
r, c = i
x, y = np.meshgrid([-1, 0, 1], [-1, 0, 1])
x, y = x + r, y + c
if (np.any(x < 0) or np.any(y < 0) or np.any(x >= MAX_ROWS)
or np.any(y >= MAX_COLS)):
valid.append(False)
elif (np.any(self.exploredMap[x[0, 0]:x[0, 2] + 1,
y[0, 0]:y[2, 0] + 1] != 1)):
valid.append(False)
else:
valid.append(True)
return [inds[i] for i in range(len(inds)) if valid[i]]
def __getNeighbours(self, loc):
"""Returns the coordinates of a 3x3 neighbourhood around the passed location
Only those neighbours are returned that have been explored and are not obstacles
Args:
loc (list): Coordinates of a cell
Returns:
list of list: Coordinates of all valid neighbours of the cell
"""
r, c = loc.coord
inds = [(r - 1, c), (r + 1, c), (r, c - 1), (r, c + 1)]
inds = self.__validInds(inds)
neighbours = [self.graph[n[0]][n[1]] for n in inds]
# if its explored and not an obstacle
return [n for n in neighbours if n.value == 1]
def __getCost(self, current_pos, next_pos):
if self.direction in [NORTH, SOUTH]:
if current_pos[1] == next_pos[1]:
return 0
else:
return 1
else:
if current_pos[0] == next_pos[0]:
return 0
else:
return 1
return 1
def __setDirection(self, prev_pos, current_pos):
if prev_pos[0] < current_pos[0]:
self.direction = SOUTH
elif prev_pos[1] < current_pos[1]:
self.direction = EAST
elif prev_pos[1] > current_pos[1]:
self.direction = WEST
else:
self.direction = NORTH
def __astar(self, start, goal):
"""Implementation of the a* algorithm for finding the shortest path in a 2d grid maze
Args:
start (list): Coordinates of the starting cell
goal (list): Coordinates of the goal cell
Returns:
list of list: A list of coordinates specifying the path from start to goal
Raises:
ValueError: If no path is found between the given start and goal
"""
goal = self.graph[goal[0]][goal[1]]
# set of nodes already evaluated
closedSet = set()
# set of discovered nodes that have not been evaluated yet
openSet = set()
current = self.graph[start[0]][start[1]]
current.G = 0
# add start node to openSet
openSet.add(current)
prev = None
while (openSet):
current = min(openSet, key=lambda o: o.G + o.H)
if prev:
self.__setDirection(prev.coord, current.coord)
# if goal is reached trace back the path
if (current == goal):
path = []
while (current.parent):
path.append(current.coord)
current = current.parent
path.append(current.coord)
return path[::-1]
else:
openSet.remove(current)
closedSet.add(current)
for node in self.__getNeighbours(current):
if node in closedSet:
continue
if node in openSet:
# calculate new G cost
new_g = current.G + self.__getCost(
current.coord, node.coord)
if node.G > new_g:
node.G = new_g
node.parent = current
else:
# if neither in openSet nor in closedSet
# cost of moving between neighbours is 1
node.G = current.G + self.__getCost(
current.coord, node.coord)
node.parent = current
openSet.add(node)
prev = copy.deepcopy(current)
# exception is no path is found
raise ValueError('No Path Found')
def __initGraph(self, h_n):
"""To create a grid of graph nodes from the exploration map
Args:
h_n (Numpy array): The heuristic cost of each node to goal node
"""
self.graph = []
for row in xrange(MAX_ROWS):
self.graph.append([])
for col in xrange(MAX_COLS):
self.graph[row].append(
Node(self.exploredMap[row][col], (row, col),
h_n[row][col]))
def getFastestPath(self):
"""To call the fastest path algorithm and handle the case if there is a way-point or not
"""
path = []
start = copy.copy(self.start)
if (self.waypoint):
h_n = self.__getHeuristicCosts(self.waypoint)
self.__initGraph(h_n)
fsp = self.__astar(start, self.waypoint)
start = copy.copy(self.waypoint)
# Leaves out the last element as it will be the starting node for the next fastest path
# (Otherwise the way-point will be added twice)
path.extend(fsp[:-1])
h_n = self.__getHeuristicCosts(self.goal)
self.__initGraph(h_n)
fsp = self.__astar(start, self.goal)
path.extend(fsp)
self.path = path
self.markMap()
def markMap(self):
"""To mark the path on the exploration map
"""
for ind in self.path:
self.exploredMap[tuple(ind)] = 6
def moveStep(self):
"""To take the fastest path, robot location and determine the next
move for the robot to follow the path
"""
movement = []
if (self.robot.center.tolist() != self.path[self.index]):
diff = self.robot.center - np.asarray(self.path[self.index])
if (diff[0] == -1 and diff[1] == 0): # Going south
if self.robot.direction == NORTH:
movement.extend((RIGHT, RIGHT, FORWARD))
elif self.robot.direction == EAST:
movement.extend((RIGHT, FORWARD))
elif self.robot.direction == SOUTH:
movement.append(FORWARD)
else:
movement.extend((LEFT, FORWARD))
elif (diff[0] == 0 and diff[1] == 1): # Going west
if self.robot.direction == NORTH:
movement.extend((LEFT, FORWARD))
elif self.robot.direction == EAST:
movement.extend((RIGHT, RIGHT, FORWARD))
elif self.robot.direction == SOUTH:
movement.extend((RIGHT, FORWARD))
else:
movement.append(FORWARD)
elif (diff[0] == 0 and diff[1] == -1): # Going east
if self.robot.direction == NORTH:
movement.extend((RIGHT, FORWARD))
elif self.robot.direction == EAST:
movement.append(FORWARD)
elif self.robot.direction == SOUTH:
movement.extend((LEFT, FORWARD))
else:
movement.extend((RIGHT, RIGHT, FORWARD))
else: # Going north
if self.robot.direction == NORTH:
movement.append(FORWARD)
elif self.robot.direction == EAST:
movement.extend((LEFT, FORWARD))
elif self.robot.direction == SOUTH:
movement.extend((RIGHT, RIGHT, FORWARD))
else:
movement.extend((RIGHT, FORWARD))
for move in movement:
self.robot.moveBot(move)
self.movement.extend(movement)
# if not (self.sim):
# if (self.robot.stepCounter + len(movement) > self.calibrateLim):
# calibrate = self.robot.can_calibrate()
# if (calibrate[0]):
# movement.append(calibrate[1])
# self.robot.stepCounter = 0
self.index += 1
