init

.gitignore

@@ -0,0 +1,4 @@
+*.jpg
+*.exe
+
+input/

planner/hybridastar/astar.py

@@ -0,0 +1,264 @@
+import heapq
+import math
+import numpy as np
+import matplotlib.pyplot as plt
+
+
+class Node:
+    def __init__(self, x, y, cost, pind):
+        self.x = x  # x position of node
+        self.y = y  # y position of node
+        self.cost = cost  # g cost of node
+        self.pind = pind  # parent index of node
+
+
+class Para:
+    def __init__(self, minx, miny, maxx, maxy, xw, yw, reso, motion):
+        self.minx = minx
+        self.miny = miny
+        self.maxx = maxx
+        self.maxy = maxy
+        self.xw = xw
+        self.yw = yw
+        self.reso = reso  # resolution of grid world
+        self.motion = motion  # motion set
+
+
+def astar_planning(sx, sy, gx, gy, ox, oy, reso, rr):
+    """
+    return path of A*.
+    :param sx: starting node x [m]
+    :param sy: starting node y [m]
+    :param gx: goal node x [m]
+    :param gy: goal node y [m]
+    :param ox: obstacles x positions [m]
+    :param oy: obstacles y positions [m]
+    :param reso: xy grid resolution
+    :param rr: robot radius
+    :return: path
+    """
+
+    n_start = Node(round(sx / reso), round(sy / reso), 0.0, -1)
+    n_goal = Node(round(gx / reso), round(gy / reso), 0.0, -1)
+
+    ox = [x / reso for x in ox]
+    oy = [y / reso for y in oy]
+
+    P, obsmap = calc_parameters(ox, oy, rr, reso)
+
+    open_set, closed_set = dict(), dict()
+    open_set[calc_index(n_start, P)] = n_start
+
+    q_priority = []
+    heapq.heappush(q_priority,
+                   (fvalue(n_start, n_goal), calc_index(n_start, P)))
+
+    while True:
+        if not open_set:
+            break
+
+        _, ind = heapq.heappop(q_priority)
+        n_curr = open_set[ind]
+        closed_set[ind] = n_curr
+        open_set.pop(ind)
+
+        for i in range(len(P.motion)):
+            node = Node(n_curr.x + P.motion[i][0],
+                        n_curr.y + P.motion[i][1],
+                        n_curr.cost + u_cost(P.motion[i]), ind)
+
+            if not check_node(node, P, obsmap):
+                continue
+
+            n_ind = calc_index(node, P)
+            if n_ind not in closed_set:
+                if n_ind in open_set:
+                    if open_set[n_ind].cost > node.cost:
+                        open_set[n_ind].cost = node.cost
+                        open_set[n_ind].pind = ind
+                else:
+                    open_set[n_ind] = node
+                    heapq.heappush(q_priority,
+                                   (fvalue(node, n_goal), calc_index(node, P)))
+
+    pathx, pathy = extract_path(closed_set, n_start, n_goal, P)
+
+    return pathx, pathy
+
+
+def calc_holonomic_heuristic_with_obstacle(node, ox, oy, reso, rr):
+    n_goal = Node(round(node.x[-1] / reso), round(node.y[-1] / reso), 0.0, -1)
+
+    ox = [x / reso for x in ox]
+    oy = [y / reso for y in oy]
+
+    P, obsmap = calc_parameters(ox, oy, reso, rr)
+
+    open_set, closed_set = dict(), dict()
+    open_set[calc_index(n_goal, P)] = n_goal
+
+    q_priority = []
+    heapq.heappush(q_priority, (n_goal.cost, calc_index(n_goal, P)))
+
+    while True:
+        if not open_set:
+            break
+
+        _, ind = heapq.heappop(q_priority)
+        n_curr = open_set[ind]
+        closed_set[ind] = n_curr
+        open_set.pop(ind)
+
+        for i in range(len(P.motion)):
+            node = Node(n_curr.x + P.motion[i][0],
+                        n_curr.y + P.motion[i][1],
+                        n_curr.cost + u_cost(P.motion[i]), ind)
+
+            if not check_node(node, P, obsmap):
+                continue
+
+            n_ind = calc_index(node, P)
+            if n_ind not in closed_set:
+                if n_ind in open_set:
+                    if open_set[n_ind].cost > node.cost:
+                        open_set[n_ind].cost = node.cost
+                        open_set[n_ind].pind = ind
+                else:
+                    open_set[n_ind] = node
+                    heapq.heappush(q_priority, (node.cost, calc_index(node, P)))
+
+    hmap = [[np.inf for _ in range(P.yw)] for _ in range(P.xw)]
+
+    for n in closed_set.values():
+        hmap[n.x - P.minx][n.y - P.miny] = n.cost
+
+    return hmap
+
+
+def check_node(node, P, obsmap):
+    if node.x <= P.minx or node.x >= P.maxx or \
+            node.y <= P.miny or node.y >= P.maxy:
+        return False
+
+    if obsmap[node.x - P.minx][node.y - P.miny]:
+        return False
+
+    return True
+
+
+def u_cost(u):
+    return math.hypot(u[0], u[1])
+
+
+def fvalue(node, n_goal):
+    return node.cost + h(node, n_goal)
+
+
+def h(node, n_goal):
+    return math.hypot(node.x - n_goal.x, node.y - n_goal.y)
+
+
+def calc_index(node, P):
+    return (node.y - P.miny) * P.xw + (node.x - P.minx)
+
+
+def calc_parameters(ox, oy, rr, reso):
+    minx, miny = round(min(ox)), round(min(oy))
+    maxx, maxy = round(max(ox)), round(max(oy))
+    xw, yw = maxx - minx, maxy - miny
+
+    motion = get_motion()
+    P = Para(minx, miny, maxx, maxy, xw, yw, reso, motion)
+    obsmap = calc_obsmap(ox, oy, rr, P)
+
+    return P, obsmap
+
+
+def calc_obsmap(ox, oy, rr, P):
+    obsmap = [[False for _ in range(P.yw)] for _ in range(P.xw)]
+
+    for x in range(P.xw):
+        xx = x + P.minx
+        for y in range(P.yw):
+            yy = y + P.miny
+            for oxx, oyy in zip(ox, oy):
+                if math.hypot(oxx - xx, oyy - yy) <= rr / P.reso:
+                    obsmap[x][y] = True
+                    break
+
+    return obsmap
+
+
+def extract_path(closed_set, n_start, n_goal, P):
+    pathx, pathy = [n_goal.x], [n_goal.y]
+    n_ind = calc_index(n_goal, P)
+
+    while True:
+        node = closed_set[n_ind]
+        pathx.append(node.x)
+        pathy.append(node.y)
+        n_ind = node.pind
+
+        if node == n_start:
+            break
+
+    pathx = [x * P.reso for x in reversed(pathx)]
+    pathy = [y * P.reso for y in reversed(pathy)]
+
+    return pathx, pathy
+
+
+def get_motion():
+    motion = [[-1, 0], [-1, 1], [0, 1], [1, 1],
+              [1, 0], [1, -1], [0, -1], [-1, -1]]
+
+    return motion
+
+
+def get_env():
+    ox, oy = [], []
+
+    for i in range(60):
+        ox.append(i)
+        oy.append(0.0)
+    for i in range(60):
+        ox.append(60.0)
+        oy.append(i)
+    for i in range(61):
+        ox.append(i)
+        oy.append(60.0)
+    for i in range(61):
+        ox.append(0.0)
+        oy.append(i)
+    for i in range(40):
+        ox.append(20.0)
+        oy.append(i)
+    for i in range(40):
+        ox.append(40.0)
+        oy.append(60.0 - i)
+
+    return ox, oy
+
+
+def main():
+    sx = 10.0  # [m]
+    sy = 10.0  # [m]
+    gx = 50.0  # [m]
+    gy = 50.0  # [m]
+
+    robot_radius = 2.0
+    grid_resolution = 1.0
+    ox, oy = get_env()
+
+    pathx, pathy = astar_planning(sx, sy, gx, gy, ox, oy, grid_resolution, robot_radius)
+
+    plt.plot(ox, oy, 'sk')
+    plt.plot(pathx, pathy, '-r')
+    plt.plot(sx, sy, 'sg')
+    plt.plot(gx, gy, 'sb')
+    plt.axis("equal")
+    plt.show()
+
+
+if __name__ == '__main__':
+    main()

planner/hybridastar/planer_reeds_shepp.py

