From 27a83dddeb9884fa8cd2d29543a3740fd42432e0 Mon Sep 17 00:00:00 2001 From: fishyy119 <1830979240@qq.com> Date: Tue, 24 Jun 2025 20:03:28 +0800 Subject: [PATCH 1/2] perf: optimize Reeds-Shepp path planning with NumPy vectorization --- .../reeds_shepp_path_planning.py | 130 +++++++++++------- 1 file changed, 81 insertions(+), 49 deletions(-) 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 From b5a39216703f77d875c324e40b466b74e4005798 Mon Sep 17 00:00:00 2001 From: yyy-win11 <1830979240@qq.com> Date: Mon, 5 Oct 2026 22:24:36 +0800 Subject: [PATCH 2/2] test(reeds-shepp): verify vectorized interpolation Add a scalar interpolation reference to preserve the behavior from before vectorization. Cover straight, left, and right segments in both forward and reverse motion using reproducible randomized inputs. Compare positions, yaw, and directions against the scalar results. --- tests/test_reeds_shepp_path_planning.py | 72 +++++++++++++++++++++++++ 1 file changed, 72 insertions(+) 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()