def inEllipse(self, center, base, area):
inEllipse = False
x, y = center.getX(), center.getY()
diff = 0.5 * base
corners = [
center,
Point(x + diff, y + diff),
Point(x - diff, y + diff),
Point(x + diff, y - diff),
Point(x - diff, y - diff)
]
for corner in corners:
inEllipse = inEllipse or self.radiusFromRootCenter(corner,
area) <= 1
return inEllipse
def updatePose(self, numberOfTimesteps):
for i in range(numberOfTimesteps):
p = self.getPosition()
currentHeading = self.getHeading()
if currentHeading != self.desiredHeading:
diff = Util.unwrap(self.desiredHeading - currentHeading)
sign = diff / abs(diff)
if abs(diff) > self.turningRadius:
currentHeading = currentHeading + sign * self.turningRadius
else:
currentHeading = self.desiredHeading % 360
if self.currentSpeed != self.desiredSpeed:
diff = self.desiredSpeed - self.currentSpeed
if abs(diff) > self.acceleration:
sign = diff / abs(diff)
self.currentSpeed = self.currentSpeed + sign * self.acceleration
else:
self.currentSpeed = self.desiredSpeed
course = np.radians((-currentHeading) + 90)
hypo = self.currentSpeed * self.timestepLength
dx = hypo * np.cos(course)
dy = hypo * np.sin(course)
x, y = p.getX(), p.getY()
self.pose = Pose(currentHeading % 360, Point(x + dx, y + dy))
self.updateLog()
def __init__(self, a):
self.height = int(round(a.getHeight()))
self.width = int(round(a.getWidth()))
self.gridsize = a.getGridsize()
t = a.getTarget()
self.halfSideLength = 0.6 * self.bigDia()
targetx = None
targety = None
try:
targetx = float(t.getX())
targety = float(t.getY())
except:
targetx, targety = self.randTarget()
while self.radiusFromCenter(targetx, targety) >= 1:
targetx, targety = self.randTarget()
print('Target is at ' + Point(targetx, targety).toString())
self.setTarget(targetx, targety)
self.cells = int(round((self.halfSideLength * 2) / self.gridsize)) + 1
self.middle = int(round(self.halfSideLength / self.gridsize))
self.data = [0] * self.cells
for i in range(self.cells):
self.data[i] = [0] * self.cells
for yindex in range(self.cells):
dy = yindex * self.gridsize
for xindex in range(self.cells):
dx = xindex * self.gridsize
y = dy - self.halfSideLength - (np.sign(dy - self.halfSideLength) * float(self.gridsize) * 0.5)
x = dx - self.halfSideLength - (np.sign(dx - self.halfSideLength) * float(self.gridsize) * 0.5)
if self.radiusFromCenter(x, y) <= 1:
self.data[yindex][xindex] = self.getDataForDist(self.radiusFromCenter(x, y))
def makeCounterClockwiseSpiral(self, dia):
pos = self.vehicle.getPosition()
self.wps = []
r = 0
a = 0.0
turns = 0
newWp = Point(0, 0)
while r < self.area.getHalfSideLength():
turns = int(a / 360)
a += 360 / (8 + 8 * turns)
r = dia * (a / 360.0)
x = r * np.cos(np.deg2rad(a))
y = r * np.sin(np.deg2rad(a))
newWp = Point(-x, y).translate(pos.getX(), pos.getY())
if self.area.inArea(newWp):
self.wps.append(newWp)
self.target = self.wps.pop(0)
def __init__(self, center, base, minGridsize, area, inputKey):
self.center = center
self.base = base
self.minGridsize = minGridsize
self.area = area
self.key = inputKey
self.root = area.getTree()
self.children = dict()
self.keys = ["topLeft", "topRight", "botLeft", "botRight"]
if self.inEllipse(center, base, area):
if base * 0.5 > minGridsize and base * 0.5 > 1:
diff = base * 0.25
for key in self.keys:
tempCenter = None
x = center.getX()
y = center.getY()
if key == "topLeft":
x = x - diff
y = y + diff
elif key == "topRight":
x = x + diff
y = y + diff
elif key == "botLeft":
x = x - diff
y = y - diff
elif key == "botRight":
x = x + diff
y = y - diff
tempCenter = Point(x, y)
tree = Tree(tempCenter, base * 0.5, minGridsize, area, key)
self.children[key] = tree
self.children[key].setParent(self)
prob = self.children[key].getMaxProb()
if prob > self.maxProb:
self.maxProb = prob
self.maxKey = key
self.totProb += prob
else:
self.keys = ["leaf"]
dist = self.radiusFromRootCenter(center, area)
prob = area.getDataForDist(dist)
self.totProb = prob
self.maxProb = prob
self.maxKey = "leaf"
t = area.getTarget()
hasTarget = self.posInCell(t)
#hasTarget = hasTarget or pos.getX() <= self.center.getY() + (self.base * 0.5)
#hasTarget = hasTarget or pos.getY() <= self.center.getY() + (self.base * 0.5)
self.children["leaf"] = Leaf(
Cell(prob, center.getX(), center.getY(), hasTarget),
"leaf")
self.children["leaf"].setParent(self)
'''print('leaf created')
print('keys: ' + repr(self.keys))
print('children: ' + repr(self.children))
print('----------------------------------------------')'''
else:
self.keys = []
def __init__(self, a, sensor):
self.sensor = sensor
self.height = int(round(a.getHeight()))
self.width = int(round(a.getWidth()))
self.gridsize = a.getGridsize()
t = a.getTarget()
self.halfSideLength = 0.6 * self.bigDia()
targetx = None
targety = None
try:
targetx = float(t.getX())
targety = float(t.getY())
except:
targetx, targety = self.randTarget()
while self.radiusFromCenter(targetx, targety) >= 1:
targetx, targety = self.randTarget()
print('Target is at ' + Point(targetx, targety).toString())
self.setTarget(targetx, -targety)
center = Point(0, 0)
base = self.halfSideLength * 2.0
minGridsize = self.gridsize
area = self
self.tree = Tree(center, base, minGridsize, area, 'root')
#print('tree is made')
self.gridsize = self.tree.getSmallestBase()
self.cells = int(round((self.halfSideLength * 2) / self.gridsize)) + 1
self.middle = int(round(self.halfSideLength / self.gridsize))
self.data = [None] * self.cells
for i in range(self.cells):
self.data[i] = [None] * self.cells
for yindex in range(self.cells):
for xindex in range(self.cells):
pos = self.cellIndexToPos(xindex, yindex)
leaf = self.tree.getLeafForPos(pos)
self.data[yindex][xindex] = copy.copy(leaf)
def updateSearch(self, showProb):
sublog = self.vehicle.logFrom(self.lastEntry)
data = None
if not showProb:
if self.foundTarget():
data = self.area.updateSearchBasedOnLog(
sublog, showProb, self.area.getTarget())
data.append([])
else:
data = self.area.updateSearchBasedOnLog(
sublog, showProb, Point(0, 0))
else:
data = self.area.updateSearchBasedOnLog(sublog, showProb, None)
for changes in data:
self.dto.appendChanges(changes)
self.updateLatestLogEntry()
def __init__(self, args):
if len(args) == 1:
area = copy.deepcopy(args[0])
self.height = int(round(area.getHeight()))
self.width = int(round(area.getWidth()))
self.gridsize = area.getGridsize()
self.data = area.getData()
self.cells = len(self.data[0])
self.halfSideLength = area.getHalfSideLength()
self.target = area.getTarget()
self.coverage = area.isCoverage()
else:
self.height = args[0]
self.width = args[1]
self.gridsize = args[2]
self.target = Point(args[3], args[4])
def setTarget(self, targetx, targety): self.realTarget = Point(targetx, -targety) targetx = targetx + self.halfSideLength targety = self.halfSideLength - targety self.target = Point(targetx, targety)
def processSearch(self):
startTime = str(datetime.datetime.now())
self.readInputFields()
randCourse = False
try:
c = self.entries[self._COURSEINDEX].get()
f = float(c)
i = int(c)
except:
randCourse = True
try:
tx = self.entries[self._TARGETXINDEX].get()
ty = self.entries[self._TARGETYINDEX].get()
f = float(tx)
i = int(tx)
f = float(ty)
i = int(ty)
except:
self.values[self._TARGETXINDEX] = 'r'
self.values[self._TARGETYINDEX] = 'r'
ackSimLength = dict()
ackProcTime = dict()
gridsizes = dict()
targets = []
originalTargetX = self.values[self._TARGETXINDEX]
originalTargetY = self.values[self._TARGETYINDEX]
for strat in self.strategies:
ackSimLength[strat] = 0
ackProcTime[strat] = 0
runs = int(self.values[self._RUNSINDEX])
executedRuns = 0
latestDTO = None
try:
gc.disable()
for i in range(runs):
print(repr(i) + '.0')
if randCourse:
self.values[self._COURSEINDEX] = int(random.random_sample() * 360)
self.values[self._TARGETXINDEX] = originalTargetX
self.values[self._TARGETYINDEX] = originalTargetY
strat = self.strategies[0]
premises = self.getPremises(strat)
self.contr.simulate(premises)
sa = self.contr.getSearcharea()
latestDTO = self.contr.getAckumulatedSearch()
self.processedSearches[latestDTO.toString()] = latestDTO
self.searchSelector.insert(0, latestDTO.toString())
ackSimLength[strat] = ackSimLength[strat] + latestDTO.len()
rt = latestDTO.getRTS() + (0.000001 * latestDTO.getRTMS())
print('running time: ' + repr(rt) + '\n')
ackProcTime[strat] = ackProcTime[strat] + rt
target = sa.getTarget()
x, y = target.getX(), target.getY()
orTar = Point(x, y)
targets.append(orTar)
if i == 0:
gridsizes[strat] = latestDTO.getGridsize()
if len(self.strategies) > 1:
self.values[self._TARGETXINDEX] = x
self.values[self._TARGETYINDEX] = -y
for j in range(1, len(self.strategies)):
print(repr(i) + '.' + repr(j))
strat = self.strategies[j]
premises = self.getPremises(strat)
self.contr.simulate(premises)
latestDTO = self.contr.getAckumulatedSearch()
rt = latestDTO.getRTS() + (0.000001 * latestDTO.getRTMS())
print('running time: ' + repr(rt) + '\n')
self.processedSearches[latestDTO.toString()] = latestDTO
self.searchSelector.insert(0, latestDTO.toString())
ackSimLength[strat] = ackSimLength[strat] + latestDTO.len()
ackProcTime[strat] = ackProcTime[strat] + rt
if i == 0:
gridsizes[strat] = latestDTO.getGridsize()
executedRuns += 1
if executedRuns % 3 == 0 and not executedRuns > runs - 4:
self.searchSelector.delete(0, END)
self.processedSearches.clear()
gc.collect()
print(gc.garbage)
except:
print "Exception in user code:"
print '-'*60
traceback.print_exc(file=sys.stdout)
print '-'*60
finally:
if not gc.isenabled():
gc.collect()
gc.enable()
self.values[self._TARGETXINDEX] = originalTargetX
self.values[self._TARGETYINDEX] = originalTargetY
totDist = 0
origo = Point(0, 0)
for p in targets:
totDist = totDist + p.distTo(origo)
mttds = dict()
averageProcTime = dict()
h = str(latestDTO.getHeight())
w = str(latestDTO.getWidth())
filename = "../../mttds/" + h + 'x' + w
for strat in self.strategies:
mttds[strat] = ackSimLength[strat] / float(executedRuns)
averageProcTime[strat] = ackProcTime[strat] / float(executedRuns)
tmp = strat[:-3]
filename = filename + '_' + tmp[:2] + tmp[-2:]
file = open(filename + ".txt", "a")
file.write('*************************************************************************\n')
file.write('STARTED: \t' + startTime + '\n')
endTime = str(datetime.datetime.now())
file.write('FINISHED:\t' + endTime + '\n')
file.write('PARAMETERS: ')
if randCourse:
self.values[self._COURSEINDEX] = 'r'
for i in range(len(self.values)):
if i % 2 == 0:
file.write('\n' + self.textFields[i] + ' : ' + str(self.values[i]))
else:
file.write('\t' + self.textFields[i] + ' : ' + str(self.values[i]))
file.write('\n\nUNITS : SECONDS AND METERS')
file.write('\nAVERAGE DISTANCE TO TARGET FROM ORIGO: ' + str(totDist / executedRuns))
file.write('\nRUNS PROCESSED: ' + str(executedRuns))
file.write('\nHEIGHTxWIDTH: ' + h + 'x' + w + '\n')
file.write('-------------------------------------------------------------------------\n')
file.write('Strategy\t|MTTD\t\t|MEAN PROC TIME\t\t|GRIDSIZE\t|\n')
file.write('-------------------------------------------------------------------------\n')
for strat in self.strategies:
stratname = strat[:2] + strat[-5:-3]
apt = str(averageProcTime[strat])
mttd = str(round(mttds[strat], 2))
if len(mttd) < 7:
mttd = mttd + '\t'
if len(apt) < 7:
apt = apt + '\t'
gs = str(gridsizes[strat])
if len(gs) < 7:
gs = gs + '\t'
file.write(stratname + '\t\t|' + mttd + '\t|' + apt + '\t\t|' + gs + '\t|\n')
file.write('*************************************************************************\n')
print('processing done')
def setTarget(self, x, y):
self.target = Point(x, y)
def updateSearchBasedOnLog(self, log, showProb, maxPos):
returnData = []
dt = log.getTimestepLength()
for i in range(log.length() - 1):
changes = []
logFrom = log.get(i)
sensor = logFrom.getSensor()
posFrom = logFrom.getPose().getPosition()
fX = posFrom.getX()
fY = posFrom.getY()
xPos = int(round((fX + self.halfSideLength) / self.gridsize))
yPos = int(round((fY + self.halfSideLength) / self.gridsize))
logTo = log.get(i + 1)
posTo = logTo.getPose().getPosition()
found = self.getCellForPos(posTo).hasTarget()
found = found or self.getCellForPos(posTo).hasTarget()
sensorRange = sensor.getRadius()
depth = int(round(sensorRange)) + 1
adjCells = self.getAdjacentCells(posFrom, depth)
priorProb = self.data[yPos][xPos]
self.data[yPos][xPos] = 0.5
if showProb and not priorProb == self.data[yPos][xPos]:
changes.append(
self.getCellDTO(self.data[yPos][xPos], xPos, yPos))
for cell in adjCells:
cpos = cell.getPosition()
dist = posFrom.distTo(cpos)
if dist <= sensorRange:
x, y = self.posToCellIndex(cpos)
probOfDetection = sensor.probabilityOfDetection(dist)
priorProb = self.data[y][x]
if probOfDetection == 1:
self.data[y][x] = 0.5
if showProb and not priorProb == self.data[y][x]:
if self.firstTimeZeroProb:
changes.append(
self.getCellDTO(self.data[y][x], x, y))
else:
for yy in range(self.cells):
for xx in range(self.cells):
changes.append(
self.getCellDTO(
self.data[yy][xx], xx, yy))
self.firstTimeZeroProb = True
if not showProb and not found:
x, y = self.posToCellIndex(Point(0, 0))
zeroProbs = [0] * self.cells
for i in range(self.cells):
zeroProbs[i] = [0] * self.cells
zeroProbs[y][x] = 1
if self.firstTimeZeroProb:
for i in range(self.cells):
for j in range(self.cells):
changes.append(
self.getCellDTO(zeroProbs[i][j], j, i))
self.firstTimeZeroProb = False
returnData.append(changes)
return returnData
class NavigationStrategy:
__metaclass__ = ABCMeta
vehicle = None
area = None
target = Point(0, 0)
localSearch = False
@abstractmethod
def nextCourse(self, vehicle, area):
pass
@abstractmethod
def nextPos(self, vehicle, area):
pass
@abstractmethod
def makeArea(self, area, sensor, depth):
pass
def setVehicleAndArea(self, vehicle, area):
self.vehicle = vehicle
self.area = area
def getCourseTowards(self, that):
this = self.vehicle.getPosition()
return self.getCourseFromTo(this, that)
def getCourseFromTo(self, this, that):
dx = that.getX() - this.getX()
dy = that.getY() - this.getY()
deg = np.degrees(np.arctan2(dy, dx))
course = self.degToNav(deg)
return course
def degToNav(self, deg):
return (-(deg - 90)) % 360
def navToDeg(self, nav):
return ((-nav) + 90) % 360
def atPosition(self, vehicle, area, target):
margin = area.getMargin()
tarx = target.getX()
tary = target.getY()
pos = vehicle.getPosition()
posx = pos.getX()
posy = pos.getY()
correctx = posx > tarx - margin and posx < tarx + margin
correcty = posy > tary - margin and posy < tary + margin
return correctx and correcty
def updateSpeed(self, target):
vehicle = self.vehicle
desiredCourse = self.getCourseTowards(target)
tr = vehicle.getTurningRadius()
pos = vehicle.getPosition()
desiredSpeed = pos.distTo(target) / float(vehicle.getTimestepLength())
course = vehicle.getHeading()
diff = (course - desiredCourse) % 360
steps = int(diff / tr) + 1
desiredSpeed = desiredSpeed / steps
vehicle.setDesiredSpeed(desiredSpeed)
def getTarget(self):
return self.target
def localSearch(self, localSearch):
self.localSearch = localSearch
self.target = self.vehicle.getPosition()
def startSearch(self):
i = 0
self.updateLatestLogEntry()
self.strategy.setVehicleAndArea(self.vehicle, self.area)
foundTarget = False
nextPos = Point(0, 0)
course = self.strategy.getCourseTowards(nextPos)
self.vehicle.setInitialCourse(course)
while not self.vehicle.near(nextPos):
self.vehicle.updatePose(1)
foundTarget = self.foundTarget() and self.strongConnection()
if foundTarget:
break
showProb = False
self.updateSearch(showProb)
if foundTarget:
self.moveVehicleTowardsTarget()
return
currentSpeed = self.vehicle.getCurrentSpeed()
self.vehicle.setDesiredSpeed(0)
while not self.vehicle.atPosition(nextPos) and not int(
round(currentSpeed)) == 0:
self.vehicle.updatePose(1)
currentSpeed = self.vehicle.getCurrentSpeed()
foundTarget = self.foundTarget() and self.strongConnection()
if foundTarget:
break
if foundTarget:
self.vehicle.setDesiredSpeed(self.vehicle.getMaxSpeed())
self.moveVehicleTowardsTarget()
return
self.vehicle.updatePose(5)
self.updateSearch(showProb)
showProb = True
self.dto.showProb(SearchareaDTO([self.area]))
while not foundTarget:
if isinstance(self.strategy, Greedy):
tmpPos = self.strategy.nextPos(self.vehicle, self.area)
course = self.strategy.getCourseTowards(tmpPos)
if not tmpPos.equals(nextPos):
foundTarget = self.foundTarget()
self.updateSearch(showProb)
nextPos = tmpPos
if not foundTarget:
self.strategy.updateSpeed(tmpPos)
self.vehicle.updatePose(1)
else:
self.vehicle.setCourse(course)
self.strategy.updateSpeed(tmpPos)
self.vehicle.updatePose(1)
if self.foundTarget() and self.strongConnection():
break
else:
course = self.strategy.nextCourse(self.vehicle, self.area)
self.vehicle.setCourse(course)
if not self.area.isCoverage():
nextPos = self.strategy.getTarget()
self.strategy.updateSpeed(nextPos)
self.vehicle.updatePose(1)
self.updateSearch(showProb)
foundTarget = self.foundTarget()
self.moveVehicleTowardsTarget()
def currAngle(self, pos):
return np.degrees(np.arccos(
pos.getX() / pos.distTo(Point(0, 0)))) % 360
def cellIndexToPos(self, xindex, yindex):
dy = yindex * self.getGridsize()
dx = xindex * self.getGridsize()
y = dy - self.getHalfSideLength()
x = dx - self.getHalfSideLength()
return Point(x, y)
def cellIndexToPos(self, xindex, yindex):
dy = yindex * self.gridsize
dx = xindex * self.gridsize
y = dy - self.halfSideLength # - (np.sign(dy - self.halfSideLength) * float(self.gridsize) * 0.5)
x = dx - self.halfSideLength # - (np.sign(dx - self.halfSideLength) * float(self.gridsize) * 0.5)
return Point(x, y)
def getPosition(self):
pos = self.pose.getPosition()
return Point(pos.getX(), pos.getY())
def cellIndexToPos(self, xindex, yindex): dy = yindex * self.gridsize dx = xindex * self.gridsize y = dy - self.halfSideLength x = dx - self.halfSideLength return Point(x, y)
def getPosition(self):
return Point(self.x, self.y)