def getPath(self, start):
"""
This function uses A* to generate a from start to end and then returns it.
:param start:
:return: list of points (x,y touples) that is the path to take to the goal
"""
came_from = self.aStarSearch(start) ## Get dictionary from astar
path = [self._goal_pos] ## Initialize path
## Prime the statements for iterating to the end of the path
current_location = self._goal_pos
prev = came_from[current_location]
path.append(prev)
current_location = prev
## Traverse the tree from leaves to trunk
while prev is not None:
prev = came_from[current_location]
if prev is None:
continue
path.append(prev)
current_location = prev
path.reverse() ## Reverse the list so it goes from start to finish
## Convert the path list to a publishable list so we can view the path in rViz
self.path_list = tools.publishListfromTouple(path)
self._repaint_map(path=True) ## Repaint the map so we can see it.
return path
def getWaypoint(self):
self._updateLocation()
start = (self.current_x,self.current_y)
pathToNodify = self.getPath(start) # Get path from A*
nodePath = [] # What will become the path of only relevant nodes.
prevSlope = 1337.0 # Create an impossible to match previous slope
for node in range (0, len(pathToNodify)-1): # For every node in the path created by A*:
if pathToNodify[node] != self._goal_pos: # Do not try to calculate a slope for the last node
#calculate current slope, from current position to next position.
deltax = 1.0*(pathToNodify[node][0]-pathToNodify[node+1][0])#Calcualte delta x based on node and the node ahead
deltay = 1.0*(pathToNodify[node][1]-pathToNodify[node+1][1])#calculate delta y
currentSlope = deltax/(deltay+0.000001) #So that we don't have devide by 0 errors. Dunno how else to fix
if currentSlope != prevSlope: # If we see a slope change
nodePath.append(pathToNodify[node])# Add the new node
prevSlope=currentSlope # Update th slope
#Append the very last node, so that we have a complete path.
nodePath.append(pathToNodify[len(pathToNodify)-1])
self.waypoint_list = tools.publishListfromTouple(nodePath)
self._repaint_map(path=True)
# print nodePath
return nodePath
