def plan(self, start_state, dest_state, max_num_steps, max_steering_radius, dest_reached_radius, live_view=True): """ Returns a path as a sequence of states [start_state, ..., dest_state] if dest_state is reachable from start_state. Otherwise returns [start_state]. Assume both source and destination are in free space. """ assert (self.state_is_free(start_state)) assert (self.state_is_free(dest_state)) # The set containing the nodes of the tree tree_nodes = set() tree_nodes.add(start_state) # image to be used to display the tree img = np.copy(self.world) cv2.circle(img, (start_state.x, start_state.y), 2, (255, 0, 0)) plan = [start_state] for step in range(max_num_steps): # TODO: Use the methods of this class as in the slides to # compute s_new correctly. The code here has several problems, as you'll see # in the output. s_nearest = start_state s_new = self.sample_state() s_new.parent = start_state if self.path_is_obstacle_free(s_nearest, s_new): tree_nodes.add(s_new) s_nearest.children.append(s_new) # If we approach the destination within a few pixels # we're done. Return the path. if s_new.euclidean_distance(dest_state) < dest_reached_radius: dest_state.parent = s_new plan = self._follow_parent_pointers(dest_state) break # plot the new node and edge cv2.circle(img, (s_new.x, s_new.y), 3, (0, 0, 255), 3) cv2.line(img, (s_nearest.x, s_nearest.y), (s_new.x, s_new.y), (255, 0, 0), 2) # Keep showing the image for a bit even # if we don't add a new node and edge if live_view: cv2.imshow('image', img) cv2.waitKey(100) if live_view: draw_plan(img, plan, bgr=(0, 0, 255), thickness=2, show_live=True) cv2.waitKey(0) return plan
def plan(self, start_state, dest_state, max_num_steps, max_steering_radius, dest_reached_radius): """ Returns a path as a sequence of states [start_state, ..., dest_state] if dest_state is reachable from start_state. Otherwise returns [start_state]. Assume both source and destination are in free space. """ ## n is max_num_steps ## V is Set of nodes ## E is Set of edges assert (self.state_is_free(start_state)) assert (self.state_is_free(dest_state)) # The set containing the nodes of the tree tree_nodes = set() tree_nodes.add(start_state) # image to be used to display the tree img = np.copy(self.world) plan = [start_state] for step in xrange(max_num_steps): # TODO: Use the methods of this class as in the slides to # compute s_new ## Set variable nodes s_rand = self.sample_state() s_nearest = self.find_closest_state(tree_nodes, s_rand) s_new = self.steer_towards(s_nearest, s_rand, max_steering_radius) if self.path_is_obstacle_free(s_nearest, s_new): tree_nodes.add(s_new) s_nearest.children.append(s_new) # If we approach the destination within a few pixels # we're done. Return the path. if s_new.euclidean_distance(dest_state) < dest_reached_radius: dest_state.parent = s_new plan = self._follow_parent_pointers(dest_state) break # plot the new node and edge cv2.circle(img, (s_new.x, s_new.y), 2, (0, 0, 0)) cv2.line(img, (s_nearest.x, s_nearest.y), (s_new.x, s_new.y), (255, 0, 0)) ##Red #time.sleep(3) # Keep showing the image for a bit even # if we don't add a new node and edge cv2.imshow('image', img) cv2.waitKey(10) draw_plan(img, plan, bgr=(0, 0, 255), thickness=2) cv2.waitKey(0) return [start_state]
def run_helper(world, planning_model, start_state, dest_state, model_type, filepath): start = datetime.now() try: start_state = start_state dest_state = dest_state plan = astar.plan(start_state, dest_state, model_type="ASTAR") draw_plan(world=world, plan=plan, filepath=filepath) except Exception as e: print(e) print("Model runtime is {} for model {} ".format(datetime.now() - start, model_type))
# load the image image = Image.open(sys.argv[1]) # convert image to numpy array world = asarray(image) astar = AStarPlanner(world) start_state = State(10, 10) # TODO: Change the goal state to 3 different values, saving and running between each # in order to produce your images to submit dest_state = State(275, 350) print("astar_planner.py is attempting to find a path from (" + str(start_state.x) + ',' + str(start_state.y) + ') to (' + str(dest_state.x) + ',' + str(dest_state.y) + ') in the image named ' + sys.argv[1] + '.') print('This may take a few moments...') #Time used for testing to see speedup compared to Dijkstra's start_time = time.time() plan = astar.plan(start_state, dest_state) end_time = time.time() draw_plan(world, plan, bgr=(0, 0, 255), thickness=2, show_live=False, filename='astar_result.png') print('Planning complete in ' + str(end_time - start_time) + '. See image astar_result.png to check the plan.')
max_num_steps = 1000 # max number of nodes to be added to the tree max_steering_radius = 30 # pixels dest_reached_radius = 50 # pixels plan_2 = rrt.plan(start_state, dest_state_2, max_num_steps, max_steering_radius, dest_reached_radius, live_view=True) print('RRT planning complete. Saving image.') draw_plan(world, plan_2, bgr=(0, 0, 255), thickness=2, show_live=False, filename='rrt_result2_XinranLi.png') plan_1 = rrt.plan(start_state, dest_state_1, max_num_steps, max_steering_radius, dest_reached_radius, live_view=True) print('RRT planning complete. Saving image.') draw_plan(world, plan_1, bgr=(0, 0, 255), thickness=2, show_live=False,
# to visit it then update its priority in the queue, and also its # distance to come and its parent if (ns not in Q) or (alternative_dist_to_come_to_ns < dist_to_come[ns.x, ns.y]): Q[ns] = alternative_dist_to_come_to_ns + self.euclidean_dist( ns, dest_state) dist_to_come[ns.x, ns.y] = alternative_dist_to_come_to_ns parents[ns] = s return [start_state] if __name__ == "__main__": if len(sys.argv) < 2: print "Usage: astar_planner.py occupancy_grid.pkl" sys.exit(1) pkl_file = open(sys.argv[1], 'rb') # world is a numpy array with dimensions (rows, cols, 3 color channels) world = pickle.load(pkl_file) pkl_file.close() astar = AStarPlanner(world) start_state = State(10, 10) dest_state = State(500, 100) plan, visited = astar.plan(start_state, dest_state) draw_plan(world, plan) draw_visited(world, visited)