From cfed23253d4851f9c75438d6a9d3490175040e0e Mon Sep 17 00:00:00 2001 From: Shilong Date: Fri, 21 Aug 2020 12:26:34 -0700 Subject: [PATCH] unify bfs, dfs, Dijkstra, best_first, Astar --- Search_based_Planning/Search_2D/Astar.py | 4 +- Search_based_Planning/Search_2D/Best_First.py | 103 ++++------------ Search_based_Planning/Search_2D/Dijkstra.py | 102 +++------------- Search_based_Planning/Search_2D/bfs.py | 101 ++++------------ Search_based_Planning/Search_2D/dfs.py | 110 +++++------------- 5 files changed, 92 insertions(+), 328 deletions(-) diff --git a/Search_based_Planning/Search_2D/Astar.py b/Search_based_Planning/Search_2D/Astar.py index 7f040ec..adf676b 100644 --- a/Search_based_Planning/Search_2D/Astar.py +++ b/Search_based_Planning/Search_2D/Astar.py @@ -11,10 +11,12 @@ import heapq sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") -from Search_based_Planning.Search_2D import plotting, env +from Search_2D import plotting, env class AStar: + """AStar set the cost + heuristics as the priority + """ def __init__(self, s_start, s_goal, heuristic_type): self.s_start = s_start self.s_goal = s_goal diff --git a/Search_based_Planning/Search_2D/Best_First.py b/Search_based_Planning/Search_2D/Best_First.py index 1ca2bb6..0c85fba 100644 --- a/Search_based_Planning/Search_2D/Best_First.py +++ b/Search_based_Planning/Search_2D/Best_First.py @@ -11,116 +11,53 @@ import heapq sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") -from Search_based_Planning.Search_2D import plotting, env +from Search_2D import plotting, env +from Search_2D.Astar import AStar -class BestFirst: - def __init__(self, s_start, s_goal): - self.s_start = s_start - self.s_goal = s_goal - - self.Env = env.Env() - self.plotting = plotting.Plotting(self.s_start, self.s_goal) - - self.u_set = self.Env.motions # feasible input set - self.obs = self.Env.obs # position of obstacles - - self.OPEN = [] # OPEN set: visited nodes - self.CLOSED = [] # CLOSED set / visited order - self.PARENT = dict() # recorded parent - +class BestFirst(AStar): + """BestFirst set the heuristics as the priority + """ def searching(self): """ - Best-first Searching - :return: planning path, visited order + 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 - self.CLOSED.append(s) for s_n in self.get_neighbor(s): - if self.is_collision(s, s_n): - continue + new_cost = self.g[s] + self.cost(s, s_n) - if s_n not in self.PARENT: # node not explored - heapq.heappush(self.OPEN, (self.heuristic(s_n), 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 - return self.extract_path(), self.CLOSED + # best first set the heuristics as the priority + heapq.heappush(self.OPEN, (self.heuristic(s_n), s_n)) - def heuristic(self, s): - """ - estimated distance between current state and goal state. - :param s: current state - :return: Euclidean distance - """ - - return math.hypot(s[0] - self.s_goal[0], s[1] - self.s_goal[1]) - - 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 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 extract_path(self): - """ - Extract the path based on the relationship of nodes. - :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) + return self.extract_path(self.PARENT), self.CLOSED def main(): s_start = (5, 5) s_goal = (45, 25) - BF = BestFirst(s_start, s_goal) + BF = BestFirst(s_start, s_goal, 'euclidean') plot = plotting.Plotting(s_start, s_goal) path, visited = BF.searching() diff --git a/Search_based_Planning/Search_2D/Dijkstra.py b/Search_based_Planning/Search_2D/Dijkstra.py index 1f9414a..e5e7b68 100644 --- a/Search_based_Planning/Search_2D/Dijkstra.py +++ b/Search_based_Planning/Search_2D/Dijkstra.py @@ -11,35 +11,25 @@ import heapq sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") -from Search_based_Planning.Search_2D import plotting, env +from Search_2D import plotting, env + +from Search_2D.Astar import AStar -class Dijkstra: - def __init__(self, s_start, s_goal): - self.s_start = s_start - self.s_goal = s_goal - - self.Env = env.Env() - self.plotting = plotting.Plotting(self.s_start, self.s_goal) - - self.u_set = self.Env.motions # feasible input set - self.obs = self.Env.obs # position of obstacles - - self.OPEN = [] # priority queue / OPEN set - self.CLOSED = [] # closed set & visited - self.PARENT = dict() # record parent - self.g = dict() # Cost to come - +class Dijkstra(AStar): + """Dijkstra set the cost as the priority + """ def searching(self): """ - Dijkstra Searching. + 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)) + heapq.heappush(self.OPEN, + (0, self.s_start)) while self.OPEN: _, s = heapq.heappop(self.OPEN) @@ -50,85 +40,25 @@ class Dijkstra: 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]: + + if new_cost < self.g[s_n]: # conditions for updating Cost self.g[s_n] = new_cost - heapq.heappush(self.OPEN, (new_cost, s_n)) self.PARENT[s_n] = s - return self.extract_path(), self.CLOSED + # best first set the heuristics as the priority + heapq.heappush(self.OPEN, (new_cost, s_n)) - 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): - """ - Extract the path based on 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 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 - """ - - 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 + return self.extract_path(self.PARENT), self.CLOSED def main(): s_start = (5, 5) s_goal = (45, 25) - dijkstra = Dijkstra(s_start, s_goal) + dijkstra = Dijkstra(s_start, s_goal, 'None') plot = plotting.Plotting(s_start, s_goal) path, visited = dijkstra.searching() diff --git a/Search_based_Planning/Search_2D/bfs.py b/Search_based_Planning/Search_2D/bfs.py index 7acd282..881e7ff 100644 --- a/Search_based_Planning/Search_2D/bfs.py +++ b/Search_based_Planning/Search_2D/bfs.py @@ -10,24 +10,14 @@ from collections import deque sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") -from Search_based_Planning.Search_2D import plotting, env - - -class BFS: - def __init__(self, s_start, s_goal): - self.s_start = s_start - self.s_goal = s_goal - - self.Env = env.Env() - self.plotting = plotting.Plotting(self.s_start, self.s_goal) - - self.u_set = self.Env.motions # feasible input set - self.obs = self.Env.obs # position of obstacles - - self.OPEN = deque() # OPEN set: visited nodes - self.PARENT = dict() # recorded parent - self.CLOSED = [] # CLOSED set: explored nodes +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. @@ -35,81 +25,40 @@ class BFS: """ self.PARENT[self.s_start] = self.s_start - self.OPEN.append(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 = self.OPEN.popleft() + _, s = heapq.heappop(self.OPEN) + self.CLOSED.append(s) if s == self.s_goal: break - self.CLOSED.append(s) for s_n in self.get_neighbor(s): - if self.is_collision(s, s_n): - continue - if s_n not in self.PARENT: # node not explored - self.OPEN.append(s_n) + 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 - return self.extract_path(), self.CLOSED + # 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)) - def get_neighbor(self, s): - """ - find neighbors of state s that not in obstacles. - :param s: state - :return: neighbors : [nodes] - """ - - return [(s[0] + u[0], s[1] + u[1]) for u in self.u_set] - - 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 extract_path(self): - """ - Extract the path based on the PARENT set. - :return: The planning path : [nodes] - """ - - 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) + return self.extract_path(self.PARENT), self.CLOSED def main(): s_start = (5, 5) s_goal = (45, 25) - bfs = BFS(s_start, s_goal) + bfs = BFS(s_start, s_goal, 'None') plot = plotting.Plotting(s_start, s_goal) path, visited = bfs.searching() diff --git a/Search_based_Planning/Search_2D/dfs.py b/Search_based_Planning/Search_2D/dfs.py index c6e43dd..3b30b03 100644 --- a/Search_based_Planning/Search_2D/dfs.py +++ b/Search_based_Planning/Search_2D/dfs.py @@ -1,117 +1,63 @@ -""" -Depth-first Searching_2D (DFS) -@author: huiming zhou -""" import os import sys -from collections import deque +import math +import heapq sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") -from Search_based_Planning.Search_2D import plotting, env - - -class DFS: - def __init__(self, s_start, s_goal): - self.s_start = s_start - self.s_goal = s_goal - - self.Env = env.Env() - self.plotting = plotting.Plotting(self.s_start, self.s_goal) - - self.u_set = self.Env.motions # feasible input set - self.obs = self.Env.obs # position of obstacles - - self.OPEN = deque() # OPEN set: visited nodes - self.PARENT = dict() # recorded parent - self.CLOSED = [] # CLOSED set / visited order +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): """ - Depth-first Searching - :return: planning path, visited order + Breadth-first Searching. + :return: path, visited order """ self.PARENT[self.s_start] = self.s_start - self.OPEN.append(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 = self.OPEN.pop() + _, s = heapq.heappop(self.OPEN) + self.CLOSED.append(s) if s == self.s_goal: break - self.CLOSED.append(s) for s_n in self.get_neighbor(s): - if self.is_collision(s, s_n): - continue - if s_n not in self.PARENT: # node not explored - self.OPEN.append(s_n) + 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 - return self.extract_path(), self.CLOSED + # 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)) - def get_neighbor(self, s): - """ - find neighbors of state s that not in obstacles. - :param s: state - :return: neighbors : [nodes] - """ - - return [(s[0] + u[0], s[1] + u[1]) for u in self.u_set] - - 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 extract_path(self): - """ - Extract the path based on the relationship of nodes. - :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) + return self.extract_path(self.PARENT), self.CLOSED def main(): s_start = (5, 5) s_goal = (45, 25) - dfs = DFS(s_start, s_goal) + 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