mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-04-22 03:00:22 -04:00
mypy fix test
This commit is contained in:
Atsushi Sakai
@@ -41,7 +41,8 @@ class AStarPlanner:
|
||||
self.pind = pind
|
||||
|
||||
def __str__(self):
|
||||
return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind)
|
||||
return str(self.x) + "," + str(self.y) + "," + str(
|
||||
self.cost) + "," + str(self.pind)
|
||||
|
||||
def planning(self, sx, sy, gx, gy):
|
||||
"""
|
||||
@@ -72,7 +73,10 @@ class AStarPlanner:
|
||||
break
|
||||
|
||||
c_id = min(
|
||||
open_set, key=lambda o: open_set[o].cost + self.calc_heuristic(ngoal, open_set[o]))
|
||||
open_set,
|
||||
key=lambda o: open_set[o].cost + self.calc_heuristic(ngoal,
|
||||
open_set[
|
||||
o]))
|
||||
current = open_set[c_id]
|
||||
|
||||
# show graph
|
||||
@@ -81,7 +85,8 @@ class AStarPlanner:
|
||||
self.calc_grid_position(current.y, self.miny), "xc")
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
lambda event: [exit(
|
||||
0) if event.key == 'escape' else None])
|
||||
if len(closed_set.keys()) % 10 == 0:
|
||||
plt.pause(0.001)
|
||||
|
||||
@@ -104,7 +109,6 @@ class AStarPlanner:
|
||||
current.cost + self.motion[i][2], c_id)
|
||||
n_id = self.calc_grid_index(node)
|
||||
|
||||
|
||||
# If the node is not safe, do nothing
|
||||
if not self.verify_node(node):
|
||||
continue
|
||||
|
||||
@@ -11,7 +11,7 @@ import matplotlib.pyplot as plt
|
||||
import scipy.interpolate as scipy_interpolate
|
||||
|
||||
|
||||
def approximate_b_spline_path(x: int, y: list, n_path_points: int,
|
||||
def approximate_b_spline_path(x: list, y: list, n_path_points: int,
|
||||
degree: int = 3) -> tuple:
|
||||
"""
|
||||
approximate points with a B-Spline path
|
||||
|
||||
0
PathPlanning/CubicSpline/__init__.py
Normal file
0
PathPlanning/CubicSpline/__init__.py
Normal file
@@ -1,239 +0,0 @@
|
||||
"""
|
||||
cubic spline planner
|
||||
|
||||
Author: Atsushi Sakai
|
||||
|
||||
"""
|
||||
import math
|
||||
import numpy as np
|
||||
import bisect
|
||||
|
||||
|
||||
class Spline:
|
||||
"""
|
||||
Cubic Spline class
|
||||
"""
|
||||
|
||||
def __init__(self, x, y):
|
||||
self.b, self.c, self.d, self.w = [], [], [], []
|
||||
|
||||
self.x = x
|
||||
self.y = y
|
||||
|
||||
self.nx = len(x) # dimension of x
|
||||
h = np.diff(x)
|
||||
|
||||
# calc coefficient c
|
||||
self.a = [iy for iy in y]
|
||||
|
||||
# calc coefficient c
|
||||
A = self.__calc_A(h)
|
||||
B = self.__calc_B(h)
|
||||
self.c = np.linalg.solve(A, B)
|
||||
# print(self.c1)
|
||||
|
||||
# calc spline coefficient b and d
|
||||
for i in range(self.nx - 1):
|
||||
self.d.append((self.c[i + 1] - self.c[i]) / (3.0 * h[i]))
|
||||
tb = (self.a[i + 1] - self.a[i]) / h[i] - h[i] * \
|
||||
(self.c[i + 1] + 2.0 * self.c[i]) / 3.0
|
||||
self.b.append(tb)
|
||||
|
||||
def calc(self, t):
|
||||
"""
|
||||
Calc position
|
||||
|
||||
if t is outside of the input x, return None
|
||||
|
||||
"""
|
||||
|
||||
if t < self.x[0]:
|
||||
return None
|
||||
elif t > self.x[-1]:
|
||||
return None
|
||||
|
||||
i = self.__search_index(t)
|
||||
dx = t - self.x[i]
|
||||
result = self.a[i] + self.b[i] * dx + \
|
||||
self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0
|
||||
|
||||
return result
|
||||
|
||||
def calcd(self, t):
|
||||
"""
|
||||
Calc first derivative
|
||||
|
||||
if t is outside of the input x, return None
|
||||
"""
|
||||
|
||||
if t < self.x[0]:
|
||||
return None
|
||||
elif t > self.x[-1]:
|
||||
return None
|
||||
|
||||
i = self.__search_index(t)
|
||||
dx = t - self.x[i]
|
||||
result = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0
|
||||
return result
|
||||
|
||||
def calcdd(self, t):
|
||||
"""
|
||||
Calc second derivative
|
||||
"""
|
||||
|
||||
if t < self.x[0]:
|
||||
return None
|
||||
elif t > self.x[-1]:
|
||||
return None
|
||||
|
||||
i = self.__search_index(t)
|
||||
dx = t - self.x[i]
|
||||
result = 2.0 * self.c[i] + 6.0 * self.d[i] * dx
|
||||
return result
|
||||
|
||||
def __search_index(self, x):
|
||||
"""
|
||||
search data segment index
|
||||
"""
|
||||
return bisect.bisect(self.x, x) - 1
|
||||
|
||||
def __calc_A(self, h):
|
||||
"""
|
||||
calc matrix A for spline coefficient c
|
||||
"""
|
||||
A = np.zeros((self.nx, self.nx))
|
||||
A[0, 0] = 1.0
|
||||
for i in range(self.nx - 1):
|
||||
if i != (self.nx - 2):
|
||||
A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1])
|
||||
A[i + 1, i] = h[i]
|
||||
A[i, i + 1] = h[i]
|
||||
|
||||
A[0, 1] = 0.0
|
||||
A[self.nx - 1, self.nx - 2] = 0.0
|
||||
A[self.nx - 1, self.nx - 1] = 1.0
|
||||
# print(A)
|
||||
return A
|
||||
|
||||
def __calc_B(self, h):
|
||||
"""
|
||||
calc matrix B for spline coefficient c
|
||||
"""
|
||||
B = np.zeros(self.nx)
|
||||
for i in range(self.nx - 2):
|
||||
B[i + 1] = 3.0 * (self.a[i + 2] - self.a[i + 1]) / \
|
||||
h[i + 1] - 3.0 * (self.a[i + 1] - self.a[i]) / h[i]
|
||||
# print(B)
|
||||
return B
|
||||
|
||||
|
||||
class Spline2D:
|
||||
"""
|
||||
2D Cubic Spline class
|
||||
|
||||
"""
|
||||
|
||||
def __init__(self, x, y):
|
||||
self.s = self.__calc_s(x, y)
|
||||
self.sx = Spline(self.s, x)
|
||||
self.sy = Spline(self.s, y)
|
||||
|
||||
def __calc_s(self, x, y):
|
||||
dx = np.diff(x)
|
||||
dy = np.diff(y)
|
||||
self.ds = [math.sqrt(idx ** 2 + idy ** 2)
|
||||
for (idx, idy) in zip(dx, dy)]
|
||||
s = [0]
|
||||
s.extend(np.cumsum(self.ds))
|
||||
return s
|
||||
|
||||
def calc_position(self, s):
|
||||
"""
|
||||
calc position
|
||||
"""
|
||||
x = self.sx.calc(s)
|
||||
y = self.sy.calc(s)
|
||||
|
||||
return x, y
|
||||
|
||||
def calc_curvature(self, s):
|
||||
"""
|
||||
calc curvature
|
||||
"""
|
||||
dx = self.sx.calcd(s)
|
||||
ddx = self.sx.calcdd(s)
|
||||
dy = self.sy.calcd(s)
|
||||
ddy = self.sy.calcdd(s)
|
||||
k = (ddy * dx - ddx * dy) / (dx ** 2 + dy ** 2)
|
||||
return k
|
||||
|
||||
def calc_yaw(self, s):
|
||||
"""
|
||||
calc yaw
|
||||
"""
|
||||
dx = self.sx.calcd(s)
|
||||
dy = self.sy.calcd(s)
|
||||
yaw = math.atan2(dy, dx)
|
||||
return yaw
|
||||
|
||||
|
||||
def calc_spline_course(x, y, ds=0.1):
|
||||
sp = Spline2D(x, y)
|
||||
s = list(np.arange(0, sp.s[-1], ds))
|
||||
|
||||
rx, ry, ryaw, rk = [], [], [], []
|
||||
for i_s in s:
|
||||
ix, iy = sp.calc_position(i_s)
|
||||
rx.append(ix)
|
||||
ry.append(iy)
|
||||
ryaw.append(sp.calc_yaw(i_s))
|
||||
rk.append(sp.calc_curvature(i_s))
|
||||
|
||||
return rx, ry, ryaw, rk, s
|
||||
|
||||
|
||||
def main(): # pragma: no cover
|
||||
print("Spline 2D test")
|
||||
import matplotlib.pyplot as plt
|
||||
x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0]
|
||||
y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0]
|
||||
|
||||
sp = Spline2D(x, y)
|
||||
s = np.arange(0, sp.s[-1], 0.1)
|
||||
|
||||
rx, ry, ryaw, rk = [], [], [], []
|
||||
for i_s in s:
|
||||
ix, iy = sp.calc_position(i_s)
|
||||
rx.append(ix)
|
||||
ry.append(iy)
|
||||
ryaw.append(sp.calc_yaw(i_s))
|
||||
rk.append(sp.calc_curvature(i_s))
|
||||
|
||||
plt.subplots(1)
|
||||
plt.plot(x, y, "xb", label="input")
|
||||
plt.plot(rx, ry, "-r", label="spline")
|
||||
plt.grid(True)
|
||||
plt.axis("equal")
|
||||
plt.xlabel("x[m]")
|
||||
plt.ylabel("y[m]")
|
||||
plt.legend()
|
||||
|
||||
plt.subplots(1)
|
||||
plt.plot(s, [np.rad2deg(iyaw) for iyaw in ryaw], "-r", label="yaw")
|
||||
plt.grid(True)
|
||||
plt.legend()
|
||||
plt.xlabel("line length[m]")
|
||||
plt.ylabel("yaw angle[deg]")
|
||||
|
||||
plt.subplots(1)
|
||||
plt.plot(s, rk, "-r", label="curvature")
|
||||
plt.grid(True)
|
||||
plt.legend()
|
||||
plt.xlabel("line length[m]")
|
||||
plt.ylabel("curvature [1/m]")
|
||||
|
||||
plt.show()
|
||||
|
||||
|
||||
if __name__ == '__main__': # pragma: no cover
|
||||
main()
|
||||
@@ -6,9 +6,11 @@ author: Atsushi Sakai (@Atsushi_twi)
|
||||
|
||||
Ref:
|
||||
|
||||
- [Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame](https://www.researchgate.net/profile/Moritz_Werling/publication/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame/links/54f749df0cf210398e9277af.pdf)
|
||||
- [Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame]
|
||||
(https://www.researchgate.net/profile/Moritz_Werling/publication/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame/links/54f749df0cf210398e9277af.pdf)
|
||||
|
||||
- [Optimal trajectory generation for dynamic street scenarios in a Frenet Frame](https://www.youtube.com/watch?v=Cj6tAQe7UCY)
|
||||
- [Optimal trajectory generation for dynamic street scenarios in a Frenet Frame]
|
||||
(https://www.youtube.com/watch?v=Cj6tAQe7UCY)
|
||||
|
||||
"""
|
||||
|
||||
@@ -16,19 +18,20 @@ import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
import copy
|
||||
import math
|
||||
import cubic_spline_planner
|
||||
import sys
|
||||
import os
|
||||
|
||||
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
|
||||
"/../QuinticPolynomialsPlanner/")
|
||||
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
|
||||
"/../CubicSpline/")
|
||||
|
||||
try:
|
||||
from quintic_polynomials_planner import QuinticPolynomial
|
||||
import cubic_spline_planner
|
||||
except ImportError:
|
||||
raise
|
||||
|
||||
|
||||
SIM_LOOP = 500
|
||||
|
||||
# Parameter
|
||||
@@ -38,36 +41,35 @@ MAX_CURVATURE = 1.0 # maximum curvature [1/m]
|
||||
MAX_ROAD_WIDTH = 7.0 # maximum road width [m]
|
||||
D_ROAD_W = 1.0 # road width sampling length [m]
|
||||
DT = 0.2 # time tick [s]
|
||||
MAXT = 5.0 # max prediction time [m]
|
||||
MINT = 4.0 # min prediction time [m]
|
||||
MAX_T = 5.0 # max prediction time [m]
|
||||
MIN_T = 4.0 # min prediction time [m]
|
||||
TARGET_SPEED = 30.0 / 3.6 # target speed [m/s]
|
||||
D_T_S = 5.0 / 3.6 # target speed sampling length [m/s]
|
||||
N_S_SAMPLE = 1 # sampling number of target speed
|
||||
ROBOT_RADIUS = 2.0 # robot radius [m]
|
||||
|
||||
# cost weights
|
||||
KJ = 0.1
|
||||
KT = 0.1
|
||||
KD = 1.0
|
||||
KLAT = 1.0
|
||||
KLON = 1.0
|
||||
K_J = 0.1
|
||||
K_T = 0.1
|
||||
K_D = 1.0
|
||||
K_LAT = 1.0
|
||||
K_LON = 1.0
|
||||
|
||||
show_animation = True
|
||||
|
||||
|
||||
class quartic_polynomial:
|
||||
|
||||
def __init__(self, xs, vxs, axs, vxe, axe, T):
|
||||
class QuarticPolynomial:
|
||||
|
||||
def __init__(self, xs, vxs, axs, vxe, axe, time):
|
||||
# calc coefficient of quartic polynomial
|
||||
|
||||
self.a0 = xs
|
||||
self.a1 = vxs
|
||||
self.a2 = axs / 2.0
|
||||
|
||||
A = np.array([[3 * T ** 2, 4 * T ** 3],
|
||||
[6 * T, 12 * T ** 2]])
|
||||
b = np.array([vxe - self.a1 - 2 * self.a2 * T,
|
||||
A = np.array([[3 * time ** 2, 4 * time ** 3],
|
||||
[6 * time, 12 * time ** 2]])
|
||||
b = np.array([vxe - self.a1 - 2 * self.a2 * time,
|
||||
axe - 2 * self.a2])
|
||||
x = np.linalg.solve(A, b)
|
||||
|
||||
@@ -75,19 +77,19 @@ class quartic_polynomial:
|
||||
self.a4 = x[1]
|
||||
|
||||
def calc_point(self, t):
|
||||
xt = self.a0 + self.a1 * t + self.a2 * t**2 + \
|
||||
self.a3 * t**3 + self.a4 * t**4
|
||||
xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \
|
||||
self.a3 * t ** 3 + self.a4 * t ** 4
|
||||
|
||||
return xt
|
||||
|
||||
def calc_first_derivative(self, t):
|
||||
xt = self.a1 + 2 * self.a2 * t + \
|
||||
3 * self.a3 * t**2 + 4 * self.a4 * t**3
|
||||
3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3
|
||||
|
||||
return xt
|
||||
|
||||
def calc_second_derivative(self, t):
|
||||
xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t**2
|
||||
xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2
|
||||
|
||||
return xt
|
||||
|
||||
@@ -97,7 +99,7 @@ class quartic_polynomial:
|
||||
return xt
|
||||
|
||||
|
||||
class Frenet_path:
|
||||
class FrenetPath:
|
||||
|
||||
def __init__(self):
|
||||
self.t = []
|
||||
@@ -121,15 +123,14 @@ class Frenet_path:
|
||||
|
||||
|
||||
def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0):
|
||||
|
||||
frenet_paths = []
|
||||
|
||||
# generate path to each offset goal
|
||||
for di in np.arange(-MAX_ROAD_WIDTH, MAX_ROAD_WIDTH, D_ROAD_W):
|
||||
|
||||
# Lateral motion planning
|
||||
for Ti in np.arange(MINT, MAXT, DT):
|
||||
fp = Frenet_path()
|
||||
for Ti in np.arange(MIN_T, MAX_T, DT):
|
||||
fp = FrenetPath()
|
||||
|
||||
# lat_qp = quintic_polynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)
|
||||
lat_qp = QuinticPolynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)
|
||||
@@ -141,9 +142,10 @@ def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0):
|
||||
fp.d_ddd = [lat_qp.calc_third_derivative(t) for t in fp.t]
|
||||
|
||||
# Longitudinal motion planning (Velocity keeping)
|
||||
for tv in np.arange(TARGET_SPEED - D_T_S * N_S_SAMPLE, TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S):
|
||||
for tv in np.arange(TARGET_SPEED - D_T_S * N_S_SAMPLE,
|
||||
TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S):
|
||||
tfp = copy.deepcopy(fp)
|
||||
lon_qp = quartic_polynomial(s0, c_speed, 0.0, tv, 0.0, Ti)
|
||||
lon_qp = QuarticPolynomial(s0, c_speed, 0.0, tv, 0.0, Ti)
|
||||
|
||||
tfp.s = [lon_qp.calc_point(t) for t in fp.t]
|
||||
tfp.s_d = [lon_qp.calc_first_derivative(t) for t in fp.t]
|
||||
@@ -154,11 +156,11 @@ def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0):
|
||||
Js = sum(np.power(tfp.s_ddd, 2)) # square of jerk
|
||||
|
||||
# square of diff from target speed
|
||||
ds = (TARGET_SPEED - tfp.s_d[-1])**2
|
||||
ds = (TARGET_SPEED - tfp.s_d[-1]) ** 2
|
||||
|
||||
tfp.cd = KJ * Jp + KT * Ti + KD * tfp.d[-1]**2
|
||||
tfp.cv = KJ * Js + KT * Ti + KD * ds
|
||||
tfp.cf = KLAT * tfp.cd + KLON * tfp.cv
|
||||
tfp.cd = K_J * Jp + K_T * Ti + K_D * tfp.d[-1] ** 2
|
||||
tfp.cv = K_J * Js + K_T * Ti + K_D * ds
|
||||
tfp.cf = K_LAT * tfp.cd + K_LON * tfp.cv
|
||||
|
||||
frenet_paths.append(tfp)
|
||||
|
||||
@@ -166,7 +168,6 @@ def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0):
|
||||
|
||||
|
||||
def calc_global_paths(fplist, csp):
|
||||
|
||||
for fp in fplist:
|
||||
|
||||
# calc global positions
|
||||
@@ -174,10 +175,10 @@ def calc_global_paths(fplist, csp):
|
||||
ix, iy = csp.calc_position(fp.s[i])
|
||||
if ix is None:
|
||||
break
|
||||
iyaw = csp.calc_yaw(fp.s[i])
|
||||
i_yaw = csp.calc_yaw(fp.s[i])
|
||||
di = fp.d[i]
|
||||
fx = ix + di * math.cos(iyaw + math.pi / 2.0)
|
||||
fy = iy + di * math.sin(iyaw + math.pi / 2.0)
|
||||
fx = ix + di * math.cos(i_yaw + math.pi / 2.0)
|
||||
fy = iy + di * math.sin(i_yaw + math.pi / 2.0)
|
||||
fp.x.append(fx)
|
||||
fp.y.append(fy)
|
||||
|
||||
@@ -199,12 +200,11 @@ def calc_global_paths(fplist, csp):
|
||||
|
||||
|
||||
def check_collision(fp, ob):
|
||||
|
||||
for i in range(len(ob[:, 0])):
|
||||
d = [((ix - ob[i, 0])**2 + (iy - ob[i, 1])**2)
|
||||
d = [((ix - ob[i, 0]) ** 2 + (iy - ob[i, 1]) ** 2)
|
||||
for (ix, iy) in zip(fp.x, fp.y)]
|
||||
|
||||
collision = any([di <= ROBOT_RADIUS**2 for di in d])
|
||||
collision = any([di <= ROBOT_RADIUS ** 2 for di in d])
|
||||
|
||||
if collision:
|
||||
return False
|
||||
@@ -213,38 +213,38 @@ def check_collision(fp, ob):
|
||||
|
||||
|
||||
def check_paths(fplist, ob):
|
||||
|
||||
okind = []
|
||||
ok_ind = []
|
||||
for i, _ in enumerate(fplist):
|
||||
if any([v > MAX_SPEED for v in fplist[i].s_d]): # Max speed check
|
||||
continue
|
||||
elif any([abs(a) > MAX_ACCEL for a in fplist[i].s_dd]): # Max accel check
|
||||
elif any([abs(a) > MAX_ACCEL for a in
|
||||
fplist[i].s_dd]): # Max accel check
|
||||
continue
|
||||
elif any([abs(c) > MAX_CURVATURE for c in fplist[i].c]): # Max curvature check
|
||||
elif any([abs(c) > MAX_CURVATURE for c in
|
||||
fplist[i].c]): # Max curvature check
|
||||
continue
|
||||
elif not check_collision(fplist[i], ob):
|
||||
continue
|
||||
|
||||
okind.append(i)
|
||||
ok_ind.append(i)
|
||||
|
||||
return [fplist[i] for i in okind]
|
||||
return [fplist[i] for i in ok_ind]
|
||||
|
||||
|
||||
def frenet_optimal_planning(csp, s0, c_speed, c_d, c_d_d, c_d_dd, ob):
|
||||
|
||||
fplist = calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0)
|
||||
fplist = calc_global_paths(fplist, csp)
|
||||
fplist = check_paths(fplist, ob)
|
||||
|
||||
# find minimum cost path
|
||||
mincost = float("inf")
|
||||
bestpath = None
|
||||
min_cost = float("inf")
|
||||
best_path = None
|
||||
for fp in fplist:
|
||||
if mincost >= fp.cf:
|
||||
mincost = fp.cf
|
||||
bestpath = fp
|
||||
if min_cost >= fp.cf:
|
||||
min_cost = fp.cf
|
||||
best_path = fp
|
||||
|
||||
return bestpath
|
||||
return best_path
|
||||
|
||||
|
||||
def generate_target_course(x, y):
|
||||
@@ -282,7 +282,7 @@ def main():
|
||||
c_speed = 10.0 / 3.6 # current speed [m/s]
|
||||
c_d = 2.0 # current lateral position [m]
|
||||
c_d_d = 0.0 # current lateral speed [m/s]
|
||||
c_d_dd = 0.0 # current latral acceleration [m/s]
|
||||
c_d_dd = 0.0 # current lateral acceleration [m/s]
|
||||
s0 = 0.0 # current course position
|
||||
|
||||
area = 20.0 # animation area length [m]
|
||||
@@ -304,8 +304,9 @@ def main():
|
||||
if show_animation: # pragma: no cover
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
plt.gcf().canvas.mpl_connect(
|
||||
'key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
plt.plot(tx, ty)
|
||||
plt.plot(ob[:, 0], ob[:, 1], "xk")
|
||||
plt.plot(path.x[1:], path.y[1:], "-or")
|
||||
|
||||
0
PathPlanning/HybridAStar/__init__.py
Normal file
0
PathPlanning/HybridAStar/__init__.py
Normal file
@@ -14,7 +14,7 @@ import matplotlib.pyplot as plt
|
||||
import sys
|
||||
sys.path.append("../ReedsSheppPath/")
|
||||
try:
|
||||
from a_star import dp_planning # , calc_obstacle_map
|
||||
from a_star_heuristic import dp_planning # , calc_obstacle_map
|
||||
import reeds_shepp_path_planning as rs
|
||||
from car import move, check_car_collision, MAX_STEER, WB, plot_car
|
||||
except:
|
||||
|
||||
Reference in New Issue
Block a user