@@ -0,0 +1,687 @@
+import time
+import math
+import numpy as np
+
+
+# parameters initiation
+STEP_SIZE = 0.2
+MAX_LENGTH = 1000.0
+PI = math.pi
+
+# class for PATH element
+class PATH:
+    def __init__(self, lengths, ctypes, L, x, y, yaw, directions):
+        self.lengths = lengths              # lengths of each part of path (+: forward, -: backward) [float]
+        self.ctypes = ctypes                # type of each part of the path [string]
+        self.L = L                          # total path length [float]
+        self.x = x                          # final x positions [m]
+        self.y = y                          # final y positions [m]
+        self.yaw = yaw                      # final yaw angles [rad]
+        self.directions = directions        # forward: 1, backward:-1
+
+
+def calc_optimal_path(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=STEP_SIZE):
+    paths = calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=step_size)
+
+    minL = paths[0].L
+    mini = 0
+
+    for i in range(len(paths)):
+        if paths[i].L <= minL:
+            minL, mini = paths[i].L, i
+
+    return paths[mini]
+
+
+def calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=STEP_SIZE):
+    q0 = [sx, sy, syaw]
+    q1 = [gx, gy, gyaw]
+
+    paths = generate_path(q0, q1, maxc)
+
+    for path in paths:
+        x, y, yaw, directions = \
+            generate_local_course(path.L, path.lengths,
+                                  path.ctypes, maxc, step_size * maxc)
+
+        # convert global coordinate
+        path.x = [math.cos(-q0[2]) * ix + math.sin(-q0[2]) * iy + q0[0] for (ix, iy) in zip(x, y)]
+        path.y = [-math.sin(-q0[2]) * ix + math.cos(-q0[2]) * iy + q0[1] for (ix, iy) in zip(x, y)]
+        path.yaw = [pi_2_pi(iyaw + q0[2]) for iyaw in yaw]
+        path.directions = directions
+        path.lengths = [l / maxc for l in path.lengths]
+        path.L = path.L / maxc
+
+    return paths
+
+
+def set_path(paths, lengths, ctypes):
+    path = PATH([], [], 0.0, [], [], [], [])
+    path.ctypes = ctypes
+    path.lengths = lengths
+    # check same path exist
+    for path_e in paths:
+        if path_e.ctypes == path.ctypes:
+            if sum([x - y for x, y in zip(path_e.lengths, path.lengths)]) <= 0.01:
+                return paths  # not insert path
+
+    path.L = sum([abs(i) for i in lengths])
+
+    if path.L >= MAX_LENGTH:
+        return paths
+
+    assert path.L >= 0.01
+    paths.append(path)
+
+    return paths
+
+
+def LSL(x, y, phi):
+    u, t = R(x - math.sin(phi), y - 1.0 + math.cos(phi))
+
+    if t >= 0.0:
+        v = M(phi - t)
+        if v >= 0.0:
+            return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def LSR(x, y, phi):
+    u1, t1 = R(x + math.sin(phi), y - 1.0 - math.cos(phi))
+    u1 = u1 ** 2
+
+    if u1 >= 4.0:
+        u = math.sqrt(u1 - 4.0)
+        theta = math.atan2(2.0, u)
+        t = M(t1 + theta)
+        v = M(t - phi)
+
+        if t >= 0.0 and v >= 0.0:
+            return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def LRL(x, y, phi):
+    u1, t1 = R(x - math.sin(phi), y - 1.0 + math.cos(phi))
+
+    if u1 <= 4.0:
+        u = -2.0 * math.asin(0.25 * u1)
+        t = M(t1 + 0.5 * u + PI)
+        v = M(phi - t + u)
+
+        if t >= 0.0 and u <= 0.0:
+            return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def SCS(x, y, phi, paths):
+    flag, t, u, v = SLS(x, y, phi)
+
+    if flag:
+        paths = set_path(paths, [t, u, v], ["S", "WB", "S"])
+
+    flag, t, u, v = SLS(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, u, v], ["S", "R", "S"])
+
+    return paths
+
+
+def SLS(x, y, phi):
+    phi = M(phi)
+
+    if y > 0.0 and 0.0 < phi < PI * 0.99:
+        xd = -y / math.tan(phi) + x
+        t = xd - math.tan(phi / 2.0)
+        u = phi
+        v = math.sqrt((x - xd) ** 2 + y ** 2) - math.tan(phi / 2.0)
+        return True, t, u, v
+    elif y < 0.0 and 0.0 < phi < PI * 0.99:
+        xd = -y / math.tan(phi) + x
+        t = xd - math.tan(phi / 2.0)
+        u = phi
+        v = -math.sqrt((x - xd) ** 2 + y ** 2) - math.tan(phi / 2.0)
+        return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def CSC(x, y, phi, paths):
+    flag, t, u, v = LSL(x, y, phi)
+    if flag:
+        paths = set_path(paths, [t, u, v], ["WB", "S", "WB"])
+
+    flag, t, u, v = LSL(-x, y, -phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, -v], ["WB", "S", "WB"])
+
+    flag, t, u, v = LSL(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, u, v], ["R", "S", "R"])
+
+    flag, t, u, v = LSL(-x, -y, phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, -v], ["R", "S", "R"])
+
+    flag, t, u, v = LSR(x, y, phi)
+    if flag:
+        paths = set_path(paths, [t, u, v], ["WB", "S", "R"])
+
+    flag, t, u, v = LSR(-x, y, -phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, -v], ["WB", "S", "R"])
+
+    flag, t, u, v = LSR(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, u, v], ["R", "S", "WB"])
+
+    flag, t, u, v = LSR(-x, -y, phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, -v], ["R", "S", "WB"])
+
+    return paths
+
+
+def CCC(x, y, phi, paths):
+    flag, t, u, v = LRL(x, y, phi)
+    if flag:
+        paths = set_path(paths, [t, u, v], ["WB", "R", "WB"])
+
+    flag, t, u, v = LRL(-x, y, -phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, -v], ["WB", "R", "WB"])
+
+    flag, t, u, v = LRL(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, u, v], ["R", "WB", "R"])
+
+    flag, t, u, v = LRL(-x, -y, phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, -v], ["R", "WB", "R"])
+
+    # backwards
+    xb = x * math.cos(phi) + y * math.sin(phi)
+    yb = x * math.sin(phi) - y * math.cos(phi)
+
+    flag, t, u, v = LRL(xb, yb, phi)
+    if flag:
+        paths = set_path(paths, [v, u, t], ["WB", "R", "WB"])
+
+    flag, t, u, v = LRL(-xb, yb, -phi)
+    if flag:
+        paths = set_path(paths, [-v, -u, -t], ["WB", "R", "WB"])
+
+    flag, t, u, v = LRL(xb, -yb, -phi)
+    if flag:
+        paths = set_path(paths, [v, u, t], ["R", "WB", "R"])
+
+    flag, t, u, v = LRL(-xb, -yb, phi)
+    if flag:
+        paths = set_path(paths, [-v, -u, -t], ["R", "WB", "R"])
+
+    return paths
+
+
+def calc_tauOmega(u, v, xi, eta, phi):
+    delta = M(u - v)
+    A = math.sin(u) - math.sin(delta)
+    B = math.cos(u) - math.cos(delta) - 1.0
+
+    t1 = math.atan2(eta * A - xi * B, xi * A + eta * B)
+    t2 = 2.0 * (math.cos(delta) - math.cos(v) - math.cos(u)) + 3.0
+
+    if t2 < 0:
+        tau = M(t1 + PI)
+    else:
+        tau = M(t1)
+
+    omega = M(tau - u + v - phi)
+
+    return tau, omega
+
+
+def LRLRn(x, y, phi):
+    xi = x + math.sin(phi)
+    eta = y - 1.0 - math.cos(phi)
+    rho = 0.25 * (2.0 + math.sqrt(xi * xi + eta * eta))
+
+    if rho <= 1.0:
+        u = math.acos(rho)
+        t, v = calc_tauOmega(u, -u, xi, eta, phi)
+        if t >= 0.0 and v <= 0.0:
+            return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def LRLRp(x, y, phi):
+    xi = x + math.sin(phi)
+    eta = y - 1.0 - math.cos(phi)
+    rho = (20.0 - xi * xi - eta * eta) / 16.0
+
+    if 0.0 <= rho <= 1.0:
+        u = -math.acos(rho)
+        if u >= -0.5 * PI:
+            t, v = calc_tauOmega(u, u, xi, eta, phi)
+            if t >= 0.0 and v >= 0.0:
+                return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def CCCC(x, y, phi, paths):
+    flag, t, u, v = LRLRn(x, y, phi)
+    if flag:
+        paths = set_path(paths, [t, u, -u, v], ["WB", "R", "WB", "R"])
+
+    flag, t, u, v = LRLRn(-x, y, -phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, u, -v], ["WB", "R", "WB", "R"])
+
+    flag, t, u, v = LRLRn(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, u, -u, v], ["R", "WB", "R", "WB"])
+
+    flag, t, u, v = LRLRn(-x, -y, phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, u, -v], ["R", "WB", "R", "WB"])
+
+    flag, t, u, v = LRLRp(x, y, phi)
+    if flag:
+        paths = set_path(paths, [t, u, u, v], ["WB", "R", "WB", "R"])
+
+    flag, t, u, v = LRLRp(-x, y, -phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, -u, -v], ["WB", "R", "WB", "R"])
+
+    flag, t, u, v = LRLRp(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, u, u, v], ["R", "WB", "R", "WB"])
+
+    flag, t, u, v = LRLRp(-x, -y, phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, -u, -v], ["R", "WB", "R", "WB"])
+
+    return paths
+
+
+def LRSR(x, y, phi):
+    xi = x + math.sin(phi)
+    eta = y - 1.0 - math.cos(phi)
+    rho, theta = R(-eta, xi)
+
+    if rho >= 2.0:
+        t = theta
+        u = 2.0 - rho
+        v = M(t + 0.5 * PI - phi)
+        if t >= 0.0 and u <= 0.0 and v <= 0.0:
+            return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def LRSL(x, y, phi):
+    xi = x - math.sin(phi)
+    eta = y - 1.0 + math.cos(phi)
+    rho, theta = R(xi, eta)
+
+    if rho >= 2.0:
+        r = math.sqrt(rho * rho - 4.0)
+        u = 2.0 - r
+        t = M(theta + math.atan2(r, -2.0))
+        v = M(phi - 0.5 * PI - t)
+        if t >= 0.0 and u <= 0.0 and v <= 0.0:
+            return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def CCSC(x, y, phi, paths):
+    flag, t, u, v = LRSL(x, y, phi)
+    if flag:
+        paths = set_path(paths, [t, -0.5 * PI, u, v], ["WB", "R", "S", "WB"])
+
+    flag, t, u, v = LRSL(-x, y, -phi)
+    if flag:
+        paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["WB", "R", "S", "WB"])
+
+    flag, t, u, v = LRSL(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, -0.5 * PI, u, v], ["R", "WB", "S", "R"])
+
+    flag, t, u, v = LRSL(-x, -y, phi)
+    if flag:
+        paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["R", "WB", "S", "R"])
+
+    flag, t, u, v = LRSR(x, y, phi)
+    if flag:
+        paths = set_path(paths, [t, -0.5 * PI, u, v], ["WB", "R", "S", "R"])
+
+    flag, t, u, v = LRSR(-x, y, -phi)
+    if flag:
+        paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["WB", "R", "S", "R"])
+
+    flag, t, u, v = LRSR(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, -0.5 * PI, u, v], ["R", "WB", "S", "WB"])
+
+    flag, t, u, v = LRSR(-x, -y, phi)
+    if flag:
+        paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["R", "WB", "S", "WB"])
+
+    # backwards
+    xb = x * math.cos(phi) + y * math.sin(phi)
+    yb = x * math.sin(phi) - y * math.cos(phi)
+
+    flag, t, u, v = LRSL(xb, yb, phi)
+    if flag:
+        paths = set_path(paths, [v, u, -0.5 * PI, t], ["WB", "S", "R", "WB"])
+
+    flag, t, u, v = LRSL(-xb, yb, -phi)
+    if flag:
+        paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["WB", "S", "R", "WB"])
+
+    flag, t, u, v = LRSL(xb, -yb, -phi)
+    if flag:
+        paths = set_path(paths, [v, u, -0.5 * PI, t], ["R", "S", "WB", "R"])
+
+    flag, t, u, v = LRSL(-xb, -yb, phi)
+    if flag:
+        paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["R", "S", "WB", "R"])
+
+    flag, t, u, v = LRSR(xb, yb, phi)
+    if flag:
+        paths = set_path(paths, [v, u, -0.5 * PI, t], ["R", "S", "R", "WB"])
+
+    flag, t, u, v = LRSR(-xb, yb, -phi)
+    if flag:
+        paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["R", "S", "R", "WB"])
+
+    flag, t, u, v = LRSR(xb, -yb, -phi)
+    if flag:
+        paths = set_path(paths, [v, u, -0.5 * PI, t], ["WB", "S", "WB", "R"])
+
+    flag, t, u, v = LRSR(-xb, -yb, phi)
+    if flag:
+        paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["WB", "S", "WB", "R"])
+
+    return paths
+
+
+def LRSLR(x, y, phi):
+    # formula 8.11 *** TYPO IN PAPER ***
+    xi = x + math.sin(phi)
+    eta = y - 1.0 - math.cos(phi)
+    rho, theta = R(xi, eta)
+
+    if rho >= 2.0:
+        u = 4.0 - math.sqrt(rho * rho - 4.0)
+        if u <= 0.0:
+            t = M(math.atan2((4.0 - u) * xi - 2.0 * eta, -2.0 * xi + (u - 4.0) * eta))
+            v = M(t - phi)
+
+            if t >= 0.0 and v >= 0.0:
+                return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def CCSCC(x, y, phi, paths):
+    flag, t, u, v = LRSLR(x, y, phi)
+    if flag:
+        paths = set_path(paths, [t, -0.5 * PI, u, -0.5 * PI, v], ["WB", "R", "S", "WB", "R"])
+
+    flag, t, u, v = LRSLR(-x, y, -phi)
+    if flag:
+        paths = set_path(paths, [-t, 0.5 * PI, -u, 0.5 * PI, -v], ["WB", "R", "S", "WB", "R"])
+
+    flag, t, u, v = LRSLR(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, -0.5 * PI, u, -0.5 * PI, v], ["R", "WB", "S", "R", "WB"])
+
+    flag, t, u, v = LRSLR(-x, -y, phi)
+    if flag:
+        paths = set_path(paths, [-t, 0.5 * PI, -u, 0.5 * PI, -v], ["R", "WB", "S", "R", "WB"])
+
+    return paths
+
+
+def generate_local_course(L, lengths, mode, maxc, step_size):
+    point_num = int(L / step_size) + len(lengths) + 3
+
+    px = [0.0 for _ in range(point_num)]
+    py = [0.0 for _ in range(point_num)]
+    pyaw = [0.0 for _ in range(point_num)]
+    directions = [0 for _ in range(point_num)]
+    ind = 1
+
+    if lengths[0] > 0.0:
+        directions[0] = 1
+    else:
+        directions[0] = -1
+
+    if lengths[0] > 0.0:
+        d = step_size
+    else:
+        d = -step_size
+
+    ll = 0.0
+
+    for m, l, i in zip(mode, lengths, range(len(mode))):
+        if l > 0.0:
+            d = step_size
+        else:
+            d = -step_size
+
+        ox, oy, oyaw = px[ind], py[ind], pyaw[ind]
+
+        ind -= 1
+        if i >= 1 and (lengths[i - 1] * lengths[i]) > 0:
+            pd = -d - ll
+        else:
+            pd = d - ll
+
+        while abs(pd) <= abs(l):
+            ind += 1
+            px, py, pyaw, directions = \
+                interpolate(ind, pd, m, maxc, ox, oy, oyaw, px, py, pyaw, directions)
+            pd += d
+
+        ll = l - pd - d  # calc remain length
+
+        ind += 1
+        px, py, pyaw, directions = \
+            interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions)
+
+    if len(px) <= 1:
+        return [], [], [], []
+
+    # remove unused data
+    while len(px) >= 1 and px[-1] == 0.0:
+        px.pop()
+        py.pop()
+        pyaw.pop()
+        directions.pop()
+
+    return px, py, pyaw, directions
+
+
+def interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions):
+    if m == "S":
+        px[ind] = ox + l / maxc * math.cos(oyaw)
+        py[ind] = oy + l / maxc * math.sin(oyaw)
+        pyaw[ind] = oyaw
+    else:
+        ldx = math.sin(l) / maxc
+        if m == "WB":
+            ldy = (1.0 - math.cos(l)) / maxc
+        elif m == "R":
+            ldy = (1.0 - math.cos(l)) / (-maxc)
+
+        gdx = math.cos(-oyaw) * ldx + math.sin(-oyaw) * ldy
+        gdy = -math.sin(-oyaw) * ldx + math.cos(-oyaw) * ldy
+        px[ind] = ox + gdx
+        py[ind] = oy + gdy
+
+    if m == "WB":
+        pyaw[ind] = oyaw + l
+    elif m == "R":
+        pyaw[ind] = oyaw - l
+
+    if l > 0.0:
+        directions[ind] = 1
+    else:
+        directions[ind] = -1
+
+    return px, py, pyaw, directions
+
+
+def generate_path(q0, q1, maxc):
+    dx = q1[0] - q0[0]
+    dy = q1[1] - q0[1]
+    dth = q1[2] - q0[2]
+    c = math.cos(q0[2])
+    s = math.sin(q0[2])
+    x = (c * dx + s * dy) * maxc
+    y = (-s * dx + c * dy) * maxc
+
+    paths = []
+    paths = SCS(x, y, dth, paths)
+    paths = CSC(x, y, dth, paths)
+    paths = CCC(x, y, dth, paths)
+    paths = CCCC(x, y, dth, paths)
+    paths = CCSC(x, y, dth, paths)
+    paths = CCSCC(x, y, dth, paths)
+
+    return paths
+
+
+# utils
+def pi_2_pi(theta):
+    while theta > PI:
+        theta -= 2.0 * PI
+
+    while theta < -PI:
+        theta += 2.0 * PI
+
+    return theta
+
+
+def R(x, y):
+    """
+    Return the polar coordinates (r, theta) of the point (x, y)
+    """
+    r = math.hypot(x, y)
+    theta = math.atan2(y, x)
+
+    return r, theta
+
+
+def M(theta):
+    """
+    Regulate theta to -pi <= theta < pi
+    """
+    phi = theta % (2.0 * PI)
+
+    if phi < -PI:
+        phi += 2.0 * PI
+    if phi > PI:
+        phi -= 2.0 * PI
+
+    return phi
+
+
+def get_label(path):
+    label = ""
+
+    for m, l in zip(path.ctypes, path.lengths):
+        label = label + m
+        if l > 0.0:
+            label = label + "+"
+        else:
+            label = label + "-"
+
+    return label
+
+
+def calc_curvature(x, y, yaw, directions):
+    c, ds = [], []
+
+    for i in range(1, len(x) - 1):
+        dxn = x[i] - x[i - 1]
+        dxp = x[i + 1] - x[i]
+        dyn = y[i] - y[i - 1]
+        dyp = y[i + 1] - y[i]
+        dn = math.hypot(dxn, dyn)
+        dp = math.hypot(dxp, dyp)
+        dx = 1.0 / (dn + dp) * (dp / dn * dxn + dn / dp * dxp)
+        ddx = 2.0 / (dn + dp) * (dxp / dp - dxn / dn)
+        dy = 1.0 / (dn + dp) * (dp / dn * dyn + dn / dp * dyp)
+        ddy = 2.0 / (dn + dp) * (dyp / dp - dyn / dn)
+        curvature = (ddy * dx - ddx * dy) / (dx ** 2 + dy ** 2)
+        d = (dn + dp) / 2.0
+
+        if np.isnan(curvature):
+            curvature = 0.0
+
+        if directions[i] <= 0.0:
+            curvature = -curvature
+
+        if len(c) == 0:
+            ds.append(d)
+            c.append(curvature)
+
+        ds.append(d)
+        c.append(curvature)
+
+    ds.append(ds[-1])
+    c.append(c[-1])
+
+    return c, ds
+
+
+def check_path(sx, sy, syaw, gx, gy, gyaw, maxc):
+    paths = calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc)
+
+    assert len(paths) >= 1
+
+    for path in paths:
+        assert abs(path.x[0] - sx) <= 0.01
+        assert abs(path.y[0] - sy) <= 0.01
+        assert abs(path.yaw[0] - syaw) <= 0.01
+        assert abs(path.x[-1] - gx) <= 0.01
+        assert abs(path.y[-1] - gy) <= 0.01
+        assert abs(path.yaw[-1] - gyaw) <= 0.01
+
+        # course distance check
+        d = [math.hypot(dx, dy)
+             for dx, dy in zip(np.diff(path.x[0:len(path.x) - 1]),
+                               np.diff(path.y[0:len(path.y) - 1]))]
+
+        for i in range(len(d)):
+            assert abs(d[i] - STEP_SIZE) <= 0.001
+
+
+def main():
+    start_x = 3.0  # [m]
+    start_y = 10.0  # [m]
+    start_yaw = np.deg2rad(40.0)  # [rad]
+    end_x = 0.0  # [m]
+    end_y = 1.0  # [m]
+    end_yaw = np.deg2rad(0.0)  # [rad]
+    max_curvature = 0.1
+
+    t0 = time.time()
+
+    for i in range(1000):
+        _ = calc_optimal_path(start_x, start_y, start_yaw, end_x, end_y, end_yaw, max_curvature)
+
+    t1 = time.time()
+    print(t1 - t0)
+
+
+if __name__ == '__main__':
+    main()

