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()
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()
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
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()
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