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