This commit is contained in:
zhm-real
2020-08-05 12:53:19 -07:00
parent b3f190ec36
commit 8e232410ed
31 changed files with 296 additions and 219 deletions
+4 -4
View File
@@ -25,7 +25,7 @@ class Spline:
self.x = x
self.y = y
self.nx = len(x) # dimension of x
self.nx = len(x) # dimension of s
h = np.diff(x)
# calc coefficient cBest
@@ -48,7 +48,7 @@ class Spline:
u"""
Calc position
if t is outside of the input x, return None
if t is outside of the input s, return None
"""
@@ -68,7 +68,7 @@ class Spline:
u"""
Calc first derivative
if t is outside of the input x, return None
if t is outside of the input s, return None
"""
if t < self.x[0]:
@@ -219,7 +219,7 @@ def test_spline2d():
plt.plot(rx, ry, "-r", label="spline")
plt.grid(True)
plt.axis("equal")
plt.xlabel("x[m]")
plt.xlabel("s[m]")
plt.ylabel("y[m]")
plt.legend()
+2 -2
View File
@@ -14,7 +14,7 @@ class PATH:
def __init__(self, L, mode, x, y, yaw):
self.L = L # total path length [float]
self.mode = mode # type of each part of the path [string]
self.x = x # final x positions [m]
self.x = x # final s positions [m]
self.y = y # final y positions [m]
self.yaw = yaw # final yaw angles [rad]
@@ -298,7 +298,7 @@ def calc_dubins_path(sx, sy, syaw, gx, gy, gyaw, curv, step_size=0.1):
def main():
# choose states pairs: (x, y, yaw)
# choose states pairs: (s, y, yaw)
# simulation-1
states = [(0, 0, 0), (10, 10, -90), (20, 5, 60), (30, 10, 120),
(35, -5, 30), (25, -10, -120), (15, -15, 100), (0, -10, -90)]
+3 -3
View File
@@ -16,7 +16,7 @@ class PATH:
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.x = x # final s positions [m]
self.y = y # final y positions [m]
self.yaw = yaw # final yaw angles [rad]
self.directions = directions # forward: 1, backward:-1
@@ -573,7 +573,7 @@ def pi_2_pi(theta):
def R(x, y):
"""
Return the polar coordinates (r, theta) of the point (x, y)
Return the polar coordinates (r, theta) of the point (s, y)
"""
r = math.hypot(x, y)
theta = math.atan2(y, x)
@@ -667,7 +667,7 @@ def check_path(sx, sy, syaw, gx, gy, gyaw, maxc):
def main():
# choose states pairs: (x, y, yaw)
# choose states pairs: (s, y, yaw)
# simulation-1
# states = [(0, 0, 0), (10, 10, -90), (20, 5, 60), (30, 10, 120),
# (35, -5, 30), (25, -10, -120), (15, -15, 100), (0, -10, -90)]