diff --git a/Search_based_Planning/Search_2D/Anytime_D_star.py b/Search_based_Planning/Search_2D/Anytime_D_star.py index 76b3944..cd1d62b 100644 --- a/Search_based_Planning/Search_2D/Anytime_D_star.py +++ b/Search_based_Planning/Search_2D/Anytime_D_star.py @@ -45,7 +45,7 @@ class ADStar: self.count_env_change = 0 self.obs_add = set() self.obs_remove = set() - self.title = "Anytime D*: Small changes" # significant changes + self.title = "Anytime D*: Small changes" # Significant changes self.fig = plt.figure() def run(self): @@ -85,17 +85,20 @@ class ADStar: if self.title == "Anytime D*: Small changes": if (x, y) not in self.obs: self.obs.add((x, y)) - plt.plot(x, y, 'sk') self.g[(x, y)] = float("inf") self.rhs[(x, y)] = float("inf") else: self.obs.remove((x, y)) - plt.plot(x, y, marker='s', color='white') self.UpdateState((x, y)) + self.Plot.update_obs(self.obs) + for sn in self.get_neighbor((x, y)): self.UpdateState(sn) + plt.cla() + self.Plot.plot_grid(self.title) + while True: if len(self.INCONS) == 0: break @@ -126,6 +129,8 @@ class ADStar: if (x, y) in self.obs_add: self.obs_add.remove((x, y)) + self.Plot.update_obs(self.obs) + if self.count_env_change >= 15: self.count_env_change = 0 self.eps += 2.0 @@ -141,6 +146,9 @@ class ADStar: self.UpdateState(sn) self.UpdateState(s) + plt.cla() + self.Plot.plot_grid(self.title) + while True: if self.eps <= 1.0: break diff --git a/Search_based_Planning/gif/ADstar_sig.gif b/Search_based_Planning/gif/ADstar_sig.gif index 8f40972..339dfbf 100644 Binary files a/Search_based_Planning/gif/ADstar_sig.gif and b/Search_based_Planning/gif/ADstar_sig.gif differ diff --git a/Search_based_Planning/gif/ADstar_small.gif b/Search_based_Planning/gif/ADstar_small.gif index 8dc559c..106ce17 100644 Binary files a/Search_based_Planning/gif/ADstar_small.gif and b/Search_based_Planning/gif/ADstar_small.gif differ