fix(behavior_path_planner): update root lanele if it is not in route lanelet

[kosuke55]

Description

Update root_lanelet_ if it is not in route lanelet.

Rerouging with modified goal update route_laneles, but if following lanes of previous root_lalet contains new root_lanelet_, that is not updated. This is happen when the previos root_lanelet is before new root lanelets.

  // check ego is in next lane
  const auto next_lanelets = route_handler->getRoutingGraphPtr()->following(root_lanelet_.get());
  for (const auto & next : next_lanelets) {
    if (next.id() == root_lanelet.id()) {
      return;
    }
  }

In a result root_lanelet is not in route_lanels and this causes empty lanelet sequence in

    const auto lanelet_sequence = route_handler->getLaneletSequence(
      root_lanelet_.get(), pose, backward_length, std::numeric_limits<double>::max());

https://github.com/autowarefoundation/autoware.universe/blob/main/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp#L281-L282

and generate empty reference path. In result a module (e.g. avoidance) uses the null previous output and dies.

Related links

Tests performed

psim

tier4 interanl scenario test

https://evaluation.tier4.jp/evaluation/reports/0e24127b-f63f-56af-88af-880fddcce8c9?project_id=prd_jt
1568/1588

compare
https://evaluation.tier4.jp/evaluation/reports/tables/new?catalog_id=cb497016-2e2f-4ce0-9ac8-68d77a27a55d&filter=&job_id=0e24127b-f63f-56af-88af-880fddcce8c9&project_id=prd_jt&table_config=date%2Chide&target_ids=769105c1-379c-5291-96ef-a1906180cb37

Notes for reviewers

none

Interface changes

none

Effects on system behavior

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.

[shmpwk]

LGTM

[codecov]

Codecov Report

Patch coverage: 40.00% and project coverage change: +0.30 🎉

Comparison is base (01cea10) 15.05% compared to head (841b6b4) 15.36%.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #4360      +/-   ##
==========================================
+ Coverage   15.05%   15.36%   +0.30%     
==========================================
  Files        1508     1508              
  Lines      103794   105531    +1737     
  Branches    31715    33001    +1286     
==========================================
+ Hits        15626    16213     +587     
- Misses      71137    71944     +807     
- Partials    17031    17374     +343     

Flag	Coverage Δ		*Carryforward flag
differential	16.09% <40.00%> (?)
total	15.05% <ø> (+<0.01%)	⬆️	Carriedforward from c42c82a

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

Impacted Files	Coverage Δ
...ning/behavior_path_planner/src/planner_manager.cpp	19.78% <0.00%> (-0.15%)	⬇️
planning/route_handler/src/route_handler.cpp	24.80% <100.00%> (+0.16%)	⬆️

... and 7 files with indirect coverage changes

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

[zulfaqar-azmi-t4]

LGTM