planner/hybridastar/planner.py

@@ -0,0 +1,373 @@
+import os
+import sys
+import math
+import heapq
+from heapdict import heapdict
+import time
+import numpy as np
+import matplotlib.pyplot as plt
+import scipy.spatial.kdtree as kd
+from planner.hybridastar import astar as astar
+from planner.hybridastar import planer_reeds_shepp as rs
+from input import make_car
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../../Onsite_Parking/")
+
+C = make_car.C
+class Node:
+    def __init__(self, xind, yind, yawind, direction, x, y,
+                 yaw, directions, steer, cost, pind):
+        self.xind = xind
+        self.yind = yind
+        self.yawind = yawind
+        self.direction = direction
+        self.x = x
+        self.y = y
+        self.yaw = yaw
+        self.directions = directions
+        self.steer = steer
+        self.cost = cost
+        self.pind = pind
+
+class Para:
+    def __init__(self, minx, miny, minyaw, maxx, maxy, maxyaw,
+                 xw, yw, yaww, xyreso, yawreso, ox, oy, kdtree):
+        self.minx = minx
+        self.miny = miny
+        self.minyaw = minyaw
+        self.maxx = maxx
+        self.maxy = maxy
+        self.maxyaw = maxyaw
+        self.xw = xw
+        self.yw = yw
+        self.yaww = yaww
+        self.xyreso = xyreso
+        self.yawreso = yawreso
+        self.ox = ox
+        self.oy = oy
+        self.kdtree = kdtree
+
+class QueuePrior:
+    def __init__(self):
+        self.queue = heapdict()
+
+    def empty(self):
+        return len(self.queue) == 0  # if Q is empty
+
+    def put(self, item, priority):
+        self.queue[item] = priority  # push
+
+    def get(self):
+        return self.queue.popitem()[0]  # pop out element with smallest priority
+
+class Path:
+    def __init__(self, x, y, yaw, direction, cost):
+        self.x = x
+        self.y = y
+        self.yaw = yaw
+        self.direction = direction
+        self.cost = cost
+
+def calc_parameters(ox, oy, xyreso, yawreso, kdtree):
+    minx = round(min(ox) / xyreso)
+    miny = round(min(oy) / xyreso)
+    maxx = round(max(ox) / xyreso)
+    maxy = round(max(oy) / xyreso)
+    xw, yw = maxx - minx, maxy - miny
+    minyaw = round(-C.PI / yawreso) - 1
+    maxyaw = round(C.PI / yawreso)
+    yaww = maxyaw - minyaw
+
+    return Para(minx, miny, minyaw, maxx, maxy, maxyaw,
+                xw, yw, yaww, xyreso, yawreso, ox, oy, kdtree)
+
+def calc_motion_set():
+    s = np.arange(C.MAX_STEER / C.N_STEER,
+                  C.MAX_STEER, C.MAX_STEER / C.N_STEER)
+    steer = list(s) + [0.0] + list(-s)
+    direc = [1.0 for _ in range(len(steer))] + [-1.0 for _ in range(len(steer))]
+    steer = steer + steer
+    return steer, direc
+
+def calc_index(node, P):
+    ind = (node.yawind - P.minyaw) * P.xw * P.yw + \
+          (node.yind - P.miny) * P.xw + \
+          (node.xind - P.minx)
+
+    return ind
+
+def calc_hybrid_cost(node, hmap, P):
+    cost = node.cost + \
+           C.H_COST * hmap[node.xind - P.minx][node.yind - P.miny]
+    return cost
+
+def update_node_with_analystic_expantion(n_curr, ngoal, P):
+    path = analystic_expantion(n_curr, ngoal, P)  # rs path: n -> ngoal
+
+    if not path:
+        return False, None
+
+    fx = path.x[1:-1]
+    fy = path.y[1:-1]
+    fyaw = path.yaw[1:-1]
+    fd = path.directions[1:-1]
+
+    fcost = n_curr.cost + calc_rs_path_cost(path)
+    fpind = calc_index(n_curr, P)
+    fsteer = 0.0
+    fpath = Node(n_curr.xind, n_curr.yind, n_curr.yawind, n_curr.direction,
+                 fx, fy, fyaw, fd, fsteer, fcost, fpind)
+
+    return True, fpath
+
+def calc_rs_path_cost(rspath):
+    cost = 0.0
+    for lr in rspath.lengths:
+        if lr >= 0:
+            cost += 1
+        else:
+            cost += abs(lr) * C.BACKWARD_COST
+    for i in range(len(rspath.lengths) - 1):
+        if rspath.lengths[i] * rspath.lengths[i + 1] < 0.0:
+            cost += C.GEAR_COST
+    for ctype in rspath.ctypes:
+        if ctype != "S":
+            cost += C.STEER_ANGLE_COST * abs(C.MAX_STEER)
+    nctypes = len(rspath.ctypes)
+    ulist = [0.0 for _ in range(nctypes)]
+    for i in range(nctypes):
+        if rspath.ctypes[i] == "R":
+            ulist[i] = -C.MAX_STEER
+        elif rspath.ctypes[i] == "WB":
+            ulist[i] = C.MAX_STEER
+    for i in range(nctypes - 1):
+        cost += C.STEER_CHANGE_COST * abs(ulist[i + 1] - ulist[i])
+    return cost
+
+def calc_motion_set():
+    s = np.arange(C.MAX_STEER / C.N_STEER,
+                  C.MAX_STEER, C.MAX_STEER / C.N_STEER)
+    steer = list(s) + [0.0] + list(-s)
+    direc = [1.0 for _ in range(len(steer))] + [-1.0 for _ in range(len(steer))]
+    steer = steer + steer
+    return steer, direc
+
+def is_same_grid(node1, node2):
+    if node1.xind != node2.xind or \
+            node1.yind != node2.yind or \
+            node1.yawind != node2.yawind:
+        return False
+
+    return True
+
+def calc_index(node, P):
+    ind = (node.yawind - P.minyaw) * P.xw * P.yw + \
+          (node.yind - P.miny) * P.xw + \
+          (node.xind - P.minx)
+
+    return ind
+
+def is_collision(x, y, yaw, P):
+    for ix, iy, iyaw in zip(x, y, yaw):
+        d = 20
+        dl = (C.RF - C.RB) / 2.0
+        r = (C.RF + C.RB) / 2.0 + d
+        cx = ix + dl * math.cos(iyaw)
+        cy = iy + dl * math.sin(iyaw)
+        ids = P.kdtree.query_ball_point([cx, cy], r)
+
+        if not ids:
+            continue
+
+        for i in ids:
+            xo = P.ox[i] - cx
+            yo = P.oy[i] - cy
+            dx = xo * math.cos(iyaw) + yo * math.sin(iyaw)
+            dy = -xo * math.sin(iyaw) + yo * math.cos(iyaw)
+
+            if abs(dx) < r and abs(dy) < C.W / 2 + d:
+                return True
+    return False
+
+def calc_parameters(ox, oy, xyreso, yawreso, kdtree):
+    minx = round(min(ox) / xyreso)
+    miny = round(min(oy) / xyreso)
+    maxx = round(max(ox) / xyreso)
+    maxy = round(max(oy) / xyreso)
+    xw, yw = maxx - minx, maxy - miny
+    minyaw = round(-C.PI / yawreso) - 1
+    maxyaw = round(C.PI / yawreso)
+    yaww = maxyaw - minyaw
+
+    return Para(minx, miny, minyaw, maxx, maxy, maxyaw,
+                xw, yw, yaww, xyreso, yawreso, ox, oy, kdtree)
+
+def analystic_expantion(node, ngoal, P):
+    sx, sy, syaw = node.x[-1], node.y[-1], node.yaw[-1]
+    gx, gy, gyaw = ngoal.x[-1], ngoal.y[-1], ngoal.yaw[-1]
+
+    maxc = math.tan(C.MAX_STEER) / C.WB
+    paths = rs.calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=C.MOVE_STEP)
+
+    if not paths:
+        return None
+
+    pq = QueuePrior()
+    for path in paths:
+        pq.put(path, calc_rs_path_cost(path))
+
+    while not pq.empty():
+        path = pq.get()
+        ind = range(0, len(path.x), C.COLLISION_CHECK_STEP)
+
+        pathx = [path.x[k] for k in ind]
+        pathy = [path.y[k] for k in ind]
+        pathyaw = [path.yaw[k] for k in ind]
+
+        if not is_collision(pathx, pathy, pathyaw, P):
+            return path
+
+    return None
+
+def is_index_ok(xind, yind, xlist, ylist, yawlist, P):
+    if xind <= P.minx or \
+            xind >= P.maxx or \
+            yind <= P.miny or \
+            yind >= P.maxy:
+        return False
+
+    ind = range(0, len(xlist), C.COLLISION_CHECK_STEP)
+
+    nodex = [xlist[k] for k in ind]
+    nodey = [ylist[k] for k in ind]
+    nodeyaw = [yawlist[k] for k in ind]
+
+    if is_collision(nodex, nodey, nodeyaw, P):
+        return False
+
+    return True
+
+def extract_path(closed, ngoal, nstart):
+    rx, ry, ryaw, direc = [], [], [], []
+    cost = 0.0
+    node = ngoal
+
+    while True:
+        rx += node.x[::-1]
+        ry += node.y[::-1]
+        ryaw += node.yaw[::-1]
+        direc += node.directions[::-1]
+        cost += node.cost
+
+        if is_same_grid(node, nstart):
+            break
+
+        node = closed[node.pind]
+
+    rx = rx[::-1]
+    ry = ry[::-1]
+    ryaw = ryaw[::-1]
+    direc = direc[::-1]
+
+    direc[0] = direc[1]
+    path = Path(rx, ry, ryaw, direc, cost)
+
+    return path
+def calc_next_node(n_curr, c_id, u, d, P):
+    step = C.XY_RESO * 2
+
+    nlist = math.ceil(step / C.MOVE_STEP)
+    xlist = [n_curr.x[-1] + d * C.MOVE_STEP * math.cos(n_curr.yaw[-1])]
+    ylist = [n_curr.y[-1] + d * C.MOVE_STEP * math.sin(n_curr.yaw[-1])]
+    yawlist = [rs.pi_2_pi(n_curr.yaw[-1] + d * C.MOVE_STEP / C.WB * math.tan(u))]
+
+    for i in range(nlist - 1):
+        xlist.append(xlist[i] + d * C.MOVE_STEP * math.cos(yawlist[i]))
+        ylist.append(ylist[i] + d * C.MOVE_STEP * math.sin(yawlist[i]))
+        yawlist.append(rs.pi_2_pi(yawlist[i] + d * C.MOVE_STEP / C.WB * math.tan(u)))
+
+    xind = round(xlist[-1] / P.xyreso)
+    yind = round(ylist[-1] / P.xyreso)
+    yawind = round(yawlist[-1] / P.yawreso)
+
+    if not is_index_ok(xind, yind, xlist, ylist, yawlist, P):
+        return None
+
+    cost = 0.0
+
+    if d > 0:
+        direction = 1
+        cost += abs(step)
+    else:
+        direction = -1
+        cost += abs(step) * C.BACKWARD_COST
+
+    if direction != n_curr.direction:  # switch back penalty
+        cost += C.GEAR_COST
+
+    cost += C.STEER_ANGLE_COST * abs(u)  # steer angle penalyty
+    cost += C.STEER_CHANGE_COST * abs(n_curr.steer - u)  # steer change penalty
+    cost = n_curr.cost + cost
+
+    directions = [direction for _ in range(len(xlist))]
+
+    node = Node(xind, yind, yawind, direction, xlist, ylist,
+                yawlist, directions, u, cost, c_id)
+
+    return node
+
+def hybrid_astar_planning(sx, sy, syaw, gx, gy, gyaw, ox, oy, xyreso, yawreso):
+    sxr, syr = round(sx / xyreso), round(sy / xyreso)
+    gxr, gyr = round(gx / xyreso), round(gy / xyreso)
+    syawr = round(rs.pi_2_pi(syaw) / yawreso)
+    gyawr = round(rs.pi_2_pi(gyaw) / yawreso)
+
+    nstart = Node(sxr, syr, syawr, 1, [sx], [sy], [syaw], [1], 0.0, 0.0, -1)
+    ngoal = Node(gxr, gyr, gyawr, 1, [gx], [gy], [gyaw], [1], 0.0, 0.0, -1)
+
+    kdtree = kd.KDTree([[x, y] for x, y in zip(ox, oy)])
+    P = calc_parameters(ox, oy, xyreso, yawreso, kdtree)
+
+    hmap = astar.calc_holonomic_heuristic_with_obstacle(ngoal, P.ox, P.oy, P.xyreso, 1.0)
+    steer_set, direc_set = calc_motion_set()
+    open_set, closed_set = {calc_index(nstart, P): nstart}, {}
+
+    qp = QueuePrior()
+    qp.put(calc_index(nstart, P), calc_hybrid_cost(nstart, hmap, P))
+
+    while True:
+        if not open_set:
+            return None
+
+        ind = qp.get()
+        n_curr = open_set[ind]
+        closed_set[ind] = n_curr
+        open_set.pop(ind)
+
+        update, fpath = update_node_with_analystic_expantion(n_curr, ngoal, P)
+
+        if update:
+            fnode = fpath
+            break
+
+        for i in range(len(steer_set)):
+            node = calc_next_node(n_curr, ind, steer_set[i], direc_set[i], P)
+
+            if not node:
+                continue
+
+            node_ind = calc_index(node, P)
+
+            if node_ind in closed_set:
+                continue
+
+            if node_ind not in open_set:
+                open_set[node_ind] = node
+                qp.put(node_ind, calc_hybrid_cost(node, hmap, P))
+            else:
+                if open_set[node_ind].cost > node.cost:
+                    open_set[node_ind] = node
+                    qp.put(node_ind, calc_hybrid_cost(node, hmap, P))
+
+    return extract_path(closed_set, fnode, nstart)
+

