diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml
index b161f78..c5b32cd 100644
--- a/Search-based Planning/.idea/workspace.xml
+++ b/Search-based Planning/.idea/workspace.xml
@@ -19,7 +19,15 @@
-
+
+
+
+
+
+
+
+
+
@@ -70,7 +78,7 @@
-
+
@@ -92,27 +100,6 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
@@ -176,6 +163,27 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -199,19 +207,19 @@
+
diff --git a/Search-based Planning/Search_2D/Hybrid_Astar/ReedsSheppPlanner/ReedsShepp.py b/Search-based Planning/Search_2D/Hybrid_Astar/ReedsSheppPlanner/ReedsShepp.py
deleted file mode 100644
index b423963..0000000
--- a/Search-based Planning/Search_2D/Hybrid_Astar/ReedsSheppPlanner/ReedsShepp.py
+++ /dev/null
@@ -1,445 +0,0 @@
-"""
-This Reeds-Shepp Curves is from nathanlct's work
-(https://github.com/nathanlct/reeds-shepp-curves)
-"""
-
-import os
-import sys
-from enum import Enum
-
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../../../../Search-based Planning/")
-
-from Search_2D.Hybrid_Astar.ReedsSheppPlanner.utils import *
-
-
-class Steering(Enum):
- LEFT = 1
- RIGHT = 2
- STRAIGHT = 3
-
-
-class Gear(Enum):
- FORWARD = 1
- BACKWARD = 2
-
-
-class PathElement():
- def __init__(self, param, steering, gear):
- self.param = param
- self.steering = steering
- self.gear = gear
-
- def __repr__(self):
- if self.steering == Steering.LEFT:
- steering_str = "left"
- elif self.steering == Steering.RIGHT:
- steering_str = "right"
- else:
- steering_str = "straight"
-
- if self.gear == Gear.FORWARD:
- gear_str = "forward"
- else:
- gear_str = "backward"
-
- s = "{ Steering: " + steering_str + "\tGear: " + gear_str \
- + "\tdistance: " + str(round(self.param, 2)) + " }"
-
- return s
-
- def reverse_steering(self):
- if self.steering == Steering.LEFT:
- self.steering = Steering.RIGHT
- elif self.steering == Steering.RIGHT:
- self.steering = Steering.LEFT
-
- def reverse_gear(self):
- if self.gear == Gear.FORWARD:
- self.gear = Gear.BACKWARD
- else:
- self.gear = Gear.FORWARD
-
-
-def path_length(path):
- """
- this one's obvious
- """
- return sum([e.param for e in path])
-
-
-def get_optimal_path(start, end):
- """
- Return the shortest path from start to end among those that exist
- """
- paths = get_all_paths(start, end)
- i_min = 0
- L_min = path_length(paths[0])
- for i in range(1, len(paths) - 1):
- L = path_length(paths[i])
- if L <= L_min:
- L_min, i_min = L, i
- return paths[i_min]
-
-
-def get_all_paths(start, end):
- """
- Return a list of all the paths from start to end generated by the
- 12 functions and their variants
- """
- path_fns = [path1, path2, path3, path4, path5, path6, path7, path8, path9, path10, path11, path12]
- paths = []
-
- # get coordinates of end in the set of axis where start is (0,0,0)
- x, y, theta = change_of_basis(start, end)
-
- for get_path in path_fns:
- # get the four variants for each path type, cf article
- paths.append(get_path(x, y, theta))
- paths.append(timeflip(get_path(-x, y, -theta)))
- paths.append(reflect(get_path(x, -y, -theta)))
- paths.append(reflect(timeflip(get_path(-x, -y, theta))))
-
- # remove path elements that have parameter 0
- for i in range(len(paths)):
- paths[i] = list(filter(lambda e: e.param != 0, paths[i]))
-
- # remove empty paths
- paths = list(filter(None, paths))
-
- return paths
-
-
-def timeflip(path):
- """
- timeflip transform described around the end of the article
- """
- new_path = path.copy()
- for e in new_path:
- e.reverse_gear()
- return new_path
-
-
-def reflect(path):
- """
- reflect transform described around the end of the article
- """
- new_path = path.copy()
- for e in new_path:
- e.reverse_steering()
- return new_path
-
-
-def path1(x, y, phi):
- """
- Formula 8.1: CSC (same turns)
- """
- r, theta = R(x, y)
- phi = deg2rad(phi)
- path = []
-
- u, t = R(x - math.sin(phi), y - 1 + math.cos(phi))
- v = M(phi - t)
-
- if t >= 0 and u >= 0 and v >= 0:
- path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
- path.append(PathElement(u, Steering.STRAIGHT, Gear.FORWARD))
- path.append(PathElement(v, Steering.LEFT, Gear.FORWARD))
-
- return path
-
-
-def path2(x, y, phi):
- """
- Formula 8.2: CSC (opposite turns)
- """
- r, theta = R(x, y)
- phi = M(deg2rad(phi))
- path = []
-
- rho, t1 = R(x + math.sin(phi), y - 1 - math.cos(phi))
-
- if rho * rho >= 4:
- u = math.sqrt(rho * rho - 4)
- t = M(t1 + math.atan2(2, u))
- v = M(t - phi)
-
- if t >= 0 and u >= 0 and v >= 0:
- path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
- path.append(PathElement(u, Steering.STRAIGHT, Gear.FORWARD))
- path.append(PathElement(v, Steering.RIGHT, Gear.FORWARD))
-
- return path
-
-
-def path3(x, y, phi):
- """
- Formula 8.3: C|C|C
- """
- r, theta = R(x, y)
- phi = deg2rad(phi)
- path = []
-
- xi = x - math.sin(phi)
- eta = y - 1 + math.cos(phi)
- rho, theta = R(xi, eta)
-
- if rho <= 4:
- A = math.acos(rho / 4)
- t = M(theta + math.pi / 2 + A)
- u = M(math.pi - 2 * A)
- v = M(phi - t - u)
-
- if t >= 0 and u >= 0 and v >= 0:
- path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
- path.append(PathElement(u, Steering.RIGHT, Gear.BACKWARD))
- path.append(PathElement(v, Steering.LEFT, Gear.FORWARD))
-
- return path
-
-
-def path4(x, y, phi):
- """
- Formula 8.4 (1): C|CC
- """
- r, theta = R(x, y)
- phi = deg2rad(phi)
- path = []
-
- xi = x - math.sin(phi)
- eta = y - 1 + math.cos(phi)
- rho, theta = R(xi, eta)
-
- if rho <= 4:
- A = math.acos(rho / 4)
- t = M(theta + math.pi / 2 + A)
- u = M(math.pi - 2 * A)
- v = M(t + u - phi)
-
- if t >= 0 and u >= 0 and v >= 0:
- path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
- path.append(PathElement(u, Steering.RIGHT, Gear.BACKWARD))
- path.append(PathElement(v, Steering.LEFT, Gear.BACKWARD))
-
- return path
-
-
-def path5(x, y, phi):
- """
- Formula 8.4 (2): CC|C
- """
- r, theta = R(x, y)
- phi = deg2rad(phi)
- path = []
-
- xi = x - math.sin(phi)
- eta = y - 1 + math.cos(phi)
- rho, theta = R(xi, eta)
-
- if rho <= 4:
- u = math.acos(1 - rho * rho / 8)
- A = math.asin(2 * math.sin(u) / rho)
- t = M(theta + math.pi / 2 - A)
- v = M(t - u - phi)
-
- if t >= 0 and u >= 0 and v >= 0:
- path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
- path.append(PathElement(u, Steering.RIGHT, Gear.FORWARD))
- path.append(PathElement(v, Steering.LEFT, Gear.BACKWARD))
-
- return path
-
-
-def path6(x, y, phi):
- """
- Formula 8.7: CCu|CuC
- """
- r, theta = R(x, y)
- phi = deg2rad(phi)
- path = []
-
- xi = x + math.sin(phi)
- eta = y - 1 - math.cos(phi)
- rho, theta = R(xi, eta)
-
- if rho <= 4:
- if rho <= 2:
- A = math.acos((rho + 2) / 4)
- t = M(theta + math.pi / 2 + A)
- u = M(A)
- v = M(phi - t + 2 * u)
- else:
- A = math.acos((rho - 2) / 4)
- t = M(theta + math.pi / 2 - A)
- u = M(math.pi - A)
- v = M(phi - t + 2 * u)
-
- if t >= 0 and u >= 0 and v >= 0:
- path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
- path.append(PathElement(u, Steering.RIGHT, Gear.FORWARD))
- path.append(PathElement(u, Steering.LEFT, Gear.BACKWARD))
- path.append(PathElement(v, Steering.RIGHT, Gear.BACKWARD))
-
- return path
-
-
-def path7(x, y, phi):
- """
- Formula 8.8: C|CuCu|C
- """
- r, theta = R(x, y)
- phi = deg2rad(phi)
- path = []
-
- xi = x + math.sin(phi)
- eta = y - 1 - math.cos(phi)
- rho, theta = R(xi, eta)
- u1 = (20 - rho * rho) / 16
-
- if rho <= 6 and 0 <= u1 <= 1:
- u = math.acos(u1)
- A = math.asin(2 * math.sin(u) / rho)
- t = M(theta + math.pi / 2 + A)
- v = M(t - phi)
-
- if t >= 0 and u >= 0 and v >= 0:
- path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
- path.append(PathElement(u, Steering.RIGHT, Gear.BACKWARD))
- path.append(PathElement(u, Steering.LEFT, Gear.BACKWARD))
- path.append(PathElement(v, Steering.RIGHT, Gear.FORWARD))
-
- return path
-
-
-def path8(x, y, phi):
- """
- Formula 8.9 (1): C|C[pi/2]SC
- """
- r, theta = R(x, y)
- phi = deg2rad(phi)
- path = []
-
- xi = x - math.sin(phi)
- eta = y - 1 + math.cos(phi)
- rho, theta = R(xi, eta)
-
- if rho >= 2:
- u = math.sqrt(rho * rho - 4) - 2
- A = math.atan2(2, u + 2)
- t = M(theta + math.pi / 2 + A)
- v = M(t - phi + math.pi / 2)
-
- if t >= 0 and u >= 0 and v >= 0:
- path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
- path.append(PathElement(math.pi / 2, Steering.RIGHT, Gear.BACKWARD))
- path.append(PathElement(u, Steering.STRAIGHT, Gear.BACKWARD))
- path.append(PathElement(v, Steering.LEFT, Gear.BACKWARD))
-
- return path
-
-
-def path9(x, y, phi):
- """
- Formula 8.9 (2): CSC[pi/2]|C
- """
- r, theta = R(x, y)
- phi = deg2rad(phi)
- path = []
-
- xi = x - math.sin(phi)
- eta = y - 1 + math.cos(phi)
- rho, theta = R(xi, eta)
-
- if rho >= 2:
- u = math.sqrt(rho * rho - 4) - 2
- A = math.atan2(u + 2, 2)
- t = M(theta + math.pi / 2 - A)
- v = M(t - phi - math.pi / 2)
-
- if t >= 0 and u >= 0 and v >= 0:
- path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
- path.append(PathElement(u, Steering.STRAIGHT, Gear.FORWARD))
- path.append(PathElement(math.pi / 2, Steering.RIGHT, Gear.FORWARD))
- path.append(PathElement(v, Steering.LEFT, Gear.BACKWARD))
-
- return path
-
-
-def path10(x, y, phi):
- """
- Formula 8.10 (1): C|C[pi/2]SC
- """
- r, theta = R(x, y)
- phi = deg2rad(phi)
- path = []
-
- xi = x + math.sin(phi)
- eta = y - 1 - math.cos(phi)
- rho, theta = R(xi, eta)
-
- if rho >= 2:
- t = M(theta + math.pi / 2)
- u = rho - 2
- v = M(phi - t - math.pi / 2)
-
- if t >= 0 and u >= 0 and v >= 0:
- path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
- path.append(PathElement(math.pi / 2, Steering.RIGHT, Gear.BACKWARD))
- path.append(PathElement(u, Steering.STRAIGHT, Gear.BACKWARD))
- path.append(PathElement(v, Steering.RIGHT, Gear.BACKWARD))
-
- return path
-
-
-def path11(x, y, phi):
- """
- Formula 8.10 (2): CSC[pi/2]|C
- """
- r, theta = R(x, y)
- phi = deg2rad(phi)
- path = []
-
- xi = x + math.sin(phi)
- eta = y - 1 - math.cos(phi)
- rho, theta = R(xi, eta)
-
- if rho >= 2:
- t = M(theta)
- u = rho - 2
- v = M(phi - t - math.pi / 2)
-
- if t >= 0 and u >= 0 and v >= 0:
- path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
- path.append(PathElement(u, Steering.STRAIGHT, Gear.FORWARD))
- path.append(PathElement(math.pi / 2, Steering.LEFT, Gear.FORWARD))
- path.append(PathElement(v, Steering.RIGHT, Gear.BACKWARD))
-
- return path
-
-
-def path12(x, y, phi):
- """
- Formula 8.11: C|C[pi/2]SC[pi/2]|C
- """
- r, theta = R(x, y)
- phi = deg2rad(phi)
- path = []
-
- xi = x + math.sin(phi)
- eta = y - 1 - math.cos(phi)
- rho, theta = R(xi, eta)
-
- if rho >= 4:
- u = math.sqrt(rho * rho - 4) - 4
- A = math.atan2(2, u + 4)
- t = M(theta + math.pi / 2 + A)
- v = M(t - phi)
-
- if t >= 0 and u >= 0 and v >= 0:
- path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
- path.append(PathElement(math.pi / 2, Steering.RIGHT, Gear.BACKWARD))
- path.append(PathElement(u, Steering.STRAIGHT, Gear.BACKWARD))
- path.append(PathElement(math.pi / 2, Steering.LEFT, Gear.BACKWARD))
- path.append(PathElement(v, Steering.RIGHT, Gear.FORWARD))
-
- return path
diff --git a/Search-based Planning/Search_2D/Hybrid_Astar/ReedsSheppPlanner/__pycache__/ReedsShepp.cpython-37.pyc b/Search-based Planning/Search_2D/Hybrid_Astar/ReedsSheppPlanner/__pycache__/ReedsShepp.cpython-37.pyc
deleted file mode 100644
index c1f82af..0000000
Binary files a/Search-based Planning/Search_2D/Hybrid_Astar/ReedsSheppPlanner/__pycache__/ReedsShepp.cpython-37.pyc and /dev/null differ
diff --git a/Search-based Planning/Search_2D/Hybrid_Astar/ReedsSheppPlanner/__pycache__/draw.cpython-37.pyc b/Search-based Planning/Search_2D/Hybrid_Astar/ReedsSheppPlanner/__pycache__/draw.cpython-37.pyc
deleted file mode 100644
index 9b88272..0000000
Binary files a/Search-based Planning/Search_2D/Hybrid_Astar/ReedsSheppPlanner/__pycache__/draw.cpython-37.pyc and /dev/null differ
diff --git a/Search-based Planning/Search_2D/Hybrid_Astar/ReedsSheppPlanner/__pycache__/utils.cpython-37.pyc b/Search-based Planning/Search_2D/Hybrid_Astar/ReedsSheppPlanner/__pycache__/utils.cpython-37.pyc
deleted file mode 100644
index 68c776e..0000000
Binary files a/Search-based Planning/Search_2D/Hybrid_Astar/ReedsSheppPlanner/__pycache__/utils.cpython-37.pyc and /dev/null differ
diff --git a/Search-based Planning/Search_2D/Hybrid_Astar/ReedsSheppPlanner/draw.py b/Search-based Planning/Search_2D/Hybrid_Astar/ReedsSheppPlanner/draw.py
deleted file mode 100644
index 1b57042..0000000
--- a/Search-based Planning/Search_2D/Hybrid_Astar/ReedsSheppPlanner/draw.py
+++ /dev/null
@@ -1,89 +0,0 @@
-import turtle
-import random as rd
-import os
-import sys
-
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../../../../Search-based Planning/")
-
-from Search_2D.Hybrid_Astar.ReedsSheppPlanner.utils import *
-import Search_2D.Hybrid_Astar.ReedsSheppPlanner.ReedsShepp as rs
-
-
-# drawing n units (eg turtle.forward(n)) will draw n * SCALE pixels
-SCALE = 40
-
-
-def scale(x):
- """
- Scale the input coordinate(s).
- """
- if type(x) is tuple or type(x) is list:
- return [p * SCALE for p in x]
- return x * SCALE
-
-
-def unscale(x):
- """
- Unscale the input coordinate(s).
- """
- if type(x) is tuple or type(x) is list:
- return [p / SCALE for p in x]
- return x / SCALE
-
-
-# note: bob is a turtle
-
-def vec(bob):
- """
- Draw an arrow.
- """
- bob.down()
- bob.pensize(3)
- bob.forward(scale(1.2))
- bob.right(25)
- bob.backward(scale(.4))
- bob.forward(scale(.4))
- bob.left(50)
- bob.backward(scale(.4))
- bob.forward(scale(.4))
- bob.right(25)
- bob.pensize(1)
- bob.up()
-
-
-def goto(bob, pos, scale_pos=True):
- """
- Go to a position without drawing.
- """
- bob.up()
- if scale_pos:
- bob.setpos(scale(pos[:2]))
- else:
- bob.setpos(pos[:2])
- bob.setheading(pos[2])
- bob.down()
-
-
-def draw_path(bob, path):
- """
- Draw the path (list of RS.PathElements).
- """
- for e in path:
- gear = 1 if e.gear == rs.Gear.FORWARD else -1
- if e.steering == rs.Steering.LEFT:
- bob.circle(scale(1), gear * rad2deg(e.param))
- elif e.steering == rs.Steering.RIGHT:
- bob.circle(- scale(1), gear * rad2deg(e.param))
- elif e.steering == rs.Steering.STRAIGHT:
- bob.forward(gear * scale(e.param))
-
-
-def set_random_pencolor(bob):
- """
- Draws noodles.
- """
- r, g, b = 1, 1, 1
- while r + g + b > 2.5:
- r, g, b = rd.uniform(0, 1), rd.uniform(0, 1), rd.uniform(0, 1)
- bob.pencolor(r, g, b)
diff --git a/Search-based Planning/Search_2D/Hybrid_Astar/ReedsSheppPlanner/simulation.py b/Search-based Planning/Search_2D/Hybrid_Astar/ReedsSheppPlanner/simulation.py
deleted file mode 100644
index d1d7fb6..0000000
--- a/Search-based Planning/Search_2D/Hybrid_Astar/ReedsSheppPlanner/simulation.py
+++ /dev/null
@@ -1,76 +0,0 @@
-import turtle
-import random as rd
-import math
-
-import os
-import sys
-
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../../../../Search-based Planning/")
-
-import Search_2D.Hybrid_Astar.ReedsSheppPlanner.ReedsShepp as RS
-from Search_2D.Hybrid_Astar.ReedsSheppPlanner import draw
-
-
-def main():
- # points to be followed
- # pts = [(-6, -7), (-6, 0), (-4, 6), (0, 5), (0, -2), (-2, -6), (3, -5), (3, 6), (6, 4)]
- #
- # # generate PATH so the vectors are pointing at each other
- # PATH = []
- # for i in range(len(pts) - 1):
- # dx = pts[i + 1][0] - pts[i][0]
- # dy = pts[i + 1][1] - pts[i][1]
- # theta = math.atan2(dy, dx)
- # PATH.append((pts[i][0], pts[i][1], utils.rad2deg(theta)))
- # PATH.append((pts[-1][0], pts[-1][1], 0))
-
- # or you can also manually set the angles:
- PATH = [(-5, 5, 90), (-5, 5, -90), (1, 4, 180), (5, 4, 0), (6, -3, 90), (4, -4, -40), (-2, 0, 240),
- (-6, -7, 160), (-7, -1, 80)]
-
- # or just generate a random route:
- # PATH = []
- # for _ in range(10):
- # PATH.append((rd.randint(-7, 7), rd.randint(-7, 7), rd.randint(0, 359)))
-
- # init turtle
- tesla = turtle.Turtle()
- tesla.speed(0) # 0: fast; 1: slow, 8.4: cool
- tesla.shape('arrow')
- tesla.resizemode('user')
- tesla.shapesize(1, 1)
-
- # draw vectors representing points in PATH
- for pt in PATH:
- draw.goto(tesla, pt)
- draw.vec(tesla)
-
- # draw all routes found
- tesla.speed(0)
- for i in range(len(PATH) - 1):
- paths = RS.get_all_paths(PATH[i], PATH[i + 1])
-
- for path in paths:
- draw.set_random_pencolor(tesla)
- draw.goto(tesla, PATH[i])
- draw.draw_path(tesla, path)
-
- # draw shortest route
- tesla.pencolor(1, 0, 0)
- tesla.pensize(3)
- tesla.speed(2)
- draw.goto(tesla, PATH[0])
- path_length = 0
- for i in range(len(PATH) - 1):
- path = RS.get_optimal_path(PATH[i], PATH[i + 1])
- path_length += RS.path_length(path)
- draw.draw_path(tesla, path)
-
- print("Shortest path length: {} px.".format(int(draw.scale(path_length))))
-
- turtle.done()
-
-
-if __name__ == '__main__':
- main()
diff --git a/Search-based Planning/Search_2D/Hybrid_Astar/ReedsSheppPlanner/utils.py b/Search-based Planning/Search_2D/Hybrid_Astar/ReedsSheppPlanner/utils.py
deleted file mode 100644
index efb5dd5..0000000
--- a/Search-based Planning/Search_2D/Hybrid_Astar/ReedsSheppPlanner/utils.py
+++ /dev/null
@@ -1,50 +0,0 @@
-import math
-
-
-def M(theta):
- """
- Return the angle phi = theta mod (2 pi) such that -pi <= theta < pi.
- """
- theta = theta % (2 * math.pi)
- if theta < -math.pi:
- return theta + 2 * math.pi
- if theta >= math.pi:
- return theta - 2 * math.pi
- return theta
-
-
-def R(x, y):
- """
- Return the polar coordinates (r, theta) of the point (x, y).
- """
- r = math.sqrt(x * x + y * y)
- theta = math.atan2(y, x)
- return r, theta
-
-
-def change_of_basis(p1, p2):
- """
- Given p1 = (x1, y1, theta1) and p2 = (x2, y2, theta2) represented in a
- coordinate system with origin (0, 0) and rotation 0 (in degrees), return
- the position and rotation of p2 in the coordinate system which origin
- (x1, y1) and rotation theta1.
- """
- theta1 = deg2rad(p1[2])
- dx = p2[0] - p1[0]
- dy = p2[1] - p1[1]
- new_x = dx * math.cos(theta1) + dy * math.sin(theta1)
- new_y = -dx * math.sin(theta1) + dy * math.cos(theta1)
- new_theta = p2[2] - p1[2]
- return new_x, new_y, new_theta
-
-
-def rad2deg(rad):
- return 180 * rad / math.pi
-
-
-def deg2rad(deg):
- return math.pi * deg / 180
-
-
-def sign(x):
- return 1 if x >= 0 else -1
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 de55fab..033d81b 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 b11ed3b..86c10d3 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/__pycache__/queue.cpython-37.pyc b/Search-based Planning/Search_2D/__pycache__/queue.cpython-37.pyc
index 183bce0..bcbb608 100644
Binary files a/Search-based Planning/Search_2D/__pycache__/queue.cpython-37.pyc and b/Search-based Planning/Search_2D/__pycache__/queue.cpython-37.pyc differ
diff --git a/Search-based Planning/Search_2D/best_first.py b/Search-based Planning/Search_2D/best_first.py
new file mode 100644
index 0000000..326e73b
--- /dev/null
+++ b/Search-based Planning/Search_2D/best_first.py
@@ -0,0 +1,127 @@
+"""
+Best-First Searching
+@author: huiming zhou
+"""
+
+import os
+import sys
+import math
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+ "/../../Search-based Planning/")
+
+from Search_2D import queue
+from Search_2D import plotting
+from Search_2D import env
+
+
+class BestFirst:
+ def __init__(self, s_start, s_goal):
+ self.s_start, self.s_goal = s_start, 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 = queue.QueuePrior() # OPEN set
+ self.OPEN.put(self.s_start, self.Heuristic(self.s_start))
+ self.CLOSED = [] # CLOSED set / visited order
+ self.PARENT = {self.s_start: self.s_start}
+
+ def searching(self):
+ """
+ Best-first Searching
+ :return: planning path, visited order
+ """
+
+ while self.OPEN:
+ s = self.OPEN.get()
+
+ if s == self.s_goal:
+ break
+ self.CLOSED.append(s)
+
+ for s_n in self.get_neighbor(s):
+ if s_n not in self.PARENT: # node not explored
+ self.OPEN.put(s_n, self.Heuristic(s_n))
+ self.PARENT[s_n] = s
+
+ return self.extract_path(), self.CLOSED
+
+ def Heuristic(self, s):
+ """
+ estimated distance between current state and goal state.
+ :param s: current state
+ :return: estimated distance
+ """
+
+ h = math.hypot(s[0] - self.s_goal[0], s[1] - self.s_goal[1])
+
+ return h
+
+ 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 not self.is_collision(s, s_next):
+ s_list.append(s_next)
+
+ return s_list
+
+ 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 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)
+
+
+def main():
+ s_start = (5, 5)
+ s_goal = (45, 25)
+
+ BF = BestFirst(s_start, s_goal)
+ 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/Search-based Planning/gif/BF.gif b/Search-based Planning/gif/BF.gif
new file mode 100644
index 0000000..1352ba2
Binary files /dev/null and b/Search-based Planning/gif/BF.gif differ