################################################################################
# obstaclesInPocket.py: utility methods for dealing with pockets
# This file was specifically created to be used when dealing with obstacles
# that occur inside pockets (Matysik 2008: goalObstPerim.py)
from hexsim import *
from robot import *
################################################################################
def findPocketCells(obstacleCells,goalCells,start):
"""
Given a cell array of contigious cells, usually of obstacle cells,
and a starting perimeter cell, return all pockets cells in a
new cell array.
A pocket cell falls along a straight
line that goes through the center of two cells in the
list of cells, where the line is normal to the sides
of the the hexagons it passes through. For example,
in the below figure, if O are the obstacle cells
listed in 'cells', all the cells labeled P would
be pocket cells.
__
__/O \
__/P \__/
__/P \__/
__/P \__/
__/P \__/
/O \__/
\__/
"""
pocketCells = newCellArray()
# move along the perimeter of the cells, finding pocket cells
curPerim = None
while curPerim != start:
if curPerim == None: curPerim = start
# move outward in all 6 directions
for direction in rotation():
curCell = curPerim
isPath=0
isObs=0
isPerim=0
if curCell.nearby(opposite(direction)).path:
isPath = 1
if curCell.nearby(opposite(direction)).obstacle:
isObs = 1
if curCell.nearby(opposite(direction)).perim:
isPerim = 1
# do not move in directions that do not have
# an obstacle on the opposite side
if isPath ==0 and isObs ==0 and isPerim ==0:
continue
# continue looking out in this direction until we hit the
# bounds of this cell array, or we know the cells we've
# crossed are pocket cells.
possiblePocketCells = []
while curCell.x >= obstacleCells.minX and \
curCell.x <= obstacleCells.maxX and \
curCell.y >= obstacleCells.minY and \
curCell.y <= obstacleCells.maxY:
# if we find another cell in the array at the other end
# we mark all the cells in between as pockets
if obstacleCells.at(curCell.x,curCell.y) != None:
for cell in possiblePocketCells:
if cell.goal:
pocketCells.add(cell)
break
# add this cell as a possible pocket cell and
# move to the next cell in this direction
possiblePocketCells.append(curCell)
curCell = curCell.nearby(direction)
# move to the next cell in the perimeter
curPerim = obstacleCells.findNextPerimeter(CW,curPerim)
if curPerim == None:
print "ERROR: curPerim was set to None"
break
# define the pocket flag
defineCellAttribute("pocket",(0.5,0.7,0.0))
for cell in pocketCells: cell.pocket = 1
# return all the pocket cells
return pocketCells
################################################################################
def seperatePockets(pocketCells):
"""
Given all the pocket cells, divide them into several contiguous
pocket groups.
PARAMS:
pocketCells - a cell array of all pocket cells. The flag pocket is
assumed to be set to 1 for all such cells, and all cells with the
flag pocket set to 1 are assumed to be in the cell array.
RESULTS:
Returns a list of several HSCellArray objects, each a contiguous
collection of pocket cells. The 'pocket' flag for
all pocket cells are set to 1 + index where index is the index
of the HSCellArray in the returned list that contains that pocket cell.
"""
# do a breadth first search to find all cells
# in all contiguous blocks
pockets = []
while len(pocketCells) > 0:
firstCell = pocketCells.first
pocketCells.remove(firstCell)
pocketQueue = [firstCell]
newPocket = newCellArray()
pockets.append(newPocket)
while len(pocketQueue) > 0:
curCell = pocketQueue.pop(0)
for neighbor in curCell.neighbors():
if neighbor.pocket and pocketCells.at(neighbor.x,neighbor.y):
pocketQueue.append(neighbor)
pocketCells.remove(neighbor)
newPocket.add(curCell)
curCell.pocket = len(pockets)
return pockets
################################################################################
def separateObstacles(obstacleCells):
"""
Given all the obstacle cells, divide them into several contiguous
obstacle groups.
PARAMS:
obstacleCells - a cell array of all obstacle cells. The flag obstacle is
assumed to be set to 1 for all such cells, and all cells with the
flag pocket set to 1 are assumed to be in the cell array.
RESULTS:
Returns a list of several HSCellArray objects, each a contiguous
collection of obstacle cells. The 'obstacle' flag for
all obstacle cells is set to 1 + index where index is the index
of the HSCellArray in the returned list that contains that obstacle cell.
"""
# do a breadth first search to find all cells
# in all contiguous blocks
obstacles= []
while len(obstacleCells) > 0:
firstCell = obstacleCells.first
obstacleCells.remove(firstCell)
obQueue = [firstCell]
newObst = newCellArray()
obstacles.append(newObst)
while len(obQueue) > 0:
curCell = obQueue.pop(0)
for neighbor in curCell.neighbors():
if neighbor.obstacle and obstacleCells.at(neighbor.x,neighbor.y):
obQueue.append(neighbor)
obstacleCells.remove(neighbor)
newObst.add(curCell)
curCell.obstacle = len(obstacles)
return obstacles
################################################################################
def orderPocket(firstPocketCell,pocketDir,pocketCells,perimeterCells,obstacleCells):
"""
Orders the cells of a pocket
PARAMS:
firstPocketCell - a valid perimeter pocket cell of this pocket
where modules will enter from.
pocketDir - the direction module will rotate into this cell
pocketCells - the pocket cells of this pocket
perimeterCells - the goal cells that make this pocket.
obstacleCells - the freestanding obstacles inside this pocket.
RESULT:
The ordered pocket as a list of HSCell objects.
"""
obstacleList = []
obstaclePerimeterList = []
# Call method to separate obstacle cells, returning a list of obstacles
obstacleList = countObstacles(obstacleCells)
obstaclePerimeterList = findObstaclePerimeters(obstacleList)
for cell in perimeterCells:
for nCell in cell.neighbors():
if nCell.obnb:
print "Perimeter cell adjacent to obstacle boundary"
return None
# print "inside pocket.orderPocket, pocket cells are "+str(pocketCells)
# create the pocket perimeter
# Code below implements the Algorithm FillPocket from the WBA paper on
# filling a pocket. This code is modified to include pockets that
# contain obstacles.
pocketPerim = []
curPerim = firstPocketCell
#print "first pocket cell = "+str(firstPocketCell)
perimeterIndex = 1
for cell in pocketCells: cell.perimeter = 0
# initially, find perimeter lining the perimeterCells HSCellArray
while curPerim.pocket:
pocketPerim.append(curPerim)
curPerim = perimeterCells.findNextPerimeter(pocketDir,curPerim)
curPerim.perimeter = perimeterIndex
perimeterIndex += 1
#print "current perimeter is "+str(curPerim)
#print "perimeter length is "+str(perimeterIndex)
##print "perimeter is "+str(pocketPerim)
# calculate the free sides for all pocket cells
for cell in pocketCells:
cell.freeSides = 6
for neighbor in cell.neighbors():
if perimeterCells.at(neighbor.x,neighbor.y): cell.freeSides -= 1
if obstacleCells.at(neighbor.x,neighbor.y):cell.freeSides -= 1
#for cell in pocketCells:
# print "Cell "+str(cell)+" has "+str(cell.freeSides)+" free sides"
# while there is still a perimeter
pocketOrder = []
while len(pocketPerim) > 0:
# find the cell to fill and add it to the pocket order
cellIndex = findNextPocketIndex(pocketPerim)
cellToAdd = pocketPerim[cellIndex]
#print "Inside pocket.orderPocket, just adding cell "+str(cellToAdd)
#print "added cell has perimeter # "+str(cellIndex)
#print "cell thinks its perimeter number is "+str(cellToAdd.perimeter)
cellToAdd.perimeter = 0
pocketOrder.append(cellToAdd)
perimeterCells.add(cellToAdd)
# remove the cell from the perimeter
del pocketPerim[cellIndex]
### Insert code to check how close last perimeter cell is to any
### freestanding obstacles inside pocket. If last filled cell is within
### one cell of a freestanding obstacle:
### find a linking cell to obstacle that does not block pocket,
### add linking cell to the pocket order, and
### add linking cell and all cells in linked obstacle to obstacle
# renumber the perimeter
if len(pocketPerim) > 0:
curPerim = pocketPerim[0]
pocketPerim = []
perimeterIndex = 1
if not curPerim == None:
while not curPerim == None and curPerim.pocket:
pocketPerim.append(curPerim)
curPerim.perimeter = perimeterIndex
curPerim = perimeterCells.findNextPerimeter(pocketDir,curPerim)
perimeterIndex = perimeterIndex + 1
#print "Cells on pocket perimeter are "+str(pocketPerim)
# reset the number of freesides for all neighbors of cellToAdd that
# are not on the pocketPerim to 6
#for cell in cellToAdd.neighbors():
# if not cell.freeSides:
# print "cell "+str(cell)+" has "+str(cell.freeSides)+" free sides"
# cell.freeSides = 6
# then remove a free side for each of the neighbors of the cell we added
for cell in cellToAdd.neighbors():
if cell.pocket:
cell.freeSides -= 1
# if the newly numbered perimeter contains a obnb cell, find that cell and
# add it to the surface (perimeterCells) and renumber the perimeter around
# obstacle
if len(pocketPerim) > 0:
curPerim = pocketPerim[0]
cellIndex = 0
if not curPerim == None:
while not curPerim == None and curPerim.pocket:
curPerim = perimeterCells.findNextPerimeter(pocketDir,curPerim)
cellIndex += 1
if curPerim.obnb:
obnbIndex = cellIndex
break
if curPerim.obnb:
newObNbCell = curPerim
curPerim.perimeter = 0
pocketOrder.append(curPerim)
perimeterCells.add(curPerim)
#print "Just added cell "+str(curPerim)
#print "Just adding obstacle "+str(curPerim.perim-1)
for cell in obstacleList[curPerim.perim-1]:
perimeterCells.add(cell)
for cell in obstaclePerimeterList[curPerim.perim-1]:
cell.obnb = 0
del pocketPerim[obnbIndex]
# renumber perimeter around bridge and obstacle
if len(pocketPerim) > 0:
curPerim = pocketPerim[0]
pocketPerim = []
perimeterIndex = 1
if not curPerim == None:
while not curPerim == None and curPerim.pocket:
pocketPerim.append(curPerim)
curPerim.perimeter = perimeterIndex
curPerim = perimeterCells.findNextPerimeter(pocketDir,curPerim)
perimeterIndex = perimeterIndex + 1
#print "Cells on pocket perimeter after obnb are "+str(pocketPerim)
# reset the number of freesides for all neighbors of cellToAdd that
# are not in perimeter to 6
#for cell in newObNbCell.neighbors():
# if not cell.freeSides and cell.pocket:
# cell.freeSides = 6
# then remove a free side for each of the neighbors of the cell we added
for cell in newObNbCell.neighbors():
if cell.pocket:
cell.freeSides -= 1
### END OF WHILE len(pocket) > 0
# remove pocket cells from the perimeterCells
for cell in pocketOrder: perimeterCells.remove(cell)
for cell in pocketCells:
if not cell in pocketOrder:
pocketOrder.append(cell)
##print "The pocket order is: " + str(pocketOrder) + "."
# return the pocket order
return pocketOrder
################################################################################
def findObstaclePerimeters(obstacleList):
obstPerimeter = []
defineCellAttribute("obnb",(0.5,0.0,0.05),4)
i = 1
# Steve Matysik: Find obstacle perimeter
for obst in obstacleList:
#Find the perimeter of obstacles cells
obstacleNB = []
for obstacleCell in obst:
for cell in obstacleCell.neighbors():
if not cell.obstacle:
obstacleNB.append(cell)
cell.obnb = 1
cell.perim = i
i = i + 1
#print "ObstacleNB: " + str(obstacleNB)
obstPerimeter.append(obstacleNB)
return obstPerimeter
################################################################################
def findNextPocketIndex(pocketPerim):
minFreeSides = 6
minFreeSideIndex = 0
startIndex, endIndex = findNPPRange(pocketPerim)
#print "Range of indicies is: " + str(startIndex)+", "+str(pocketPerim[startIndex]) + " - " + \
# str(endIndex)+", "+str(pocketPerim[endIndex]) + "."
for index in range(startIndex,endIndex+1):
if pocketPerim[index].freeSides < minFreeSides:
minFreeSides = pocketPerim[index].freeSides
minFreeSideIndex = index
if minFreeSides == 2:
break
return minFreeSideIndex
################################################################################
# This method is used for pocket filling, when finding the deepest NPP that
# is closest to the entry point. Last modified for 9/15/08 icra09 deadline.
def findNPPRange(pocketPerim):
startIndex = index = 0
endIndex = len(pocketPerim)-1
nppEnd = 0
while index < endIndex:
# for each pocket cell on the perimeter,
# check for neighbors with nonconsecutive perimeter values
lowest = endIndex
for cell in pocketPerim[index].neighbors():
if cell.pocket and cell.perimeter and \
abs(pocketPerim[index].perimeter - cell.perimeter) > 1 and \
pocketPerim[index].perimeter < cell.perimeter:
# The next line was crucial for correctness in icra09 paper
if cell.perimeter < lowest:
nppEnd = cell.perimeter
lowest = nppEnd
if nppEnd > 0 and nppEnd <= endIndex:
startIndex = index
endIndex = nppEnd
index += 1
nppEnd = 0
if startIndex == 0 and endIndex == len(pocketPerim)-1:
return startIndex, endIndex
else:
return startIndex+1, endIndex-1
###############################################################################
def findNPPDelay(firstPocketCell,pocketCells,obstacleCells,pocketDir):
curPerim = firstPocketCell
pocketPerim = []
while curPerim.pocket:
pocketPerim.append(curPerim)
curPerim = obstacleCells.findNextPerimeter(pocketDir,curPerim)
#print "current perimeter is "+str(pocketPerim)
startIndex, endIndex = findNPPRange(pocketPerim)
return endIndex - startIndex
###############################################################################
# As appears in the Multiple Pocket paper, count pockets
# separates pocket cells into discrete pockets.
def countPockets(pocketCells):
pc = []
for i in pocketCells:
pc.append(i)
i = 0
q = []
pockets = []
while len(pc) > 0:
x = pc[0]
q.append(x)
pc.remove(x)
pockets.append([])
while len(q) > 0:
#print q
u = q.pop(0)
contacts = []
for cell in u.neighbors():
#print cell
if cell in pc:
contacts.append(cell)
for c in contacts:
q.append(c)
pc.remove(c)
pockets[i].append(u)
i += 1
return pockets
############################################################################
# Takes in a HSCellArray and copies it into a list. Then does a BFS to
# find each connected cluster of obstacle cells and returns the clusters
# as lists within a list
def countObstacles(obstacleCells):
ob = []
for i in obstacleCells:
ob.append(i)
i = 0
q = []
obstacles = []
while len(ob) > 0:
x = ob[0]
q.append(x)
ob.remove(x)
obstacles.append([])
while len(q) > 0:
u = q.pop(0)
contacts = []
for cell in u.neighbors():
if cell in ob:
contacts.append(cell)
for c in contacts:
q.append(c)
ob.remove(c)
obstacles[i].append(u)
i += 1
return obstacles
#############################################################################
def orderObstacles(obstacles):
northeastarray = [];
i = 0
for obs in obstacles:
northeastarray.append((findNortheast(obs), i))
i += 1
sortedNE = sortNEarray(northeastarray)
orderedObstacles = []
for ob in sortedNE:
orderedObstacles.append(obstacles[ob[1]])
return orderedObstacles
def sortNEarray(array):
if len(array) < 2:
return array
middle = len(array)/2
end = len(array)
first = []
second = []
for i in range(0, middle):
first.append(array[i])
for i in range(middle, end):
second.append(array[i])
return merge(sortNEarray(first), sortNEarray(second))
def merge(a1, a2):
counter1 = 0
counter2 = 0
newarray = []
while counter1 < len(a1) and counter2 < len(a2):
if a1[counter1][0][0] < a2[counter2][0][0]:
newarray.append(a1[counter1])
counter1 = counter1 + 1
elif a1[counter1][0][0] == a2[counter2][0][0] and a1[counter1][0][1] > a2[counter2][0][1]:
newarray.append(a1[counter1])
counter1 = counter1 + 1
else:
newarray.append(a2[counter2])
counter2 = counter2 + 1
while counter1 < len(a1):
newarray.append(a1[counter1])
counter1 = counter1 + 1
while counter2 < len(a2):
newarray.append(a2[counter2])
counter2 = counter2 + 1
return newarray
def findNortheast(obstacle):
if len(obstacle) < 1:
print "no obstacle given to findNortheast"
northmost = obstacle[0].y
eastmost = obstacle[0].x
for obs in obstacle:
if obs.x < eastmost:
northmost = obs.y
eastmost = obs.x
elif obs.x == eastmost and obs.y > northmost:
northmost = obs.y
return (eastmost,northmost)
###############################################################################
def pocketMember(cell, pockets):
for pocket in pockets:
for pCell in pocket:
if cell.x == pCell.x and cell.y == pCell.y:
return pocket
# Last change 8/27/08
#
# Problem was still in finding the lowest NPP that modules
# must traverse first. I changed the code to check that
# the cell.perimeter < lowest before resetting nppEnd.
# If this was not done, the method would return the wrong
# NPP and modules would collide in more "inner" NPPs