perf: optimize Reeds-Shepp path planning with NumPy vectorization

[fishyy119]

Summary

This PR optimizes the Reeds-Shepp path planning module by leveraging NumPy's vectorized computations. The built-in test function in reeds_shepp_path_planning.py was executed 100 times to benchmark performance. The runtime improvements of key functions are as follows:

Function	Before Optimization (s)	After Optimization (s)	Relative Time (%)
calc_paths	6.4861	0.2676	4.13%
generate_local_course	2.3774	0.1041	4.37%
calc_interpolate_dists_list	0.0307	0.0125	40.72%

Performance was measured using line_profiler with Python 3.10.16.

Optimization Details

calc_interpolate_dists_list

The main bottleneck in this function was the use of np.append, which accounted for about 70% of the runtime due to repeated memory reallocation. This has been eliminated by restructuring the function to avoid unnecessary array copying.

generate_local_course

1. Replaced the per-element interpolate loop with a vectorized version interpolate_vectorized, enabling batch computation with NumPy.
2. Precomputed the length of result arrays to avoid dynamic resizing during the loop.
3. The function now returns NumPy arrays instead of Python lists to facilitate downstream vectorized processing.

calc_paths

Constructed SE(2) transformation matrices to accelerate coordinate transformations using NumPy matrix operations.

CheckList

• Did you add an unittest for your new example or defect fix?
• Did you add documents for your new example?
• All CIs are green? (You can check it after submitting)

[Copilot]

Pull Request Overview

This PR optimizes the Reeds-Shepp path planning module by leveraging NumPy’s vectorized computations to significantly reduce runtime in key functions.

• Replaces iterative loops with vectorized operations in functions calc_interpolate_dists_list, generate_local_course, and a new interpolate_vectorized.
• Updates coordinate transformations in calc_paths to use NumPy matrix operations for improved performance.

Comments suppressed due to low confidence (2)

PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py:350

• Consider adding a docstring to calc_interpolate_dists_list to explain its role in precomputing distance arrays and how the endpoint is handled.

def calc_interpolate_dists_list(lengths: List[float], step_size: float) -> List[NDArray[np.floating]]:

PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py:399

• Add a docstring for interpolate_vectorized to detail its behavior for different modes ('S', 'L', 'R') and clarify how the vectorized computations are performed.

def interpolate_vectorized(

[AtsushiSakai]

@fishyy119 Thank you for your PR. Could you please add unit tests to check that the vectorized interpolation preserves the positions, yaw and directions for straight, left and right segments, including reverse motion?

Please check How to contribute.

[fishyy119]

Could you please add unit tests to check that the vectorized interpolation preserves the positions, yaw and directions for straight, left and right segments, including reverse motion?

Thank you for the feedback. I’ve added unit tests comparing the vectorized interpolation with the original scalar behavior for straight, left, and right segments in both forward and reverse motion, covering positions, yaw, and directions.