def extractPathEndingAtCell(self, pathEndCell, colour):
# Construct the path object and mark if the goal was reached
path = PlannedPath()
path.goalReached = self.goalReached
# Initial condition - the goal cell
path.waypoints.append(pathEndCell)
# Start at the goal and find the parent. Find the cost associated with the parent
cell = pathEndCell.parent
path.travelCost = self.computeLStageAdditiveCost(
pathEndCell.parent, pathEndCell)
path.totalAngleTurned = self.computeAngleTurned(
pathEndCell.parent, pathEndCell) # for task 1.1
path.maxLenOfQueue = self.getMaxLenOfQueue() # for task 1.1
path.numberOfCellsVisited = self._numberOfCellsVisited
# Iterate back through and extract each parent in turn and add
# it to the path. To work out the travel length along the
# path, you'll also have to add self at self stage.
while (cell is not None):
path.waypoints.appendleft(cell)
path.travelCost = path.travelCost + self.computeLStageAdditiveCost(
cell.parent, cell)
path.totalAngleTurned += self.computeAngleTurned(
cell.parent, cell) # for task 1.1
# print cell.coords # debug del
cell = cell.parent
# Update the stats on the size of the path
self._numberOfCellsVisited = path.numberOfWaypoints = len(
path.waypoints) # for task 1.1
# Note that if we failed to reach the goal, the above mechanism computes a path length of 0.
# Therefore, if we didn't reach the goal, change it to infinity
if path.goalReached is False:
path.travelCost = float("inf")
self._totalTravelCost = path.travelCost # for task 1.1
print "Path travel cost = " + str(path.travelCost)
print "Path cardinality = " + str(path.numberOfWaypoints)
print "Angle Turned = " + str(path.totalAngleTurned)
print "Maximum Queue Size =" + str(path.maxLenOfQueue)
print "numberOfCellsVisited =" + str(path.numberOfCellsVisited)
# Draw the path if requested
if (self.showGraphics == True):
self.plannerDrawer.update()
self.plannerDrawer.drawPathGraphicsWithCustomColour(path, colour)
self.plannerDrawer.waitForKeyPress()
# Return the path
return path
def extractPathEndingAtCell(self, pathEndCell, colour):
# Construct the path object and mark if the goal was reached
path = PlannedPath()
path.goalReached = self.goalReached
# Initial condition - the goal cell
path.waypoints.append(pathEndCell)
# Start at the goal and find the parent. Find the cost associated with the parent
previousCell = pathEndCell
cell = pathEndCell.parent
parentParentCell = cell.parent
path.travelCost = self.computeLStageAdditiveCost(
pathEndCell.parent, pathEndCell)
pathAngle = self.computeAngle(pathEndCell, cell, parentParentCell)
print(pathAngle)
print(cell.coords[0])
# Iterate back through and extract each parent in turn and add
# it to the path. To work out the travel length along the
# path, you'll also have to add self at self stage.
while (cell is not None):
if (cell.parent == None):
break
path.waypoints.appendleft(cell)
path.travelCost = path.travelCost + self.computeLStageAdditiveCost(
cell.parent, cell)
pathAngle += self.computeAngle(previousCell, cell, cell.parent)
cell = cell.parent
previousCell = previousCell.parent
# Update the stats on the size of the path
path.numberOfWaypoints = len(path.waypoints)
# Note that if we failed to reach the goal, the above mechanism computes a path length of 0.
# Therefore, if we didn't reach the goal, change it to infinity
if path.goalReached is False:
path.travelCost = float("inf")
print("The total path Angle is {} degrees".format(pathAngle))
print "Path travel cost = " + str(path.travelCost)
print "Path cardinality = " + str(path.numberOfWaypoints)
# Draw the path if requested
if (self.showGraphics == True):
self.plannerDrawer.update()
self.plannerDrawer.drawPathGraphicsWithCustomColour(path, colour)
self.plannerDrawer.waitForKeyPress()
# Return the path
return path
def extractPathEndingAtCell(self, pathEndCell, colour):
# Construct the path object and mark if the goal was reached
path = PlannedPath()
path.goalReached = self.goalReached
# Add the first cell to the path. We actually grow the path
# backwards from the goal to the start. For the goal cell, we
# do not include it if (a) the goal cell was occupied when we
# started planning and (b) trimming the path is enabled.
if (self.removeGoalCellFromPathIfOccupied is False) or \
(self.goalCellLabel is not CellLabel.OBSTRUCTED):
path.waypoints.append(pathEndCell)
# Start at the goal and find the parent. Find the cost associated with the parent
cell = pathEndCell.parent
path.travelCost = self.computeLStageAdditiveCost(
pathEndCell.parent, pathEndCell)
# Iterate back through and extract each parent in turn and add
# it to the path. To work out the travel length along the
# path, you'll also have to add self at self stage.
while (cell is not None):
path.waypoints.appendleft(cell)
path.travelCost = path.travelCost + self.computeLStageAdditiveCost(
cell.parent, cell)
cell = cell.parent
# Update the stats on the size of the path
path.numberOfWaypoints = len(path.waypoints)
# Note that if we failed to reach the goal, the above mechanism computes a path length of 0.
# Therefore, if we didn't reach the goal, change it to infinity
if path.goalReached is False:
path.travelCost = float("inf")
print "Path travel cost = " + str(path.travelCost)
print "Path cardinality = " + str(path.numberOfWaypoints)
self.total_travel_cost += path.travelCost
print "Total travel cost = " + str(self.total_travel_cost)
# Draw the path if requested
if (self.showGraphics == True):
self.searchGridDrawer.update()
self.searchGridDrawer.drawPathGraphicsWithCustomColour(
path, colour)
self.searchGridDrawer.waitForKeyPress()
# Return the path
return path