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)