diff --git a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py index 618d1d99ba..6db3ced466 100644 --- a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py +++ b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py @@ -4,16 +4,21 @@ author Atsushi Sakai(@Atsushi_twi) co-author Videh Patel(@videh25) : Added the missing RS paths +co-author fishyy119(@fishyy119) : Improved runtime efficiency """ -import sys import pathlib +import sys + sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) import math +from typing import List, Tuple import matplotlib.pyplot as plt import numpy as np +from numpy.typing import NDArray + from utils.angle import angle_mod show_animation = True @@ -342,63 +347,86 @@ def generate_path(q0, q1, max_curvature, step_size): return paths -def calc_interpolate_dists_list(lengths, step_size): - interpolate_dists_list = [] +def calc_interpolate_dists_list(lengths: List[float], step_size: float) -> List[NDArray[np.floating]]: + interpolate_dists_list: List[NDArray[np.floating]] = [] for length in lengths: d_dist = step_size if length >= 0.0 else -step_size - interp_dists = np.arange(0.0, length, d_dist) - interp_dists = np.append(interp_dists, length) + + interp_core = np.arange(0.0, length, d_dist, dtype=np.float64) + interp_dists = np.empty(len(interp_core) + 1, dtype=np.float64) + interp_dists[:-1] = interp_core + interp_dists[-1] = length + interpolate_dists_list.append(interp_dists) return interpolate_dists_list -def generate_local_course(lengths, modes, max_curvature, step_size): +def generate_local_course( + lengths: List[float], + modes: List[str], + max_curvature: float, + step_size: float, +) -> Tuple[NDArray[np.floating], NDArray[np.floating], NDArray[np.floating], NDArray[np.signedinteger]]: interpolate_dists_list = calc_interpolate_dists_list(lengths, step_size * max_curvature) + total_len = sum(len(arr) for arr in interpolate_dists_list) + xs = np.empty(total_len, dtype=np.float64) + ys = np.empty_like(xs) + yaws = np.empty_like(xs) + directions = np.empty_like(xs, dtype=np.int32) origin_x, origin_y, origin_yaw = 0.0, 0.0, 0.0 - - xs, ys, yaws, directions = [], [], [], [] - for (interp_dists, mode, length) in zip(interpolate_dists_list, modes, - lengths): - - for dist in interp_dists: - x, y, yaw, direction = interpolate(dist, length, mode, - max_curvature, origin_x, - origin_y, origin_yaw) - xs.append(x) - ys.append(y) - yaws.append(yaw) - directions.append(direction) - origin_x = xs[-1] - origin_y = ys[-1] - origin_yaw = yaws[-1] + idx = 0 + + for interp_dists, mode, length in zip(interpolate_dists_list, modes, lengths): + n = len(interp_dists) + x_arr, y_arr, yaw_arr, dir_arr = interpolate_vectorized( + interp_dists, length, mode, max_curvature, origin_x, origin_y, origin_yaw + ) + xs[idx : idx + n] = x_arr + ys[idx : idx + n] = y_arr + yaws[idx : idx + n] = yaw_arr + directions[idx : idx + n] = dir_arr + + origin_x = x_arr[-1] + origin_y = y_arr[-1] + origin_yaw = yaw_arr[-1] + idx += n return xs, ys, yaws, directions -def interpolate(dist, length, mode, max_curvature, origin_x, origin_y, - origin_yaw): +def interpolate_vectorized( + dists: NDArray[np.floating], + length: float, + mode: str, + max_curvature: float, + origin_x: float, + origin_y: float, + origin_yaw: float, +) -> Tuple[NDArray[np.floating], NDArray[np.floating], NDArray[np.floating], NDArray[np.signedinteger]]: if mode == "S": - x = origin_x + dist / max_curvature * math.cos(origin_yaw) - y = origin_y + dist / max_curvature * math.sin(origin_yaw) - yaw = origin_yaw + x = origin_x + dists / max_curvature * math.cos(origin_yaw) + y = origin_y + dists / max_curvature * math.sin(origin_yaw) + yaw = np.full_like(dists, origin_yaw) else: # curve - ldx = math.sin(dist) / max_curvature - ldy = 0.0 - yaw = None + ldx = np.sin(dists) / max_curvature if mode == "L": # left turn - ldy = (1.0 - math.cos(dist)) / max_curvature - yaw = origin_yaw + dist - elif mode == "R": # right turn - ldy = (1.0 - math.cos(dist)) / -max_curvature - yaw = origin_yaw - dist - gdx = math.cos(-origin_yaw) * ldx + math.sin(-origin_yaw) * ldy - gdy = -math.sin(-origin_yaw) * ldx + math.cos(-origin_yaw) * ldy + ldy = (1.0 - np.cos(dists)) / max_curvature + yaw = origin_yaw + dists + else: # elif mode == "R": # right turn + ldy = (1.0 - np.cos(dists)) / (-max_curvature) + yaw = origin_yaw - dists + + cos_oy = math.cos(-origin_yaw) + sin_oy = math.sin(-origin_yaw) + gdx = cos_oy * ldx + sin_oy * ldy + gdy = -sin_oy * ldx + cos_oy * ldy x = origin_x + gdx y = origin_y + gdy - return x, y, yaw, 1 if length > 0.0 else -1 + direction = 1 if length > 0 else -1 + return x, y, yaw, np.full_like(dists, direction, dtype=np.int32) def calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size): @@ -407,19 +435,23 @@ def calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size): paths = generate_path(q0, q1, maxc, step_size) for path in paths: - xs, ys, yaws, directions = generate_local_course(path.lengths, - path.ctypes, maxc, - step_size) + xs, ys, yaws, directions = generate_local_course(path.lengths, path.ctypes, maxc, step_size) # convert global coordinate - path.x = [math.cos(-q0[2]) * ix + math.sin(-q0[2]) * iy + q0[0] for - (ix, iy) in zip(xs, ys)] - path.y = [-math.sin(-q0[2]) * ix + math.cos(-q0[2]) * iy + q0[1] for - (ix, iy) in zip(xs, ys)] - path.yaw = [pi_2_pi(yaw + q0[2]) for yaw in yaws] - path.directions = directions - path.lengths = [length / maxc for length in path.lengths] - path.L = path.L / maxc + local_pts = np.vstack([xs, ys, np.ones_like(xs)]) # shape: [3, N] + cos_y = np.cos(syaw) + sin_y = np.sin(syaw) + se2 = np.array([[cos_y, -sin_y, sx],[sin_y, cos_y, sy],[0, 0, 1]]) + global_pts = se2 @ local_pts # shape: [3, N] + + path.x = global_pts[0, :].tolist() + path.y = global_pts[1, :].tolist() + + path.yaw = ((yaws + syaw + np.pi) % (2 * np.pi) - np.pi).tolist() + + path.directions = directions.tolist() + path.lengths = [l / maxc for l in path.lengths] + path.L /= maxc return paths diff --git a/tests/test_reeds_shepp_path_planning.py b/tests/test_reeds_shepp_path_planning.py index 34ccfe7730..aa67c2f13a 100644 --- a/tests/test_reeds_shepp_path_planning.py +++ b/tests/test_reeds_shepp_path_planning.py @@ -1,4 +1,7 @@ +import math + import numpy as np +import pytest import conftest # Add root path to sys.path from PathPlanning.ReedsSheppPath import reeds_shepp_path_planning as m @@ -24,6 +27,75 @@ def check_path_length(px, py, lengths): assert (diff_len <= 0.01) +def _interpolate_reference(dist, length, mode, max_curvature, origin_x, + origin_y, origin_yaw): + """Scalar interpolation implementation used before vectorization.""" + if mode == "S": + x = origin_x + dist / max_curvature * math.cos(origin_yaw) + y = origin_y + dist / max_curvature * math.sin(origin_yaw) + yaw = origin_yaw + else: # curve + ldx = math.sin(dist) / max_curvature + ldy = 0.0 + yaw = None + if mode == "L": # left turn + ldy = (1.0 - math.cos(dist)) / max_curvature + yaw = origin_yaw + dist + elif mode == "R": # right turn + ldy = (1.0 - math.cos(dist)) / -max_curvature + yaw = origin_yaw - dist + gdx = math.cos(-origin_yaw) * ldx + math.sin(-origin_yaw) * ldy + gdy = -math.sin(-origin_yaw) * ldx + math.cos(-origin_yaw) * ldy + x = origin_x + gdx + y = origin_y + gdy + + return x, y, yaw, 1 if length > 0.0 else -1 + + +@pytest.mark.parametrize( + ("mode", "direction"), + [ + pytest.param("S", 1, id="straight-forward"), + pytest.param("S", -1, id="straight-reverse"), + pytest.param("L", 1, id="left-forward"), + pytest.param("L", -1, id="left-reverse"), + pytest.param("R", 1, id="right-forward"), + pytest.param("R", -1, id="right-reverse"), + ], +) +def test_interpolate_vectorized_matches_scalar_reference(mode, direction): + rng = np.random.default_rng(1234) + + for _ in range(10): + length = direction * rng.uniform(0.1, 2.0) + dists = np.sort(rng.uniform(0.0, abs(length), size=10)) + dists = direction * np.concatenate(([0.0], dists, [abs(length)])) + max_curvature = rng.uniform(0.1, 2.0) + origin_x, origin_y = rng.uniform(-10.0, 10.0, size=2) + origin_yaw = rng.uniform(-np.pi, np.pi) + + expected = [ + _interpolate_reference( + dist, length, mode, max_curvature, origin_x, origin_y, + origin_yaw + ) + for dist in dists + ] + expected_x, expected_y, expected_yaw, expected_directions = ( + np.asarray(values) for values in zip(*expected) + ) + + xs, ys, yaws, directions = m.interpolate_vectorized( + dists, length, mode, max_curvature, origin_x, origin_y, + origin_yaw + ) + + np.testing.assert_allclose(xs, expected_x) + np.testing.assert_allclose(ys, expected_y) + np.testing.assert_allclose(yaws, expected_yaw) + np.testing.assert_array_equal(directions, expected_directions) + + def test1(): m.show_animation = False m.main()