perf(freespace_planning_algorithms): improve freespace planning performance

[NorahXiong]

Description

See #2039

Related links

Tests performed

Test environment: planning simulator as default

• start point: "pose: {position: {x: 3713.99, y: 73732.0, z: 0.664812}, orientation: {x: 0.0, y: 0.0, z: 0.850511, w: 0.525957}"
• goal point: "pose: {position: {x: 3727.39, y: 73750.0, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.850592, w: 0.525827}"
• In test 1 costmap resolution is 0.2m and in test 2 which is 0.1m.
• Results are average value of at least 3 replications.

Use unordered_map to store node data instead of vector
The memory cost sharply decreases. The efficiency of data access is almost the same as before.

Eliminate unnecessary calculation steps in collision detection function
Delete duplicate indexes which corresponds to vehicle shape. More than 3/4 indexes are not unique.

Notes for reviewers

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.
• The PR has been properly tested.
• The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.
• The PR is ready for merge.

After all checkboxes are checked, anyone who has write access can merge the PR.

[codecov]

Codecov Report

Base: 10.89% // Head: 10.98% // Increases project coverage by +0.09% 🎉

Coverage data is based on head (0ffc7be) compared to base (1862ee7).
Patch coverage: 89.47% of modified lines in pull request are covered.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2040      +/-   ##
==========================================
+ Coverage   10.89%   10.98%   +0.09%     
==========================================
  Files        1179     1179              
  Lines       84936    85042     +106     
  Branches    20043    20094      +51     
==========================================
+ Hits         9250     9344      +94     
+ Misses      65912    65911       -1     
- Partials     9774     9787      +13     

Flag	Coverage Δ		*Carryforward flag
differential	62.35% <90.00%> (?)
total	10.86% <83.33%> (ø)		Carriedforward from 1862ee7

*This pull request uses carry forward flags. Click here to find out more.

Impacted Files	Coverage Δ
...eespace_planning_algorithms/abstract_algorithm.hpp	76.92% <ø> (+6.92%)	⬆️
...ace_planning_algorithms/src/abstract_algorithm.cpp	85.82% <87.09%> (+2.13%)	⬆️
...ude/freespace_planning_algorithms/astar_search.hpp	92.30% <100.00%> (+4.80%)	⬆️
...freespace_planning_algorithms/src/astar_search.cpp	76.14% <100.00%> (+3.02%)	⬆️

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

☔ View full report at Codecov.
📢 Do you have feedback about the report comment? Let us know in this issue.

[maxime-clem]

The change makes sense, the code looks good, and parking is working well in simulation.

[TakaHoribe]

@NorahXiong Thank you for your contribution! Your report for the performance improvement is very impressive!

Recently, we merged a large PR for the freespace planner, so there is now a conflict in this PR. Would you solve the conflict? Then we'd like to merge this PR soon.

[liuXinGangChina]

@NorahXiong Thank you for your contribution! Your report for the performance improvement is very impressive!

Recently, we merged a large PR for the freespace planner, so there is now a conflict in this PR. Would you solve the conflict? Then we'd like to merge this PR soon.

@TakaHoribe ,hi. NorthXiong has resolved the conflict , please take a look and give your valuable advice.

[maxime-clem]

@liuXinGangChina @NorahXiong One test is failing in the CI: https://github.com/autowarefoundation/autoware.universe/actions/runs/3376624609/jobs/5605343502#step:7:380
Please fix it as we cannot merge until all tests are passing.

[NorahXiong]

@liuXinGangChina @NorahXiong One test is failing in the CI: https://github.com/autowarefoundation/autoware.universe/actions/runs/3376624609/jobs/5605343502#step:7:380 Please fix it as we cannot merge until all tests are passing.

@maxime-clem @TakaHoribe I've fixed the problem. PR is ready to merge now.

[TakaHoribe]

@NorahXiong Thank you so much. I'd like to merge this PR soon. Do you have a test script to measure performance like you put in the top of this PR? We need to track the performance since the OSS is frequently updated. Your script would be very useful.

[NorahXiong]

@NorahXiong Thank you so much. I'd like to merge this PR soon. Do you have a test script to measure performance like you put in the top of this PR? We need to track the performance since the OSS is frequently updated. Your script would be very useful.

Sorry I don't have that script. I counted executing time of each step by debug log. I have a simple script to precisely set the start and goal point in order to repeat tests. I'd like to share it if you need.

[NorahXiong]

@NorahXiong Thank you so much. I'd like to merge this PR soon. Do you have a test script to measure performance like you put in the top of this PR? We need to track the performance since the OSS is frequently updated. Your script would be very useful.

Sorry I don't have that script. I counted executing time of each step by debug log. I have a simple script to precisely set the start and goal point in order to repeat tests. I'd like to share it if you need.

@TakaHoribe, the attached script can precisely set start/goal point and the usage is as below. Hope it will be helpful for you.

# source env
./astar_set s n # set start point with the nth start point
./astar_set g n # set goal point with the nth goal point

It seems that the set initial pose service does not work anymore. I changed the script accordingly.
astar_set.zip

[TakaHoribe]

@NorahXiong Thank you. I could confirm the great improvement of the freespace planner.