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
