diff --git a/Search_based_Planning/Search_2D/ARAstar.py b/Search_based_Planning/Search_2D/ARAstar.py index 1f0ab3d..c014616 100644 --- a/Search_based_Planning/Search_2D/ARAstar.py +++ b/Search_based_Planning/Search_2D/ARAstar.py @@ -14,7 +14,7 @@ import math 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 AraStar: diff --git a/Search_based_Planning/Search_2D/Bidirectional_a_star.py b/Search_based_Planning/Search_2D/Bidirectional_a_star.py index 3b6bf12..3580c1a 100644 --- a/Search_based_Planning/Search_2D/Bidirectional_a_star.py +++ b/Search_based_Planning/Search_2D/Bidirectional_a_star.py @@ -11,7 +11,7 @@ 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 BidirectionalAStar: diff --git a/Search_based_Planning/Search_2D/D_star.py b/Search_based_Planning/Search_2D/D_star.py index 300059b..60b6c7e 100644 --- a/Search_based_Planning/Search_2D/D_star.py +++ b/Search_based_Planning/Search_2D/D_star.py @@ -11,7 +11,7 @@ import matplotlib.pyplot as plt 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 DStar: diff --git a/Search_based_Planning/Search_2D/D_star_Lite.py b/Search_based_Planning/Search_2D/D_star_Lite.py index fb7677e..4996be2 100644 --- a/Search_based_Planning/Search_2D/D_star_Lite.py +++ b/Search_based_Planning/Search_2D/D_star_Lite.py @@ -11,7 +11,7 @@ import matplotlib.pyplot as plt 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 DStar: diff --git a/Search_based_Planning/Search_2D/LPAstar.py b/Search_based_Planning/Search_2D/LPAstar.py index 22e56df..4fd70ae 100644 --- a/Search_based_Planning/Search_2D/LPAstar.py +++ b/Search_based_Planning/Search_2D/LPAstar.py @@ -11,7 +11,7 @@ import matplotlib.pyplot as plt 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 LPAStar: diff --git a/Search_based_Planning/Search_2D/LRTAstar.py b/Search_based_Planning/Search_2D/LRTAstar.py index 71418d3..108903b 100644 --- a/Search_based_Planning/Search_2D/LRTAstar.py +++ b/Search_based_Planning/Search_2D/LRTAstar.py @@ -11,7 +11,7 @@ import math sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") -from Search_based_Planning.Search_2D import queue, plotting, env +from Search_2D import queue, plotting, env class LrtAStarN: diff --git a/Search_based_Planning/Search_2D/PotentialField.py b/Search_based_Planning/Search_2D/PotentialField.py deleted file mode 100644 index dfa82e7..0000000 --- a/Search_based_Planning/Search_2D/PotentialField.py +++ /dev/null @@ -1,172 +0,0 @@ -""" -Potential_Field -@author: huiming zhou -""" - -import os -import sys -import numpy as np -import matplotlib.pyplot as plt -from collections import deque - -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Search_based_Planning/") - -from Search_2D import plotting -from Search_2D import env - - -class PF: - def __init__(self, s_start, s_goal): - self.s_start = s_start - self.s_goal = s_goal - self.kp = 5.0 - self.eta = 400 - self.r = 30.0 - self.OL = 10 - self.rr = 2 - - self.Env = env.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 - - def run(self): - pmap, minx, miny = self.calc_potential_field() - - d = np.hypot(self.s_start[0] - self.s_goal[0], - self.s_start[1] - self.s_goal[1]) - - ix = self.s_start[0] - minx - iy = self.s_start[1] - miny - gix = self.s_goal[0] - minx - giy = self.s_goal[1] - miny - - self.draw_heatmap(pmap) - plt.gcf().canvas.mpl_connect('key_release_event', - lambda event: [exit(0) if event.key == 'escape' else None]) - plt.plot(ix, iy, "sk") - plt.plot(gix, giy, "sm") - - rx, ry = [self.s_start[0]], [self.s_goal[0]] - ids_rec = deque() - - while d >= 1: - minp = float("inf") - minix, miniy = -1, -1 - - for u in self.u_set: - inx = int(ix + u[0]) - iny = int(iy + u[1]) - - if inx >= len(pmap) or iny >= len(pmap[0]) or inx < 0 or iny < 0: - p = float("inf") - print("test") - else: - p = pmap[inx][iny] - - if minp > p: - minp = p - minix = inx - miniy = iny - - ix = minix - iy = miniy - xp = ix + minx - yp = iy + miny - d = np.hypot(self.s_goal[0] - xp, self.s_goal[1] - yp) - rx.append(xp) - ry.append(yp) - - if self.is_oscillation(ids_rec, ix, iy): - print("Oscillation detected at ({},{})!".format(ix, iy)) - break - - plt.plot(ix, iy, ".r") - plt.pause(0.01) - - return rx, ry - - def calc_potential_field(self): - minx = 0 - miny = 0 - maxx = self.x - 1 - maxy = self.y - 1 - xw = maxx - minx - yw = maxy - miny - - pmap = [[0.0 for i in range(yw)] for i in range(xw)] - - for ix in range(xw): - x = ix + minx - - for iy in range(yw): - y = iy + miny - ug = self.calc_attractive_potential(x, y) - uo = self.calc_repulsive_potential(x, y) - uf = ug + uo - pmap[ix][iy] = uf - - return pmap, minx, miny - - def calc_attractive_potential(self, x, y): - return 0.5 * self.kp * \ - np.hypot(x - self.s_goal[0], y - self.s_goal[1]) - - def calc_repulsive_potential(self, x, y): - dmin = float("inf") - - for s in self.obs: - d = np.hypot(x - s[0], y - s[1]) - if dmin >= d: - dmin = d - - # calc repulsive potential - dq = dmin - - if dq <= self.rr: - if dq <= 0.8: - dq = 0.8 - - return 0.5 * self.eta * (1.0 / dq - 1.0 / self.rr) ** 2 - else: - return 0.0 - - def is_oscillation(self, ids_rec, ix, iy): - ids_rec.append((ix, iy)) - - if len(ids_rec) > self.OL: - ids_rec.popleft() - - ids_set = set() - for s in ids_rec: - if s in ids_set: - return True - else: - ids_set.add(s) - return False - - @staticmethod - def draw_heatmap(data): - data = np.array(data).T - plt.pcolor(data, vmax=100.0, cmap=plt.cm.Blues) - - -def main(): - s_start = (5, 5) - s_goal = (45, 25) - - pf = PF(s_start, s_goal) - - plt.grid(True) - plt.axis("equal") - _, _ = pf.run() - plt.show() - - -if __name__ == '__main__': - print(__file__ + " start!!") - main() diff --git a/Search_based_Planning/Search_2D/RTAAStar.py b/Search_based_Planning/Search_2D/RTAAStar.py index 42eb90d..de0a615 100644 --- a/Search_based_Planning/Search_2D/RTAAStar.py +++ b/Search_based_Planning/Search_2D/RTAAStar.py @@ -11,7 +11,7 @@ import math sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") -from Search_based_Planning.Search_2D import queue, plotting, env +from Search_2D import queue, plotting, env class RTAAStar: diff --git a/Search_based_Planning/Search_2D/__pycache__/Astar.cpython-38.pyc b/Search_based_Planning/Search_2D/__pycache__/Astar.cpython-38.pyc new file mode 100644 index 0000000..7d90ba3 Binary files /dev/null and b/Search_based_Planning/Search_2D/__pycache__/Astar.cpython-38.pyc differ diff --git a/Search_based_Planning/Search_2D/__pycache__/env.cpython-38.pyc b/Search_based_Planning/Search_2D/__pycache__/env.cpython-38.pyc new file mode 100644 index 0000000..e45c75b Binary files /dev/null and b/Search_based_Planning/Search_2D/__pycache__/env.cpython-38.pyc differ diff --git a/Search_based_Planning/Search_2D/__pycache__/plotting.cpython-38.pyc b/Search_based_Planning/Search_2D/__pycache__/plotting.cpython-38.pyc new file mode 100644 index 0000000..5e8cff3 Binary files /dev/null and b/Search_based_Planning/Search_2D/__pycache__/plotting.cpython-38.pyc differ diff --git a/Search_based_Planning/Search_2D/__pycache__/queue.cpython-38.pyc b/Search_based_Planning/Search_2D/__pycache__/queue.cpython-38.pyc new file mode 100644 index 0000000..69c46c6 Binary files /dev/null and b/Search_based_Planning/Search_2D/__pycache__/queue.cpython-38.pyc differ