planner/test_run.py

@@ -0,0 +1,52 @@
+import os
+import sys
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../../onsite_parking/")
+
+import json
+import input.make_map as mp
+from planner.hybridastar import planner as planner
+from input import make_car
+from utils import replay
+from utils import map_display as mdp
+import matplotlib.pyplot as plt
+
+
+def main():
+
+    # 输入input文件夹下场景文件
+    map_path = '../input/Atest.json'
+    mdp.map_display(map_path) #  仅绘制地图
+
+    ox, oy,sp,gp = mp.make_map(map_path)
+    sx, sy, syaw0 = sp['x'], sp['y'], sp['yaw']
+    C = make_car.C
+
+    # 获取目标停车位
+    park = '3'
+    gx, gy, gyaw0 = gp[park]['x_end'], gp[park]['y_end'], gp[park]['yaw']
+
+
+
+
+
+    # 规划算法
+    path = planner.hybrid_astar_planning(sx, sy, syaw0, gx, gy, gyaw0, ox, oy, C.XY_RESO, C.YAW_RESO)
+    # 算法测试结果保存
+    if not path:
+        print("Searching failed!")
+        return
+    output_dit={
+        "output_x":path.x,
+        "output_y": path.y,
+        "output_yaw": path.yaw,
+        "output_dir": path.direction,
+    }
+    with open(f"../output/result_{map_path.split('/')[-1].split('.json')[0]}_{park}.json", "w") as file:
+        json.dump(output_dit, file)
+
+    # 仿真回放
+    result_path = f"../output/result_{map_path.split('/')[-1].split('.json')[0]}_{park}.json"
+    replay.replay(map_path, result_path)
+if __name__ == '__main__':
+    main()

