diff --git a/doc/实践资源-软件开发项目的团队自评报告模板.xlsx b/doc/实践资源-软件开发项目的团队自评报告模板.xlsx index cdbe869..61204cc 100644 Binary files a/doc/实践资源-软件开发项目的团队自评报告模板.xlsx and b/doc/实践资源-软件开发项目的团队自评报告模板.xlsx differ diff --git a/doc/宣传彩页.png b/doc/宣传彩页.png new file mode 100644 index 0000000..c2c0a15 Binary files /dev/null and b/doc/宣传彩页.png differ diff --git a/model/功能描述用例图.eddx b/model/功能描述用例图.eddx new file mode 100644 index 0000000..718589c Binary files /dev/null and b/model/功能描述用例图.eddx differ diff --git a/model/软件部署图模型.svg b/model/软件部署图模型.svg new file mode 100644 index 0000000..d9af9ee --- /dev/null +++ b/model/软件部署图模型.svg @@ -0,0 +1 @@ +<<OS>>Windows平台<<artifact>>PathPlanningApp.apk<<device>>前端电脑<<device>>后端电脑<<artifact>>TelloControlPerceive<<OS>>Windows<<DBMS>>MySQL<<UAS>>Pytello<<device>>无人机Tello无人机 \ No newline at end of file diff --git a/src/Search_2D/ARAstar.py b/src/Search_2D/ARAstar.py deleted file mode 100644 index c014616..0000000 --- a/src/Search_2D/ARAstar.py +++ /dev/null @@ -1,222 +0,0 @@ -""" -ARA_star 2D (Anytime Repairing A*) -@author: huiming zhou - -@description: local inconsistency: g-value decreased. -g(s) decreased introduces a local inconsistency between s and its successors. - -""" - -import os -import sys -import math - -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Search_based_Planning/") - -from Search_2D import plotting, env - - -class AraStar: - def __init__(self, s_start, s_goal, e, heuristic_type): - self.s_start, self.s_goal = s_start, s_goal - self.heuristic_type = heuristic_type - - self.Env = env.Env() # class Env - - self.u_set = self.Env.motions # feasible input set - self.obs = self.Env.obs # position of obstacles - self.e = e # weight - - self.g = dict() # Cost to come - self.OPEN = dict() # priority queue / OPEN set - self.CLOSED = set() # CLOSED set - self.INCONS = {} # INCONSISTENT set - self.PARENT = dict() # relations - self.path = [] # planning path - self.visited = [] # order of visited nodes - - def init(self): - """ - initialize each set. - """ - - self.g[self.s_start] = 0.0 - self.g[self.s_goal] = math.inf - self.OPEN[self.s_start] = self.f_value(self.s_start) - self.PARENT[self.s_start] = self.s_start - - def searching(self): - self.init() - self.ImprovePath() - self.path.append(self.extract_path()) - - while self.update_e() > 1: # continue condition - self.e -= 0.4 # increase weight - self.OPEN.update(self.INCONS) - self.OPEN = {s: self.f_value(s) for s in self.OPEN} # update f_value of OPEN set - - self.INCONS = dict() - self.CLOSED = set() - self.ImprovePath() # improve path - self.path.append(self.extract_path()) - - return self.path, self.visited - - def ImprovePath(self): - """ - :return: a e'-suboptimal path - """ - - visited_each = [] - - while True: - s, f_small = self.calc_smallest_f() - - if self.f_value(self.s_goal) <= f_small: - break - - self.OPEN.pop(s) - self.CLOSED.add(s) - - for s_n in self.get_neighbor(s): - if s_n in self.obs: - continue - - new_cost = self.g[s] + self.cost(s, s_n) - - if s_n not in self.g or new_cost < self.g[s_n]: - self.g[s_n] = new_cost - self.PARENT[s_n] = s - visited_each.append(s_n) - - if s_n not in self.CLOSED: - self.OPEN[s_n] = self.f_value(s_n) - else: - self.INCONS[s_n] = 0.0 - - self.visited.append(visited_each) - - def calc_smallest_f(self): - """ - :return: node with smallest f_value in OPEN set. - """ - - s_small = min(self.OPEN, key=self.OPEN.get) - - return s_small, self.OPEN[s_small] - - def get_neighbor(self, s): - """ - find neighbors of state s that not in obstacles. - :param s: state - :return: neighbors - """ - - return {(s[0] + u[0], s[1] + u[1]) for u in self.u_set} - - def update_e(self): - v = float("inf") - - if self.OPEN: - v = min(self.g[s] + self.h(s) for s in self.OPEN) - if self.INCONS: - v = min(v, min(self.g[s] + self.h(s) for s in self.INCONS)) - - return min(self.e, self.g[self.s_goal] / v) - - def f_value(self, x): - """ - f = g + e * h - f = cost-to-come + weight * cost-to-go - :param x: current state - :return: f_value - """ - - return self.g[x] + self.e * self.h(x) - - def extract_path(self): - """ - Extract the path based on the PARENT set. - :return: The planning path - """ - - path = [self.s_goal] - s = self.s_goal - - while True: - s = self.PARENT[s] - path.append(s) - - if s == self.s_start: - break - - return list(path) - - def h(self, s): - """ - Calculate heuristic. - :param s: current node (state) - :return: heuristic function value - """ - - heuristic_type = self.heuristic_type # heuristic type - goal = self.s_goal # goal node - - if heuristic_type == "manhattan": - return abs(goal[0] - s[0]) + abs(goal[1] - s[1]) - else: - return math.hypot(goal[0] - s[0], goal[1] - s[1]) - - def cost(self, s_start, s_goal): - """ - Calculate Cost for this motion - :param s_start: starting node - :param s_goal: end node - :return: Cost for this motion - :note: Cost function could be more complicate! - """ - - if self.is_collision(s_start, s_goal): - return math.inf - - return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1]) - - def is_collision(self, s_start, s_end): - """ - check if the line segment (s_start, s_end) is collision. - :param s_start: start node - :param s_end: end node - :return: True: is collision / False: not collision - """ - - if s_start in self.obs or s_end in self.obs: - return True - - if s_start[0] != s_end[0] and s_start[1] != s_end[1]: - if s_end[0] - s_start[0] == s_start[1] - s_end[1]: - s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1])) - s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1])) - else: - s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1])) - s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1])) - - if s1 in self.obs or s2 in self.obs: - return True - - return False - - -def main(): - s_start = (5, 5) - s_goal = (45, 25) - - arastar = AraStar(s_start, s_goal, 2.5, "euclidean") - plot = plotting.Plotting(s_start, s_goal) - - path, visited = arastar.searching() - plot.animation_ara_star(path, visited, "Anytime Repairing A* (ARA*)") - - -if __name__ == '__main__': - main() diff --git a/src/Search_2D/Anytime_D_star.py b/src/Search_2D/Anytime_D_star.py deleted file mode 100644 index cd1d62b..0000000 --- a/src/Search_2D/Anytime_D_star.py +++ /dev/null @@ -1,317 +0,0 @@ -""" -Anytime_D_star 2D -@author: huiming zhou -""" - -import os -import sys -import math -import matplotlib.pyplot as plt - -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Search_based_Planning/") - -from Search_2D import plotting -from Search_2D import env - - -class ADStar: - def __init__(self, s_start, s_goal, eps, heuristic_type): - self.s_start, self.s_goal = s_start, s_goal - self.heuristic_type = heuristic_type - - self.Env = env.Env() # class Env - self.Plot = plotting.Plotting(s_start, s_goal) - - self.u_set = self.Env.motions # feasible input set - self.obs = self.Env.obs # position of obstacles - self.x = self.Env.x_range - self.y = self.Env.y_range - - self.g, self.rhs, self.OPEN = {}, {}, {} - - for i in range(1, self.Env.x_range - 1): - for j in range(1, self.Env.y_range - 1): - self.rhs[(i, j)] = float("inf") - self.g[(i, j)] = float("inf") - - self.rhs[self.s_goal] = 0.0 - self.eps = eps - self.OPEN[self.s_goal] = self.Key(self.s_goal) - self.CLOSED, self.INCONS = set(), dict() - - self.visited = set() - self.count = 0 - self.count_env_change = 0 - self.obs_add = set() - self.obs_remove = set() - self.title = "Anytime D*: Small changes" # Significant changes - self.fig = plt.figure() - - def run(self): - self.Plot.plot_grid(self.title) - self.ComputeOrImprovePath() - self.plot_visited() - self.plot_path(self.extract_path()) - self.visited = set() - - while True: - if self.eps <= 1.0: - break - self.eps -= 0.5 - self.OPEN.update(self.INCONS) - for s in self.OPEN: - self.OPEN[s] = self.Key(s) - self.CLOSED = set() - self.ComputeOrImprovePath() - self.plot_visited() - self.plot_path(self.extract_path()) - self.visited = set() - plt.pause(0.5) - - self.fig.canvas.mpl_connect('button_press_event', self.on_press) - plt.show() - - def on_press(self, event): - x, y = event.xdata, event.ydata - if x < 0 or x > self.x - 1 or y < 0 or y > self.y - 1: - print("Please choose right area!") - else: - self.count_env_change += 1 - x, y = int(x), int(y) - print("Change position: s =", x, ",", "y =", y) - - # for small changes - if self.title == "Anytime D*: Small changes": - if (x, y) not in self.obs: - self.obs.add((x, y)) - self.g[(x, y)] = float("inf") - self.rhs[(x, y)] = float("inf") - else: - self.obs.remove((x, y)) - 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 - self.OPEN.update(self.INCONS) - for s in self.OPEN: - self.OPEN[s] = self.Key(s) - self.CLOSED = set() - self.ComputeOrImprovePath() - self.plot_visited() - self.plot_path(self.extract_path()) - # plt.plot(self.title) - self.visited = set() - - if self.eps <= 1.0: - break - - else: - if (x, y) not in self.obs: - self.obs.add((x, y)) - self.obs_add.add((x, y)) - plt.plot(x, y, 'sk') - if (x, y) in self.obs_remove: - self.obs_remove.remove((x, y)) - else: - self.obs.remove((x, y)) - self.obs_remove.add((x, y)) - plt.plot(x, y, marker='s', color='white') - 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 - for s in self.obs_add: - self.g[(x, y)] = float("inf") - self.rhs[(x, y)] = float("inf") - - for sn in self.get_neighbor(s): - self.UpdateState(sn) - - for s in self.obs_remove: - for sn in self.get_neighbor(s): - self.UpdateState(sn) - self.UpdateState(s) - - plt.cla() - self.Plot.plot_grid(self.title) - - while True: - if self.eps <= 1.0: - break - self.eps -= 0.5 - self.OPEN.update(self.INCONS) - for s in self.OPEN: - self.OPEN[s] = self.Key(s) - self.CLOSED = set() - self.ComputeOrImprovePath() - self.plot_visited() - self.plot_path(self.extract_path()) - plt.title(self.title) - self.visited = set() - plt.pause(0.5) - - self.fig.canvas.draw_idle() - - def ComputeOrImprovePath(self): - while True: - s, v = self.TopKey() - if v >= self.Key(self.s_start) and \ - self.rhs[self.s_start] == self.g[self.s_start]: - break - - self.OPEN.pop(s) - self.visited.add(s) - - if self.g[s] > self.rhs[s]: - self.g[s] = self.rhs[s] - self.CLOSED.add(s) - for sn in self.get_neighbor(s): - self.UpdateState(sn) - else: - self.g[s] = float("inf") - for sn in self.get_neighbor(s): - self.UpdateState(sn) - self.UpdateState(s) - - def UpdateState(self, s): - if s != self.s_goal: - self.rhs[s] = float("inf") - for x in self.get_neighbor(s): - self.rhs[s] = min(self.rhs[s], self.g[x] + self.cost(s, x)) - if s in self.OPEN: - self.OPEN.pop(s) - - if self.g[s] != self.rhs[s]: - if s not in self.CLOSED: - self.OPEN[s] = self.Key(s) - else: - self.INCONS[s] = 0 - - def Key(self, s): - if self.g[s] > self.rhs[s]: - return [self.rhs[s] + self.eps * self.h(self.s_start, s), self.rhs[s]] - else: - return [self.g[s] + self.h(self.s_start, s), self.g[s]] - - def TopKey(self): - """ - :return: return the min key and its value. - """ - - s = min(self.OPEN, key=self.OPEN.get) - return s, self.OPEN[s] - - def h(self, s_start, s_goal): - heuristic_type = self.heuristic_type # heuristic type - - if heuristic_type == "manhattan": - return abs(s_goal[0] - s_start[0]) + abs(s_goal[1] - s_start[1]) - else: - return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1]) - - def cost(self, s_start, s_goal): - """ - Calculate Cost for this motion - :param s_start: starting node - :param s_goal: end node - :return: Cost for this motion - :note: Cost function could be more complicate! - """ - - if self.is_collision(s_start, s_goal): - return float("inf") - - return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1]) - - def is_collision(self, s_start, s_end): - if s_start in self.obs or s_end in self.obs: - return True - - if s_start[0] != s_end[0] and s_start[1] != s_end[1]: - if s_end[0] - s_start[0] == s_start[1] - s_end[1]: - s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1])) - s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1])) - else: - s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1])) - s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1])) - - if s1 in self.obs or s2 in self.obs: - return True - - return False - - def get_neighbor(self, s): - nei_list = set() - for u in self.u_set: - s_next = tuple([s[i] + u[i] for i in range(2)]) - if s_next not in self.obs: - nei_list.add(s_next) - - return nei_list - - def extract_path(self): - """ - Extract the path based on the PARENT set. - :return: The planning path - """ - - path = [self.s_start] - s = self.s_start - - for k in range(100): - g_list = {} - for x in self.get_neighbor(s): - if not self.is_collision(s, x): - g_list[x] = self.g[x] - s = min(g_list, key=g_list.get) - path.append(s) - if s == self.s_goal: - break - - return list(path) - - def plot_path(self, path): - px = [x[0] for x in path] - py = [x[1] for x in path] - plt.plot(px, py, linewidth=2) - plt.plot(self.s_start[0], self.s_start[1], "bs") - plt.plot(self.s_goal[0], self.s_goal[1], "gs") - - def plot_visited(self): - self.count += 1 - - color = ['gainsboro', 'lightgray', 'silver', 'darkgray', - 'bisque', 'navajowhite', 'moccasin', 'wheat', - 'powderblue', 'skyblue', 'lightskyblue', 'cornflowerblue'] - - if self.count >= len(color) - 1: - self.count = 0 - - for x in self.visited: - plt.plot(x[0], x[1], marker='s', color=color[self.count]) - - -def main(): - s_start = (5, 5) - s_goal = (45, 25) - - dstar = ADStar(s_start, s_goal, 2.5, "euclidean") - dstar.run() - - -if __name__ == '__main__': - main() diff --git a/src/Search_2D/Best_First.py b/src/Search_2D/Best_First.py deleted file mode 100644 index 0c85fba..0000000 --- a/src/Search_2D/Best_First.py +++ /dev/null @@ -1,68 +0,0 @@ -""" -Best-First Searching -@author: huiming zhou -""" - -import os -import sys -import math -import heapq - -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Search_based_Planning/") - -from Search_2D import plotting, env -from Search_2D.Astar import AStar - - -class BestFirst(AStar): - """BestFirst set the heuristics as the priority - """ - def searching(self): - """ - Breadth-first Searching. - :return: path, visited order - """ - - self.PARENT[self.s_start] = self.s_start - self.g[self.s_start] = 0 - self.g[self.s_goal] = math.inf - heapq.heappush(self.OPEN, - (self.heuristic(self.s_start), self.s_start)) - - while self.OPEN: - _, s = heapq.heappop(self.OPEN) - self.CLOSED.append(s) - - if s == self.s_goal: - break - - for s_n in self.get_neighbor(s): - new_cost = self.g[s] + self.cost(s, s_n) - - if s_n not in self.g: - self.g[s_n] = math.inf - - if new_cost < self.g[s_n]: # conditions for updating Cost - self.g[s_n] = new_cost - self.PARENT[s_n] = s - - # best first set the heuristics as the priority - heapq.heappush(self.OPEN, (self.heuristic(s_n), s_n)) - - return self.extract_path(self.PARENT), self.CLOSED - - -def main(): - s_start = (5, 5) - s_goal = (45, 25) - - BF = BestFirst(s_start, s_goal, 'euclidean') - plot = plotting.Plotting(s_start, s_goal) - - path, visited = BF.searching() - plot.animation(path, visited, "Best-first Searching") # animation - - -if __name__ == '__main__': - main() diff --git a/src/Search_2D/Bidirectional_a_star.py b/src/Search_2D/Bidirectional_a_star.py deleted file mode 100644 index 3580c1a..0000000 --- a/src/Search_2D/Bidirectional_a_star.py +++ /dev/null @@ -1,229 +0,0 @@ -""" -Bidirectional_a_star 2D -@author: huiming zhou -""" - -import os -import sys -import math -import heapq - -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Search_based_Planning/") - -from Search_2D import plotting, env - - -class BidirectionalAStar: - def __init__(self, s_start, s_goal, heuristic_type): - self.s_start = s_start - self.s_goal = s_goal - self.heuristic_type = heuristic_type - - self.Env = env.Env() # class Env - - self.u_set = self.Env.motions # feasible input set - self.obs = self.Env.obs # position of obstacles - - self.OPEN_fore = [] # OPEN set for forward searching - self.OPEN_back = [] # OPEN set for backward searching - self.CLOSED_fore = [] # CLOSED set for forward - self.CLOSED_back = [] # CLOSED set for backward - self.PARENT_fore = dict() # recorded parent for forward - self.PARENT_back = dict() # recorded parent for backward - self.g_fore = dict() # cost to come for forward - self.g_back = dict() # cost to come for backward - - def init(self): - """ - initialize parameters - """ - - self.g_fore[self.s_start] = 0.0 - self.g_fore[self.s_goal] = math.inf - self.g_back[self.s_goal] = 0.0 - self.g_back[self.s_start] = math.inf - self.PARENT_fore[self.s_start] = self.s_start - self.PARENT_back[self.s_goal] = self.s_goal - heapq.heappush(self.OPEN_fore, - (self.f_value_fore(self.s_start), self.s_start)) - heapq.heappush(self.OPEN_back, - (self.f_value_back(self.s_goal), self.s_goal)) - - def searching(self): - """ - Bidirectional A* - :return: connected path, visited order of forward, visited order of backward - """ - - self.init() - s_meet = self.s_start - - while self.OPEN_fore and self.OPEN_back: - # solve foreward-search - _, s_fore = heapq.heappop(self.OPEN_fore) - - if s_fore in self.PARENT_back: - s_meet = s_fore - break - - self.CLOSED_fore.append(s_fore) - - for s_n in self.get_neighbor(s_fore): - new_cost = self.g_fore[s_fore] + self.cost(s_fore, s_n) - - if s_n not in self.g_fore: - self.g_fore[s_n] = math.inf - - if new_cost < self.g_fore[s_n]: - self.g_fore[s_n] = new_cost - self.PARENT_fore[s_n] = s_fore - heapq.heappush(self.OPEN_fore, - (self.f_value_fore(s_n), s_n)) - - # solve backward-search - _, s_back = heapq.heappop(self.OPEN_back) - - if s_back in self.PARENT_fore: - s_meet = s_back - break - - self.CLOSED_back.append(s_back) - - for s_n in self.get_neighbor(s_back): - new_cost = self.g_back[s_back] + self.cost(s_back, s_n) - - if s_n not in self.g_back: - self.g_back[s_n] = math.inf - - if new_cost < self.g_back[s_n]: - self.g_back[s_n] = new_cost - self.PARENT_back[s_n] = s_back - heapq.heappush(self.OPEN_back, - (self.f_value_back(s_n), s_n)) - - return self.extract_path(s_meet), self.CLOSED_fore, self.CLOSED_back - - def get_neighbor(self, s): - """ - find neighbors of state s that not in obstacles. - :param s: state - :return: neighbors - """ - - return [(s[0] + u[0], s[1] + u[1]) for u in self.u_set] - - def extract_path(self, s_meet): - """ - extract path from start and goal - :param s_meet: meet point of bi-direction a* - :return: path - """ - - # extract path for foreward part - path_fore = [s_meet] - s = s_meet - - while True: - s = self.PARENT_fore[s] - path_fore.append(s) - if s == self.s_start: - break - - # extract path for backward part - path_back = [] - s = s_meet - - while True: - s = self.PARENT_back[s] - path_back.append(s) - if s == self.s_goal: - break - - return list(reversed(path_fore)) + list(path_back) - - def f_value_fore(self, s): - """ - forward searching: f = g + h. (g: Cost to come, h: heuristic value) - :param s: current state - :return: f - """ - - return self.g_fore[s] + self.h(s, self.s_goal) - - def f_value_back(self, s): - """ - backward searching: f = g + h. (g: Cost to come, h: heuristic value) - :param s: current state - :return: f - """ - - return self.g_back[s] + self.h(s, self.s_start) - - def h(self, s, goal): - """ - Calculate heuristic value. - :param s: current node (state) - :param goal: goal node (state) - :return: heuristic value - """ - - heuristic_type = self.heuristic_type - - if heuristic_type == "manhattan": - return abs(goal[0] - s[0]) + abs(goal[1] - s[1]) - else: - return math.hypot(goal[0] - s[0], goal[1] - s[1]) - - def cost(self, s_start, s_goal): - """ - Calculate Cost for this motion - :param s_start: starting node - :param s_goal: end node - :return: Cost for this motion - :note: Cost function could be more complicate! - """ - - if self.is_collision(s_start, s_goal): - return math.inf - - return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1]) - - def is_collision(self, s_start, s_end): - """ - check if the line segment (s_start, s_end) is collision. - :param s_start: start node - :param s_end: end node - :return: True: is collision / False: not collision - """ - - if s_start in self.obs or s_end in self.obs: - return True - - if s_start[0] != s_end[0] and s_start[1] != s_end[1]: - if s_end[0] - s_start[0] == s_start[1] - s_end[1]: - s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1])) - s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1])) - else: - s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1])) - s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1])) - - if s1 in self.obs or s2 in self.obs: - return True - - return False - - -def main(): - x_start = (5, 5) - x_goal = (45, 25) - - bastar = BidirectionalAStar(x_start, x_goal, "euclidean") - plot = plotting.Plotting(x_start, x_goal) - - path, visited_fore, visited_back = bastar.searching() - plot.animation_bi_astar(path, visited_fore, visited_back, "Bidirectional-A*") # animation - - -if __name__ == '__main__': - main() diff --git a/src/Search_2D/D_star.py b/src/Search_2D/D_star.py deleted file mode 100644 index 60b6c7e..0000000 --- a/src/Search_2D/D_star.py +++ /dev/null @@ -1,304 +0,0 @@ -""" -D_star 2D -@author: huiming zhou -""" - -import os -import sys -import math -import matplotlib.pyplot as plt - -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Search_based_Planning/") - -from Search_2D import plotting, env - - -class DStar: - def __init__(self, s_start, s_goal): - self.s_start, self.s_goal = s_start, s_goal - - self.Env = env.Env() - self.Plot = plotting.Plotting(self.s_start, self.s_goal) - - self.u_set = self.Env.motions - self.obs = self.Env.obs - self.x = self.Env.x_range - self.y = self.Env.y_range - - self.fig = plt.figure() - - self.OPEN = set() - self.t = dict() - self.PARENT = dict() - self.h = dict() - self.k = dict() - self.path = [] - self.visited = set() - self.count = 0 - - def init(self): - for i in range(self.Env.x_range): - for j in range(self.Env.y_range): - self.t[(i, j)] = 'NEW' - self.k[(i, j)] = 0.0 - self.h[(i, j)] = float("inf") - self.PARENT[(i, j)] = None - - self.h[self.s_goal] = 0.0 - - 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) - self.fig.canvas.mpl_connect('button_press_event', self.on_press) - plt.show() - - def on_press(self, event): - x, y = event.xdata, event.ydata - if x < 0 or x > self.x - 1 or y < 0 or y > self.y - 1: - print("Please choose right area!") - else: - x, y = int(x), int(y) - 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) - - 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.fig.canvas.draw_idle() - - def extract_path(self, s_start, s_end): - path = [s_start] - s = s_start - while True: - s = self.PARENT[s] - path.append(s) - if s == s_end: - return path - - def process_state(self): - s = self.min_state() # get node in OPEN set with min k value - self.visited.add(s) - - if s is None: - return -1 # OPEN set is empty - - 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): - - # 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): - - # 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): - """ - choose the node with the minimum k value in OPEN set. - :return: state - """ - - if not self.OPEN: - return None - - return min(self.OPEN, key=lambda x: self.k[x]) - - def get_k_min(self): - """ - calc the min k value for nodes in OPEN set. - :return: k value - """ - - if not self.OPEN: - return -1 - - 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': - self.k[s] = min(self.k[s], h_new) - elif self.t[s] == 'CLOSED': - self.k[s] = min(self.h[s], h_new) - - self.h[s] = h_new - self.t[s] = 'OPEN' - self.OPEN.add(s) - - def delete(self, s): - """ - delete: move state s from OPEN set to CLOSED set. - :param s: state should be deleted - """ - - if self.t[s] == 'OPEN': - self.t[s] = 'CLOSED' - - 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])) - - def get_neighbor(self, s): - nei_list = set() - - for u in self.u_set: - s_next = tuple([s[i] + u[i] for i in range(2)]) - if s_next not in self.obs: - nei_list.add(s_next) - - return nei_list - - def cost(self, s_start, s_goal): - """ - Calculate Cost for this motion - :param s_start: starting node - :param s_goal: end node - :return: Cost for this motion - :note: Cost function could be more complicate! - """ - - if self.is_collision(s_start, s_goal): - return float("inf") - - return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1]) - - def is_collision(self, s_start, s_end): - if s_start in self.obs or s_end in self.obs: - return True - - if s_start[0] != s_end[0] and s_start[1] != s_end[1]: - if s_end[0] - s_start[0] == s_start[1] - s_end[1]: - s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1])) - s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1])) - else: - s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1])) - s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1])) - - if s1 in self.obs or s2 in self.obs: - return True - - return False - - def plot_path(self, path): - px = [x[0] for x in path] - py = [x[1] for x in path] - plt.plot(px, py, linewidth=2) - plt.plot(self.s_start[0], self.s_start[1], "bs") - plt.plot(self.s_goal[0], self.s_goal[1], "gs") - - def plot_visited(self, visited): - color = ['gainsboro', 'lightgray', 'silver', 'darkgray', - 'bisque', 'navajowhite', 'moccasin', 'wheat', - 'powderblue', 'skyblue', 'lightskyblue', 'cornflowerblue'] - - if self.count >= len(color) - 1: - self.count = 0 - - for x in visited: - plt.plot(x[0], x[1], marker='s', color=color[self.count]) - - -def main(): - s_start = (5, 5) - s_goal = (45, 25) - dstar = DStar(s_start, s_goal) - dstar.run(s_start, s_goal) - - -if __name__ == '__main__': - main() diff --git a/src/Search_2D/D_star_Lite.py b/src/Search_2D/D_star_Lite.py deleted file mode 100644 index 4996be2..0000000 --- a/src/Search_2D/D_star_Lite.py +++ /dev/null @@ -1,239 +0,0 @@ -""" -D_star_Lite 2D -@author: huiming zhou -""" - -import os -import sys -import math -import matplotlib.pyplot as plt - -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Search_based_Planning/") - -from Search_2D import plotting, env - - -class DStar: - def __init__(self, s_start, s_goal, heuristic_type): - self.s_start, self.s_goal = s_start, s_goal - self.heuristic_type = heuristic_type - - self.Env = env.Env() # class Env - self.Plot = plotting.Plotting(s_start, s_goal) - - self.u_set = self.Env.motions # feasible input set - self.obs = self.Env.obs # position of obstacles - self.x = self.Env.x_range - self.y = self.Env.y_range - - self.g, self.rhs, self.U = {}, {}, {} - self.km = 0 - - for i in range(1, self.Env.x_range - 1): - for j in range(1, self.Env.y_range - 1): - self.rhs[(i, j)] = float("inf") - self.g[(i, j)] = float("inf") - - self.rhs[self.s_goal] = 0.0 - self.U[self.s_goal] = self.CalculateKey(self.s_goal) - self.visited = set() - self.count = 0 - self.fig = plt.figure() - - def run(self): - self.Plot.plot_grid("D* Lite") - self.ComputePath() - self.plot_path(self.extract_path()) - self.fig.canvas.mpl_connect('button_press_event', self.on_press) - plt.show() - - def on_press(self, event): - x, y = event.xdata, event.ydata - if x < 0 or x > self.x - 1 or y < 0 or y > self.y - 1: - print("Please choose right area!") - else: - x, y = int(x), int(y) - print("Change position: s =", x, ",", "y =", y) - - s_curr = self.s_start - s_last = self.s_start - i = 0 - path = [self.s_start] - - while s_curr != self.s_goal: - s_list = {} - - for s in self.get_neighbor(s_curr): - s_list[s] = self.g[s] + self.cost(s_curr, s) - s_curr = min(s_list, key=s_list.get) - path.append(s_curr) - - if i < 1: - self.km += self.h(s_last, s_curr) - s_last = s_curr - 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.UpdateVertex((x, y)) - for s in self.get_neighbor((x, y)): - self.UpdateVertex(s) - i += 1 - - self.count += 1 - self.visited = set() - self.ComputePath() - - self.plot_visited(self.visited) - self.plot_path(path) - self.fig.canvas.draw_idle() - - def ComputePath(self): - while True: - s, v = self.TopKey() - if v >= self.CalculateKey(self.s_start) and \ - self.rhs[self.s_start] == self.g[self.s_start]: - break - - k_old = v - self.U.pop(s) - self.visited.add(s) - - if k_old < self.CalculateKey(s): - self.U[s] = self.CalculateKey(s) - elif self.g[s] > self.rhs[s]: - self.g[s] = self.rhs[s] - for x in self.get_neighbor(s): - self.UpdateVertex(x) - else: - self.g[s] = float("inf") - self.UpdateVertex(s) - for x in self.get_neighbor(s): - self.UpdateVertex(x) - - def UpdateVertex(self, s): - if s != self.s_goal: - self.rhs[s] = float("inf") - for x in self.get_neighbor(s): - self.rhs[s] = min(self.rhs[s], self.g[x] + self.cost(s, x)) - if s in self.U: - self.U.pop(s) - - if self.g[s] != self.rhs[s]: - self.U[s] = self.CalculateKey(s) - - def CalculateKey(self, s): - return [min(self.g[s], self.rhs[s]) + self.h(self.s_start, s) + self.km, - min(self.g[s], self.rhs[s])] - - def TopKey(self): - """ - :return: return the min key and its value. - """ - - s = min(self.U, key=self.U.get) - return s, self.U[s] - - def h(self, s_start, s_goal): - heuristic_type = self.heuristic_type # heuristic type - - if heuristic_type == "manhattan": - return abs(s_goal[0] - s_start[0]) + abs(s_goal[1] - s_start[1]) - else: - return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1]) - - def cost(self, s_start, s_goal): - """ - Calculate Cost for this motion - :param s_start: starting node - :param s_goal: end node - :return: Cost for this motion - :note: Cost function could be more complicate! - """ - - if self.is_collision(s_start, s_goal): - return float("inf") - - return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1]) - - def is_collision(self, s_start, s_end): - if s_start in self.obs or s_end in self.obs: - return True - - if s_start[0] != s_end[0] and s_start[1] != s_end[1]: - if s_end[0] - s_start[0] == s_start[1] - s_end[1]: - s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1])) - s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1])) - else: - s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1])) - s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1])) - - if s1 in self.obs or s2 in self.obs: - return True - - return False - - def get_neighbor(self, s): - nei_list = set() - for u in self.u_set: - s_next = tuple([s[i] + u[i] for i in range(2)]) - if s_next not in self.obs: - nei_list.add(s_next) - - return nei_list - - def extract_path(self): - """ - Extract the path based on the PARENT set. - :return: The planning path - """ - - path = [self.s_start] - s = self.s_start - - for k in range(100): - g_list = {} - for x in self.get_neighbor(s): - if not self.is_collision(s, x): - g_list[x] = self.g[x] - s = min(g_list, key=g_list.get) - path.append(s) - if s == self.s_goal: - break - - return list(path) - - def plot_path(self, path): - px = [x[0] for x in path] - py = [x[1] for x in path] - plt.plot(px, py, linewidth=2) - plt.plot(self.s_start[0], self.s_start[1], "bs") - plt.plot(self.s_goal[0], self.s_goal[1], "gs") - - def plot_visited(self, visited): - color = ['gainsboro', 'lightgray', 'silver', 'darkgray', - 'bisque', 'navajowhite', 'moccasin', 'wheat', - 'powderblue', 'skyblue', 'lightskyblue', 'cornflowerblue'] - - if self.count >= len(color) - 1: - self.count = 0 - - for x in visited: - plt.plot(x[0], x[1], marker='s', color=color[self.count]) - - -def main(): - s_start = (5, 5) - s_goal = (45, 25) - - dstar = DStar(s_start, s_goal, "euclidean") - dstar.run() - - -if __name__ == '__main__': - main() diff --git a/src/Search_2D/Dijkstra.py b/src/Search_2D/Dijkstra.py deleted file mode 100644 index e5e7b68..0000000 --- a/src/Search_2D/Dijkstra.py +++ /dev/null @@ -1,69 +0,0 @@ -""" -Dijkstra 2D -@author: huiming zhou -""" - -import os -import sys -import math -import heapq - -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Search_based_Planning/") - -from Search_2D import plotting, env - -from Search_2D.Astar import AStar - - -class Dijkstra(AStar): - """Dijkstra set the cost as the priority - """ - def searching(self): - """ - Breadth-first Searching. - :return: path, visited order - """ - - self.PARENT[self.s_start] = self.s_start - self.g[self.s_start] = 0 - self.g[self.s_goal] = math.inf - heapq.heappush(self.OPEN, - (0, self.s_start)) - - while self.OPEN: - _, s = heapq.heappop(self.OPEN) - self.CLOSED.append(s) - - if s == self.s_goal: - break - - for s_n in self.get_neighbor(s): - new_cost = self.g[s] + self.cost(s, s_n) - - if s_n not in self.g: - self.g[s_n] = math.inf - - if new_cost < self.g[s_n]: # conditions for updating Cost - self.g[s_n] = new_cost - self.PARENT[s_n] = s - - # best first set the heuristics as the priority - heapq.heappush(self.OPEN, (new_cost, s_n)) - - return self.extract_path(self.PARENT), self.CLOSED - - -def main(): - s_start = (5, 5) - s_goal = (45, 25) - - dijkstra = Dijkstra(s_start, s_goal, 'None') - plot = plotting.Plotting(s_start, s_goal) - - path, visited = dijkstra.searching() - plot.animation(path, visited, "Dijkstra's") # animation generate - - -if __name__ == '__main__': - main() diff --git a/src/Search_2D/LPAstar.py b/src/Search_2D/LPAstar.py deleted file mode 100644 index 4fd70ae..0000000 --- a/src/Search_2D/LPAstar.py +++ /dev/null @@ -1,256 +0,0 @@ -""" -LPA_star 2D -@author: huiming zhou -""" - -import os -import sys -import math -import matplotlib.pyplot as plt - -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Search_based_Planning/") - -from Search_2D import plotting, env - - -class LPAStar: - def __init__(self, s_start, s_goal, heuristic_type): - self.s_start, self.s_goal = s_start, s_goal - self.heuristic_type = heuristic_type - - self.Env = env.Env() - self.Plot = plotting.Plotting(self.s_start, self.s_goal) - - self.u_set = self.Env.motions - self.obs = self.Env.obs - self.x = self.Env.x_range - self.y = self.Env.y_range - - self.g, self.rhs, self.U = {}, {}, {} - - for i in range(self.Env.x_range): - for j in range(self.Env.y_range): - self.rhs[(i, j)] = float("inf") - self.g[(i, j)] = float("inf") - - self.rhs[self.s_start] = 0 - self.U[self.s_start] = self.CalculateKey(self.s_start) - self.visited = set() - self.count = 0 - - self.fig = plt.figure() - - def run(self): - self.Plot.plot_grid("Lifelong Planning A*") - - self.ComputeShortestPath() - self.plot_path(self.extract_path()) - self.fig.canvas.mpl_connect('button_press_event', self.on_press) - - plt.show() - - def on_press(self, event): - x, y = event.xdata, event.ydata - if x < 0 or x > self.x - 1 or y < 0 or y > self.y - 1: - print("Please choose right area!") - else: - x, y = int(x), int(y) - print("Change position: s =", x, ",", "y =", y) - - self.visited = set() - self.count += 1 - - if (x, y) not in self.obs: - self.obs.add((x, y)) - else: - self.obs.remove((x, y)) - self.UpdateVertex((x, y)) - - self.Plot.update_obs(self.obs) - - for s_n in self.get_neighbor((x, y)): - self.UpdateVertex(s_n) - - self.ComputeShortestPath() - - plt.cla() - self.Plot.plot_grid("Lifelong Planning A*") - self.plot_visited(self.visited) - self.plot_path(self.extract_path()) - self.fig.canvas.draw_idle() - - def ComputeShortestPath(self): - while True: - s, v = self.TopKey() - - if v >= self.CalculateKey(self.s_goal) and \ - self.rhs[self.s_goal] == self.g[self.s_goal]: - break - - self.U.pop(s) - self.visited.add(s) - - if self.g[s] > self.rhs[s]: - - # Condition: over-consistent (eg: deleted obstacles) - # So, rhs[s] decreased -- > rhs[s] < g[s] - self.g[s] = self.rhs[s] - else: - - # Condition: # under-consistent (eg: added obstacles) - # So, rhs[s] increased --> rhs[s] > g[s] - self.g[s] = float("inf") - self.UpdateVertex(s) - - for s_n in self.get_neighbor(s): - self.UpdateVertex(s_n) - - def UpdateVertex(self, s): - """ - update the status and the current cost to come of state s. - :param s: state s - """ - - if s != self.s_start: - - # Condition: cost of parent of s changed - # Since we do not record the children of a state, we need to enumerate its neighbors - self.rhs[s] = min(self.g[s_n] + self.cost(s_n, s) - for s_n in self.get_neighbor(s)) - - if s in self.U: - self.U.pop(s) - - if self.g[s] != self.rhs[s]: - - # Condition: current cost to come is different to that of last time - # state s should be added into OPEN set (set U) - self.U[s] = self.CalculateKey(s) - - def TopKey(self): - """ - :return: return the min key and its value. - """ - - s = min(self.U, key=self.U.get) - - return s, self.U[s] - - def CalculateKey(self, s): - - return [min(self.g[s], self.rhs[s]) + self.h(s), - min(self.g[s], self.rhs[s])] - - def get_neighbor(self, s): - """ - find neighbors of state s that not in obstacles. - :param s: state - :return: neighbors - """ - - s_list = set() - - for u in self.u_set: - s_next = tuple([s[i] + u[i] for i in range(2)]) - if s_next not in self.obs: - s_list.add(s_next) - - return s_list - - def h(self, s): - """ - Calculate heuristic. - :param s: current node (state) - :return: heuristic function value - """ - - heuristic_type = self.heuristic_type # heuristic type - goal = self.s_goal # goal node - - if heuristic_type == "manhattan": - return abs(goal[0] - s[0]) + abs(goal[1] - s[1]) - else: - return math.hypot(goal[0] - s[0], goal[1] - s[1]) - - def cost(self, s_start, s_goal): - """ - Calculate Cost for this motion - :param s_start: starting node - :param s_goal: end node - :return: Cost for this motion - :note: Cost function could be more complicate! - """ - - if self.is_collision(s_start, s_goal): - return float("inf") - - return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1]) - - def is_collision(self, s_start, s_end): - if s_start in self.obs or s_end in self.obs: - return True - - if s_start[0] != s_end[0] and s_start[1] != s_end[1]: - if s_end[0] - s_start[0] == s_start[1] - s_end[1]: - s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1])) - s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1])) - else: - s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1])) - s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1])) - - if s1 in self.obs or s2 in self.obs: - return True - - return False - - def extract_path(self): - """ - Extract the path based on the PARENT set. - :return: The planning path - """ - - path = [self.s_goal] - s = self.s_goal - - for k in range(100): - g_list = {} - for x in self.get_neighbor(s): - if not self.is_collision(s, x): - g_list[x] = self.g[x] - s = min(g_list, key=g_list.get) - path.append(s) - if s == self.s_start: - break - - return list(reversed(path)) - - def plot_path(self, path): - px = [x[0] for x in path] - py = [x[1] for x in path] - plt.plot(px, py, linewidth=2) - plt.plot(self.s_start[0], self.s_start[1], "bs") - plt.plot(self.s_goal[0], self.s_goal[1], "gs") - - def plot_visited(self, visited): - color = ['gainsboro', 'lightgray', 'silver', 'darkgray', - 'bisque', 'navajowhite', 'moccasin', 'wheat', - 'powderblue', 'skyblue', 'lightskyblue', 'cornflowerblue'] - - if self.count >= len(color) - 1: - self.count = 0 - - for x in visited: - plt.plot(x[0], x[1], marker='s', color=color[self.count]) - - -def main(): - x_start = (5, 5) - x_goal = (45, 25) - - lpastar = LPAStar(x_start, x_goal, "Euclidean") - lpastar.run() - - -if __name__ == '__main__': - main() diff --git a/src/Search_2D/LRTAstar.py b/src/Search_2D/LRTAstar.py deleted file mode 100644 index 108903b..0000000 --- a/src/Search_2D/LRTAstar.py +++ /dev/null @@ -1,230 +0,0 @@ -""" -LRTA_star 2D (Learning Real-time A*) -@author: huiming zhou -""" - -import os -import sys -import copy -import math - -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Search_based_Planning/") - -from Search_2D import queue, plotting, env - - -class LrtAStarN: - def __init__(self, s_start, s_goal, N, heuristic_type): - self.s_start, self.s_goal = s_start, s_goal - self.heuristic_type = heuristic_type - - self.Env = env.Env() - - self.u_set = self.Env.motions # feasible input set - self.obs = self.Env.obs # position of obstacles - - self.N = N # number of expand nodes each iteration - self.visited = [] # order of visited nodes in planning - self.path = [] # path of each iteration - self.h_table = {} # h_value table - - def init(self): - """ - initialize the h_value of all nodes in the environment. - it is a global table. - """ - - for i in range(self.Env.x_range): - for j in range(self.Env.y_range): - self.h_table[(i, j)] = self.h((i, j)) - - def searching(self): - self.init() - s_start = self.s_start # initialize start node - - while True: - OPEN, CLOSED = self.AStar(s_start, self.N) # OPEN, CLOSED sets in each iteration - - if OPEN == "FOUND": # reach the goal node - self.path.append(CLOSED) - break - - h_value = self.iteration(CLOSED) # h_value table of CLOSED nodes - - for x in h_value: - self.h_table[x] = h_value[x] - - s_start, path_k = self.extract_path_in_CLOSE(s_start, h_value) # x_init -> expected node in OPEN set - self.path.append(path_k) - - def extract_path_in_CLOSE(self, s_start, h_value): - path = [s_start] - s = s_start - - while True: - h_list = {} - - for s_n in self.get_neighbor(s): - if s_n in h_value: - h_list[s_n] = h_value[s_n] - else: - h_list[s_n] = self.h_table[s_n] - - s_key = min(h_list, key=h_list.get) # move to the smallest node with min h_value - path.append(s_key) # generate path - s = s_key # use end of this iteration as the start of next - - if s_key not in h_value: # reach the expected node in OPEN set - return s_key, path - - def iteration(self, CLOSED): - h_value = {} - - for s in CLOSED: - h_value[s] = float("inf") # initialize h_value of CLOSED nodes - - while True: - h_value_rec = copy.deepcopy(h_value) - for s in CLOSED: - h_list = [] - for s_n in self.get_neighbor(s): - if s_n not in CLOSED: - h_list.append(self.cost(s, s_n) + self.h_table[s_n]) - else: - h_list.append(self.cost(s, s_n) + h_value[s_n]) - h_value[s] = min(h_list) # update h_value of current node - - if h_value == h_value_rec: # h_value table converged - return h_value - - def AStar(self, x_start, N): - OPEN = queue.QueuePrior() # OPEN set - OPEN.put(x_start, self.h(x_start)) - CLOSED = [] # CLOSED set - g_table = {x_start: 0, self.s_goal: float("inf")} # Cost to come - PARENT = {x_start: x_start} # relations - count = 0 # counter - - while not OPEN.empty(): - count += 1 - s = OPEN.get() - CLOSED.append(s) - - if s == self.s_goal: # reach the goal node - self.visited.append(CLOSED) - return "FOUND", self.extract_path(x_start, PARENT) - - for s_n in self.get_neighbor(s): - if s_n not in CLOSED: - new_cost = g_table[s] + self.cost(s, s_n) - if s_n not in g_table: - g_table[s_n] = float("inf") - if new_cost < g_table[s_n]: # conditions for updating Cost - g_table[s_n] = new_cost - PARENT[s_n] = s - OPEN.put(s_n, g_table[s_n] + self.h_table[s_n]) - - if count == N: # expand needed CLOSED nodes - break - - self.visited.append(CLOSED) # visited nodes in each iteration - - return OPEN, CLOSED - - def get_neighbor(self, s): - """ - find neighbors of state s that not in obstacles. - :param s: state - :return: neighbors - """ - - s_list = [] - - for u in self.u_set: - s_next = tuple([s[i] + u[i] for i in range(2)]) - if s_next not in self.obs: - s_list.append(s_next) - - return s_list - - def extract_path(self, x_start, parent): - """ - Extract the path based on the relationship of nodes. - - :return: The planning path - """ - - path_back = [self.s_goal] - x_current = self.s_goal - - while True: - x_current = parent[x_current] - path_back.append(x_current) - - if x_current == x_start: - break - - return list(reversed(path_back)) - - def h(self, s): - """ - Calculate heuristic. - :param s: current node (state) - :return: heuristic function value - """ - - heuristic_type = self.heuristic_type # heuristic type - goal = self.s_goal # goal node - - if heuristic_type == "manhattan": - return abs(goal[0] - s[0]) + abs(goal[1] - s[1]) - else: - return math.hypot(goal[0] - s[0], goal[1] - s[1]) - - def cost(self, s_start, s_goal): - """ - Calculate Cost for this motion - :param s_start: starting node - :param s_goal: end node - :return: Cost for this motion - :note: Cost function could be more complicate! - """ - - if self.is_collision(s_start, s_goal): - return float("inf") - - return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1]) - - def is_collision(self, s_start, s_end): - if s_start in self.obs or s_end in self.obs: - return True - - if s_start[0] != s_end[0] and s_start[1] != s_end[1]: - if s_end[0] - s_start[0] == s_start[1] - s_end[1]: - s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1])) - s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1])) - else: - s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1])) - s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1])) - - if s1 in self.obs or s2 in self.obs: - return True - - return False - - -def main(): - s_start = (10, 5) - s_goal = (45, 25) - - lrta = LrtAStarN(s_start, s_goal, 250, "euclidean") - plot = plotting.Plotting(s_start, s_goal) - - lrta.searching() - plot.animation_lrta(lrta.path, lrta.visited, - "Learning Real-time A* (LRTA*)") - - -if __name__ == '__main__': - main() diff --git a/src/Search_2D/RTAAStar.py b/src/Search_2D/RTAAStar.py deleted file mode 100644 index de0a615..0000000 --- a/src/Search_2D/RTAAStar.py +++ /dev/null @@ -1,237 +0,0 @@ -""" -RTAAstar 2D (Real-time Adaptive A*) -@author: huiming zhou -""" - -import os -import sys -import copy -import math - -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Search_based_Planning/") - -from Search_2D import queue, plotting, env - - -class RTAAStar: - def __init__(self, s_start, s_goal, N, heuristic_type): - self.s_start, self.s_goal = s_start, s_goal - self.heuristic_type = heuristic_type - - self.Env = env.Env() - - self.u_set = self.Env.motions # feasible input set - self.obs = self.Env.obs # position of obstacles - - self.N = N # number of expand nodes each iteration - self.visited = [] # order of visited nodes in planning - self.path = [] # path of each iteration - self.h_table = {} # h_value table - - def init(self): - """ - initialize the h_value of all nodes in the environment. - it is a global table. - """ - - for i in range(self.Env.x_range): - for j in range(self.Env.y_range): - self.h_table[(i, j)] = self.h((i, j)) - - def searching(self): - self.init() - s_start = self.s_start # initialize start node - - while True: - OPEN, CLOSED, g_table, PARENT = \ - self.Astar(s_start, self.N) - - if OPEN == "FOUND": # reach the goal node - self.path.append(CLOSED) - break - - s_next, h_value = self.cal_h_value(OPEN, CLOSED, g_table, PARENT) - - for x in h_value: - self.h_table[x] = h_value[x] - - s_start, path_k = self.extract_path_in_CLOSE(s_start, s_next, h_value) - self.path.append(path_k) - - def cal_h_value(self, OPEN, CLOSED, g_table, PARENT): - v_open = {} - h_value = {} - for (_, x) in OPEN.enumerate(): - v_open[x] = g_table[PARENT[x]] + 1 + self.h_table[x] - s_open = min(v_open, key=v_open.get) - f_min = v_open[s_open] - for x in CLOSED: - h_value[x] = f_min - g_table[x] - - return s_open, h_value - - def iteration(self, CLOSED): - h_value = {} - - for s in CLOSED: - h_value[s] = float("inf") # initialize h_value of CLOSED nodes - - while True: - h_value_rec = copy.deepcopy(h_value) - for s in CLOSED: - h_list = [] - for s_n in self.get_neighbor(s): - if s_n not in CLOSED: - h_list.append(self.cost(s, s_n) + self.h_table[s_n]) - else: - h_list.append(self.cost(s, s_n) + h_value[s_n]) - h_value[s] = min(h_list) # update h_value of current node - - if h_value == h_value_rec: # h_value table converged - return h_value - - def Astar(self, x_start, N): - OPEN = queue.QueuePrior() # OPEN set - OPEN.put(x_start, self.h_table[x_start]) - CLOSED = [] # CLOSED set - g_table = {x_start: 0, self.s_goal: float("inf")} # Cost to come - PARENT = {x_start: x_start} # relations - count = 0 # counter - - while not OPEN.empty(): - count += 1 - s = OPEN.get() - CLOSED.append(s) - - if s == self.s_goal: # reach the goal node - self.visited.append(CLOSED) - return "FOUND", self.extract_path(x_start, PARENT), [], [] - - for s_n in self.get_neighbor(s): - if s_n not in CLOSED: - new_cost = g_table[s] + self.cost(s, s_n) - if s_n not in g_table: - g_table[s_n] = float("inf") - if new_cost < g_table[s_n]: # conditions for updating Cost - g_table[s_n] = new_cost - PARENT[s_n] = s - OPEN.put(s_n, g_table[s_n] + self.h_table[s_n]) - - if count == N: # expand needed CLOSED nodes - break - - self.visited.append(CLOSED) # visited nodes in each iteration - - return OPEN, CLOSED, g_table, PARENT - - def get_neighbor(self, s): - """ - find neighbors of state s that not in obstacles. - :param s: state - :return: neighbors - """ - - s_list = set() - - for u in self.u_set: - s_next = tuple([s[i] + u[i] for i in range(2)]) - if s_next not in self.obs: - s_list.add(s_next) - - return s_list - - def extract_path_in_CLOSE(self, s_end, s_start, h_value): - path = [s_start] - s = s_start - - while True: - h_list = {} - for s_n in self.get_neighbor(s): - if s_n in h_value: - h_list[s_n] = h_value[s_n] - s_key = max(h_list, key=h_list.get) # move to the smallest node with min h_value - path.append(s_key) # generate path - s = s_key # use end of this iteration as the start of next - - if s_key == s_end: # reach the expected node in OPEN set - return s_start, list(reversed(path)) - - def extract_path(self, x_start, parent): - """ - Extract the path based on the relationship of nodes. - :return: The planning path - """ - - path = [self.s_goal] - s = self.s_goal - - while True: - s = parent[s] - path.append(s) - if s == x_start: - break - - return list(reversed(path)) - - def h(self, s): - """ - Calculate heuristic. - :param s: current node (state) - :return: heuristic function value - """ - - heuristic_type = self.heuristic_type # heuristic type - goal = self.s_goal # goal node - - if heuristic_type == "manhattan": - return abs(goal[0] - s[0]) + abs(goal[1] - s[1]) - else: - return math.hypot(goal[0] - s[0], goal[1] - s[1]) - - def cost(self, s_start, s_goal): - """ - Calculate Cost for this motion - :param s_start: starting node - :param s_goal: end node - :return: Cost for this motion - :note: Cost function could be more complicate! - """ - - if self.is_collision(s_start, s_goal): - return float("inf") - - return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1]) - - def is_collision(self, s_start, s_end): - if s_start in self.obs or s_end in self.obs: - return True - - if s_start[0] != s_end[0] and s_start[1] != s_end[1]: - if s_end[0] - s_start[0] == s_start[1] - s_end[1]: - s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1])) - s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1])) - else: - s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1])) - s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1])) - - if s1 in self.obs or s2 in self.obs: - return True - - return False - - -def main(): - s_start = (10, 5) - s_goal = (45, 25) - - rtaa = RTAAStar(s_start, s_goal, 240, "euclidean") - plot = plotting.Plotting(s_start, s_goal) - - rtaa.searching() - plot.animation_lrta(rtaa.path, rtaa.visited, - "Real-time Adaptive A* (RTAA*)") - - -if __name__ == '__main__': - main() diff --git a/src/Search_2D/bfs.py b/src/Search_2D/bfs.py deleted file mode 100644 index 881e7ff..0000000 --- a/src/Search_2D/bfs.py +++ /dev/null @@ -1,69 +0,0 @@ -""" -Breadth-first Searching_2D (BFS) -@author: huiming zhou -""" - -import os -import sys -from collections import deque - -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Search_based_Planning/") - -from Search_2D import plotting, env -from Search_2D.Astar import AStar -import math -import heapq - -class BFS(AStar): - """BFS add the new visited node in the end of the openset - """ - def searching(self): - """ - Breadth-first Searching. - :return: path, visited order - """ - - self.PARENT[self.s_start] = self.s_start - self.g[self.s_start] = 0 - self.g[self.s_goal] = math.inf - heapq.heappush(self.OPEN, - (0, self.s_start)) - - while self.OPEN: - _, s = heapq.heappop(self.OPEN) - self.CLOSED.append(s) - - if s == self.s_goal: - break - - for s_n in self.get_neighbor(s): - new_cost = self.g[s] + self.cost(s, s_n) - - if s_n not in self.g: - self.g[s_n] = math.inf - - if new_cost < self.g[s_n]: # conditions for updating Cost - self.g[s_n] = new_cost - self.PARENT[s_n] = s - - # bfs, add new node to the end of the openset - prior = self.OPEN[-1][0]+1 if len(self.OPEN)>0 else 0 - heapq.heappush(self.OPEN, (prior, s_n)) - - return self.extract_path(self.PARENT), self.CLOSED - - -def main(): - s_start = (5, 5) - s_goal = (45, 25) - - bfs = BFS(s_start, s_goal, 'None') - plot = plotting.Plotting(s_start, s_goal) - - path, visited = bfs.searching() - plot.animation(path, visited, "Breadth-first Searching (BFS)") - - -if __name__ == '__main__': - main() diff --git a/src/Search_2D/dfs.py b/src/Search_2D/dfs.py deleted file mode 100644 index 3b30b03..0000000 --- a/src/Search_2D/dfs.py +++ /dev/null @@ -1,65 +0,0 @@ - -import os -import sys -import math -import heapq - -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Search_based_Planning/") - -from Search_2D import plotting, env -from Search_2D.Astar import AStar - -class DFS(AStar): - """DFS add the new visited node in the front of the openset - """ - def searching(self): - """ - Breadth-first Searching. - :return: path, visited order - """ - - self.PARENT[self.s_start] = self.s_start - self.g[self.s_start] = 0 - self.g[self.s_goal] = math.inf - heapq.heappush(self.OPEN, - (0, self.s_start)) - - while self.OPEN: - _, s = heapq.heappop(self.OPEN) - self.CLOSED.append(s) - - if s == self.s_goal: - break - - for s_n in self.get_neighbor(s): - new_cost = self.g[s] + self.cost(s, s_n) - - if s_n not in self.g: - self.g[s_n] = math.inf - - if new_cost < self.g[s_n]: # conditions for updating Cost - self.g[s_n] = new_cost - self.PARENT[s_n] = s - - # dfs, add new node to the front of the openset - prior = self.OPEN[0][0]-1 if len(self.OPEN)>0 else 0 - heapq.heappush(self.OPEN, (prior, s_n)) - - return self.extract_path(self.PARENT), self.CLOSED - - -def main(): - s_start = (5, 5) - s_goal = (45, 25) - - dfs = DFS(s_start, s_goal, 'None') - plot = plotting.Plotting(s_start, s_goal) - - path, visited = dfs.searching() - visited = list(dict.fromkeys(visited)) - plot.animation(path, visited, "Depth-first Searching (DFS)") # animation - - -if __name__ == '__main__': - main() diff --git a/src/Search_2D/queue.py b/src/Search_2D/queue.py deleted file mode 100644 index 51703ae..0000000 --- a/src/Search_2D/queue.py +++ /dev/null @@ -1,62 +0,0 @@ -import collections -import heapq - - -class QueueFIFO: - """ - Class: QueueFIFO - Description: QueueFIFO is designed for First-in-First-out rule. - """ - - def __init__(self): - self.queue = collections.deque() - - def empty(self): - return len(self.queue) == 0 - - def put(self, node): - self.queue.append(node) # enter from back - - def get(self): - return self.queue.popleft() # leave from front - - -class QueueLIFO: - """ - Class: QueueLIFO - Description: QueueLIFO is designed for Last-in-First-out rule. - """ - - def __init__(self): - self.queue = collections.deque() - - def empty(self): - return len(self.queue) == 0 - - def put(self, node): - self.queue.append(node) # enter from back - - def get(self): - return self.queue.pop() # leave from back - - -class QueuePrior: - """ - Class: QueuePrior - Description: QueuePrior reorders elements using value [priority] - """ - - def __init__(self): - self.queue = [] - - def empty(self): - return len(self.queue) == 0 - - def put(self, item, priority): - heapq.heappush(self.queue, (priority, item)) # reorder s using priority - - def get(self): - return heapq.heappop(self.queue)[1] # pop out the smallest item - - def enumerate(self): - return self.queue