def plot_rrt(env, bounds, path, vertices, parents, radius, start_pose, end_region, time): 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) # plot the full tree for v in vertices: if v in parents: x, y = LineString([parents[v], v]).xy ax.plot(x, y, color='blue', linewidth=1, solid_capstyle='round', zorder=1) # plot the final path for i in range(1, len(path)): x, y = LineString([path[i - 1], path[i]]).xy ax.plot(x, y, color='green', linewidth=3, solid_capstyle='round', zorder=1) # tree nodes, time, num obs, path length (nodes) pathLength = getPathLength(path) plt.title( 'Nodes: %d Time: %.3f sec Obstacles: %d Path Length: %.3f (%d nodes)' % (len(vertices), time, len(env.obstacles), pathLength, len(path))) plt.show()
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_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)
def plotGoalPath(path, radius, ax, end_region): ''' path: list of (x,y)-tuples ax: matplotlib.axes object Purpose: is meant to plot RRT tree on drone ''' plot_poly(ax, end_region, 'green', alpha=0.3) plot_poly(ax, Point(path[0]).buffer(radius / 2, resolution=3), 'blue', alpha=0.3) for i in range(0, len(path) - 1): line = LineString([path[i], path[i + 1]]) # plot_line_mine(ax, line) expanded_line = line.buffer(radius, resolution=3) plot_poly(ax, expanded_line, 'green', alpha=0.025)
def plot_current_pos(self, ax, pos, id): """ Plot the x-y position of an agent with id `id`. """ plot_poly(ax, Point(pos[0], pos[1]).buffer(0.2, resolution=3), "blue")
def rrt(bounds, environment, start_pose, radius, end_region): ''' - bounds: (minx, miny, maxx, maxy) tuple over region - environment: instance of the Environment class that describes the placement of the obstacles in the scene - start_pose: start_pose = (x,y) is a tuple indicating the starting position of the robot - radius: radius is the radius of the robot (used for collision checking) - end_region: end_region is a shapely Polygon that describes the region that the robot needs to reach ''' # Adding tuples to nodes -> represent all nodes expanded by tree nodes = [start_pose] # Creating graph of SearchNodes from the tuples to represent the tree with parents. Used to order and graph = Graph() graph.add_node(SearchNode(start_pose)) goalPath = Path( SearchNode(start_pose)) # for initialization in case of no path found # Draw the environment (with its obstacles) and with the start node + end region if plotting: ax = plot_environment(environment, bounds) plot_poly(ax, Point(start_pose).buffer(radius, resolution=3), 'blue') plot_poly(ax, end_region, 'red', alpha=0.2) for i in range(10000): # random high number # this range must be evaluated according to size of problem. RRT do not ensure any solution paths # sample a random node inside bounds (random number between xmin/max and ymin/max). all bound-"checking" is donw here: makes sure we don't have to do ant checks later rand_xval = random.uniform(bounds[0], bounds[2]) rand_yval = random.uniform(bounds[1], bounds[3]) node_rand = (rand_xval, rand_yval) ''' --- For every x'th iteration - aim towards goal. A good choice varies, and depends on the step length that I decided in steerpath(). Not having this makes total number of nodes blow up. I have kept choices of when to goal bias and the travel distance in steering function to perform well in large environments. TODO: This number depends on complexity of space. Easy space: lower number --- Then, find out which node in our list that is the nearest to the random node. Returns a searchNode and a float distance. --- Steer towards the new node -> correct parents and add cost --- Check if the new steered node is in bounds. --- If not visited before (avoid cycle) and no obstacles are in the path: add to tree ''' sampling_rate = 12 if not (i % sampling_rate): node_rand = end_region.centroid.coords[0] node_nearest, node_dist = nearestSNode(graph, node_rand) ## TODO: this steering function should somehow consider dynamics steered_node = steerPath(node_nearest.state, node_rand) if not (bounds[0] < steered_node[0] < bounds[2]) or not (bounds[1] < steered_node[1] < bounds[3]): continue # sometimes not checking for this made the path go out of bounds node_steered = SearchNode(steered_node, node_nearest, node_nearest.cost + node_dist) if node_steered.state not in nodes: if not obstacleIsInPath(node_nearest.state, node_steered.state, environment, radius): # Keep track of total number of nodes in tree # Add edge (also adds new nodes to graph) and weight # Plot non-colliding edge to show all searched nodes nodes.append(node_steered.state) graph.add_edge(node_nearest, node_steered, node_dist) if plotting: line = LineString([node_nearest.state, node_steered.state]) plot_line_mine(ax, line) else: continue # Avoid goal check if collision is found # Check last addition for goal state # TODO: ADDED EXTRA SAFETY IN GOAL CHECK if goalReached(node_steered.state, 1.5 * radius, end_region): goalPath = Path(node_steered) break # break the while loop when solution is found! # No. of nodes in total - No. of nodes in sol path - Sol path length noOfTotalNodes = len(nodes) noOfNodesInSol = len(goalPath.path) pathLength = goalPath.cost if plotting: for i in range(noOfNodesInSol - 1): # Draw goal path line = LineString([goalPath.path[i], goalPath.path[i + 1]]) plot_line_mine(ax, line) # Draw the expanded line expanded_line = line.buffer(radius, resolution=3) plot_poly(ax, expanded_line, 'green', alpha=0.2) # plotting last node in goalPath and setting title to format in task plot_poly(ax, Point(goalPath.path[-1]).buffer(radius, resolution=3), 'blue') titleString = "Nodes total / in solution path: %s/ %s \nPath length: %0.3f" ax.set_title(titleString % (noOfTotalNodes, noOfNodesInSol, pathLength)) return goalPath.path