planner/user_run.py

@@ -0,0 +1,36 @@
+import json
+import input.make_map as mp
+from input import make_car
+from utils import replay
+from utils import map_display as mdp
+def main():
+    # 请在此输入input文件夹下场景文件
+    map_path = '../Onsite_Parking/input/Atest.json'
+    # mdp.map_display(map_path) #  仅绘制地图
+    ox, oy,sp,gp = mp.make_map(map_path)
+    sx, sy, syaw0 = sp['x'], sp['y'], sp['yaw']
+    C = make_car.C
+    # 遍历所有停车位
+    for i in range(1,len(gp)+1):
+        gx, gy, gyaw0 = gp[str(i)]['x_end'], gp[str(i)]['y_end'], gp[str(i)]['yaw']
+        # 请在此调用planer文件夹下的规控算法
+        path = " "
+        # 算法测试结果保存
+        if not path:
+            print("Searching failed!")
+            return
+        output_dit={
+            "output_x": path.x,
+            "output_y": path.y,
+            "output_yaw": path.yaw,
+            "output_dir": path.direction,
+        }
+        with open(f"../Onsite_Parking/output/result_{map_path.split('/')[-1].split('.json')[0]}_{str(i)}.json", "w") as file:
+            json.dump(output_dit, file)
+
+        # 仿真回放
+        result_path = f"../Onsite_Parking/output/result_{map_path.split('/')[-1].split('.json')[0]}_{str(i)}.json"
+        replay.replay(map_path, result_path)
+
+if __name__ == '__main__':
+    main()

utils/draw.py

@@ -0,0 +1,119 @@
+import matplotlib.pyplot as plt
+import numpy as np
+import math
+PI = np.pi
+
+
+class Arrow:
+    def __init__(self,ax, x, y, theta, L, c):
+        angle = np.deg2rad(30)
+        d = 0.3 * L
+        w = 2
+
+        x_start = x
+        y_start = y
+        x_end = x + L * np.cos(theta)
+        y_end = y + L * np.sin(theta)
+
+        theta_hat_L = theta + PI - angle
+        theta_hat_R = theta + PI + angle
+
+        x_hat_start = x_end
+        x_hat_end_L = x_hat_start + d * np.cos(theta_hat_L)
+        x_hat_end_R = x_hat_start + d * np.cos(theta_hat_R)
+
+        y_hat_start = y_end
+        y_hat_end_L = y_hat_start + d * np.sin(theta_hat_L)
+        y_hat_end_R = y_hat_start + d * np.sin(theta_hat_R)
+
+        # plt.plot([x_start, x_end], [y_start, y_end], color=c, linewidth=w)
+        # plt.plot([x_hat_start, x_hat_end_L],
+        #          [y_hat_start, y_hat_end_L], color=c, linewidth=w)
+        # plt.plot([x_hat_start, x_hat_end_R],
+        #          [y_hat_start, y_hat_end_R], color=c, linewidth=w)
+        ax.plot([x_start, x_end], [y_start, y_end], color=c, linewidth=w)
+        ax.plot([x_hat_start, x_hat_end_L],
+                 [y_hat_start, y_hat_end_L], color=c, linewidth=w)
+        ax.plot([x_hat_start, x_hat_end_R],
+                 [y_hat_start, y_hat_end_R], color=c, linewidth=w)
+
+
+class Car:
+    def __init__(self, x, y, yaw, w, L):
+        theta_B = PI + yaw
+
+        xB = x + L / 4 * np.cos(theta_B)
+        yB = y + L / 4 * np.sin(theta_B)
+
+        theta_BL = theta_B + PI / 2
+        theta_BR = theta_B - PI / 2
+
+        x_BL = xB + w / 2 * np.cos(theta_BL)        # Bottom-Left vertex
+        y_BL = yB + w / 2 * np.sin(theta_BL)
+        x_BR = xB + w / 2 * np.cos(theta_BR)        # Bottom-Right vertex
+        y_BR = yB + w / 2 * np.sin(theta_BR)
+
+        x_FL = x_BL + L * np.cos(yaw)               # Front-Left vertex
+        y_FL = y_BL + L * np.sin(yaw)
+        x_FR = x_BR + L * np.cos(yaw)               # Front-Right vertex
+        y_FR = y_BR + L * np.sin(yaw)
+
+        plt.plot([x_BL, x_BR, x_FR, x_FL, x_BL],
+                 [y_BL, y_BR, y_FR, y_FL, y_BL],
+                 linewidth=1, color='black')
+
+        Arrow(x, y, yaw, L / 2, 'black')
+        # plt.axis("equal")
+        # plt.show()
+
+
+def draw_car(x, y, yaw, steer, C, color='black'):
+    car = np.array([[-C.RB, -C.RB, C.RF, C.RF, -C.RB],
+                    [C.W / 2, -C.W / 2, -C.W / 2, C.W / 2, C.W / 2]])
+
+    wheel = np.array([[-C.TR, -C.TR, C.TR, C.TR, -C.TR],
+                      [C.TW / 4, -C.TW / 4, -C.TW / 4, C.TW / 4, C.TW / 4]])
+
+    rlWheel = wheel.copy()
+    rrWheel = wheel.copy()
+    frWheel = wheel.copy()
+    flWheel = wheel.copy()
+
+    Rot1 = np.array([[math.cos(yaw), -math.sin(yaw)],
+                     [math.sin(yaw), math.cos(yaw)]])
+
+    Rot2 = np.array([[math.cos(steer), math.sin(steer)],
+                     [-math.sin(steer), math.cos(steer)]])
+
+    frWheel = np.dot(Rot2, frWheel)
+    flWheel = np.dot(Rot2, flWheel)
+
+    frWheel += np.array([[C.WB], [-C.WD / 2]])
+    flWheel += np.array([[C.WB], [C.WD / 2]])
+    rrWheel[1, :] -= C.WD / 2
+    rlWheel[1, :] += C.WD / 2
+
+    frWheel = np.dot(Rot1, frWheel)
+    flWheel = np.dot(Rot1, flWheel)
+
+    rrWheel = np.dot(Rot1, rrWheel)
+    rlWheel = np.dot(Rot1, rlWheel)
+    car = np.dot(Rot1, car)
+
+    frWheel += np.array([[x], [y]])
+    flWheel += np.array([[x], [y]])
+    rrWheel += np.array([[x], [y]])
+    rlWheel += np.array([[x], [y]])
+    car += np.array([[x], [y]])
+
+    plt.plot(car[0, :], car[1, :], color)
+    plt.plot(frWheel[0, :], frWheel[1, :], color)
+    plt.plot(rrWheel[0, :], rrWheel[1, :], color)
+    plt.plot(flWheel[0, :], flWheel[1, :], color)
+    plt.plot(rlWheel[0, :], rlWheel[1, :], color)
+    Arrow(x, y, yaw, C.WB * 0.8, color)
+
+
+if __name__ == '__main__':
+    # Arrow(-1, 2, 60)
+    Car(0, 0, 1, 2, 60)

