feature(freespace_planner): enhance freespace planner search algorithm and add reparking feature for improved parking accuracy

[YCLiu-TD]

Checklist

• I've read the contribution guidelines.

• I've searched other issues and no duplicate issues were found.

• I'm convinced that this is not my fault but a bug.

Current Behavior

The current parking algorithm used by the Autoware freespace planner can constrain the planned yaw angle to within 3°. However, because this is purely a global‐planning result, the vehicle’s actual final orientation in PSIM simulations may still exceed that threshold. In five test runs, the final yaw error exceeded the 3° limit in four cases.

parking.exp.mp4

Demo video of the experiment scenario

Validation results of parking accuracy over five experiments: in four runs, the final yaw error exceeded 3°.

The root cause is that the planned path still includes a rotational maneuver near the final goal. Although the path is kinematically feasible, it does not compensate for the execution error during actual vehicle movement. Additionally, the current design of the path tracking controller decouples steering and throttle control. As a result, the vehicle may be considered to have met the stopping condition when its position is close to the goal pose, even though the steering alignment has not yet been completed.

Expected Behavior

In PSIM parking scenarios, the vehicle’s yaw angle error relative to the user‑specified target pose is supposed to stay within 3°.

Actual Behavior

It often exceeds this 3° threshold, yet the system still deems the parking maneuver complete.

Proposed Enhancement

1. Optimize the heuristic function of the A star searching algorithm
   Refine the A* algorithm’s heuristic function so that the generated trajectory becomes progressively straighter as it approaches the final goal, reducing the controller’s tracking burden and thereby minimizing the final pose error.

Improved.mp4

In the demo with the adjusted algorithm, the final orientation error was reduced to 0.497° (the baseline average error over 10 parking tests in this scenario was 3.8°).

2. Introduce a reparking function
   Emulate human behavior by adding a reparking feature: if the vehicle’s final parking orientation exceeds a specified deviation threshold, trigger a secondary parking maneuver that generates a combined pull‑out-and‑alignment trajectory to correct the angle. Allow up to three corrective attempts to iteratively reduce the error, while alleviating path constraints imposed by nearby obstacles.

reparking.webm

After executing the reparking function once to realign with the goal pose, the final orientation error was 0.9°.

Versions

OS: Ubuntu 22.04
ROS2: Humble
Autoware: Autoware.universe:main

[maxime-clem]

Thank you for sharing your observations and results. Your enhancements look good and it would be great to add them to the universe repository.
Are you willing to open pull requests with your modifications ?

[YCLiu-TD]

Hi,
Thank you for your kind response! I'm currently working on preparing the pull request and will open it once it's ready. Really appreciate your support!

[YCLiu-TD]

Hi,

I have a question regarding opening a Pull Request.
I’ve completed testing for a new feature, but I noticed that in the current autoware main branch, the autoware.repos file points to submodules where autoware_universe uses the tag 0.45.0 instead of the main branch, and there is no corresponding branch for it. The same applies to autoware_launch.

If I modify autoware.repos in the main branch to point both universe and launch to their respective main branches, I encounter build errors and can’t proceed with testing.

Would it be possible to submit a Pull Request against the current tag (0.45.0) instead?

Thanks for your guidance!

[maxime-clem]

Pull requests should always be made for the main branches.
The way to use the most up to date version of Autoware is to use the autoware-nightly.repos file instead of autoware.repos: https://github.com/autowarefoundation/autoware/blob/main/autoware-nightly.repos

[YCLiu-TD]

Thank you for the suggestion. I have tested this method with autoware-nightly.repos and it works perfectly. I appreciate your help!