diff --git a/Search_based_Planning/Search_2D/D_star.py b/Search_based_Planning/Search_2D/D_star.py index a413c4f..300059b 100644 --- a/Search_based_Planning/Search_2D/D_star.py +++ b/Search_based_Planning/Search_2D/D_star.py @@ -50,10 +50,12 @@ class DStar: def run(self, s_start, s_end): self.init() self.insert(s_end, 0) + while True: self.process_state() if self.t[s_start] == 'CLOSED': break + self.path = self.extract_path(s_start, s_end) self.Plot.plot_grid("Dynamic A* (D*)") self.plot_path(self.path) @@ -66,22 +68,28 @@ class DStar: print("Please choose right area!") else: x, y = int(x), int(y) - print("Add obstacle at: s =", x, ",", "y =", y) - self.obs.add((x, y)) - plt.plot(x, y, 'sk') - s = self.s_start - self.visited = set() - self.count += 1 + if (x, y) not in self.obs: + print("Add obstacle at: s =", x, ",", "y =", y) + self.obs.add((x, y)) + self.Plot.update_obs(self.obs) - while s != self.s_goal: - if self.is_collision(s, self.PARENT[s]): - self.modify(s) - continue - s = self.PARENT[s] + s = self.s_start + self.visited = set() + self.count += 1 + + while s != self.s_goal: + if self.is_collision(s, self.PARENT[s]): + self.modify(s) + continue + s = self.PARENT[s] + + self.path = self.extract_path(self.s_start, self.s_goal) + + plt.cla() + self.Plot.plot_grid("Dynamic A* (D*)") + self.plot_visited(self.visited) + self.plot_path(self.path) - self.path = self.extract_path(self.s_start, self.s_goal) - self.plot_visited(self.visited) - self.plot_path(self.path) self.fig.canvas.draw_idle() def extract_path(self, s_start, s_end): @@ -94,42 +102,63 @@ class DStar: return path def process_state(self): - s = self.min_state() + s = self.min_state() # get node in OPEN set with min k value self.visited.add(s) if s is None: - return -1 + return -1 # OPEN set is empty - k_old = self.get_k_min() - self.delete(s) + k_old = self.get_k_min() # record the min k value of this iteration (min path cost) + self.delete(s) # move state s from OPEN set to CLOSED set + # k_min < h[s] --> s: RAISE state (increased cost) if k_old < self.h[s]: for s_n in self.get_neighbor(s): - if self.h[s_n] <= k_old and self.h[s] > self.h[s_n] + self.cost(s_n, s): + if self.h[s_n] <= k_old and \ + self.h[s] > self.h[s_n] + self.cost(s_n, s): + + # update h_value and choose parent self.PARENT[s] = s_n self.h[s] = self.h[s_n] + self.cost(s_n, s) + + # s: k_min >= h[s] -- > s: LOWER state (cost reductions) if k_old == self.h[s]: for s_n in self.get_neighbor(s): if self.t[s_n] == 'NEW' or \ (self.PARENT[s_n] == s and self.h[s_n] != self.h[s] + self.cost(s, s_n)) or \ (self.PARENT[s_n] != s and self.h[s_n] > self.h[s] + self.cost(s, s_n)): + + # Condition: + # 1) t[s_n] == 'NEW': not visited + # 2) s_n's parent: cost reduction + # 3) s_n find a better parent self.PARENT[s_n] = s self.insert(s_n, self.h[s] + self.cost(s, s_n)) else: for s_n in self.get_neighbor(s): if self.t[s_n] == 'NEW' or \ (self.PARENT[s_n] == s and self.h[s_n] != self.h[s] + self.cost(s, s_n)): + + # Condition: + # 1) t[s_n] == 'NEW': not visited + # 2) s_n's parent: cost reduction self.PARENT[s_n] = s self.insert(s_n, self.h[s] + self.cost(s, s_n)) else: - if self.PARENT[s_n] != s and self.h[s_n] > self.h[s] + self.cost(s, s_n): + if self.PARENT[s_n] != s and \ + self.h[s_n] > self.h[s] + self.cost(s, s_n): + + # Condition: LOWER happened in OPEN set (s), s should be explored again self.insert(s, self.h[s]) else: if self.PARENT[s_n] != s and \ self.h[s] > self.h[s_n] + self.cost(s_n, s) and \ self.t[s_n] == 'CLOSED' and \ self.h[s_n] > k_old: + + # Condition: LOWER happened in CLOSED set (s_n), s_n should be explored again self.insert(s_n, self.h[s_n]) + return self.get_k_min() def min_state(self): @@ -155,6 +184,12 @@ class DStar: return min([self.k[x] for x in self.OPEN]) def insert(self, s, h_new): + """ + insert node into OPEN set. + :param s: node + :param h_new: new or better cost to come value + """ + if self.t[s] == 'NEW': self.k[s] = h_new elif self.t[s] == 'OPEN': @@ -178,13 +213,23 @@ class DStar: self.OPEN.remove(s) def modify(self, s): + """ + start processing from state s. + :param s: is a node whose status is RAISE or LOWER. + """ + self.modify_cost(s) + while True: k_min = self.process_state() + if k_min >= self.h[s]: break def modify_cost(self, s): + # if node in CLOSED set, put it into OPEN set. + # Since cost may be changed between s - s.parent, calc cost(s, s.p) again + if self.t[s] == 'CLOSED': self.insert(s, self.h[self.PARENT[s]] + self.cost(s, self.PARENT[s])) diff --git a/Search_based_Planning/Search_2D/__pycache__/env.cpython-37.pyc b/Search_based_Planning/Search_2D/__pycache__/env.cpython-37.pyc index b0d7aae..945aa4d 100644 Binary files a/Search_based_Planning/Search_2D/__pycache__/env.cpython-37.pyc and b/Search_based_Planning/Search_2D/__pycache__/env.cpython-37.pyc differ diff --git a/Search_based_Planning/Search_2D/__pycache__/plotting.cpython-37.pyc b/Search_based_Planning/Search_2D/__pycache__/plotting.cpython-37.pyc index 2fdee55..8a41db2 100644 Binary files a/Search_based_Planning/Search_2D/__pycache__/plotting.cpython-37.pyc and b/Search_based_Planning/Search_2D/__pycache__/plotting.cpython-37.pyc differ diff --git a/Search_based_Planning/Search_2D/env.py b/Search_based_Planning/Search_2D/env.py index 0cd2424..753a8c8 100644 --- a/Search_based_Planning/Search_2D/env.py +++ b/Search_based_Planning/Search_2D/env.py @@ -12,6 +12,9 @@ class Env: (1, 0), (1, -1), (0, -1), (-1, -1)] self.obs = self.obs_map() + def update_obs(self, obs): + self.obs = obs + def obs_map(self): """ Initialize obstacles' positions diff --git a/Search_based_Planning/Search_2D/plotting.py b/Search_based_Planning/Search_2D/plotting.py index f952876..1cf98a3 100644 --- a/Search_based_Planning/Search_2D/plotting.py +++ b/Search_based_Planning/Search_2D/plotting.py @@ -19,6 +19,9 @@ class Plotting: self.env = env.Env() self.obs = self.env.obs_map() + def update_obs(self, obs): + self.obs = obs + def animation(self, path, visited, name): self.plot_grid(name) self.plot_visited(visited)