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%

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)