def getChildrenWithIslandsAndCosts(self, node):
if (not self.graph.has_key(node.getNodeId())):
self.graph[node.getNodeId()] = node
point = self.getPointFromId(node.getNodeId())
children, edgeCosts = self.getNeighbours(point[0], point[1])
childrenNodes = []
for child in children:
nodeId = self.getIdFromPoint(child)
if (self.graph.has_key(nodeId)):
childNode = self.graph[nodeId]
childrenNodes.append(childNode)
else:
childNode = Node(nodeId)
self.graph[nodeId] = childNode
childrenNodes.append(childNode)
flag = 0
for nodeId in self.islandNodeIds:
if (self.distanceBetweenNodes(node.getNodeId(), nodeId) <
self.islandThresh):
# XXX Assumes that one node is near only one island node.
childIslandNode = self.graph[nodeId]
cost = self.heuristic(node, childIslandNode)
childrenNodes.append(childIslandNode)
edgeCosts.append(cost)
flag = 1
break
if node.checkDummyG():
for childNode in childrenNodes:
childNode.setHasDummyG(True)
return (childrenNodes, edgeCosts, flag)
def main():
"""Numpy array is accessed as (r, c) while a point is (x, y). The code
follows (r, c) convention everywhere. Hence, be careful whenever using a
point with opencv."""
folder = sys.argv[1]
image = folder + "/image.png"
start_goal = folder + "/start_goal.pkl"
startPoint, goalPoint = pickle.load(open(start_goal, "rb"))
occGrid = OccupancyGrid()
occMap = occGrid.getMapFromImage(image)
viz = ImageVisualizer(occMap)
viz.incrementalDisplay = True
print(occMap.shape)
print(startPoint, goalPoint)
gridEnv = GridEnvironment(occMap, occMap.shape[0], occMap.shape[1])
# List of two heuristics.
heuristics = (gridEnv.euclideanHeuristic, gridEnv.diagonalHeuristic)
gridEnv.setHeuristic(heuristics)
startNode = Node(gridEnv.getIdFromPoint(startPoint))
startNode.setParent(None)
goalNode = Node(gridEnv.getIdFromPoint(goalPoint))
gridEnv.addNode(goalNode)
gridEnv.goal(goalNode)
assert (gridEnv.isValidPoint(startPoint))
assert (gridEnv.isValidPoint(goalPoint))
# Planner
planner = MHAstar(gridEnv, w1=5, w2=5)
planFound = planner.plan(startNode, goalNode, viz=viz)
path = []
if planFound:
print("Planning successful")
currNode = goalNode
while (currNode != startNode):
path.append(currNode)
currNode = currNode.getParent()
# Reverse the list.
path = path[::-1]
planStateIds = map(lambda node: node.getNodeId(), path)
pathPoints = []
for node in path:
pathPoints.append(gridEnv.getPointFromId(node.getNodeId()))
viz.displayImage()
#viz.joinPointsInOrder(pathPoints, thickness=5)
viz.markPoints(pathPoints, color=100)
viz.displayImage()
cv.waitKey(0)
def setIslandNodes(self, islandPoints):
self.islandNodeIds = []
self.activatedIslandNodes = []
for point in islandPoints:
islandId = self.getIdFromPoint(point)
self.islandNodeIds.append(islandId)
if not self.graph.has_key(islandId):
self.graph[islandId] = Node(islandId)
def main():
folder = sys.argv[1]
image = folder + "/image.png"
start_goal = folder + "/start_goal.pkl"
startPoint, goalPoint = pickle.load(open(start_goal, "rb"))
# Build the planning environment.
occGrid = OccupancyGrid()
occMap = occGrid.getMapFromImage(image)
gridEnv = MultiIslandGridEnvironment(occMap, occMap.shape[0],
occMap.shape[1])
gridEnv.setHeuristicType(0)
assert (gridEnv.isValidPoint(startPoint))
assert (gridEnv.isValidPoint(goalPoint))
# Set up island regions.
islandRegions = constructIslandRegions(gridEnv,
folder,
radius=61,
inflation=1.2)
gridEnv.islandRegions = islandRegions
# For visualization.
viz = ImageVisualizer(occMap, True)
makePrettyViz(viz, islandRegions, startPoint, goalPoint)
startNode = Node(gridEnv.getIdFromPoint(startPoint))
startNode.setParent(None)
goalNode = Node(gridEnv.getIdFromPoint(goalPoint))
gridEnv.addNode(goalNode.getNodeId())
# Choose your planner.
planner = MultiIslandAstar(gridEnv, inflation=1.2)
# Plan!
planFound = planner.plan(startNode, goalNode, viz=viz)
path = []
if planFound:
print("Planning successful")
# Retrieve the path.
currNode = gridEnv.graph[goalNode.getNodeId()]
print("Solution cost: %f" % currNode.gValue())
while (currNode != startNode):
path.append(currNode)
currNode = currNode.getParent()
# Reverse the list.
path = path[::-1]
pathPoints = []
for node in path:
pathPoints.append(gridEnv.getPointFromId(node.getNodeId()))
#viz.joinPointsInOrder(pathPoints, thickness=2)
viz.displayImage()
def constructIslandRegions(env, folder, radius=20, inflation=10):
"""Construct island regions using saved island coordinates and region
specifications."""
islandsFile = folder + "/islands.pkl"
islandPoints = pickle.load(open(islandsFile, "rb"))
print(islandPoints)
islandRegions = []
for islandPoint in islandPoints:
node = Node(env.getIdFromPoint(islandPoint))
circularRegion = CircularRegion(islandPoint, radius)
islandRegion = IslandRegion(node, circularRegion)
islandRegion.inflation = inflation
islandRegions.append(islandRegion)
return islandRegions
def findPeaks(self, folder):
"""Numpy array is accessed as (r, c) while a point is (x, y). The code
follows (r, c) convention everywhere. Hence, be careful whenever using a
point with opencv.
Takes one command line argument: Folder that has the environment and
config.
"""
self.folder = folder
image = self.folder + "/image.png"
start_goal = folder + "/start_goal.pkl"
startPoint, goalPoint = pickle.load(open(start_goal, "rb"))
NUMTIMEPEAKS = 20
NUMHEURISTICPEAKS = 10
occGrid = OccupancyGrid()
occMap = occGrid.getMapFromImage(image)
print(occMap.shape)
self.gridEnv = GridEnvironment(occMap, occMap.shape[0],
occMap.shape[1])
self.gridEnv.setHeuristic(0)
viz = ImageVisualizer(occMap)
#startPoint = (100, 20)
#goalPoint = (201, 200)
#print("Click start point")
#startPoint = inputClickedPoint(occMap)
#print("Click end point")
#goalPoint = inputClickedPoint(occMap)
print(startPoint, goalPoint)
assert (self.gridEnv.isValidPoint(startPoint))
assert (self.gridEnv.isValidPoint(goalPoint))
startNode = Node(self.gridEnv.getIdFromPoint(startPoint))
startNode.setParent(None)
goalNode = Node(self.gridEnv.getIdFromPoint(goalPoint))
self.gridEnv.addNode(goalNode)
planner = Astar(self.gridEnv)
planFound = planner.plan(startNode, goalNode, viz=viz)
#planFound = planner.plan(startNode, goalNode)
path = []
planNodeIds = []
if planFound:
print("Planning successful")
# Retrieve the path.
currNode = goalNode
while (currNode != startNode):
path.append(currNode)
currNode = currNode.getParent()
# Reverse the list.
path = path[::-1]
planStateIds = map(lambda node: node.getNodeId(), path)
self.solutionPath = planStateIds
# Dictionary of ids and timestamps.
planStats = planner.getPlanStats()
# -----------------------------------
# Finding the local minima.
# Get the timestamps.
stateNodeIds = planStats.keys()
stateHValues = []
for i in planStats.values():
stateHValues.append(i[1])
planHValues, planTimeStamps, planTimePerState = [], [], []
for stateId in planStateIds:
planNodeIds.append(stateId)
planHValues.append(planStats[stateId][1])
planTimeStamps.append(planStats[stateId][0])
# Start state.
planTimePerState = [0]
for i in range(1, len(planTimeStamps)):
planTimePerState.append(planTimeStamps[i] -
planTimeStamps[i - 1])
planHeuristicPerState = [0]
for i in range(1, len(planHValues)):
planHeuristicPerState.append(
max(0, planHValues[i] - planHValues[i - 1]))
#planHeuristicPerState.append(planHValues[i] -
#planHValues[i-1] )
# plotStuff(planHValues, planTimePerState, stateHValues, planNodeIds,
# stateNodeIds)
#plotStuff(planHValues, planTimePerState, stateHValues)
plotStuff(planHeuristicPerState, planTimePerState, stateHValues)
# Visualize the path.
pathPoints = []
for node in path:
#print(node.getNodeId())
pathPoints.append(self.gridEnv.getPointFromId(node.getNodeId()))
viz.joinPointsInOrder(pathPoints, thickness=5)
#----------------------------------------
# Visualize the peaks
# Extract the timestamps from values.
pathPlanTime = [planStats[node][0] for node in planNodeIds]
# Peaks has the node ids sorted according to the delta t.
peaks = self.getTimePeaks(planNodeIds, pathPlanTime)
timePeaks = peaks #[:NUMTIMEPEAKS]
# Extract the heuristic values.
pathPlanHeuristic = [planStats[node][1] for node in planNodeIds]
# Peaks has the node ids sorted according to the delta t.
print("Finding heuristic peaks.")
peaks = self.getHeuristicPeaks(planNodeIds, pathPlanHeuristic)
heuristicPeaks = peaks #[:NUMHEURISTICPEAKS]
#for peak in timePeaks:
#print(self.gridEnv.getPointFromId(peak))
for peak in timePeaks:
print("Marking the minima")
#print(self.gridEnv.getPointFromId(peak))
#viz.drawCircle(self.gridEnv.getPointFromId(peak), 5, color=(200,200,200), thickness=-1)
for peak in heuristicPeaks:
print("Marking the minima")
viz.drawCircle(self.gridEnv.getPointFromId(peak),
5,
color=(150, 150, 150),
thickness=-1)
#---------------------------------------
viz.displayImage()
# Save the peaks in files.
self.savePeaks(timePeaks, heuristicPeaks)
def main():
"""Numpy array is accessed as (r, c) while a point is (x, y). The code
follows (r, c) convention everywhere. Hence, be careful whenever using a
point with opencv."""
folder = sys.argv[1]
image = folder + "/image.png"
start_goal = folder + "/start_goal.pkl"
islandsFile = folder + "/islands.pkl"
startPoint, goalPoint = pickle.load(open(start_goal, "rb"))
islands = pickle.load(open(islandsFile, "rb"))
occGrid = OccupancyGrid()
occMap = occGrid.getMapFromImage(image)
viz = ImageVisualizer(occMap)
viz.incrementalDisplay = True
print(occMap.shape)
print(startPoint, goalPoint)
gridEnv = IslandGridEnvironment(occMap, occMap.shape[0], occMap.shape[1],
islands)
gridEnv.setIslandNodes(islands)
islandHeur = IslandHeuristic()
gridEnv.setHeuristic(
partial(islandHeur.heuristic,
env=gridEnv,
metric=gridEnv.euclideanHeuristic))
startNode = Node(gridEnv.getIdFromPoint(startPoint))
startNode.setParent(None)
goalNode = Node(gridEnv.getIdFromPoint(goalPoint))
gridEnv.addNode(goalNode)
gridEnv.goal(goalNode)
assert (gridEnv.isValidPoint(startPoint))
assert (gridEnv.isValidPoint(goalPoint))
# Island visualization.
#for island in gridEnv.getIslandNodes():
# viz.drawCircle(gridEnv.getPointFromId(island.getNodeId()),
# gridEnv.islandThresh)
# viz.displayImage(1)
#cv.destroyAllWindows()
# Planner
planner = IslandAstar(gridEnv, inflation=1)
planFound = planner.plan(startNode, goalNode, viz=viz)
path = []
if planFound:
print("Planning successful")
currNode = goalNode
while (currNode != startNode):
path.append(currNode)
currNode = currNode.getParent()
# Reverse the list.
path = path[::-1]
print("Solution cost is %d" % path[-1].g)
planStateIds = map(lambda node: node.getNodeId(), path)
pathPoints = []
for node in path:
pathPoints.append(gridEnv.getPointFromId(node.getNodeId()))
viz.displayImage()
#viz.joinPointsInOrder(pathPoints, thickness=5)
viz.markPoints(pathPoints, color=100)
viz.displayImage()
cv.waitKey(0)
def main():
"""
The main function that sets up the environment and calls the planner.
Each planning run has three components:
* Environment
* Graph
* Planner
The Environment class contains details about the specific planning problem,
eg: a 2D map for 2D planning. Its primary role is to implement a
`getChildren` method that returns the successors of each node. This helps
in building the graph implicitly.
Each vertex in the graph is an instance of `Node` class. It stores details
specific to the node.
An Astar instance runs on the graph so created.
"""
folder = sys.argv[1]
image = folder + "/image.png"
start_goal = folder + "/start_goal.pkl"
startPoint, goalPoint = pickle.load(open(start_goal, "rb"))
# Build the planning environment.
occGrid = OccupancyGrid()
occMap = occGrid.getMapFromImage(image)
print(occMap.shape)
gridEnv = GridEnvironment(occMap, occMap.shape[0], occMap.shape[1])
gridEnv.setHeuristicType(0)
# For visualization.
viz = ImageVisualizer(occMap, True)
## To take input by clicking.
#startPoint = (100, 20)
#goalPoint = (201, 200)
#print("Click start point")
#startPoint = inputClickedPoint(occMap)
#print("Click end point")
#goalPoint = inputClickedPoint(occMap)
print(startPoint, goalPoint)
assert (gridEnv.isValidPoint(startPoint))
assert (gridEnv.isValidPoint(goalPoint))
startNode = Node(gridEnv.getIdFromPoint(startPoint))
startNode.setParent(None)
goalNode = Node(gridEnv.getIdFromPoint(goalPoint))
gridEnv.addNode(goalNode)
# Choose your planner.
planner = Astar(gridEnv, inflation=1)
# Plan!
planFound = planner.plan(startNode, goalNode, viz=viz)
path = []
if planFound:
print("Planning successful")
# Retrieve the path.
currNode = gridEnv.graph[goalNode.getNodeId()]
while (currNode.getNodeId() != startNode.getNodeId()):
path.append(currNode)
currNode = currNode.getParent()
# Reverse the list.
path = path[::-1]
print("Cost of solution is %f" % path[-1].g)
pathPoints = []
for node in path:
pathPoints.append(gridEnv.getPointFromId(node.getNodeId()))
viz.joinPointsInOrder(pathPoints, thickness=2)
viz.displayImage()
def main():
"""Numpy array is accessed as (r, c) while a point is (x, y). The code
follows (r, c) convention everywhere. Hence, be careful whenever using a
point with opencv.
Takes one command line argument: Folder that has the environment and
config.
"""
folder = sys.argv[1]
image = folder + "/image.png"
start_goal = folder + "/start_goal.pkl"
startPoint, goalPoint = pickle.load( open(start_goal, "rb") )
occGrid = OccupancyGrid()
occMap = occGrid.getMapFromImage(image)
print(occMap.shape)
gridEnv = GridEnvironment(occMap, occMap.shape[0], occMap.shape[1])
gridEnv.setHeuristic(0)
viz = ImageVisualizer(occMap)
#startPoint = (100, 20)
#goalPoint = (201, 200)
#print("Click start point")
#startPoint = inputClickedPoint(occMap)
#print("Click end point")
#goalPoint = inputClickedPoint(occMap)
print(startPoint, goalPoint)
assert(gridEnv.isValidPoint(startPoint))
assert(gridEnv.isValidPoint(goalPoint))
startNode = Node(gridEnv.getIdFromPoint(startPoint))
startNode.setParent(None)
goalNode = Node(gridEnv.getIdFromPoint(goalPoint))
gridEnv.addNode(goalNode)
planner = Astar(gridEnv)
planFound = planner.plan(startNode, goalNode, viz=viz)
#planFound = planner.plan(startNode, goalNode)
path = []
if planFound:
print("Planning successful")
# Retrieve the path.
currNode = goalNode
while(currNode != startNode):
path.append(currNode)
currNode = currNode.getParent()
# Reverse the list.
path = path[::-1]
planStateIds = map(lambda node : node.getNodeId(), path)
planStats = planner.getPlanStats()
# -----------------------------------
# Finding the local minima.
# Get the timestamps.
stateNodeIds = planStats.keys()
stateHValues = []
for i in planStats.values():
stateHValues.append(i[1])
planHValues, planTimeStamps, planTimePerState = [], [], []
planNodeIds = []
for stateId in planStateIds:
planNodeIds.append(stateId)
planHValues.append(planStats[stateId][1])
planTimeStamps.append(planStats[stateId][0])
# Start state.
planTimePerState = [0]
for i in range(1, len(planTimeStamps)):
planTimePerState.append(planTimeStamps[i] - planTimeStamps[i-1])
#plotStuff(planHValues, planTimePerState, stateHValues, planNodeIds,
#stateNodeIds)
plotStuff(planHValues, planTimePerState, stateHValues)
pathPoints = []
for node in path:
#print(node.getNodeId())
pathPoints.append(gridEnv.getPointFromId(node.getNodeId()))
viz.displayImage()
viz.joinPointsInOrder(pathPoints, thickness=5)
viz.displayImage()