update

Search-based Planning/.idea/workspace.xml

@@ -19,7 +19,15 @@
     <select />
   </component>
   <component name="ChangeListManager">
-    <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="" />
+    <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
+      <change afterPath="$PROJECT_DIR$/Search_2D/best_first.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/.idea/workspace.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/workspace.xml" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/Hybrid_Astar/ReedsSheppPlanner/ReedsShepp.py" beforeDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/Hybrid_Astar/ReedsSheppPlanner/draw.py" beforeDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/Hybrid_Astar/ReedsSheppPlanner/simulation.py" beforeDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/Hybrid_Astar/ReedsSheppPlanner/utils.py" beforeDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/plotting.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/plotting.py" afterDir="false" />
+    </list>
     <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
     <option name="SHOW_DIALOG" value="false" />
     <option name="HIGHLIGHT_CONFLICTS" value="true" />
@@ -70,7 +78,7 @@
       </list>
     </option>
   </component>
-  <component name="RunManager" selected="Python.Field_D_star">
+  <component name="RunManager" selected="Python.best_first">
     <configuration name="ARAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
@@ -92,7 +100,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="Astar3D" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="D_star_Lite" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -100,11 +108,11 @@
         <env name="PYTHONUNBUFFERED" value="1" />
       </envs>
       <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_3D" />
+      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_2D" />
       <option name="IS_MODULE_SDK" value="true" />
       <option name="ADD_CONTENT_ROOTS" value="true" />
       <option name="ADD_SOURCE_ROOTS" value="true" />
-      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_3D/Astar3D.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/D_star_Lite.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -113,7 +121,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="D_star_Lite" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="Field_D_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -125,7 +133,7 @@
       <option name="IS_MODULE_SDK" value="true" />
       <option name="ADD_CONTENT_ROOTS" value="true" />
       <option name="ADD_SOURCE_ROOTS" value="true" />
-      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/D_star_Lite.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/Field_D_star.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -134,7 +142,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="Field_D_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="Potential_Field" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -146,7 +154,7 @@
       <option name="IS_MODULE_SDK" value="true" />
       <option name="ADD_CONTENT_ROOTS" value="true" />
       <option name="ADD_SOURCE_ROOTS" value="true" />
-      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/Field_D_star.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/Potential_Field.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -155,7 +163,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="Potential_Field" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="best_first" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -167,7 +175,7 @@
       <option name="IS_MODULE_SDK" value="true" />
       <option name="ADD_CONTENT_ROOTS" value="true" />
       <option name="ADD_SOURCE_ROOTS" value="true" />
-      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/Potential_Field.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/best_first.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -199,19 +207,19 @@
     </configuration>
     <list>
       <item itemvalue="Python.dijkstra" />
-      <item itemvalue="Python.Astar3D" />
       <item itemvalue="Python.ARAstar" />
       <item itemvalue="Python.Potential_Field" />
       <item itemvalue="Python.D_star_Lite" />
       <item itemvalue="Python.Field_D_star" />
+      <item itemvalue="Python.best_first" />
     </list>
     <recent_temporary>
       <list>
+        <item itemvalue="Python.best_first" />
         <item itemvalue="Python.Field_D_star" />
         <item itemvalue="Python.D_star_Lite" />
         <item itemvalue="Python.Potential_Field" />
         <item itemvalue="Python.ARAstar" />
-        <item itemvalue="Python.Astar3D" />
       </list>
     </recent_temporary>
   </component>

Search-based Planning/Search_2D/Hybrid_Astar/ReedsSheppPlanner/ReedsShepp.py

@@ -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

Search-based Planning/Search_2D/Hybrid_Astar/ReedsSheppPlanner/draw.py

@@ -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)

Search-based Planning/Search_2D/Hybrid_Astar/ReedsSheppPlanner/simulation.py

@@ -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()

Search-based Planning/Search_2D/Hybrid_Astar/ReedsSheppPlanner/utils.py

@@ -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

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()