class AStarPlanning:
def readStartEndPosition(self):
with open('C:\Users\Tamkin\Documents\GitHub\Searistica\Tamkin_Work\Project\MissionPlanning\Business\PythonCodes\PathPlanning\StartEndPoint.txt') as infile:
i = 0
for line in infile:
if(i == 0):
robotPosXY = line
xy = robotPosXY.split(',')
rx = int(xy[0])
ry = int(xy[1])
self.robotPos = Position(rx,ry)
i = i + 1
continue
if(i == 1):
goalPosXY = line
xy = goalPosXY.split(',')
gx = int(xy[0])
gy = int(xy[1])
self.goalPos = Position(gx,gy)
break
def __init__(self):
self.readStartEndPosition()
self.oceanCurrentDataGrid = OceanCurrentDataGrid()
self.oceanCurrentDataGrid.loadDataFromFile()
def robotPosChangedCallback(self, robotPos):
self.robotPos = robotPos
self.cmd_robotPos_sub.unregister()
def goalPosChangedCallback(self, goalPos):
self.goalPos = goalPos
def computeEquledianDistance(self, currentCell):
xDistance = self.goalPos.x - currentCell.x
yDistance = self.goalPos.y - currentCell.y
return math.sqrt( math.pow(xDistance, 2) + math.pow(yDistance, 2) )
def computeH(self, childGridCell):
childGridCell.hVal = self.computeEquledianDistance(childGridCell)
def oneCellMoveCost(self):
return 1;
def isAnObstacle(self, gridCell):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(gridCell.x,gridCell.y)
if(uvComponent == None):
return True
return False
def getCellCost(self, gridCell):
if(self.isAnObstacle(gridCell)):
return 10000
return 1
def computeG(self, gridCell):
if(gridCell.parentGridCell is None):
gridCell.gVal = 0
else:
gridCell.gVal = gridCell.parentGridCell.gVal + self.getCellCost(gridCell)
def computeTotalCost(self, childGridCell):
self.computeG(childGridCell)
self.computeH(childGridCell)
return childGridCell.getCost()
def isGoalGridCell(self, currentCell):
return currentCell.x == self.goalPos.x and currentCell.y == self.goalPos.y
def isInRange(self, x, y):
return (x >= 0 and y >= 0) and ( x < self.oceanCurrentDataGrid.dimension.width and y < self.oceanCurrentDataGrid.dimension.height)
def getAdjacentCells(self, currentCell):
adjacentCells = []
x = currentCell.x
y = currentCell.y
if(self.isInRange(x + 1, y)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(x + 1, y)
gridCell = GridCell(x + 1, y, uvComponent ,currentCell)
if(gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if(self.isInRange(x + 1, y + 1)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(x + 1, y + 1)
gridCell = GridCell(x + 1, y + 1, uvComponent, currentCell)
if(gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if(self.isInRange(x, y + 1)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(x, y + 1)
gridCell = GridCell(x, y + 1, uvComponent, currentCell)
if(gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if(self.isInRange(x - 1, y + 1)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(x - 1, y + 1)
gridCell = GridCell(x - 1, y + 1,uvComponent, currentCell)
if(gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if(self.isInRange(x - 1, y)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(x - 1, y)
gridCell = GridCell(x - 1, y, uvComponent ,currentCell)
if(gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if(self.isInRange(x - 1, y - 1)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(x - 1, y - 1)
gridCell = GridCell(x - 1, y - 1, uvComponent, currentCell)
if(gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if(self.isInRange(x, y - 1)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(x, y - 1)
gridCell = GridCell(x, y - 1, uvComponent, currentCell)
if(gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if(self.isInRange(x + 1, y - 1)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(x + 1, y - 1)
gridCell = GridCell(x + 1, y - 1, uvComponent, currentCell)
if(gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
return adjacentCells
def getPath(self, gridCell):
self.path.append(Position(gridCell.x,gridCell.y))
if(gridCell.x == self.robotPos.x and gridCell.y == self.robotPos.y):
return
self.getPath(gridCell.parentGridCell)
def computePlan(self):
if(self.robotPos != None and self.goalPos != None):
print 'hi'
start = time.time()
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(self.robotPos.x, self.robotPos.y)
startGridCell = GridCell(self.robotPos.x, self.robotPos.y, uvComponent, None)
priorityQueue = PriorityQueue()
closeList = {}
openList = {}
openList[startGridCell.getKey()] = startGridCell
self.computeTotalCost(startGridCell)
priorityQueue.push(startGridCell.getCost(),startGridCell)
goal = None
while(len(openList) > 0):
(cost, currentCell) = priorityQueue.pop()
#print "low cell:(", currentCell.x , currentCell.y, ") cost:", cost
if(currentCell.getKey() in openList):
del openList[currentCell.getKey()]
if(self.isGoalGridCell(currentCell)):
goal = currentCell
break
else:
adjacentCells = self.getAdjacentCells(currentCell)
#self.printAdjacentCells(adjacentCells)
#print "\n"
for index in range(len(adjacentCells)):
adjacentCell = adjacentCells[index]
allReadyVisited = adjacentCell.getKey() in closeList
if(allReadyVisited): continue
self.computeTotalCost(adjacentCell)
#print "Adj:(" , adjacentCell.x , adjacentCell.y , ") " , "cost:", adjacentCell.getCost()
exploredBefore = adjacentCell.getKey() in openList
if(exploredBefore == False):
openList[adjacentCell.getKey()] = adjacentCell
priorityQueue.push(adjacentCell.getCost(),adjacentCell)
#print "Adj:(" , adjacentCell.x , adjacentCell.y , ") ", "not in open list, added to open list"
else:
oldAdjacentCell = openList[adjacentCell.getKey()]
#print "found old Adj:(" , oldAdjacentCell.x , oldAdjacentCell.y , ") " , "cost:", oldAdjacentCell.getCost(), " parent:" , oldAdjacentCell.parentGridCell.x , oldAdjacentCell.parentGridCell.y
if(adjacentCell.getCost() < oldAdjacentCell.getCost()):
oldAdjacentCell.setParent(currentCell)
self.computeTotalCost(oldAdjacentCell)
#print "old Adj upadte:(" , oldAdjacentCell.x , oldAdjacentCell.y , ") " , "new cost:", oldAdjacentCell.getCost(), " new parent:" , oldAdjacentCell.parentGridCell.x , oldAdjacentCell.parentGridCell.y
#else:
#print "Old Adj not updated"
#print "\n"
closeList[currentCell.getKey()] = currentCell
#print "low cell:(", currentCell.x , currentCell.y, ") cost:", cost, " REMOVED"
#print "-----------"
#print "Goal:", goal.x, goal.y
print 'kk'
end = time.time()
elapsed = end - start
s = "Time taken: " + str(elapsed) + " seconds.\n"
s += "Total node expanded: " + str(len(closeList)) + " out of " + "(30 * 30) = 900. \n"
self.path = []
if(goal is not None):
self.getPath(goal)
self.writePathToFile()
else:
s = "Goal not found !!!"
print s
def writePathToFile(self):
string =""
for node in self.path:
s = "%d,%d\n" % (node.x, node.y)
string = string + s
fileObject = open('C:\Users\Tamkin\Documents\GitHub\Searistica\Tamkin_Work\Project\MissionPlanning\Business\PythonCodes\PathPlanning\path.txt', "wb")
fileObject.write(string);
fileObject.close()
def makePalnning(self):
print 'Running A start planner'
self.computePlan()
def __init__(self):
self.readStartEndPosition()
self.oceanCurrentDataGrid = OceanCurrentDataGrid()
self.oceanCurrentDataGrid.loadDataFromFile()
class AStarPlanning:
def readStartEndPosition(self):
with open('C:\Users\Tamkin\Documents\GitHub\Searistica\Tamkin_Work\Project\MissionPlanning\Business\PythonCodes\PathPlanning\StartEndPoint.txt') as infile:
i = 0
for line in infile:
if(i == 0):
robotPosXY = line
xy = robotPosXY.split(',')
rx = int(xy[0])
ry = int(xy[1])
self.robotPos = Position(rx,ry)
if(i == 1):
goalPosXY = line
xy = goalPosXY.split(',')
gx = int(xy[0])
gy = int(xy[1])
self.goalPos = Position(gx,gy)
i = i + 1
print "Robot pos:" , self.robotPos.x , ',', self.robotPos.y
print "Goal pos:" , self.goalPos.x , ',', self.goalPos.y
def __init__(self):
self.readStartEndPosition()
self.oceanCurrentDataGrid = OceanCurrentDataGrid()
self.oceanCurrentDataGrid.loadDataFromFile()
def robotPosChangedCallback(self, robotPos):
self.robotPos = robotPos
self.cmd_robotPos_sub.unregister()
def goalPosChangedCallback(self, goalPos):
self.goalPos = goalPos
def computeEquledianDistance(self, currentCell):
xDistance = self.goalPos.x - currentCell.x
yDistance = self.goalPos.y - currentCell.y
return math.sqrt( math.pow(xDistance, 2) + math.pow(yDistance, 2) )
def computeHOld(self, childGridCell):
#childGridCell.hVal = self.computeEquledianDistance(childGridCell)
euclideanDistanceToGoal = self.computeEquledianDistance(childGridCell)
resultantOcceanCurrent = math.sqrt( math.pow(childGridCell.u, 2) + math.pow(childGridCell.v, 2) )
#childGridCell.hVal = euclideanDistanceToGoal / resultantOcceanCurrent
factor = 10000
resultantOcceanCurrent = None
if(childGridCell.parentGridCell == None):
resultantOcceanCurrent = math.sqrt( math.pow(childGridCell.u, 2) + math.pow(childGridCell.v, 2) )
childGridCell.addedU = childGridCell.u
childGridCell.addedV = childGridCell.v
childGridCell.hVal = (childGridCell.gVal / resultantOcceanCurrent) * factor
else:
neWaddedU = childGridCell.u + childGridCell.parentGridCell.addedU;
neWaddedV = childGridCell.v + childGridCell.parentGridCell.addedV;
childGridCell.addedU = neWaddedU
childGridCell.addedV = neWaddedV
resultantOcceanCurrent = math.sqrt( math.pow(childGridCell.addedU, 2) + math.pow(childGridCell.addedV, 2) )
childGridCell.hVal = (childGridCell.gVal / resultantOcceanCurrent) * factor
def computeAngle(self, childGridCell):
#consider cell center as 0,0
if(self.goalPos.x == childGridCell.x and self.goalPos.y == childGridCell.y):
return None
#print 'golx' , self.goalPos.x
#print 'goly' , self.goalPos.y
gx = self.goalPos.x - childGridCell.x
gy = (self.goalPos.y - childGridCell.y) * -1
#print childGridCell.x
#print childGridCell.y
#print childGridCell.ou
#print childGridCell.ov
resultantVectorX = childGridCell.addedU
resultantVectorY = childGridCell.addedV * -1
#print 'g', gx ,',', gy
#print 'rv', resultantVectorX ,',', resultantVectorY
#print 'resultantVectorX 2', math.pow(resultantVectorX, 2)
#print 'resultantVectorY 2', math.pow(resultantVectorY, 2)
aProductb = (gx * resultantVectorX) + (gy * resultantVectorY)
magnOfa = math.sqrt( math.pow(gx, 2) + math.pow(gy, 2) )
magnOfb = math.sqrt( math.pow(resultantVectorX, 2) + math.pow(resultantVectorY, 2) )
cosTheta = aProductb / (magnOfa * magnOfb)
#print "abp", aProductb
#print "magnOfa", magnOfa
#print "magnOfb", magnOfb
theta = math.degrees(math.acos(cosTheta))
if(theta < 0):
theta = theta * -1;
#print 'theta' , theta
return theta
def computeH(self, childGridCell):
#childGridCell.hVal = self.computeEquledianDistance(childGridCell)
euclideanDistanceToGoal = self.computeEquledianDistance(childGridCell)
resultantOcceanCurrent = math.sqrt( math.pow(childGridCell.u, 2) + math.pow(childGridCell.v, 2) )
#childGridCell.hVal = euclideanDistanceToGoal / resultantOcceanCurrent
factor = 10000
resultantOcceanCurrent = None
if(childGridCell.parentGridCell == None):
resultantOcceanCurrent = math.sqrt( math.pow(childGridCell.u, 2) + math.pow(childGridCell.v, 2) )
childGridCell.addedU = childGridCell.u
childGridCell.addedV = childGridCell.v
childGridCell.hVal = (childGridCell.gVal / resultantOcceanCurrent) * factor
theta = self.computeAngle(childGridCell)
if(theta != None):
childGridCell.hVal = childGridCell.hVal + theta
else:
neWaddedU = (childGridCell.u + childGridCell.parentGridCell.u) / 2;
neWaddedV = (childGridCell.v + childGridCell.parentGridCell.v) / 2;
childGridCell.addedU = neWaddedU
childGridCell.addedV = neWaddedV
resultantOcceanCurrent = math.sqrt( math.pow(childGridCell.addedU, 2) + math.pow(childGridCell.addedV, 2) )
childGridCell.hVal = (childGridCell.gVal / resultantOcceanCurrent) * factor
theta = self.computeAngle(childGridCell)
if(theta != None):
childGridCell.hVal = childGridCell.hVal + theta
def oneCellMoveCost(self):
return 1;
def isAnObstacle(self, gridCell):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(gridCell.x,gridCell.y)
if(uvComponent == None):
return True
return False
def getCellCost(self, gridCell):
if(self.isAnObstacle(gridCell)):
return 10000
return 1
def computeG(self, gridCell):
if(gridCell.parentGridCell is None):
gridCell.gVal = 0
else:
gridCell.gVal = gridCell.parentGridCell.gVal + self.getCellCost(gridCell)
def computeTotalCost(self, childGridCell):
self.computeG(childGridCell)
self.computeH(childGridCell)
return childGridCell.getCost()
def isGoalGridCell(self, currentCell):
return currentCell.x == self.goalPos.x and currentCell.y == self.goalPos.y
def isInRange(self, x, y):
return (x >= 0 and y >= 0) and ( x < self.oceanCurrentDataGrid.dimension.width and y < self.oceanCurrentDataGrid.dimension.height)
def getAdjacentCells(self, currentCell):
adjacentCells = []
x = currentCell.x
y = currentCell.y
if(self.isInRange(x + 1, y)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(x + 1, y)
gridCell = GridCell(x + 1, y, uvComponent ,currentCell)
if(gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if(self.isInRange(x + 1, y + 1)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(x + 1, y + 1)
gridCell = GridCell(x + 1, y + 1, uvComponent, currentCell)
if(gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if(self.isInRange(x, y + 1)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(x, y + 1)
gridCell = GridCell(x, y + 1, uvComponent, currentCell)
if(gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if(self.isInRange(x - 1, y + 1)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(x - 1, y + 1)
gridCell = GridCell(x - 1, y + 1,uvComponent, currentCell)
if(gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if(self.isInRange(x - 1, y)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(x - 1, y)
gridCell = GridCell(x - 1, y, uvComponent ,currentCell)
if(gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if(self.isInRange(x - 1, y - 1)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(x - 1, y - 1)
gridCell = GridCell(x - 1, y - 1, uvComponent, currentCell)
if(gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if(self.isInRange(x, y - 1)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(x, y - 1)
gridCell = GridCell(x, y - 1, uvComponent, currentCell)
if(gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if(self.isInRange(x + 1, y - 1)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(x + 1, y - 1)
gridCell = GridCell(x + 1, y - 1, uvComponent, currentCell)
if(gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
return adjacentCells
def getPath(self, gridCell):
self.path.append(gridCell)
if(gridCell.x == self.robotPos.x and gridCell.y == self.robotPos.y):
return
self.getPath(gridCell.parentGridCell)
def printAjacentCell(self,adjacentCell):
print "Adj:(" , adjacentCell.x , adjacentCell.y , adjacentCell.u , adjacentCell.v, ") " , "cost:", adjacentCell.gVal, "+", adjacentCell.hVal
def computePlan(self):
if(self.robotPos != None and self.goalPos != None):
start = time.time()
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(self.robotPos.x, self.robotPos.y)
startGridCell = GridCell(self.robotPos.x, self.robotPos.y, uvComponent, None)
priorityQueue = PriorityQueue()
closeList = {}
openList = {}
openList[startGridCell.getKey()] = startGridCell
self.computeTotalCost(startGridCell)
priorityQueue.push(startGridCell.getCost(),startGridCell)
goal = None
i =0;
while(len(openList) > 0):
i = i + 1
(cost, currentCell) = priorityQueue.pop()
#print "low cell:(", currentCell.x , currentCell.y, currentCell.u , currentCell.v, ") cost:", cost
if(currentCell.getKey() in openList):
del openList[currentCell.getKey()]
if(self.isGoalGridCell(currentCell)):
goal = currentCell
break
else:
adjacentCells = self.getAdjacentCells(currentCell)
#self.printAdjacentCells(adjacentCells)
#print "\n"
for index in range(len(adjacentCells)):
adjacentCell = adjacentCells[index]
allReadyVisited = adjacentCell.getKey() in closeList
if(allReadyVisited): continue
self.computeTotalCost(adjacentCell)
#self.printAjacentCell(adjacentCell)
exploredBefore = adjacentCell.getKey() in openList
if(exploredBefore == False):
openList[adjacentCell.getKey()] = adjacentCell
priorityQueue.push(adjacentCell.getCost(),adjacentCell)
#print "Adj:(" , adjacentCell.x , adjacentCell.y , ") ", "not in open list, added to open list"
else:
oldAdjacentCell = openList[adjacentCell.getKey()]
#print "found old Adj:(" , oldAdjacentCell.x , oldAdjacentCell.y , ") " , "cost:", oldAdjacentCell.getCost(), " parent:" , oldAdjacentCell.parentGridCell.x , oldAdjacentCell.parentGridCell.y
if(adjacentCell.getCost() < oldAdjacentCell.getCost()):
oldAdjacentCell.setParent(currentCell)
self.computeTotalCost(oldAdjacentCell)
#print "old Adj upadte:(" , oldAdjacentCell.x , oldAdjacentCell.y , ") " , "new cost:", oldAdjacentCell.getCost(), " new parent:" , oldAdjacentCell.parentGridCell.x , oldAdjacentCell.parentGridCell.y
#else:
#print "Old Adj not updated"
#print "\n"
closeList[currentCell.getKey()] = currentCell
#print "low cell:(", currentCell.x , currentCell.y, ") cost:", cost, " REMOVED"
#print "-----------"
#if(i == 1):
#break
#print "Goal:", goal.x, goal.y
#print 'ieration: ', i
end = time.time()
elapsed = end - start
s = "Time taken: " + str(elapsed) + " seconds.\n"
#s += "Total node expanded: " + str(len(closeList)) + " out of " + "(30 * 30) = 900. \n"
self.path = []
if(goal is not None):
self.getPath(goal)
self.writePathToFile()
else:
s = "Goal not found !!!"
print s
def writePathToFile(self):
string =""
for node in self.path:
s = "%d,%d\n" % (node.x, node.y)
c = "%d,%d,%d\n" % (node.x, node.y, node.hVal)
print c
string = string + s
fileObject = open('C:\Users\Tamkin\Documents\GitHub\Searistica\Tamkin_Work\Project\MissionPlanning\Business\PythonCodes\PathPlanning\path.txt', "wb")
fileObject.write(string);
fileObject.close()
def makePalnning(self):
print 'Running shortest path planning ....'
self.computePlan()
def __init__(self):
self.readStartEndPosition()
self.oceanCurrentDataGrid = OceanCurrentDataGrid()
self.oceanCurrentDataGrid.loadDataFromFile()
class PathPlanning:
def readStartEndPosition(self):
with open('C:\Users\Tamkin\Documents\GitHub\Searistica\Tamkin_Work\Project\MissionPlanning\Business\PythonCodes\PathPlanning\StartEndPoint.txt') as infile:
i = 0
for line in infile:
if(i == 0):
robotPosXY = line
xy = robotPosXY.split(',')
rx = int(xy[0])
ry = int(xy[1])
self.robotPos = Position(rx,ry)
if(i == 1):
goalPosXY = line
xy = goalPosXY.split(',')
gx = int(xy[0])
gy = int(xy[1])
self.goalPos = Position(gx,gy)
i = i + 1
print "Robot pos:" , self.robotPos.x , ',', self.robotPos.y
print "Goal pos:" , self.goalPos.x , ',', self.goalPos.y
def __init__(self):
#self.readStartEndPosition()
self.oceanCurrentDataGrid = OceanCurrentDataGrid()
self.oceanCurrentDataGrid.loadDataFromFile()
def setGoalPostion(self, gx, gy):
self.goalPos = Position(gx,gy)
def setRobotPostion(self, rx, ry):
self.robotPos = Position(rx,ry)
def robotPosChangedCallback(self, robotPos):
self.robotPos = robotPos
self.cmd_robotPos_sub.unregister()
def goalPosChangedCallback(self, goalPos):
self.goalPos = goalPos
def computeEquledianDistance(self, currentCell):
xDistance = self.goalPos.x - currentCell.x
yDistance = self.goalPos.y - currentCell.y
return math.sqrt( math.pow(xDistance, 2) + math.pow(yDistance, 2) )
def computeHOld(self, childGridCell):
#childGridCell.hVal = self.computeEquledianDistance(childGridCell)
euclideanDistanceToGoal = self.computeEquledianDistance(childGridCell)
resultantOcceanCurrent = math.sqrt( math.pow(childGridCell.u, 2) + math.pow(childGridCell.v, 2) )
#childGridCell.hVal = euclideanDistanceToGoal / resultantOcceanCurrent
factor = 10000
resultantOcceanCurrent = None
if(childGridCell.parentGridCell == None):
resultantOcceanCurrent = math.sqrt( math.pow(childGridCell.u, 2) + math.pow(childGridCell.v, 2) )
childGridCell.addedU = childGridCell.u
childGridCell.addedV = childGridCell.v
childGridCell.hVal = (childGridCell.gVal / resultantOcceanCurrent) * factor
else:
neWaddedU = childGridCell.u + childGridCell.parentGridCell.addedU;
neWaddedV = childGridCell.v + childGridCell.parentGridCell.addedV;
childGridCell.addedU = neWaddedU
childGridCell.addedV = neWaddedV
resultantOcceanCurrent = math.sqrt( math.pow(childGridCell.addedU, 2) + math.pow(childGridCell.addedV, 2) )
childGridCell.hVal = (childGridCell.gVal / resultantOcceanCurrent) * factor
def computeAngle(self, childGridCell):
#consider cell center as 0,0
if(self.goalPos.x == childGridCell.x and self.goalPos.y == childGridCell.y):
return None
#print 'golx' , self.goalPos.x
#print 'goly' , self.goalPos.y
gx = self.goalPos.x - childGridCell.x
gy = (self.goalPos.y - childGridCell.y) * -1
#print childGridCell.x
#print childGridCell.y
#print childGridCell.ou
#print childGridCell.ov
resultantVectorX = childGridCell.addedU
resultantVectorY = childGridCell.addedV * -1
#print 'g', gx ,',', gy
#print 'rv', resultantVectorX ,',', resultantVectorY
#print 'resultantVectorX 2', math.pow(resultantVectorX, 2)
#print 'resultantVectorY 2', math.pow(resultantVectorY, 2)
aProductb = (gx * resultantVectorX) + (gy * resultantVectorY)
magnOfa = math.sqrt( math.pow(gx, 2) + math.pow(gy, 2) )
magnOfb = math.sqrt( math.pow(resultantVectorX, 2) + math.pow(resultantVectorY, 2) )
cosTheta = aProductb / (magnOfa * magnOfb)
#print "abp", aProductb
#print "magnOfa", magnOfa
#print "magnOfb", magnOfb
theta = math.degrees(math.acos(cosTheta))
if(theta < 0):
theta = theta * -1;
#print 'theta' , theta
return theta
def computeH(self, childGridCell):
#childGridCell.hVal = self.computeEquledianDistance(childGridCell)
euclideanDistanceToGoal = self.computeEquledianDistance(childGridCell)
resultantOcceanCurrent = math.sqrt( math.pow(childGridCell.u, 2) + math.pow(childGridCell.v, 2) )
#childGridCell.hVal = euclideanDistanceToGoal / resultantOcceanCurrent
factor = 10000
resultantOcceanCurrent = None
if(childGridCell.parentGridCell == None):
resultantOcceanCurrent = math.sqrt( math.pow(childGridCell.u, 2) + math.pow(childGridCell.v, 2) )
childGridCell.addedU = childGridCell.u
childGridCell.addedV = childGridCell.v
childGridCell.hVal = (childGridCell.gVal / resultantOcceanCurrent) * factor
theta = self.computeAngle(childGridCell)
if(theta != None):
childGridCell.hVal = childGridCell.hVal + theta
else:
neWaddedU = (childGridCell.u + childGridCell.parentGridCell.u) / 2;
neWaddedV = (childGridCell.v + childGridCell.parentGridCell.v) / 2;
childGridCell.addedU = neWaddedU
childGridCell.addedV = neWaddedV
resultantOcceanCurrent = math.sqrt( math.pow(childGridCell.addedU, 2) + math.pow(childGridCell.addedV, 2) )
childGridCell.hVal = (childGridCell.gVal / resultantOcceanCurrent) * factor
theta = self.computeAngle(childGridCell)
if(theta != None):
childGridCell.hVal = childGridCell.hVal + theta
def oneCellMoveCost(self):
return 1;
def isAnObstacle(self, gridCell):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(gridCell.x,gridCell.y)
if(uvComponent == None):
return True
return False
def getCellCost(self, gridCell):
if(self.isAnObstacle(gridCell)):
return 10000
return 1
def computeG(self, gridCell):
if(gridCell.parentGridCell is None):
gridCell.gVal = 0
else:
gridCell.gVal = gridCell.parentGridCell.gVal + self.getCellCost(gridCell)
def computeTotalCost(self, childGridCell):
self.computeG(childGridCell)
self.computeH(childGridCell)
return childGridCell.getCost()
def isGoalGridCell(self, currentCell):
return currentCell.x == self.goalPos.x and currentCell.y == self.goalPos.y
def isInRange(self, x, y):
return (x >= 0 and y >= 0) and ( x < self.oceanCurrentDataGrid.dimension.width and y < self.oceanCurrentDataGrid.dimension.height)
def getAdjacentCells(self, currentCell):
adjacentCells = []
x = currentCell.x
y = currentCell.y
if(self.isInRange(x + 1, y)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(x + 1, y)
gridCell = GridCell(x + 1, y, uvComponent ,currentCell)
if(gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if(self.isInRange(x + 1, y + 1)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(x + 1, y + 1)
gridCell = GridCell(x + 1, y + 1, uvComponent, currentCell)
if(gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if(self.isInRange(x, y + 1)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(x, y + 1)
gridCell = GridCell(x, y + 1, uvComponent, currentCell)
if(gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if(self.isInRange(x - 1, y + 1)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(x - 1, y + 1)
gridCell = GridCell(x - 1, y + 1,uvComponent, currentCell)
if(gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if(self.isInRange(x - 1, y)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(x - 1, y)
gridCell = GridCell(x - 1, y, uvComponent ,currentCell)
if(gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if(self.isInRange(x - 1, y - 1)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(x - 1, y - 1)
gridCell = GridCell(x - 1, y - 1, uvComponent, currentCell)
if(gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if(self.isInRange(x, y - 1)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(x, y - 1)
gridCell = GridCell(x, y - 1, uvComponent, currentCell)
if(gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if(self.isInRange(x + 1, y - 1)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(x + 1, y - 1)
gridCell = GridCell(x + 1, y - 1, uvComponent, currentCell)
if(gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
return adjacentCells
def getPath(self, gridCell):
self.path.append(gridCell)
if(gridCell.x == self.robotPos.x and gridCell.y == self.robotPos.y):
return
self.getPath(gridCell.parentGridCell)
def printAjacentCell(self,adjacentCell):
print "Adj:(" , adjacentCell.x , adjacentCell.y , adjacentCell.u , adjacentCell.v, ") " , "cost:", adjacentCell.gVal, "+", adjacentCell.hVal
def computePlan(self):
if(self.robotPos != None and self.goalPos != None):
start = time.time()
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(self.robotPos.x, self.robotPos.y)
startGridCell = GridCell(self.robotPos.x, self.robotPos.y, uvComponent, None)
priorityQueue = PriorityQueue()
closeList = {}
openList = {}
openList[startGridCell.getKey()] = startGridCell
self.computeTotalCost(startGridCell)
priorityQueue.push(startGridCell.getCost(),startGridCell)
goal = None
i =0;
while(len(openList) > 0):
i = i + 1
(cost, currentCell) = priorityQueue.pop()
#print "low cell:(", currentCell.x , currentCell.y, currentCell.u , currentCell.v, ") cost:", cost
if(currentCell.getKey() in openList):
del openList[currentCell.getKey()]
if(self.isGoalGridCell(currentCell)):
goal = currentCell
break
else:
adjacentCells = self.getAdjacentCells(currentCell)
#self.printAdjacentCells(adjacentCells)
#print "\n"
for index in range(len(adjacentCells)):
adjacentCell = adjacentCells[index]
allReadyVisited = adjacentCell.getKey() in closeList
if(allReadyVisited): continue
self.computeTotalCost(adjacentCell)
#self.printAjacentCell(adjacentCell)
exploredBefore = adjacentCell.getKey() in openList
if(exploredBefore == False):
openList[adjacentCell.getKey()] = adjacentCell
priorityQueue.push(adjacentCell.getCost(),adjacentCell)
#print "Adj:(" , adjacentCell.x , adjacentCell.y , ") ", "not in open list, added to open list"
else:
oldAdjacentCell = openList[adjacentCell.getKey()]
#print "found old Adj:(" , oldAdjacentCell.x , oldAdjacentCell.y , ") " , "cost:", oldAdjacentCell.getCost(), " parent:" , oldAdjacentCell.parentGridCell.x , oldAdjacentCell.parentGridCell.y
if(adjacentCell.getCost() < oldAdjacentCell.getCost()):
oldAdjacentCell.setParent(currentCell)
self.computeTotalCost(oldAdjacentCell)
#print "old Adj upadte:(" , oldAdjacentCell.x , oldAdjacentCell.y , ") " , "new cost:", oldAdjacentCell.getCost(), " new parent:" , oldAdjacentCell.parentGridCell.x , oldAdjacentCell.parentGridCell.y
#else:
#print "Old Adj not updated"
#print "\n"
closeList[currentCell.getKey()] = currentCell
#print "low cell:(", currentCell.x , currentCell.y, ") cost:", cost, " REMOVED"
#print "-----------"
#if(i == 1):
#break
#print "Goal:", goal.x, goal.y
#print 'ieration: ', i
end = time.time()
elapsed = end - start
s = "Time taken: " + str(elapsed) + " seconds.\n"
#s += "Total node expanded: " + str(len(closeList)) + " out of " + "(30 * 30) = 900. \n"
self.path = []
if(goal is not None):
self.getPath(goal)
self.writePathToFile()
else:
s = "Goal not found !!!"
#print s
def writePathToFile(self):
factor = 10000
self.totalCost = 0
string =""
for node in self.path:
#s = "%d,%d\n" % (node.x, node.y)
#c = "%d,%d,%d\n" % (node.x, node.y, node.hVal)
if(node.parentGridCell != None):
neWaddedU = (node.u + node.parentGridCell.u) / 2;
neWaddedV = (node.v + node.parentGridCell.v) / 2;
resultantOcceanCurrent = math.sqrt( math.pow(neWaddedU, 2) + math.pow(neWaddedV, 2) )
self.totalCost += int((node.parentGridCell.gVal + 1 / resultantOcceanCurrent))
#print c
#string = string + s
#fileObject = open('C:\Users\Tamkin\Documents\GitHub\Searistica\Tamkin_Work\Project\MissionPlanning\Business\PythonCodes\PathPlanning\path.txt', "wb")
#fileObject.write(string);
#fileObject.close()
#print "total cost %d" % self.totalCost
def makePalnning(self):
#print 'Running shortest path planning ....'
self.computePlan()
def __init__(self):
self.robotPos = Position(9, 15)
self.goalPos = Position(41, 106)
self.oceanCurrentDataGrid = OceanCurrentDataGrid()
self.oceanCurrentDataGrid.loadDataFromFile()
class PathPlanning:
def __init__(self):
self.robotPos = Position(9, 15)
self.goalPos = Position(41, 106)
self.oceanCurrentDataGrid = OceanCurrentDataGrid()
self.oceanCurrentDataGrid.loadDataFromFile()
def getMarkerUniqueId(self):
self.markerUniqueId += 1
return self.markerUniqueId
def setMarkerPublishers(self):
self.markerPublisher = rospy.Publisher('/planning_markers', Marker)
self.markerArray_publisher = rospy.Publisher(
'/planning_multiple_markers', MarkerArray, latch=True)
def subscribeEvents(self):
rospy.Subscriber('/goal_pos', PoseStamped, self.rvizGoalPosCallBack)
def rvizGoalPosCallBack(self, clickedPose):
comandType = rospy.get_param('comandType')
pos_x = int(clickedPose.pose.position.x)
pos_y = int(clickedPose.pose.position.y)
if (comandType == "o"):
clickedPosition = Position(pos_x, pos_y)
self.occupancy_grid.setData(clickedPosition.x, clickedPosition.y,
100)
elif (comandType == "g"):
r = rospy.Rate(4)
self.deletePathMarkers()
r.sleep()
self.updatePlanning = True
self.goalPos = Position(pos_x, pos_y)
def deletePathMarkers(self):
for i in range(len(self.pathMarkerIds)):
pathMarkerId = self.pathMarkerIds[i]
marker = Marker()
marker.header.frame_id = '/map'
marker.ns = 'AStartPlanning'
marker.id = pathMarkerId
marker.action = Marker.DELETE
self.markerPublisher.publish(marker)
def computeEuclideanDistance(self, currentCell):
xDistance = self.goalPos.x - currentCell.x
yDistance = self.goalPos.y - currentCell.y
return math.sqrt(math.pow(xDistance, 2) + math.pow(yDistance, 2))
def computeH(self, gridCell):
euclideanDistanceToGoal = self.computeEuclideanDistance(gridCell)
#print 'ec: ' + str(euclideanDistanceToGoal)
resultantOcceanCurrent = math.sqrt(
math.pow(gridCell.u, 2) + math.pow(gridCell.v, 2))
#print 'rv: ' + str(resultantOcceanCurrent)
#gridCell.hVal = euclideanDistanceToGoal / resultantOcceanCurrent
gridCell.hVal = euclideanDistanceToGoal
def oneCellMoveCost(self):
return 1
def isAnObstacle(self, gridCell):
gridCellData = self.oceanCurrentDataGrid.data[gridCell.x][gridCell.y]
if (gridCellData == 100):
return True
return False
def getMovingToACellCost(self, gridCell):
if (gridCell.isAnObstacle):
return 10000
return 1
def computeG(self, gridCell):
if (gridCell.parentGridCell is None):
gridCell.gVal = 0
else:
gridCell.gVal = gridCell.parentGridCell.gVal + self.getMovingToACellCost(
gridCell)
def computeTotalCost(self, gridCell):
self.computeG(gridCell)
self.computeH(gridCell)
return gridCell.getCost()
def isGoalGridCell(self, currentCell):
return currentCell.x == self.goalPos.x and currentCell.y == self.goalPos.y
def isInRange(self, x, y):
return (x >= 0 and y >= 0) and (
x < self.oceanCurrentDataGrid.dimension.width
and y < self.oceanCurrentDataGrid.dimension.height)
def getAdjacentCells(self, currentCell):
adjacentCells = []
x = currentCell.x
y = currentCell.y
if (self.isInRange(x + 1, y)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(
x + 1, y)
gridCell = GridCell(x + 1, y, uvComponent, currentCell)
if (gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if (self.isInRange(x + 1, y + 1)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(
x + 1, y + 1)
gridCell = GridCell(x + 1, y + 1, uvComponent, currentCell)
if (gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if (self.isInRange(x, y + 1)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(
x, y + 1)
gridCell = GridCell(x, y + 1, uvComponent, currentCell)
if (gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if (self.isInRange(x - 1, y + 1)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(
x - 1, y + 1)
gridCell = GridCell(x - 1, y + 1, uvComponent, currentCell)
if (gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if (self.isInRange(x - 1, y)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(
x - 1, y)
gridCell = GridCell(x - 1, y, uvComponent, currentCell)
if (gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if (self.isInRange(x - 1, y - 1)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(
x - 1, y - 1)
gridCell = GridCell(x - 1, y - 1, uvComponent, currentCell)
if (gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if (self.isInRange(x, y - 1)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(
x, y - 1)
gridCell = GridCell(x, y - 1, uvComponent, currentCell)
if (gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if (self.isInRange(x + 1, y - 1)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(
x + 1, y - 1)
gridCell = GridCell(x + 1, y - 1, uvComponent, currentCell)
if (gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
return adjacentCells
def printAdjacentCells(self, adjs):
s = "Adjs : "
for i in range(len(adjs)):
adj = adjs[i]
s += "(" + str(adj.x) + "," + str(adj.y) + ") "
print s
def getPath(self, gridCell):
self.path.append(gridCell)
if (gridCell.x == self.robotPos.x and gridCell.y == self.robotPos.y):
return
self.getPath(gridCell.parentGridCell)
def computePlan(self):
obj = self.oceanCurrentDataGrid.getUVComponentOfDataCell(
self.goalPos.x, self.goalPos.y)
print obj.u
print obj.v
print "Goal is exists."
if (self.robotPos != None and self.goalPos != None):
start = time.time()
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(
self.robotPos.x, self.robotPos.y)
startGridCell = GridCell(self.robotPos.x, self.robotPos.y,
uvComponent, None)
priorityQueue = PriorityQueue()
closeList = {}
openList = {}
openList[startGridCell.getKey()] = startGridCell
self.computeTotalCost(startGridCell)
priorityQueue.push(startGridCell.getCost(), startGridCell)
goal = None
i = 0
while (len(openList) > 0):
(cost, currentCell) = priorityQueue.pop()
print "low cell:(", currentCell.x, currentCell.y, ") cost:", cost
if (currentCell.getKey() in openList):
del openList[currentCell.getKey()]
if (self.isGoalGridCell(currentCell)):
goal = currentCell
print "found !!"
break
else:
adjacentCells = self.getAdjacentCells(currentCell)
#self.printAdjacentCells(adjacentCells)
#print "\n"
for index in range(len(adjacentCells)):
adjacentCell = adjacentCells[index]
#print adjacentCell.u
allReadyVisited = adjacentCell.getKey() in closeList
if (allReadyVisited): continue
self.computeTotalCost(adjacentCell)
print "Adj:(", adjacentCell.x, adjacentCell.y, ") ", "cost:", adjacentCell.getCost(
)
exploredBefore = adjacentCell.getKey() in openList
if (exploredBefore == False):
openList[adjacentCell.getKey()] = adjacentCell
priorityQueue.push(adjacentCell.getCost(),
adjacentCell)
#print "Adj:(" , adjacentCell.x , adjacentCell.y , ") ", "not in open list, added to open list"
else:
oldAdjacentCell = openList[adjacentCell.getKey()]
#print "found old Adj:(" , oldAdjacentCell.x , oldAdjacentCell.y , ") " , "cost:", oldAdjacentCell.getCost(), " parent:" , oldAdjacentCell.parentGridCell.x , oldAdjacentCell.parentGridCell.y
if (adjacentCell.getCost() <
oldAdjacentCell.getCost()):
oldAdjacentCell.setParent(currentCell)
#if(oldAdjacentCell.isAnObstacle):
#print "o: %d, %d" % (oldAdjacentCell.x, oldAdjacentCell.y)
#print "ofp: %d, %d" % (oldAdjacentCell.parentGridCell.x, oldAdjacentCell.parentGridCell.y)
self.computeTotalCost(oldAdjacentCell)
#print "old Adj upadte:(" , oldAdjacentCell.x , oldAdjacentCell.y , ") " , "new cost:", oldAdjacentCell.getCost(), " new parent:" , oldAdjacentCell.parentGridCell.x , oldAdjacentCell.parentGridCell.y
#else:
#print "Old Adj not updated"
#print "\n"
closeList[currentCell.getKey()] = currentCell
#print "low cell:(", currentCell.x , currentCell.y, ") cost:", cost, " REMOVED"
print "-----------"
i = i + 1
#if(i == 1000):
#break
#print "Goal:", goal.x, goal.y
end = time.time()
elapsed = end - start
s = "Time taken: " + str(elapsed) + " seconds.\n"
s += "Total node expanded: " + str(
len(closeList)) + " out of " + "(30 * 30) = 900. \n"
self.path = []
print len(openList)
if (goal is not None):
self.getPath(goal)
else:
s = "Goal not found !!!"
for node in self.path:
print "%d,%d" % (node.x, node.y)
def makePalnning(self):
print 'Running ocean path planner'
self.computePlan()
def computeHNew(self, childGridCell):
#childGridCell.hVal = self.computeEquledianDistance(childGridCell)
euclideanDistanceToGoal = self.computeEquledianDistance(childGridCell)
resultantOcceanCurrent = math.sqrt(
math.pow(childGridCell.u, 2) + math.pow(childGridCell.v, 2))
#childGridCell.hVal = euclideanDistanceToGoal / resultantOcceanCurrent
resultantOcceanCurrent = None
if (childGridCell.parentGridCell == None):
resultantOcceanCurrent = math.sqrt(
math.pow(childGridCell.u, 2) + math.pow(childGridCell.v, 2))
else:
addedU = childGridCell.u + childGridCell.parentGridCell.u
addedV = childGridCell.v + childGridCell.parentGridCell.v * -1
resultantOcceanCurrent = math.sqrt(
math.pow(addedU, 2) + math.pow(addedV, 2))
childGridCell.hVal = childGridCell.gVal / resultantOcceanCurrent
def __init__(self):
self.robotPos = Position(9, 15)
self.goalPos = Position(41, 106)
self.oceanCurrentDataGrid = OceanCurrentDataGrid()
self.oceanCurrentDataGrid.loadDataFromFile()
class PathPlanning:
def __init__(self):
self.robotPos = Position(9, 15)
self.goalPos = Position(41, 106)
self.oceanCurrentDataGrid = OceanCurrentDataGrid()
self.oceanCurrentDataGrid.loadDataFromFile()
def getMarkerUniqueId(self):
self.markerUniqueId +=1
return self.markerUniqueId
def setMarkerPublishers(self):
self.markerPublisher = rospy.Publisher('/planning_markers', Marker)
self.markerArray_publisher = rospy.Publisher('/planning_multiple_markers', MarkerArray, latch=True)
def subscribeEvents(self):
rospy.Subscriber('/goal_pos', PoseStamped, self.rvizGoalPosCallBack)
def rvizGoalPosCallBack(self, clickedPose):
comandType = rospy.get_param('comandType')
pos_x = int(clickedPose.pose.position.x)
pos_y = int(clickedPose.pose.position.y)
if(comandType == "o"):
clickedPosition = Position(pos_x, pos_y)
self.occupancy_grid.setData(clickedPosition.x, clickedPosition.y, 100)
elif(comandType == "g"):
r = rospy.Rate(4)
self.deletePathMarkers()
r.sleep()
self.updatePlanning = True
self.goalPos = Position(pos_x, pos_y)
def deletePathMarkers(self):
for i in range(len(self.pathMarkerIds)):
pathMarkerId = self.pathMarkerIds[i]
marker = Marker()
marker.header.frame_id = '/map'
marker.ns = 'AStartPlanning'
marker.id = pathMarkerId
marker.action = Marker.DELETE
self.markerPublisher.publish(marker)
def computeEuclideanDistance(self, currentCell):
xDistance = self.goalPos.x - currentCell.x
yDistance = self.goalPos.y - currentCell.y
return math.sqrt( math.pow(xDistance, 2) + math.pow(yDistance, 2) )
def computeH(self, gridCell):
euclideanDistanceToGoal = self.computeEuclideanDistance(gridCell)
#print 'ec: ' + str(euclideanDistanceToGoal)
resultantOcceanCurrent = math.sqrt( math.pow(gridCell.u, 2) + math.pow(gridCell.v, 2) )
#print 'rv: ' + str(resultantOcceanCurrent)
#gridCell.hVal = euclideanDistanceToGoal / resultantOcceanCurrent
gridCell.hVal = euclideanDistanceToGoal
def oneCellMoveCost(self):
return 1;
def isAnObstacle(self, gridCell):
gridCellData = self.oceanCurrentDataGrid.data[gridCell.x][gridCell.y]
if(gridCellData == 100):
return True
return False
def getMovingToACellCost(self, gridCell):
if(gridCell.isAnObstacle):
return 10000
return 1
def computeG(self, gridCell):
if(gridCell.parentGridCell is None):
gridCell.gVal = 0
else:
gridCell.gVal = gridCell.parentGridCell.gVal + self.getMovingToACellCost(gridCell)
def computeTotalCost(self, gridCell):
self.computeG(gridCell)
self.computeH(gridCell)
return gridCell.getCost()
def isGoalGridCell(self, currentCell):
return currentCell.x == self.goalPos.x and currentCell.y == self.goalPos.y
def isInRange(self, x, y):
return (x >= 0 and y >= 0) and ( x < self.oceanCurrentDataGrid.dimension.width and y < self.oceanCurrentDataGrid.dimension.height)
def getAdjacentCells(self, currentCell):
adjacentCells = []
x = currentCell.x
y = currentCell.y
if(self.isInRange(x + 1, y)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(x + 1, y)
gridCell = GridCell(x + 1, y, uvComponent ,currentCell)
if(gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if(self.isInRange(x + 1, y + 1)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(x + 1, y + 1)
gridCell = GridCell(x + 1, y + 1, uvComponent, currentCell)
if(gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if(self.isInRange(x, y + 1)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(x, y + 1)
gridCell = GridCell(x, y + 1, uvComponent, currentCell)
if(gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if(self.isInRange(x - 1, y + 1)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(x - 1, y + 1)
gridCell = GridCell(x - 1, y + 1,uvComponent, currentCell)
if(gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if(self.isInRange(x - 1, y)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(x - 1, y)
gridCell = GridCell(x - 1, y, uvComponent ,currentCell)
if(gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if(self.isInRange(x - 1, y - 1)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(x - 1, y - 1)
gridCell = GridCell(x - 1, y - 1, uvComponent, currentCell)
if(gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if(self.isInRange(x, y - 1)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(x, y - 1)
gridCell = GridCell(x, y - 1, uvComponent, currentCell)
if(gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
if(self.isInRange(x + 1, y - 1)):
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(x + 1, y - 1)
gridCell = GridCell(x + 1, y - 1, uvComponent, currentCell)
if(gridCell.isAnObstacle == False):
adjacentCells.append(gridCell)
return adjacentCells
def printAdjacentCells(self, adjs):
s = "Adjs : "
for i in range(len(adjs)):
adj = adjs[i]
s += "(" + str(adj.x) + "," + str(adj.y) + ") "
print s
def getPath(self, gridCell):
self.path.append(gridCell)
if(gridCell.x == self.robotPos.x and gridCell.y == self.robotPos.y):
return
self.getPath(gridCell.parentGridCell)
def computePlan(self):
obj = self.oceanCurrentDataGrid.getUVComponentOfDataCell(self.goalPos.x, self.goalPos.y)
print obj.u
print obj.v
print "Goal is exists."
if(self.robotPos != None and self.goalPos != None):
start = time.time()
uvComponent = self.oceanCurrentDataGrid.getUVComponentOfDataCell(self.robotPos.x, self.robotPos.y)
startGridCell = GridCell(self.robotPos.x, self.robotPos.y, uvComponent, None)
priorityQueue = PriorityQueue()
closeList = {}
openList = {}
openList[startGridCell.getKey()] = startGridCell
self.computeTotalCost(startGridCell)
priorityQueue.push(startGridCell.getCost(),startGridCell)
goal = None
i = 0;
while(len(openList) > 0):
(cost, currentCell) = priorityQueue.pop()
print "low cell:(", currentCell.x , currentCell.y, ") cost:", cost
if(currentCell.getKey() in openList):
del openList[currentCell.getKey()]
if(self.isGoalGridCell(currentCell)):
goal = currentCell
print "found !!"
break
else:
adjacentCells = self.getAdjacentCells(currentCell)
#self.printAdjacentCells(adjacentCells)
#print "\n"
for index in range(len(adjacentCells)):
adjacentCell = adjacentCells[index]
#print adjacentCell.u
allReadyVisited = adjacentCell.getKey() in closeList
if(allReadyVisited): continue
self.computeTotalCost(adjacentCell)
print "Adj:(" , adjacentCell.x , adjacentCell.y , ") " , "cost:", adjacentCell.getCost()
exploredBefore = adjacentCell.getKey() in openList
if(exploredBefore == False):
openList[adjacentCell.getKey()] = adjacentCell
priorityQueue.push(adjacentCell.getCost(),adjacentCell)
#print "Adj:(" , adjacentCell.x , adjacentCell.y , ") ", "not in open list, added to open list"
else:
oldAdjacentCell = openList[adjacentCell.getKey()]
#print "found old Adj:(" , oldAdjacentCell.x , oldAdjacentCell.y , ") " , "cost:", oldAdjacentCell.getCost(), " parent:" , oldAdjacentCell.parentGridCell.x , oldAdjacentCell.parentGridCell.y
if(adjacentCell.getCost() < oldAdjacentCell.getCost()):
oldAdjacentCell.setParent(currentCell)
#if(oldAdjacentCell.isAnObstacle):
#print "o: %d, %d" % (oldAdjacentCell.x, oldAdjacentCell.y)
#print "ofp: %d, %d" % (oldAdjacentCell.parentGridCell.x, oldAdjacentCell.parentGridCell.y)
self.computeTotalCost(oldAdjacentCell)
#print "old Adj upadte:(" , oldAdjacentCell.x , oldAdjacentCell.y , ") " , "new cost:", oldAdjacentCell.getCost(), " new parent:" , oldAdjacentCell.parentGridCell.x , oldAdjacentCell.parentGridCell.y
#else:
#print "Old Adj not updated"
#print "\n"
closeList[currentCell.getKey()] = currentCell
#print "low cell:(", currentCell.x , currentCell.y, ") cost:", cost, " REMOVED"
print "-----------"
i = i + 1
#if(i == 1000):
#break
#print "Goal:", goal.x, goal.y
end = time.time()
elapsed = end - start
s = "Time taken: " + str(elapsed) + " seconds.\n"
s += "Total node expanded: " + str(len(closeList)) + " out of " + "(30 * 30) = 900. \n"
self.path = []
print len(openList)
if(goal is not None):
self.getPath(goal)
else:
s = "Goal not found !!!"
for node in self.path:
print "%d,%d" % (node.x, node.y)
def makePalnning(self):
print 'Running ocean path planner'
self.computePlan()
def computeHNew(self, childGridCell):
#childGridCell.hVal = self.computeEquledianDistance(childGridCell)
euclideanDistanceToGoal = self.computeEquledianDistance(childGridCell)
resultantOcceanCurrent = math.sqrt( math.pow(childGridCell.u, 2) + math.pow(childGridCell.v, 2) )
#childGridCell.hVal = euclideanDistanceToGoal / resultantOcceanCurrent
resultantOcceanCurrent = None
if(childGridCell.parentGridCell == None):
resultantOcceanCurrent = math.sqrt( math.pow(childGridCell.u, 2) + math.pow(childGridCell.v, 2) )
else:
addedU = childGridCell.u + childGridCell.parentGridCell.u;
addedV = childGridCell.v + childGridCell.parentGridCell.v * -1;
resultantOcceanCurrent = math.sqrt( math.pow(addedU, 2) + math.pow(addedV, 2) )
childGridCell.hVal = childGridCell.gVal / resultantOcceanCurrent
