fix(route_handler): route handler overlap removal is too conservative

[mkquda]

Description

Currently, overlap handling is not great, in some corner cases, a broken route might be generated, like the one shown below.

This is happening because, in the function getLaneletSequenceUpTo, if at least one of the previous lanelets is same as the inut lanelet, then it is considered a loop and search will stop.

Another issue is the logic used to get the closest lanelet within route does not respect the history of the lanelets traversed, it will just return the closest route lanelet satisfying the distance and yaw constraints, so the closest lanelet can jump suddenly in case of partially overlapping lanelets causing a break in the route, as shown below.

Screencast_2024-05-29_14-18-00.mp4

Changes

(route_handler)
Modified the loop check logic in function getLaneletSequenceUpTo():

• If previous lanelets are route lanelets, then return true only if all prev lanelets are overlapping with input lanelet
• If at least one prev lanelet is not overlapping with input lanelet then continue sequence search
• If previous lanelets are not route lanelets and at least one overlaps with input lanelet then loop check returns true

Implemented new function getClosestRouteLaneletFromCurrent() to get closest route lanelet with respect the previous computed closest route lanelets to insure continuity.

(planner_manager)
Implemented separate function updateCurrentRouteLanelet() and moved logic out of getReferencePathFunction()
Use new function getClosestRouteLaneletFromCurrent() in updateCurrentRouteLanelet()

(behavior_path_planner_common/utils)
Modified function isEgoOutOfRoute() to take closestRoadLanelet as input from planner manager rather than recomputing same logic locally

(mission_planner)
Refactored DefaultPlanner::plan() function, should not affect logic.

Tests performed

• Tested on psim
• Confirmed no degradation using autoware evaluator

Effects on system behavior

Can generate proper route:

Closest route lanelet doesn't jump suddenly causing unexpected behavior:

Screencast_2024-05-29_12-01-13.mp4

Interface changes

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.

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.

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

[zulfaqar-azmi-t4]

LGTM!

[codecov]

Codecov Report

Attention: Patch coverage is 34.14634% with 81 lines in your changes missing coverage. Please review.

Project coverage is 14.65%. Comparing base (e24f48d) to head (9e36962).
Report is 136 commits behind head on main.

❗ Current head 9e36962 differs from pull request most recent head ec1f99d

Please upload reports for the commit ec1f99d to get more accurate results.

Files	Patch %	Lines
planning/route_handler/test/test_route_handler.cpp	25.64%	0 Missing and 29 partials ⚠️
...e_behavior_path_planner_common/src/utils/utils.cpp	11.11%	17 Missing and 7 partials ⚠️
planning/route_handler/src/route_handler.cpp	48.48%	3 Missing and 14 partials ⚠️
planning/route_handler/test/test_route_handler.hpp	50.00%	0 Missing and 5 partials ⚠️
...ning/behavior_path_planner/src/planner_manager.cpp	66.66%	0 Missing and 4 partials ⚠️
...n_planner/src/lanelet2_plugins/default_planner.cpp	0.00%	0 Missing and 2 partials ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #7156      +/-   ##
==========================================
- Coverage   15.10%   14.65%   -0.45%     
==========================================
  Files        1922     2049     +127     
  Lines      134796   145844   +11048     
  Branches    43473    46635    +3162     
==========================================
+ Hits        20364    21379    +1015     
- Misses      91780   101076    +9296     
- Partials    22652    23389     +737     

Flag	Coverage Δ		*Carryforward flag
differential	14.98% <34.14%> (?)
total	14.88% <ø> (-0.22%)	⬇️	Carriedforward from e800d54

*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.

[maxime-clem]

Tested and it works well

[kosuke55]

LGTM