class Robot:
"""
Robot class. Represents the virtual robot.
Orientation refers to where the robot is facing:
0. Top
1. Right
2. Bottom
3. Left
Attributes:
pos: a 15x20 array. Contains None, 0 or 1 as values.
orientation: Centre of 3x3 start area. Default = [1,1]
explore: if True, update map after every movement. Explorer sets this to False after finishing exploration.
map: Map object. Refer to Map.py
sensors: Sensors object. Refer to sensors.py
coordinator: Coordinator object. Refer to coordinator.py
pathfinder: Pathfinder object. Refer to pathfinder.py
explorer: Explorer Object. Refer to explorer.py
"""
pos = [1, 1]
orientation = 0
explore = True
map = Map()
android = None
sensors = None
coordinator = Coordinator()
pathfinder = None
explorer = None
imagefinder = None
images = []
camera_counter = 0
sendimages = False
sendreport_counter = 0
fakeRun = False
def __init__(self,
arduino=None,
android=None,
fakeRun=False,
fakeMap=None,
stepsPerSec=1,
**kwargs):
"""
Constructor. Accepts attributes as kwargs.
Args:
fakeRun: set True if running simulation. Remember to give fake map as input. I.e: fakeMap = fakemap
fakeMap: set simulation map. If left empty, creates an empty arena.
pos: a 15x20 array. Contains None, 0 or 1 as values.
orientation: Centre of 3x3 start area. Default = [1,1]
map: Map object. Refer to Map.py
sensors: Sensors object. Refer to sensors.py
coordinator: Coordinator object. Refer to coordinator.py
"""
if fakeRun:
self.fakeRun = True
from sensors_fake import Sensors
self.sensors = Sensors(self, fakeMap) #fake sensors for simulation
self.coordinator.fakeRun = True
self.coordinator.stepsPerSec = stepsPerSec
self.imagefinder = Imagefinder(fakeRun=True)
elif arduino is None:
raise Exception("Real run requires arduino to be present")
elif android is None:
raise Exception("Real run requires arduino to be present")
else:
from sensors import Sensors
self.android = android
self.sensors = Sensors(self, arduino)
self.coordinator.arduino = arduino
self.imagefinder = Imagefinder()
#update map
self.updatemap()
goalTiles = [ #set goal as explored
[12, 19],
[13, 19],
[14, 19],
[12, 18],
[13, 18],
[14, 18],
[12, 17],
[13, 17],
[14, 17],
]
valuelist = [0] * len(goalTiles)
self.map.setTiles(goalTiles, valuelist)
#initialise pathfinder
self.pathfinder = Pathfinder(self.map)
#initialise explorer
self.explorer = Explorer(self)
def backward(self):
"""
Moves the robot backward.
"""
self.coordinator.backward()
x, y = self.pos
newpos_dict = {
0: "[x, y-1]",
1: "[x-1,y]",
2: "[x, y+1]",
3: "[x+1,y]"
}
self.pos = eval(newpos_dict[self.orientation])
if self.explore: self.updatemap()
if settings.logging:
print("Movement: Robot goes backward")
def checkPhantomBlock(self, checkpos):
print("Checking {} for phantom block".format(checkpos))
x, y = checkpos
neighbours = [
"[x+1,y]",
"[x,y-1]",
"[x-1,y]",
"[x,y+1]",
]
for n in neighbours:
tile = eval(n)
if self.map.getTile(tile) is None:
print("Unexplored terrain detected, changing {} to unexplored".
format(checkpos))
self.map.setTile(checkpos, None)
self.removeImage(
checkpos) #check whether removed block has image
return
#no unexplored neighbours
print("No unexplored terrain detected. Keeping {} as is.".format(
checkpos))
def decodeSensors(self, terrain, tiles_array, sensors_range):
newTiles = []
valuelist = []
skipUpdate = False
for row in tiles_array:
terr = terrain.pop(0)
if terr == -1: continue
for i in range(0, terr):
pos = row[i]
#check phantom block
if self.map.getTile(pos) == 1:
if terr > 1:
"""Not tested"""
print(
"Warning: Sensor attemped to wipe previous obstacle. But we are skipping reading."
)
skipUpdate = True
break
print(
"Warning: Phantom block detected and removed. Tile is {}"
.format(pos))
self.removeImage(
pos) #check whether removed block has image
newTiles.append(pos)
valuelist += [0]
"""Not tested"""
if skipUpdate:
skipUpdate = False
continue
if terr < sensors_range:
pos = row[terr]
newTiles.append(pos)
valuelist += [1] #obstacle detected. Add to map
#check phantom block
if self.map.getTile(pos) == 0:
print(
"Warning: Phantom block appeared on explored tile, {}."
.format(pos))
if terr < sensors_range - 1:
checkpos = row[terr + 1]
self.checkPhantomBlock(checkpos)
self.map.setTiles(newTiles, valuelist)
def explore(self, timer=None, exploreLimit=None):
"""
Starts exploration.
Args:
timer: Integer. Time in seconds. Total time for robot to explore. Includes time to return
exploreLimit: Float. Between 0.0 to 1.0. Percentage of map to explore before exploration is declared done.
"""
self.explorer.setTime(timer)
self.explorer.setExploreLimit(exploreLimit)
self.explorer.start()
#update android exploration done
if not self.fakeRun:
self.writeImages()
time.sleep(0.5)
self.android.write('{"action": "exploreCompleted"}')
def faceDirection(self, orient):
"""
Turns the robot to face the given direction.
Args:
orient: Integer. Direction for robot to face.
"""
if orient == self.orientation:
return
elif orient == (self.orientation + 1) % 4:
self.turnRight()
elif orient == (self.orientation + 3) % 4:
self.turnLeft()
else:
self.turnRight()
self.turnRight()
def detectImage(self, reset_counter=False):
if reset_counter:
self.camera_counter = 0
if self.isDetectImageCancelled():
return
#only check tiles that have obstacles
checktiles = []
baseline_vert = self.getBaseLineVert()
for i in range(3):
if self.map.getTile(baseline_vert[i]) == 1:
checktiles.append(i)
results = self.imagefinder.find(checktiles=checktiles)
if results is None:
return
id, location = results
pos = baseline_vert[location]
for img in self.images:
if pos == img[1]:
print(
"WARNING. Found image {} at {} but position already has an image"
.format(id, pos))
return
print("images found")
self.images.append([id, pos])
self.sendimages = True
#write to android
self.writeImages()
def isDetectImageCancelled(self):
"""method to minimise image recognition calls"""
#cancelled because settings is find no image
if settings.findallimages == 0:
return True
#cancelled because all images found
if len(self.images) == settings.images_threshold:
return True
#reduce camera usage by only taking once every 3 steps
if self.camera_counter != 0:
self.camera_counter = (self.camera_counter + 1) % 3
return True
else:
self.camera_counter = (self.camera_counter + 1) % 3
#if next to arena walls, cancel image recognition
#format is [x,y,orientation]
conditions = [[1, -1, 0], [-1, 18, 1], [13, -1, 2], [-1, 1, 3]]
for cond in conditions:
x, y, orient = cond
if (self.pos[0] == x
or self.pos[1] == y) and self.orientation == orient:
return True
return False
def findpath(self,
start=None,
goal=[13, 18],
waypoint=None,
move=True,
rowgoal=None):
"""
Method for robot to find shortest path.
Args:
start: [x,y] coordinates. Defaults to current robot position.
goal: [x,y] coordinates. Defaults to [13,18].
waypoint: [x,y] coordinates. Defaults to None.
move: Boolean. Defaults True. If True, moves robot after finding shortest path.
rowgoal: Integer. y-axis. Defaults None. Ends findpath() early if y-axis is reached.
"""
if start is None:
start = self.pos
orientation = self.orientation
path, directions = self.pathfinder.findpath(start, goal, waypoint,
orientation)
if move:
instructions = self.readDirections(directions)
for i in instructions:
if rowgoal and self.pos[1] == rowgoal: break
exec(i)
if settings.logging:
print("Movement: findpath() to " + str(goal) + " with rowgoal " +
str(rowgoal))
return [path, directions]
def forward(self, steps=1, findImage=False):
"""
Moves the robot forward.
Args:
steps: Integer. Defaults to 1. Number of steps forward to take
"""
if not self.coordinator.forward(steps):
#phantom block detected. Check sensors again
wipeTiles = self.getBaseLine()
valuelist = [None] * len(wipeTiles)
self.map.setTiles(wipeTiles, valuelist) #remove tiles
front_terrain = self.sensors.getFront()
tiles_array = self.getBaseLineRange(
length=self.sensors.front_sensors_range)
self.decodeSensors(terrain=front_terrain,
tiles_array=tiles_array,
sensors_range=self.sensors.front_sensors_range)
return
x, y = self.pos
newpos_dict = {
0: "[x, y+steps]",
1: "[x+steps,y]",
2: "[x, y-steps]",
3: "[x-steps,y]"
}
self.pos = eval(newpos_dict[self.orientation])
if self.explore: self.updatemap()
if findImage: self.detectImage()
if settings.logging:
print("Movement: Robot goes forward " + str(steps) + " steps")
#send update to android
self.writeReport()
def getBaseLine(self):
"""
Baseline refers to the left,middle & right (from the robot's perspective) tiles
immediately in front of robot and the 3x3 space the robot is occupying.
Returns a string compatible for eval(). Expects [x,y] to be declared beforehand.
Example string:
"[[x-1,y+2],[x,y+2],[x+1,y+2]]"
Example usage:
x,y = self.pos
baseline = getBaseLine()
tilelist = eval(baseline)
"""
#baseline_dict contains the tiles to search. For example, if facing right, search top, middle & bottom tiles
x, y = self.pos
baseline_dict = {
0: "[[x-1,y+2],[x,y+2],[x+1,y+2]]",
1: "[[x+2,y+1],[x+2,y],[x+2,y-1]]",
2: "[[x+1,y-2],[x,y-2],[x-1,y-2]]",
3: "[[x-2,y-1],[x-2,y],[x-2,y+1]]"
}
baseline = eval(baseline_dict[self.orientation])
return baseline
def getBaseLineRange(self, length=1):
baseline = self.getBaseLine()
tileRange = self.getTileRange()
results = []
for tile in baseline:
x, y = tile
tiles = []
for i in range(length):
tiles.append([x, y])
x, y = eval(tileRange)
results.append(tiles)
return results
def getBaseLineVert(self, right=False):
"""
baseline_vert refers to baseline, but vertical. Refer to getBaseLine() above.
"""
x, y = self.pos
if right:
baseline_vert_dict = {
0: "[[x+2,y+1], [x+2,y], [x+2,y-1]]",
1: "[[x+1,y-2], [x,y-2], [x-1,y-2]]",
2: "[[x-2,y-1], [x-2,y], [x-2,y+1]]",
3: "[[x-1,y+2], [x,y+2], [x+1,y+2]]"
}
else:
baseline_vert_dict = {
0: "[[x-2,y+1], [x-2,y], [x-2,y-1]]",
1: "[[x+1,y+2], [x,y+2], [x-1,y+2]]",
2: "[[x+2,y-1], [x+2,y], [x+2,y+1]]",
3: "[[x-1,y-2], [x,y-2], [x+1,y-2]]"
}
baseline_vert = eval(baseline_vert_dict[self.orientation])
return baseline_vert
def getBaseLineVertRange(self, length=1, exclude_mid=True, toRight=False):
baseline_vert = self.getBaseLineVert(right=toRight)
if exclude_mid: baseline_vert.pop(1)
tileRange_vert = self.getTileRangeVert(toRight=toRight)
results = []
for tile in baseline_vert:
x, y = tile
tiles = []
for i in range(length):
tiles.append([x, y])
x, y = eval(tileRange_vert)
results.append(tiles)
return results
def getTileRange(self):
"""
Range of tiles to search. If facing right, search range of tiles right of robot.
Returns a string compatible for eval(). Expects [x,y] to be declared beforehand.
Example string:
"[x,y+1]"
Example usage:
x,y = self.pos
tilerange=getTileRange
for i in range(0,5):
nextTile = eval(tilerange)
"""
tileRange_dict = {
0: "[x,y+1]",
1: "[x+1,y]",
2: "[x,y-1]",
3: "[x-1,y]",
}
tileRange = tileRange_dict[self.orientation]
return tileRange
def getTileRangeVert(self, toRight=False):
"""
Search range of tiles to left of robot. Refer to getTileRange() above.
"""
if toRight:
tileRange_vert_dict = {
0: "[x+1,y]",
1: "[x,y-1]",
2: "[x-1,y]",
3: "[x,y+1]",
}
else:
tileRange_vert_dict = {
0: "[x-1,y]",
1: "[x,y+1]",
2: "[x+1,y]",
3: "[x,y-1]",
}
tileRange_vert = tileRange_vert_dict[self.orientation]
return tileRange_vert
def isLeftBlocked(self):
"""
Checks whether left side is blocked by reading map.
"""
x, y = self.pos
tiles = self.getBaseLineVert()
for pos in tiles:
if self.map.getTile(pos) == 1:
return True
return False
def readDirections(self, directions):
"""
Reads a list of directions and converts it into instructions like forward, turn left, etc.
Args:
directions: list of directions/integers.
"""
prev = self.orientation
steps = 0
instructions = []
for d in directions:
if d == prev:
steps += 1
prev = d
elif d == (prev + 1) % 4: #turn right
if steps > 0:
instructions.append("self.forward(" + str(steps) + ")")
instructions.append("self.turnRight()")
steps = 1
elif d == (prev + 2) % 4: #U-turn
if steps > 0:
instructions.append("self.forward(" + str(steps) + ")")
instructions.append("self.turnRight()")
instructions.append("self.turnRight()")
steps = 1
else: #turn right
if steps > 0:
instructions.append("self.forward(" + str(steps) + ")")
instructions.append("self.turnLeft()")
steps = 1
prev = d
if steps > 0:
instructions.append("self.forward(" + str(steps) + ")")
if settings.logging:
print("=======Read Instructions Output========")
print(instructions)
return instructions
def removeImage(self, tile):
for im in self.images:
id, pos = im
if pos == tile:
print(
"WARNING: Image detected at removed phantom block. Removing image as well. Consider block may not be phantom?"
)
self.images.remove(im)
def setAttributes(self, **kwargs):
"""
Set class attributes. Accepts kwargs of robot attributes.
"""
for key, value in kwargs:
concat = "self." + key + " = " + value
eval(concat) #Set attributes. Evaluate self.key = value
def turnLeft(self, findImage=False):
"""
Turns the robot left.
"""
self.coordinator.turnLeft()
self.orientation = (self.orientation + 3) % 4
if self.explore: self.updatemap()
if findImage: self.detectImage()
if settings.logging:
print("Movement: Robot Turns Left")
#send update to android
self.writeReport()
def turnRight(self, findImage=False):
"""
Turns the robot right.
"""
if findImage:
self.detectImage(
reset_counter=True) #called before and after movement
self.coordinator.turnRight()
self.orientation = (self.orientation + 1) % 4
if self.explore: self.updatemap()
if findImage: self.detectImage(reset_counter=True)
if settings.logging:
print("Movement: Robot Turns Right")
#send update to android
self.writeReport()
def updatemap(self):
"""
Updates map by reading sensors.
"""
#update robot's position as free space on map
x, y = self.pos
freeTiles = [
[x - 1, y + 1],
[x, y + 1],
[x + 1, y + 1],
[x - 1, y],
[x, y],
[x + 1, y],
[x - 1, y - 1],
[x, y - 1],
[x + 1, y - 1],
]
valuelist = [0] * len(freeTiles)
self.map.setTiles(freeTiles, valuelist)
#update map with front sensors
terrain = self.sensors.getLeastSensors()
front_terrain = terrain[:3]
tiles_array = self.getBaseLineRange(
length=self.sensors.front_sensors_range)
self.decodeSensors(terrain=front_terrain,
tiles_array=tiles_array,
sensors_range=self.sensors.front_sensors_range)
#update map with left sensors
left_terrain = terrain[3:5]
tiles_array = self.getBaseLineVertRange(
length=self.sensors.left_sensors_range)
self.decodeSensors(terrain=left_terrain,
tiles_array=tiles_array,
sensors_range=self.sensors.left_sensors_range)
#update map with right sensors
right_terrain = terrain[-1]
if right_terrain == -1:
tiles_array = self.getBaseLineVertRange(
length=self.sensors.front_sensors_range, #use front sensors
exclude_mid=False,
toRight=True)
row = tiles_array.pop(self.sensors.right_sensors_position)
isRightExplored = True
for tile in row:
if self.map.getTile(tile) == 1: #explored and found obstacle
break
if self.map.getTile(tile) is None:
isRightExplored = False
if not isRightExplored:
#update map by turning right and using front sensors
self.turnRight()
front_terrain = self.sensors.getFront()
tiles_array = self.getBaseLineRange(
length=self.sensors.front_sensors_range)
self.decodeSensors(
terrain=front_terrain,
tiles_array=tiles_array,
sensors_range=self.sensors.front_sensors_range)
self.turnLeft()
else:
#update using right sensors.
tiles_array = self.getBaseLineVertRange(
length=self.sensors.right_sensors_range,
exclude_mid=False,
toRight=True)
row = tiles_array.pop(self.sensors.right_sensors_position)
self.decodeSensors(terrain=[right_terrain],
tiles_array=[row],
sensors_range=self.sensors.right_sensors_range)
if self.map.is_explored(): self.explore = False
def writeImages(self):
img_list = []
for img in self.images:
x, y = img[1]
id = img[0]
img_list.append([x, y, id])
output = {"imageDisplay": img_list}
output = json.dumps(output)
self.android.write(output)
def writeReport(self):
if self.fakeRun:
return
self.sendreport_counter = (self.sendreport_counter + 1) % 10
if self.sendreport_counter == 0:
self.writeImages()
else:
results = ""
for item in self.map.convert():
results += item[2:].upper() + ','
for coords in self.pos:
results += str(coords) + ','
orientation = 90 * self.orientation
results += str(orientation)
dict = {"robot": results}
report = json.dumps(dict)
self.android.write(report)
