def aStarSearch(self, start):
"""
This function generates a dictionary of paths where the shortest path can be found through
traversing from the goal back to the start.
This version of A* uses tools.distFormula as the heuristic. Could be changed out.
:param start: (x,y) touple of the location in the grid.
:return: Dictionary of <end point> : <starting Point> as key,value
"""
frontier = Queue.PriorityQueue()
frontier.put((0, start)) ## Put takes a touple of priority, value
came_from = {}
cost_so_far = {}
came_from[start] = None
cost_so_far[start] = 0
while not frontier.empty():
current_touple = frontier.get() ## Returns the (priority, (x,y)
_,current = current_touple ## sets current to (x,y)
x,y = current
if x == self.goalX and y == self.goalY:
break #We're there!
#unpack current
neighbors = self.getNeighbors(x,y)
# print neighbors
for next in neighbors: ## Get list of touples (points)
# print "AStar",next,current
cost_next = cost_so_far[current] + tools.distFormula(current, next)
if next not in cost_so_far or cost_so_far[next] > cost_next:
if next not in cost_so_far:
frontier.put((cost_next + tools.distFormula(next, self._goal_pos), next))## Put takes a touple of priority, value
nx,ny = next
## THis is where explored nodes would be added to a list for painting.
# self.explored_nodes_list.append(tools.makePoint(nx,ny,0))
# self._repaint_map()
cost_so_far[next] = cost_next
came_from[next] = current
if frontier.empty():
print "It's empty!!!!!"
rospy.sleep(1)
print "CAME FROM:", came_from
print "GOAL", self._goal_
print "START", start
raise RuntimeError("A* cannot find the path for some reason")
return came_from
def isAtGoalPosition(self, currentLoc):
"""
Takes in a tuple of (x, y) that is the current robot location.
Returns true if we're there, false otherwise.
"""
if (tools.distFormula(currentLoc, self._goal_pos) < 0.1): # Maybe this error bar should go out globally, or take it in.
return True
else:
return False
def driveStraight(self, speed, distance):
"""
This takes in a speed and distance and moves in the facing direction
until it has reached the distance.
NOTE: This function denotes it's travel by how far the 'odom' frame believes
it has traveled. NOT how far it has traveled in the /map.
:param speed: This is a 0 to 1 value, it will cap the values at 1 and 0.
:param distance: This is a value in meters.
:return: Nothing
"""
## Speed Error checking
if speed > 1: speed = 1
elif speed < 0: speed = 0
## Set the starting x,y
starting_x = self._x
starting_y = self._y
## Booleans for understanding location
arrived = False
slowdown = False
while ((not arrived) and (not rospy.is_shutdown())):
## Get the distance traveled
dist_so_far = tools.distFormula((starting_x,starting_y),(self._x,self._y))
## Modulate speed based off how far you've gone
if dist_so_far <= abs(distance)*0.25:
regulated_speed = speed*(0.2) + speed*(dist_so_far / distance)
elif dist_so_far >= abs(distance)*0.75:
regulated_speed = speed*(0.2) + speed*(1- (dist_so_far / distance))
else:
regulated_speed = speed
if distance > 0: self._spinWheels(regulated_speed,regulated_speed,0.1)
else: self._spinWheels(-regulated_speed,-regulated_speed,0.1)
#Set the booleans on if you're there yet
arrived = dist_so_far >= abs(distance)
self._stopRobot()
