saberbud/PathPlanning
1
0
Fork 0
mirror of https://github.com/zhm-real/PathPlanning.git synced 2026-08-31 09:30:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

update RRT*

Browse Source
This commit is contained in:
zhm-real
2020-06-23 18:46:44 -07:00
parent a22f3ef98a
commit dfee5555c5
9 changed files with 301 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Sampling-based Planning/plotting.py
+32 -15
View File
@@ -8,33 +8,44 @@ class Plotting:
self.xI, self.xG = xI, xG
self.env = env.Env()
self.obs_bound = self.env.obs_boundary
self.obs_circle = self.env.obs
self.obs_circle = self.env.obs_circle
self.obs_rectangle = self.env.obs_rectangle
def animation(self, nodelist, path):
def animation(self, nodelist, path, animation=False):
if path is None:
print("No path found!")
return
self.plot_visited(nodelist)
self.plot_grid("RRT")
self.plot_visited(nodelist, animation)
self.plot_path(path)
def plot_grid(self, name):
fig, ax = plt.subplots()
for x in self.obs_bound:
for (ox, oy, w, h) in self.obs_bound:
ax.add_patch(
patches.Rectangle(
(x[0], x[1]), x[2], x[3],
(ox, oy), w, h,
edgecolor='black',
facecolor='black',
fill=True
)
)
for x in self.obs_circle:
for (ox, oy, w, h) in self.obs_rectangle:
ax.add_patch(
patches.Rectangle(
(ox, oy), w, h,
edgecolor='black',
facecolor='gray',
fill=True
)
)
for (ox, oy, r) in self.obs_circle:
ax.add_patch(
patches.Circle(
(x[0], x[1]), x[2],
(ox, oy), r,
edgecolor='black',
facecolor='gray',
fill=True
@@ -47,13 +58,19 @@ class Plotting:
plt.axis("equal")
@staticmethod
def plot_visited(nodelist):
for node in nodelist:
if node.parent:
plt.plot(node.path_x, node.path_y, "-g")
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.pause(0.001)
def plot_visited(nodelist, animation):
if animation:
for node in nodelist:
if node.parent:
plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g")
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.pause(0.001)
else:
for node in nodelist:
if node.parent:
plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g")
@staticmethod
def plot_path(path):