perf(goal_planner): faster path sorting and selection

[kosuke55]

Description

Before:
We had done a collision check using footprints for all candidate paths and gave higher priority to those with larger margins.
Then, for those with high priority, we again checked the hard margin and confirmed that sufficient distance had been secured.

After:
For candidate paths, check whether the goal is safe and sufficient distance is secured, and delete those that do not meet the requirements. Calculate rough margins for candidate routes and sort. The rough distance is the longest distance when the object and ego are rotated. (see #8464 for details)
After that, we check if hard margin can be secured for the sorted objects in order. Also, pull over paths take -5ms each to copy, so index is used whenever possible.

depends on

• feat(behavior_path_planner_common): add calculateRoughDistanceToObjects #8464
• perf(goal_planner): reduce unnecessary recursive lock guard #8465

---

(internal)

before

after

Related links

Parent Issue:

• Link

How was this PR tested?

 webauto ci scenario run --project-id prd_jt --scenario-id e02498d1-9832-4935-8c84-026d3ab8b5ab --scenario-version-id 9 --scenario-parameters '__tier4_modifier_ego_speed=11.1111,__tier4_modifier_npc_position=10.75,__tier4_modifier_processing_time=100'

before

$ ros2 run autoware_debug_tools processing_time_visualizer
⏰ Worst Case Execution Time ⏰
updateData: 6.19 [ms]

plan: 72.93 [ms]
    └── planPullOver: 72.92 [ms]
        ├── hasDecidedPath: 0.00 [ms]
        │   └── checkDecidingPathStatus: 0.00 [ms]
        └── planPullOverAsCandidate: 72.92 [ms]
            ├── planPullOverAsOutput: 60.86 [ms]
            │   ├── hasNotDecidedPath: 0.01 [ms]
            │   │   └── checkDecidingPathStatus: 0.00 [ms]
            │   ├── sortPullOverPathCandidatesByGoalPriority: 29.89 [ms]
            │   ├── selectPullOverPath: 0.13 [ms]
            │   ├── decelerateBeforeSearchStart: 0.08 [ms]
            │   ├── setOutput: 0.77 [ms]
            │   │   ├── isSafePath: 0.45 [ms]
            │   │   ├── setDrivableAreaInfo: 0.06 [ms]
            │   │   │   └── generateDrivableLanes: 0.03 [ms]
            │   │   └── hasDecidedPath: 0.21 [ms]
            │   │       └── checkDecidingPathStatus: 0.20 [ms]
            │   │           └── isSafePath: 0.20 [ms]
            │   ├── setDebugData: 0.72 [ms]
            │   │   └── hasDecidedPath: 0.19 [ms]
            │   │       └── checkDecidingPathStatus: 0.19 [ms]
            │   │           └── isSafePath: 0.19 [ms]
            │   └── updatePreviousData: 0.39 [ms]
            │       ├── checkDecidingPathStatus: 0.20 [ms]
            │       │   └── isSafePath: 0.20 [ms]
            │       └── isSafePath: 0.19 [ms]
            ├── generateStopPath: 2.98 [ms]
            │   ├── decelerateForTurnSignal: 2.41 [ms]
            │   └── decelerateBeforeSearchStart: 0.03 [ms]
            ├── generateDrivableLanes: 0.04 [ms]
            └── setDebugData: 0.58 [ms]
                └── hasDecidedPath: 0.23 [ms]
                    └── checkDecidingPathStatus: 0.23 [ms]
                        └── isSafePath: 0.23 [ms]

postProcess: 1.23 [ms]
    ├── calcDistanceToPathChange: 0.05 [ms]
    ├── hasDecidedPath: 1.02 [ms]
    │   └── checkDecidingPathStatus: 1.02 [ms]
    │       ├── isSafePath: 0.41 [ms]
    │       └── isSafePath: 0.49 [ms]
    └── calcTurnSignalInfo: 0.11 [ms]

after (worst case)

path priority

Notes for reviewers

None.

Interface changes

None.

Effects on system behavior

None.

[github-actions]

Thank you for contributing to the Autoware project!

🚧 If your pull request is in progress, switch it to draft mode.

Please ensure:

• You've checked our contribution guidelines.
• Your PR follows our pull request guidelines.
• All required CI checks pass before marking the PR ready for review.

[codecov]

Codecov Report

Attention: Patch coverage is 0% with 82 lines in your changes missing coverage. Please review.

Project coverage is 23.87%. Comparing base (df530c2) to head (b7dbcb6).
Report is 9 commits behind head on main.

Files	Patch %	Lines
...th_goal_planner_module/src/goal_planner_module.cpp	0.00%	64 Missing ⚠️
...are_behavior_path_goal_planner_module/src/util.cpp	0.00%	18 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #8457      +/-   ##
==========================================
- Coverage   23.88%   23.87%   -0.02%     
==========================================
  Files        1381     1387       +6     
  Lines      101993   102039      +46     
  Branches    38852    38852              
==========================================
  Hits        24357    24357              
- Misses      75207    75253      +46     
  Partials     2429     2429              

Flag	Coverage Δ		*Carryforward flag
differential	15.06% <0.00%> (?)
total	23.90% <ø> (+0.01%)	⬆️	Carriedforward from df530c2

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

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[kosuke55]

@soblin
I rebased on main so this PR is ready for review.

[soblin]

Can you rename path_id_to_margin_map to path_id_to_rough_margin_map ?

[kosuke55]

that is better!
fixed in 9b43e03