def define_path(self):
self.path.reset()
max_length = self.waypoints.path_length
self.define_margin_dist()
# Find start index that corresponds to the desired smooth distance
temp_ds = 0
for i in range(0, max_length):
if not ((i + 1) > max_length - 1):
temp_ds += np.sqrt(
np.square(self.waypoints.x[i + 1] - self.waypoints.x[i]) +
np.square(self.waypoints.y[i + 1] - self.waypoints.y[i]))
if temp_ds > self.margin_dist:
self.margin_idx = i + 1
break
else:
pass
temp_waypoints_x = self.waypoints.x[:] + self.waypoints.x[:self.
margin_idx]
temp_waypoints_y = self.waypoints.y[:] + self.waypoints.y[:self.
margin_idx]
# Interpolate the waypoints
spline = sp.Spline2D(temp_waypoints_x, temp_waypoints_y)
# Arrange the waypoints with a defined distance interval self.ds_step
s = np.arange(0, spline.s[-1], self.ds_step)
# Cut parts of interpolation to smooth interpolation for start area with curvature
found_start_waypoint = False
temp_first = [[], []]
for i_s in s:
if i_s < self.margin_dist / 2.:
pass
else:
i_x, i_y = spline.calc_position(i_s)
i_k = spline.calc_curvature(i_s)
if not found_start_waypoint:
temp_first[0] = i_x
temp_first[1] = i_y
found_start_waypoint = True
if (s[-1] - i_s) < self.margin_dist / 2:
temp_ds = np.sqrt(
np.square(i_x - temp_first[0]) +
np.square(i_y - temp_first[1]))
if temp_ds < self.margin_connect_dist:
break
self.path.append(i_x, i_y, i_s, i_k)
self.idx_max = len(self.path.x)
def cubic_spline(points):
x_pts = [x[0] for x in points]
y_pts = [x[1] for x in points]
ds = 0.01
sp = spline.Spline2D(x_pts, y_pts)
s = 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))
plt.plot(x_pts, y_pts, "xb", label="input")
plt.plot(rx, ry, "-r", label="spline")
spline_pts = tuple(zip(rx, ry))
return spline_pts
import spline as sp import numpy as np import matplotlib.pyplot as plt t = np.linspace(0, 2 * np.pi, 20) kx = 2 ky = 3 x = np.cos(kx * t) y = np.sin(ky * t) curve = sp.Spline2D(t, x, y) t_range = np.linspace(0.01, 2 * np.pi - 0.01, 100) points = [curve.interpolate(t) for t in t_range] px = [p[0] for p in points] py = [p[1] for p in points] plt.figure() plt.plot(px, py, marker='.', label='interpolation') plt.scatter(x, y, c='red', marker='X', label='real') plt.legend() plt.show()
import spline as sp
import numpy as np
import matplotlib.pyplot as plt
coordinates = [(0, 0), (0.42, 0.46), (1.04, 1.33), (2, 3), (3.2, 4.8),
(5.12, 6.2), (7.25, 6.14), (8.8, 5.18), (10, 4), (10.1, 2.6),
(9.16, 2), (7.85, 2.5), (6.4, 2.7), (5.8, 1.4), (5.2, 0.2),
(4.5, -0.8), (3.4, -1.5), (2.4, -0.5), (1, -0.9), (0.14, -0.6),
(0, 0)]
c = [list(c) for c in zip(*coordinates)]
t = np.linspace(0, 10, len(coordinates))
t_range = np.arange(0, t[-1], 0.1)
curve = sp.Spline2D(t, c[0], c[1])
points = [curve.interpolate(t) for t in t_range]
px = [p[0] for p in points]
py = [p[1] for p in points]
plt.figure()
plt.scatter(px, py, marker='.', label='interpolation')
plt.scatter(c[0], c[1], c='red', marker='X', label='real')
plt.legend()
plt.show()