utils/drawcar.py

@@ -0,0 +1,103 @@
+
+
+import math
+import numpy as np
+from input import make_car
+import utils.draw as draw
+
+C=make_car.C
+def draw_car(ax, x, y, yaw, steer, color='red'):
+    car = np.array([[-C.RB, -C.RB, C.RF, C.RF, -C.RB], [C.W / 2, -C.W / 2, -C.W / 2, C.W / 2, C.W / 2]])
+    wheel = np.array([[-C.TR, -C.TR, C.TR, C.TR, -C.TR], [C.TW / 4, -C.TW / 4, -C.TW / 4, C.TW / 4, C.TW / 4]])
+
+    rlWheel = wheel.copy()
+    rrWheel = wheel.copy()
+    frWheel = wheel.copy()
+    flWheel = wheel.copy()
+
+    Rot1 = np.array([[math.cos(yaw), -math.sin(yaw)], [math.sin(yaw), math.cos(yaw)]])
+    Rot2 = np.array([[math.cos(steer), math.sin(steer)], [-math.sin(steer), math.cos(steer)]])
+
+    frWheel = np.dot(Rot2, frWheel)
+    flWheel = np.dot(Rot2, flWheel)
+
+    frWheel += np.array([[C.WB], [-C.WD / 2]])
+    flWheel += np.array([[C.WB], [C.WD / 2]])
+    rrWheel[1, :] -= C.WD / 2
+    rlWheel[1, :] += C.WD / 2
+
+    frWheel = np.dot(Rot1, frWheel)
+    flWheel = np.dot(Rot1, flWheel)
+    rrWheel = np.dot(Rot1, rrWheel)
+    rlWheel = np.dot(Rot1, rlWheel)
+    car = np.dot(Rot1, car)
+
+    frWheel += np.array([[x], [y]])
+    flWheel += np.array([[x], [y]])
+    rrWheel += np.array([[x], [y]])
+    rlWheel += np.array([[x], [y]])
+    car += np.array([[x], [y]])
+
+    ax.plot(car[0, :], car[1, :], color)
+    ax.plot(frWheel[0, :], frWheel[1, :], color)
+    ax.plot(rrWheel[0, :], rrWheel[1, :], color)
+    ax.plot(flWheel[0, :], flWheel[1, :], color)
+    ax.plot(rlWheel[0, :], rlWheel[1, :], color)
+
+    draw.Arrow(ax,x, y, yaw, C.WB * 0.8, color)  # 修改为在指定的ax上绘制箭头
+
+
+
+# import math
+# import numpy as np
+# import matplotlib.pyplot as plt
+# import utils.draw as draw
+# from input import make_car
+
+# C=make_car.C
+
+# def draw_car(x, y, yaw, steer, color='red'):
+#     car = np.array([[-C.RB, -C.RB, C.RF, C.RF, -C.RB],
+#                     [C.W / 2, -C.W / 2, -C.W / 2, C.W / 2, C.W / 2]])
+
+#     wheel = np.array([[-C.TR, -C.TR, C.TR, C.TR, -C.TR],
+#                       [C.TW / 4, -C.TW / 4, -C.TW / 4, C.TW / 4, C.TW / 4]])
+
+#     rlWheel = wheel.copy()
+#     rrWheel = wheel.copy()
+#     frWheel = wheel.copy()
+#     flWheel = wheel.copy()
+
+#     Rot1 = np.array([[math.cos(yaw), -math.sin(yaw)],
+#                      [math.sin(yaw), math.cos(yaw)]])
+
+#     Rot2 = np.array([[math.cos(steer), math.sin(steer)],
+#                      [-math.sin(steer), math.cos(steer)]])
+
+#     frWheel = np.dot(Rot2, frWheel)
+#     flWheel = np.dot(Rot2, flWheel)
+
+#     frWheel += np.array([[C.WB], [-C.WD / 2]])
+#     flWheel += np.array([[C.WB], [C.WD / 2]])
+#     rrWheel[1, :] -= C.WD / 2
+#     rlWheel[1, :] += C.WD / 2
+
+#     frWheel = np.dot(Rot1, frWheel)
+#     flWheel = np.dot(Rot1, flWheel)
+
+#     rrWheel = np.dot(Rot1, rrWheel)
+#     rlWheel = np.dot(Rot1, rlWheel)
+#     car = np.dot(Rot1, car)
+
+#     frWheel += np.array([[x], [y]])
+#     flWheel += np.array([[x], [y]])
+#     rrWheel += np.array([[x], [y]])
+#     rlWheel += np.array([[x], [y]])
+#     car += np.array([[x], [y]])
+
+#     plt.plot(car[0, :], car[1, :], color)
+#     plt.plot(frWheel[0, :], frWheel[1, :], color)
+#     plt.plot(rrWheel[0, :], rrWheel[1, :], color)
+#     plt.plot(flWheel[0, :], flWheel[1, :], color)
+#     plt.plot(rlWheel[0, :], rlWheel[1, :], color)
+#     draw.Arrow(x, y, yaw, C.WB * 0.8, color)

utils/map_display.py

@@ -0,0 +1,20 @@
+"""
+
+ 仅绘制地图
+"""
+import matplotlib.pyplot as plt
+import input.make_map as mp
+
+def map_display(map_scene):
+    picture_scene =  map_scene.replace('/', '/').replace('.json', '.jpg')
+    picture = plt.imread(picture_scene)
+    ox, oy,sp,gp = mp.make_map(map_scene)
+    plt.rcParams['xtick.direction'] = 'in'
+    plt.cla()
+    plt.plot(ox, oy, ",k")
+    plt.tick_params(axis='x', direction='in', top=True, bottom=False, labelbottom=False, labeltop=True)
+    plt.axis("equal")
+    plt.imshow(picture, extent=[min(ox), max(ox), min(oy), max(oy)])
+    print("绘制地图结束!")
+    plt.show()
+

utils/reeds_shepp.py

