#! /usr/bin/python
"""
Cubic Spline library on python
author Atsushi Sakai
usage: see test codes as below
license: MIT
"""
import bisect
import math
import carla
import numpy as np
import scipy.special
[docs]
def local_wp(wp: carla.Waypoint, max_distance: float = 50, distance_rate: float = 1.4):
"""Get waypoints ahead on the current lane.
Args
----
wp:
Starting waypoint
max_distance:
Maximum distance ahead to gather waypoints
distance_rate:
Rate at which distance increases between waypoints
Returns
-------
List of waypoints ahead on the current lane
"""
seq = 1.0
wp_l = [wp.next(0.01)[0]]
while True:
wp_l.append(wp.next(seq)[0])
seq *= distance_rate
if seq > max_distance:
break
return wp_l
[docs]
def branch_wp(wp: carla.Waypoint, max_distance: float = 80, distance_rate: float = 1.4):
"""Get all branching waypoints (including lane changes).
Args
----
wp:
Starting waypoint
max_distance:
Maximum distance ahead to gather waypoints
distance_rate:
Rate at which distance increases between waypoints
Returns
-------
List of all waypoints including branches from the starting point
"""
seq = 1.0
ap_l = [wp.next(0.01)[0]]
while True:
for p in wp.next(seq):
ap_l.append(p)
seq += distance_rate
if seq > max_distance:
break
return ap_l
[docs]
def cal_angle(vec_1: "list[float]", vec_2: "list[float]") -> "float | None":
"""Calculate angle between two 2D vectors.
Args
----
vec_1:
First vector as [x, y]
vec_2:
Second vector as [x, y]
Returns
-------
Angle in radians, or None if either vector has zero magnitude
"""
norm_1 = np.hypot(vec_1[0], vec_1[1])
norm_2 = np.hypot(vec_2[0], vec_2[1])
if norm_1 * norm_2 != 0:
cos_theta = (vec_1[0] * vec_2[0] + vec_1[1] * vec_2[1]) / (norm_1 * norm_2)
else:
return None
return math.acos(cos_theta)
[docs]
def ref_waypoint(wp: carla.Waypoint, max_dist: float = 30.0, dist_rate: float = 1.4) -> "list[carla.Waypoint]":
"""Get reference waypoints ahead with exponential spacing.
Args
----
wp:
Starting waypoint
max_dist:
Maximum distance ahead to gather waypoints
dist_rate:
Rate at which distance increases exponentially
Returns
-------
List of reference waypoints with exponential spacing
"""
start_dist = 1
wp_l = []
while True:
wp_l.append(wp.next(start_dist)[0])
start_dist *= dist_rate
if start_dist > max_dist:
break
return wp_l
[docs]
def calc_path(sx: float, sy: float, syaw: float, ex: float, ey: float, eyaw: float, offset: float, d_dist: float):
"""Compute Bezier curve path from start to end position.
Args
----
sx:
X-coordinate of the starting point
sy:
Y-coordinate of the starting point
syaw:
Yaw angle at start (radians)
ex:
X-coordinate of the ending point
ey:
Y-coordinate of the ending point
eyaw:
Yaw angle at the end (radians)
offset:
Control point offset
d_dist:
Distance between points on the path
Returns
-------
Tuple of (path_x, path_y) numpy arrays representing the bezier path
"""
length = np.hypot(sx - ex, sy - ey)
dist = length / offset
n_points = int(length / d_dist)
control_points = np.array(
[
[sx, sy],
[sx + dist * np.cos(syaw), sy + dist * np.sin(syaw)],
[ex - dist * np.cos(eyaw), ey - dist * np.sin(eyaw)],
[ex, ey],
]
)
rx, ry, ryaw = [], [], []
for t in np.linspace(0, 1, n_points):
p = bezier(t, control_points)
rx.append(p[0])
ry.append(p[1])
ryaw.append(calc_yaw(t, control_points))
s = calc_s(rx, ry)
return rx, ry, ryaw, s
[docs]
def bernstein_poly(n: int, i: int, t: float) -> float:
"""
Bernstein polynom.
:param n: (int) polynom degree
:param i: (int)
:param t: (float)
:return: (float)
"""
return math.comb(n, i) * t**i * (1 - t) ** (n - i)
return scipy.special.comb(n, i) * t**i * (1 - t) ** (n - i)
[docs]
def bezier(t, control_points):
"""
Return one point on the bezier curve.
:param t: (float) number in [0, 1]
:param control_points: (numpy array)
:return: (numpy array) Coordinates of the point
"""
n = len(control_points) - 1
return np.sum([bernstein_poly(n, i, t) * control_points[i] for i in range(n + 1)], axis=0)
[docs]
def bezier_derivatives_control_points(control_points, n_derivatives):
"""
Compute control points of the successive derivatives of a given bezier curve.
A derivative of a bezier curve is a bezier curve.
See https://pomax.github.io/bezierinfo/#derivatives
for detailed explanations
:param control_points: (numpy array)
:param n_derivatives: (int)
e.g., n_derivatives=2 -> compute control points for first and second derivatives
:return: ([numpy array])
"""
w = {0: control_points}
for i in range(n_derivatives):
n = len(w[i])
w[i + 1] = np.array([(n - 1) * (w[i][j + 1] - w[i][j]) for j in range(n - 1)])
return w
[docs]
def calc_curvature(dx, dy, ddx, ddy):
"""
Compute curvature at one point given first and second derivatives.
:param dx: (float) First derivative along x axis
:param dy: (float)
:param ddx: (float) Second derivative along x axis
:param ddy: (float)
:return: (float)
"""
return (dx * ddy - dy * ddx) / (dx**2 + dy**2) ** (3 / 2)
[docs]
def calc_yaw(t, control_points):
"""
calc yaw
"""
derivatives_cp = bezier_derivatives_control_points(control_points, 1)
dt = bezier(t, derivatives_cp[1])
yaw = math.atan2(dt[1], dt[0])
return yaw
[docs]
def calc_s(rx, ry):
dx = np.diff(rx)
dy = np.diff(ry)
ds = [math.sqrt(idx**2 + idy**2) for (idx, idy) in zip(dx, dy)]
s = [0]
s.extend(np.cumsum(ds))
return s
[docs]
def pi_2_pi(theta: float) -> float:
while theta > math.pi:
theta -= 2 * math.pi
while theta < -math.pi:
theta += 2 * math.pi
return theta
[docs]
class Spline:
"""
Cubic Spline class
"""
def __init__(self, x: "list[float]", y: "list[float]"):
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)
[docs]
def calc(self, t: float):
"""
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
[docs]
def calcd(self, t: float):
"""
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
[docs]
def calcdd(self, t: float):
"""
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: "list[float]") -> int:
"""
search data segment index
"""
return bisect.bisect(self.x, x) - 1
def __calc_A(self, h: "list[float]") -> np.ndarray:
"""
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: "list[float]") -> np.ndarray:
"""
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
[docs]
class Spline2D:
"""
2D Cubic Spline class
"""
def __init__(self, x: "list[float]", y: "list[float]"):
self.s = self.__calc_s(x, y)
self.x = x
self.y = y
self.sx = Spline(self.s, x)
self.sy = Spline(self.s, y)
def __calc_s(self, x: "list[float]", y: "list[float]") -> "list[float]":
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
[docs]
def calc_position(self, s: float):
"""
calc position
"""
x = self.sx.calc(s)
y = self.sy.calc(s)
return x, y
[docs]
def calc_curvature(self, s: float):
"""
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
[docs]
def calc_yaw(self, s: float):
"""
calc yaw
"""
dx = self.sx.calcd(s)
dy = self.sy.calcd(s)
yaw = math.atan2(dy, dx)
# i = bisect.bisect(self.s, s) - 1
# dx = self.x[i+1]-self.x[i]
# dy = self.y[i+1]-self.y[i]
# yaw = math.atan2(dy, dx)
return yaw
[docs]
def test_spline2d():
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))
flg, ax = 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()
flg, ax = plt.subplots(1)
plt.plot(s, [math.degrees(iyaw) for iyaw in ryaw], "-r", label="yaw")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("yaw angle[deg]")
flg, ax = 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()
[docs]
def test_spline():
print("Spline test")
import matplotlib.pyplot as plt
x = [-0.5, 0.0, 0.5, 1.0, 1.5]
y = [3.2, 2.7, 6, 5, 6.5]
spline = Spline(x, y)
rx = np.arange(-2.0, 4, 0.01)
ry = [spline.calc(i) for i in rx]
plt.plot(x, y, "xb")
plt.plot(rx, ry, "-r")
plt.grid(True)
plt.axis("equal")
plt.show()
if __name__ == "__main__":
test_spline()
test_spline2d()