mirror of
https://github.com/zhm-real/PathPlanning.git
synced 2026-08-30 00:50:46 +08:00
zhm-real
86 lines
2.4 KiB
Python
86 lines
2.4 KiB
Python
import matplotlib.pyplot as plt
|
|
import env
|
|
|
|
|
|
class Plotting:
|
|
def __init__(self, xI, xG):
|
|
self.xI, self.xG = xI, xG
|
|
self.env = env.Env()
|
|
self.obs = self.env.obs_map()
|
|
|
|
def animation(self, path, visited, name):
|
|
self.plot_grid(name)
|
|
self.plot_visited(visited)
|
|
self.plot_path(path)
|
|
plt.show()
|
|
|
|
def plot_grid(self, name):
|
|
obs_x = [self.obs[i][0] for i in range(len(self.obs))]
|
|
obs_y = [self.obs[i][1] for i in range(len(self.obs))]
|
|
|
|
plt.plot(self.xI[0], self.xI[1], "bs")
|
|
plt.plot(self.xG[0], self.xG[1], "gs")
|
|
plt.plot(obs_x, obs_y, "sk")
|
|
plt.title(name)
|
|
plt.axis("equal")
|
|
|
|
def plot_visited(self, visited, cl='gray'):
|
|
if self.xI in visited:
|
|
visited.remove(self.xI)
|
|
|
|
if self.xG in visited:
|
|
visited.remove(self.xG)
|
|
|
|
count = 0
|
|
|
|
for x in visited:
|
|
count += 1
|
|
plt.plot(x[0], x[1], linewidth='3', color=cl, marker='o')
|
|
plt.gcf().canvas.mpl_connect('key_release_event',
|
|
lambda event: [exit(0) if event.key == 'escape' else None])
|
|
|
|
if count < len(visited) / 3:
|
|
length = 15
|
|
elif count < len(visited) * 2 / 3:
|
|
length = 25
|
|
else:
|
|
length = 35
|
|
|
|
if count % length == 0:
|
|
plt.pause(0.001)
|
|
plt.pause(0.01)
|
|
|
|
def plot_path(self, path, cl='r', flag=False):
|
|
if self.xI in path:
|
|
path.remove(self.xI)
|
|
|
|
if self.xG in path:
|
|
path.remove(self.xG)
|
|
|
|
path_x = [path[i][0] for i in range(len(path))]
|
|
path_y = [path[i][1] for i in range(len(path))]
|
|
|
|
if not flag:
|
|
plt.plot(path_x, path_y, linewidth='3', color='r', marker='o')
|
|
else:
|
|
plt.plot(path_x, path_y, linewidth='3', color=cl, marker='o')
|
|
|
|
plt.pause(0.01)
|
|
|
|
def animation_ara_star(self, path, visited, name):
|
|
self.plot_grid(name)
|
|
cl_v, cl_p = self.color_list()
|
|
|
|
for k in range(len(path)):
|
|
self.plot_visited(visited[k], cl_v[k])
|
|
self.plot_path(path[k], cl_p[k], True)
|
|
plt.pause(0.5)
|
|
|
|
plt.show()
|
|
|
|
@staticmethod
|
|
def color_list():
|
|
cl_v = ['silver', 'wheat', 'lightskyblue', 'plum', 'slategray']
|
|
cl_p = ['gray', 'orange', 'deepskyblue', 'red', 'm']
|
|
return cl_v, cl_p
|