@@ -0,0 +1,688 @@
+import time
+import math
+import numpy as np
+
+
+# parameters initiation
+STEP_SIZE = 0.2
+MAX_LENGTH = 1000.0
+PI = math.pi
+
+
+# class for PATH element
+class PATH:
+    def __init__(self, lengths, ctypes, L, x, y, yaw, directions):
+        self.lengths = lengths              # lengths of each part of path (+: forward, -: backward) [float]
+        self.ctypes = ctypes                # type of each part of the path [string]
+        self.L = L                          # total path length [float]
+        self.x = x                          # final x positions [m]
+        self.y = y                          # final y positions [m]
+        self.yaw = yaw                      # final yaw angles [rad]
+        self.directions = directions        # forward: 1, backward:-1
+
+
+def calc_optimal_path(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=STEP_SIZE):
+    paths = calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=step_size)
+
+    minL = paths[0].L
+    mini = 0
+
+    for i in range(len(paths)):
+        if paths[i].L <= minL:
+            minL, mini = paths[i].L, i
+
+    return paths[mini]
+
+
+def calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=STEP_SIZE):
+    q0 = [sx, sy, syaw]
+    q1 = [gx, gy, gyaw]
+
+    paths = generate_path(q0, q1, maxc)
+
+    for path in paths:
+        x, y, yaw, directions = \
+            generate_local_course(path.L, path.lengths,
+                                  path.ctypes, maxc, step_size * maxc)
+
+        # convert global coordinate
+        path.x = [math.cos(-q0[2]) * ix + math.sin(-q0[2]) * iy + q0[0] for (ix, iy) in zip(x, y)]
+        path.y = [-math.sin(-q0[2]) * ix + math.cos(-q0[2]) * iy + q0[1] for (ix, iy) in zip(x, y)]
+        path.yaw = [pi_2_pi(iyaw + q0[2]) for iyaw in yaw]
+        path.directions = directions
+        path.lengths = [l / maxc for l in path.lengths]
+        path.L = path.L / maxc
+
+    return paths
+
+
+def set_path(paths, lengths, ctypes):
+    path = PATH([], [], 0.0, [], [], [], [])
+    path.ctypes = ctypes
+    path.lengths = lengths
+    # check same path exist
+    for path_e in paths:
+        if path_e.ctypes == path.ctypes:
+            if sum([x - y for x, y in zip(path_e.lengths, path.lengths)]) <= 0.01:
+                return paths  # not insert path
+
+    path.L = sum([abs(i) for i in lengths])
+
+    if path.L >= MAX_LENGTH:
+        return paths
+
+    assert path.L >= 0.01
+    paths.append(path)
+
+    return paths
+
+
+def LSL(x, y, phi):
+    u, t = R(x - math.sin(phi), y - 1.0 + math.cos(phi))
+
+    if t >= 0.0:
+        v = M(phi - t)
+        if v >= 0.0:
+            return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def LSR(x, y, phi):
+    u1, t1 = R(x + math.sin(phi), y - 1.0 - math.cos(phi))
+    u1 = u1 ** 2
+
+    if u1 >= 4.0:
+        u = math.sqrt(u1 - 4.0)
+        theta = math.atan2(2.0, u)
+        t = M(t1 + theta)
+        v = M(t - phi)
+
+        if t >= 0.0 and v >= 0.0:
+            return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def LRL(x, y, phi):
+    u1, t1 = R(x - math.sin(phi), y - 1.0 + math.cos(phi))
+
+    if u1 <= 4.0:
+        u = -2.0 * math.asin(0.25 * u1)
+        t = M(t1 + 0.5 * u + PI)
+        v = M(phi - t + u)
+
+        if t >= 0.0 and u <= 0.0:
+            return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def SCS(x, y, phi, paths):
+    flag, t, u, v = SLS(x, y, phi)
+
+    if flag:
+        paths = set_path(paths, [t, u, v], ["S", "WB", "S"])
+
+    flag, t, u, v = SLS(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, u, v], ["S", "R", "S"])
+
+    return paths
+
+
+def SLS(x, y, phi):
+    phi = M(phi)
+
+    if y > 0.0 and 0.0 < phi < PI * 0.99:
+        xd = -y / math.tan(phi) + x
+        t = xd - math.tan(phi / 2.0)
+        u = phi
+        v = math.sqrt((x - xd) ** 2 + y ** 2) - math.tan(phi / 2.0)
+        return True, t, u, v
+    elif y < 0.0 and 0.0 < phi < PI * 0.99:
+        xd = -y / math.tan(phi) + x
+        t = xd - math.tan(phi / 2.0)
+        u = phi
+        v = -math.sqrt((x - xd) ** 2 + y ** 2) - math.tan(phi / 2.0)
+        return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def CSC(x, y, phi, paths):
+    flag, t, u, v = LSL(x, y, phi)
+    if flag:
+        paths = set_path(paths, [t, u, v], ["WB", "S", "WB"])
+
+    flag, t, u, v = LSL(-x, y, -phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, -v], ["WB", "S", "WB"])
+
+    flag, t, u, v = LSL(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, u, v], ["R", "S", "R"])
+
+    flag, t, u, v = LSL(-x, -y, phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, -v], ["R", "S", "R"])
+
+    flag, t, u, v = LSR(x, y, phi)
+    if flag:
+        paths = set_path(paths, [t, u, v], ["WB", "S", "R"])
+
+    flag, t, u, v = LSR(-x, y, -phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, -v], ["WB", "S", "R"])
+
+    flag, t, u, v = LSR(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, u, v], ["R", "S", "WB"])
+
+    flag, t, u, v = LSR(-x, -y, phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, -v], ["R", "S", "WB"])
+
+    return paths
+
+
+def CCC(x, y, phi, paths):
+    flag, t, u, v = LRL(x, y, phi)
+    if flag:
+        paths = set_path(paths, [t, u, v], ["WB", "R", "WB"])
+
+    flag, t, u, v = LRL(-x, y, -phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, -v], ["WB", "R", "WB"])
+
+    flag, t, u, v = LRL(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, u, v], ["R", "WB", "R"])
+
+    flag, t, u, v = LRL(-x, -y, phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, -v], ["R", "WB", "R"])
+
+    # backwards
+    xb = x * math.cos(phi) + y * math.sin(phi)
+    yb = x * math.sin(phi) - y * math.cos(phi)
+
+    flag, t, u, v = LRL(xb, yb, phi)
+    if flag:
+        paths = set_path(paths, [v, u, t], ["WB", "R", "WB"])
+
+    flag, t, u, v = LRL(-xb, yb, -phi)
+    if flag:
+        paths = set_path(paths, [-v, -u, -t], ["WB", "R", "WB"])
+
+    flag, t, u, v = LRL(xb, -yb, -phi)
+    if flag:
+        paths = set_path(paths, [v, u, t], ["R", "WB", "R"])
+
+    flag, t, u, v = LRL(-xb, -yb, phi)
+    if flag:
+        paths = set_path(paths, [-v, -u, -t], ["R", "WB", "R"])
+
+    return paths
+
+
+def calc_tauOmega(u, v, xi, eta, phi):
+    delta = M(u - v)
+    A = math.sin(u) - math.sin(delta)
+    B = math.cos(u) - math.cos(delta) - 1.0
+
+    t1 = math.atan2(eta * A - xi * B, xi * A + eta * B)
+    t2 = 2.0 * (math.cos(delta) - math.cos(v) - math.cos(u)) + 3.0
+
+    if t2 < 0:
+        tau = M(t1 + PI)
+    else:
+        tau = M(t1)
+
+    omega = M(tau - u + v - phi)
+
+    return tau, omega
+
+
+def LRLRn(x, y, phi):
+    xi = x + math.sin(phi)
+    eta = y - 1.0 - math.cos(phi)
+    rho = 0.25 * (2.0 + math.sqrt(xi * xi + eta * eta))
+
+    if rho <= 1.0:
+        u = math.acos(rho)
+        t, v = calc_tauOmega(u, -u, xi, eta, phi)
+        if t >= 0.0 and v <= 0.0:
+            return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def LRLRp(x, y, phi):
+    xi = x + math.sin(phi)
+    eta = y - 1.0 - math.cos(phi)
+    rho = (20.0 - xi * xi - eta * eta) / 16.0
+
+    if 0.0 <= rho <= 1.0:
+        u = -math.acos(rho)
+        if u >= -0.5 * PI:
+            t, v = calc_tauOmega(u, u, xi, eta, phi)
+            if t >= 0.0 and v >= 0.0:
+                return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def CCCC(x, y, phi, paths):
+    flag, t, u, v = LRLRn(x, y, phi)
+    if flag:
+        paths = set_path(paths, [t, u, -u, v], ["WB", "R", "WB", "R"])
+
+    flag, t, u, v = LRLRn(-x, y, -phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, u, -v], ["WB", "R", "WB", "R"])
+
+    flag, t, u, v = LRLRn(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, u, -u, v], ["R", "WB", "R", "WB"])
+
+    flag, t, u, v = LRLRn(-x, -y, phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, u, -v], ["R", "WB", "R", "WB"])
+
+    flag, t, u, v = LRLRp(x, y, phi)
+    if flag:
+        paths = set_path(paths, [t, u, u, v], ["WB", "R", "WB", "R"])
+
+    flag, t, u, v = LRLRp(-x, y, -phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, -u, -v], ["WB", "R", "WB", "R"])
+
+    flag, t, u, v = LRLRp(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, u, u, v], ["R", "WB", "R", "WB"])
+
+    flag, t, u, v = LRLRp(-x, -y, phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, -u, -v], ["R", "WB", "R", "WB"])
+
+    return paths
+
+
+def LRSR(x, y, phi):
+    xi = x + math.sin(phi)
+    eta = y - 1.0 - math.cos(phi)
+    rho, theta = R(-eta, xi)
+
+    if rho >= 2.0:
+        t = theta
+        u = 2.0 - rho
+        v = M(t + 0.5 * PI - phi)
+        if t >= 0.0 and u <= 0.0 and v <= 0.0:
+            return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def LRSL(x, y, phi):
+    xi = x - math.sin(phi)
+    eta = y - 1.0 + math.cos(phi)
+    rho, theta = R(xi, eta)
+
+    if rho >= 2.0:
+        r = math.sqrt(rho * rho - 4.0)
+        u = 2.0 - r
+        t = M(theta + math.atan2(r, -2.0))
+        v = M(phi - 0.5 * PI - t)
+        if t >= 0.0 and u <= 0.0 and v <= 0.0:
+            return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def CCSC(x, y, phi, paths):
+    flag, t, u, v = LRSL(x, y, phi)
+    if flag:
+        paths = set_path(paths, [t, -0.5 * PI, u, v], ["WB", "R", "S", "WB"])
+
+    flag, t, u, v = LRSL(-x, y, -phi)
+    if flag:
+        paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["WB", "R", "S", "WB"])
+
+    flag, t, u, v = LRSL(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, -0.5 * PI, u, v], ["R", "WB", "S", "R"])
+
+    flag, t, u, v = LRSL(-x, -y, phi)
+    if flag:
+        paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["R", "WB", "S", "R"])
+
+    flag, t, u, v = LRSR(x, y, phi)
+    if flag:
+        paths = set_path(paths, [t, -0.5 * PI, u, v], ["WB", "R", "S", "R"])
+
+    flag, t, u, v = LRSR(-x, y, -phi)
+    if flag:
+        paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["WB", "R", "S", "R"])
+
+    flag, t, u, v = LRSR(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, -0.5 * PI, u, v], ["R", "WB", "S", "WB"])
+
+    flag, t, u, v = LRSR(-x, -y, phi)
+    if flag:
+        paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["R", "WB", "S", "WB"])
+
+    # backwards
+    xb = x * math.cos(phi) + y * math.sin(phi)
+    yb = x * math.sin(phi) - y * math.cos(phi)
+
+    flag, t, u, v = LRSL(xb, yb, phi)
+    if flag:
+        paths = set_path(paths, [v, u, -0.5 * PI, t], ["WB", "S", "R", "WB"])
+
+    flag, t, u, v = LRSL(-xb, yb, -phi)
+    if flag:
+        paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["WB", "S", "R", "WB"])
+
+    flag, t, u, v = LRSL(xb, -yb, -phi)
+    if flag:
+        paths = set_path(paths, [v, u, -0.5 * PI, t], ["R", "S", "WB", "R"])
+
+    flag, t, u, v = LRSL(-xb, -yb, phi)
+    if flag:
+        paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["R", "S", "WB", "R"])
+
+    flag, t, u, v = LRSR(xb, yb, phi)
+    if flag:
+        paths = set_path(paths, [v, u, -0.5 * PI, t], ["R", "S", "R", "WB"])
+
+    flag, t, u, v = LRSR(-xb, yb, -phi)
+    if flag:
+        paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["R", "S", "R", "WB"])
+
+    flag, t, u, v = LRSR(xb, -yb, -phi)
+    if flag:
+        paths = set_path(paths, [v, u, -0.5 * PI, t], ["WB", "S", "WB", "R"])
+
+    flag, t, u, v = LRSR(-xb, -yb, phi)
+    if flag:
+        paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["WB", "S", "WB", "R"])
+
+    return paths
+
+
+def LRSLR(x, y, phi):
+    # formula 8.11 *** TYPO IN PAPER ***
+    xi = x + math.sin(phi)
+    eta = y - 1.0 - math.cos(phi)
+    rho, theta = R(xi, eta)
+
+    if rho >= 2.0:
+        u = 4.0 - math.sqrt(rho * rho - 4.0)
+        if u <= 0.0:
+            t = M(math.atan2((4.0 - u) * xi - 2.0 * eta, -2.0 * xi + (u - 4.0) * eta))
+            v = M(t - phi)
+
+            if t >= 0.0 and v >= 0.0:
+                return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def CCSCC(x, y, phi, paths):
+    flag, t, u, v = LRSLR(x, y, phi)
+    if flag:
+        paths = set_path(paths, [t, -0.5 * PI, u, -0.5 * PI, v], ["WB", "R", "S", "WB", "R"])
+
+    flag, t, u, v = LRSLR(-x, y, -phi)
+    if flag:
+        paths = set_path(paths, [-t, 0.5 * PI, -u, 0.5 * PI, -v], ["WB", "R", "S", "WB", "R"])
+
+    flag, t, u, v = LRSLR(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, -0.5 * PI, u, -0.5 * PI, v], ["R", "WB", "S", "R", "WB"])
+
+    flag, t, u, v = LRSLR(-x, -y, phi)
+    if flag:
+        paths = set_path(paths, [-t, 0.5 * PI, -u, 0.5 * PI, -v], ["R", "WB", "S", "R", "WB"])
+
+    return paths
+
+
+def generate_local_course(L, lengths, mode, maxc, step_size):
+    point_num = int(L / step_size) + len(lengths) + 3
+
+    px = [0.0 for _ in range(point_num)]
+    py = [0.0 for _ in range(point_num)]
+    pyaw = [0.0 for _ in range(point_num)]
+    directions = [0 for _ in range(point_num)]
+    ind = 1
+
+    if lengths[0] > 0.0:
+        directions[0] = 1
+    else:
+        directions[0] = -1
+
+    if lengths[0] > 0.0:
+        d = step_size
+    else:
+        d = -step_size
+
+    ll = 0.0
+
+    for m, l, i in zip(mode, lengths, range(len(mode))):
+        if l > 0.0:
+            d = step_size
+        else:
+            d = -step_size
+
+        ox, oy, oyaw = px[ind], py[ind], pyaw[ind]
+
+        ind -= 1
+        if i >= 1 and (lengths[i - 1] * lengths[i]) > 0:
+            pd = -d - ll
+        else:
+            pd = d - ll
+
+        while abs(pd) <= abs(l):
+            ind += 1
+            px, py, pyaw, directions = \
+                interpolate(ind, pd, m, maxc, ox, oy, oyaw, px, py, pyaw, directions)
+            pd += d
+
+        ll = l - pd - d  # calc remain length
+
+        ind += 1
+        px, py, pyaw, directions = \
+            interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions)
+
+    if len(px) <= 1:
+        return [], [], [], []
+
+    # remove unused data
+    while len(px) >= 1 and px[-1] == 0.0:
+        px.pop()
+        py.pop()
+        pyaw.pop()
+        directions.pop()
+
+    return px, py, pyaw, directions
+
+
+def interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions):
+    if m == "S":
+        px[ind] = ox + l / maxc * math.cos(oyaw)
+        py[ind] = oy + l / maxc * math.sin(oyaw)
+        pyaw[ind] = oyaw
+    else:
+        ldx = math.sin(l) / maxc
+        if m == "WB":
+            ldy = (1.0 - math.cos(l)) / maxc
+        elif m == "R":
+            ldy = (1.0 - math.cos(l)) / (-maxc)
+
+        gdx = math.cos(-oyaw) * ldx + math.sin(-oyaw) * ldy
+        gdy = -math.sin(-oyaw) * ldx + math.cos(-oyaw) * ldy
+        px[ind] = ox + gdx
+        py[ind] = oy + gdy
+
+    if m == "WB":
+        pyaw[ind] = oyaw + l
+    elif m == "R":
+        pyaw[ind] = oyaw - l
+
+    if l > 0.0:
+        directions[ind] = 1
+    else:
+        directions[ind] = -1
+
+    return px, py, pyaw, directions
+
+
+def generate_path(q0, q1, maxc):
+    dx = q1[0] - q0[0]
+    dy = q1[1] - q0[1]
+    dth = q1[2] - q0[2]
+    c = math.cos(q0[2])
+    s = math.sin(q0[2])
+    x = (c * dx + s * dy) * maxc
+    y = (-s * dx + c * dy) * maxc
+
+    paths = []
+    paths = SCS(x, y, dth, paths)
+    paths = CSC(x, y, dth, paths)
+    paths = CCC(x, y, dth, paths)
+    paths = CCCC(x, y, dth, paths)
+    paths = CCSC(x, y, dth, paths)
+    paths = CCSCC(x, y, dth, paths)
+
+    return paths
+
+
+# utils
+def pi_2_pi(theta):
+    while theta > PI:
+        theta -= 2.0 * PI
+
+    while theta < -PI:
+        theta += 2.0 * PI
+
+    return theta
+
+
+def R(x, y):
+    """
+    Return the polar coordinates (r, theta) of the point (x, y)
+    """
+    r = math.hypot(x, y)
+    theta = math.atan2(y, x)
+
+    return r, theta
+
+
+def M(theta):
+    """
+    Regulate theta to -pi <= theta < pi
+    """
+    phi = theta % (2.0 * PI)
+
+    if phi < -PI:
+        phi += 2.0 * PI
+    if phi > PI:
+        phi -= 2.0 * PI
+
+    return phi
+
+
+def get_label(path):
+    label = ""
+
+    for m, l in zip(path.ctypes, path.lengths):
+        label = label + m
+        if l > 0.0:
+            label = label + "+"
+        else:
+            label = label + "-"
+
+    return label
+
+
+def calc_curvature(x, y, yaw, directions):
+    c, ds = [], []
+
+    for i in range(1, len(x) - 1):
+        dxn = x[i] - x[i - 1]
+        dxp = x[i + 1] - x[i]
+        dyn = y[i] - y[i - 1]
+        dyp = y[i + 1] - y[i]
+        dn = math.hypot(dxn, dyn)
+        dp = math.hypot(dxp, dyp)
+        dx = 1.0 / (dn + dp) * (dp / dn * dxn + dn / dp * dxp)
+        ddx = 2.0 / (dn + dp) * (dxp / dp - dxn / dn)
+        dy = 1.0 / (dn + dp) * (dp / dn * dyn + dn / dp * dyp)
+        ddy = 2.0 / (dn + dp) * (dyp / dp - dyn / dn)
+        curvature = (ddy * dx - ddx * dy) / (dx ** 2 + dy ** 2)
+        d = (dn + dp) / 2.0
+
+        if np.isnan(curvature):
+            curvature = 0.0
+
+        if directions[i] <= 0.0:
+            curvature = -curvature
+
+        if len(c) == 0:
+            ds.append(d)
+            c.append(curvature)
+
+        ds.append(d)
+        c.append(curvature)
+
+    ds.append(ds[-1])
+    c.append(c[-1])
+
+    return c, ds
+
+
+def check_path(sx, sy, syaw, gx, gy, gyaw, maxc):
+    paths = calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc)
+
+    assert len(paths) >= 1
+
+    for path in paths:
+        assert abs(path.x[0] - sx) <= 0.01
+        assert abs(path.y[0] - sy) <= 0.01
+        assert abs(path.yaw[0] - syaw) <= 0.01
+        assert abs(path.x[-1] - gx) <= 0.01
+        assert abs(path.y[-1] - gy) <= 0.01
+        assert abs(path.yaw[-1] - gyaw) <= 0.01
+
+        # course distance check
+        d = [math.hypot(dx, dy)
+             for dx, dy in zip(np.diff(path.x[0:len(path.x) - 1]),
+                               np.diff(path.y[0:len(path.y) - 1]))]
+
+        for i in range(len(d)):
+            assert abs(d[i] - STEP_SIZE) <= 0.001
+
+
+def main():
+    start_x = 3.0  # [m]
+    start_y = 10.0  # [m]
+    start_yaw = np.deg2rad(40.0)  # [rad]
+    end_x = 0.0  # [m]
+    end_y = 1.0  # [m]
+    end_yaw = np.deg2rad(0.0)  # [rad]
+    max_curvature = 0.1
+
+    t0 = time.time()
+
+    for i in range(1000):
+        _ = calc_optimal_path(start_x, start_y, start_yaw, end_x, end_y, end_yaw, max_curvature)
+
+    t1 = time.time()
+    print(t1 - t0)
+
+
+if __name__ == '__main__':
+    main()

