def debug_plot(env, bounds, vertices, parents, start_pose, end_region): ax = plot_environment(env, bounds=bounds) plot_poly(ax, Point(start_pose).buffer(radius, resolution=3), 'magenta') plot_poly(ax, end_region, 'brown', alpha=0.2) for v in vertices: if v in parents: plot_line(ax, LineString([parents[v], v])) plt.show()
def draw_results(algo_name, path, V, E, env, bounds, object_radius, resolution, start_pose, goal_region, elapsed_time): """ Plots the path from start node to goal region as well as the graph (or tree) searched with the Sampling Based Algorithms. Args: algo_name (str): The name of the algorithm used (used as title of the plot) path (list<(float,float), (float,float)>): The sequence of coordinates traveled to reach goal from start node V (set<(float, float)>): All nodes in the explored graph/tree E (set<(float,float), (float, float)>): The set of all edges considered in the graph/tree env (yaml environment): 2D yaml environment for the path planning to take place bounds (int, int int int): min x, min y, max x, max y of the coordinates in the environment. object_radius (float): radius of our object. resolution (int): Number of segments used to approximate a quarter circle around a point. start_pose(float,float): Coordinates of initial point of the path. goal_region (Polygon): A polygon object representing the end goal. elapsed_time (float): Time it took for the algorithm to run Return: None Action: Plots a path using the environment module. """ originalPath,pruningPath=path graph_size = len(V) path_size = len(originalPath) # Calculate path length path_length1 = 0.0 path_length2 = 0.0 for i in range(len(originalPath)-1): path_length1 += euclidian_dist(originalPath[i], originalPath[i+1]) for i in range(len(pruningPath)-1): path_length2 += euclidian_dist(pruningPath[i], pruningPath[i+1]) # Create title with descriptive information based on environment, path length, and elapsed_time title = algo_name + "\n" + str(graph_size) + " Nodes. " + str(len(env.obstacles)) + " Obstacles. Path Size: " + str(path_size) + "\n Path Length: " + str([path_length1,path_length2]) + "\n Runtime(s)= " + str(elapsed_time) # Plot environment env_plot = plot_environment(env, bounds) # Add title env_plot.set_title(title) # Plot goal plot_poly(env_plot, goal_region, 'green') # Plot start buffered_start_vertex = Point(start_pose).buffer(object_radius, resolution) plot_poly(env_plot, buffered_start_vertex, 'red') # Plot Edges explored by ploting lines between each edge for edge in E: line = LineString([edge[0], edge[1]]) plot_line(env_plot, line) # Plot path plot_path(env_plot, originalPath, object_radius,'black') plot_path(env_plot, pruningPath, object_radius,'red')
def plot_tree(self, ax, rrt): """ Plot RRT tree Args: ax: a Plot axis object on which to plot the tree. rrt: an initialized RRTstar object. """ for node in rrt.node_list: if node.parent is not None: plot_line(ax, LineString([[node.x, node.y], [node.parent.x, node.parent.y]]), "orange")
def plot_path(self, ax, path, id, color): """ Plot a path and label by id on a given axis object. Args: ax: a Plot axis object, previously plotted with environment. path: a Path object id: a unique identifier corresponding to the agent to which the path belongs. """ if path.nodes: plot_poly(ax, Point( (path.nodes[0].x, path.nodes[0].y)).buffer(0.3, resolution=3), "green") plot_poly(ax, Point( (path.goal_node.x, path.goal_node.y)).buffer(0.3, resolution=3), "red") if path is not None: path_xy = [[node.x, node.y] for node in path.nodes] line = LineString(path_xy) plot_line(ax, line, color)
def draw_results(algo_name, path, V, E, env, bounds, object_radius, resolution, start_pose, goal_region, elapsed_time): graph_size = len(V) path_size = len(path) path_length = 0.0 for i in xrange(len(path) - 1): path_length += euclidian_dist(path[i], path[i + 1]) title = algo_name + "\n" + str(graph_size) + " Nodes. " + str( len(env.obstacles)) + " Obstacles. Path Size: " + str( path_size) + "\n Path Length: " + str( path_length) + "\n Runtime(s)= " + str(elapsed_time) env_plot = plot_environment(env, bounds) env_plot.set_title(title) plot_poly(env_plot, goal_region, 'green') buffered_start_vertex = Point(start_pose).buffer(object_radius, resolution) plot_poly(env_plot, buffered_start_vertex, 'red') for edge in E: line = LineString([edge[0], edge[1]]) plot_line(env_plot, line) plot_path(env_plot, path, object_radius)