utils/replay.py

@@ -0,0 +1,70 @@
+
+"""
+算法结果回放
+
+"""
+import json
+import math
+import matplotlib.pyplot as plt
+import input.make_map as mp
+from utils import drawcar as tools, reeds_shepp as rs
+from input import make_car
+import numpy as np
+import imageio
+
+def replay(map_scene,output_result):
+    picture_scene =  map_scene.replace('/', '/').replace('.json', '.jpg')
+    gif_scene =  map_scene.replace('input', 'image_save').replace('.json', '.gif')
+    if output_result[-6].isdigit():
+        Num =output_result.rsplit('/result_', 1)[-1].rsplit('.', 1)[0]
+        if Num[-2].isdigit():
+            gif_scene = gif_scene.replace('.gif', f'{Num[-3:]}.gif')
+            
+        else:
+            gif_scene = gif_scene.replace('.gif', f'{Num[-2:]}.gif')  
+    C = make_car.C
+    ox, oy,sp,gp = mp.make_map(map_scene)
+    sx, sy, syaw0 = sp['x'], sp['y'], sp['yaw']
+    gx, gy, gyaw0 = gp['3']['x_end'], gp['3']['y_end'], gp['3']['yaw']
+    with open(output_result,'r',encoding='UTF-8') as f:
+        result=json.load(f)
+    x = result['output_x']
+    y = result['output_y']
+    yaw = result['output_yaw']
+    direction = result['output_dir']
+    plt.rcParams['xtick.direction'] = 'in'
+    picture = plt.imread(picture_scene)
+    ox1,ox2,oy1,oy2 = min(ox), max(ox), min(oy), max(oy)
+    frames = []  # 用于保存每一帧图像
+    for k in range(len(x)):
+        if k % 3 == 0:
+            fig, ax = plt.subplots()
+            ax.imshow(picture, extent=[ox1, ox2, oy1, oy2], aspect='auto') 
+            ax.lines.clear()
+            ax.plot(x, y, linewidth=0.5, color='b', linestyle='--')
+            # ax.plot(ox, oy, ",k")
+            if k < len(x) - 2:
+                dy = (yaw[k + 1] - yaw[k]) / C.MOVE_STEP
+                steer = rs.pi_2_pi(math.atan(-C.WB * dy / direction[k]))
+            else:
+                steer = 0.0
+            tools.draw_car(ax, gx, gy, gyaw0, 0.0, 'dimgray')
+            tools.draw_car(ax, x[k], y[k], yaw[k], steer)
+            ax.set_title("Simulation Result", loc='left', fontweight="heavy")
+            ax.axis("equal")    
+            # 保存当前帧图像
+            fig.canvas.draw()
+            image = np.frombuffer(fig.canvas.tostring_rgb(), dtype='uint8')
+            image = image.reshape(fig.canvas.get_width_height()[::-1] + (3,))
+            frames.append(image)
+            plt.show()
+    # 将所有保存的帧图像整合成 GIF 动态图
+    imageio.mimsave(gif_scene, frames, fps=15)
+    if output_result[-6].isdigit():
+        if Num[-2].isdigit():
+            print(f"车位{Num[-2:]}仿真完成")
+        else:
+            print(f"车位{Num[-1]}仿真完成")
+    else:
+        print("仿真结束!")
+