fix(static_obstacle_avoidance): improve turn signal output timing when the ego returns original lane

[satoshi-ota]

Description

Fix bug which caused delay of turning on signal when the ego returns original lane after avoiding. This module outputs turn signal per shift lines but sometimes they include the one whose shift length is small. In original implementation, this module didn't output turn signal for such a shift line. Additionally, since this module only checked front (=nearest) shift line, it may ignore return shift line unexpectedly.

In this PR, I fixed the logic to remove small shift line at first.

  const auto itr =
    std::remove_if(shift_lines.begin(), shift_lines.end(), [&, this](const auto & s) {
      const auto threshold = planner_data_->parameters.turn_signal_shift_length_threshold;
      return std::abs(s.start_shift_length - s.end_shift_length) < threshold ||
             is_ignore_signal(s.id);
    });
  shift_lines.erase(itr, shift_lines.end());

And, I added the logic to check not only nearest shift line but also next one, and outputs turn signal for latter one if its start point is closer than threshold.

    for (size_t i = 1; i < shift_lines.size(); i++) {
      const auto & s2 = shift_lines.at(i);

      const auto s1_relative_length = s1.start_shift_length - s1.end_shift_length;
      const auto s2_relative_length = s2.start_shift_length - s2.end_shift_length;

      // same side shift
      if (s1_relative_length > 0.0 && s2_relative_length > 0.0) {
        continue;
      }

      // same side shift
      if (s1_relative_length < 0.0 && s2_relative_length < 0.0) {
        continue;
      }

      // different side shift
      const auto & points = path_shifter_.getReferencePath().points;
      const size_t idx = planner_data_->findEgoIndex(points);

      // output turn signal for near shift line.
      if (calcSignedArcLength(points, idx, s1.start_idx) > 0.0) {
        return s1;
      }

      // output turn signal for far shift line.
      if (
        calcSignedArcLength(points, idx, s2.start_idx) <
        getEgoSpeed() * parameters_->max_prepare_time) {
        return s2;
      }

Related links

Parent Issue:

• TIER IV INTERNAL LINK

How was this PR tested?

• PASS TIER IV INTERNAL SCENARIOS

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 3.75000% with 77 lines in your changes missing coverage. Please review.

Project coverage is 25.01%. Comparing base (93e4e7d) to head (94895d1).
Report is 48 commits behind head on main.

Files with missing lines	Patch %	Lines
...ath_static_obstacle_avoidance_module/src/scene.cpp	5.66%	50 Missing ⚠️
...ath_static_obstacle_avoidance_module/src/debug.cpp	0.00%	27 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #8726      +/-   ##
==========================================
- Coverage   25.02%   25.01%   -0.02%     
==========================================
  Files        1318     1319       +1     
  Lines       97944    98014      +70     
  Branches    37804    37837      +33     
==========================================
+ Hits        24511    24516       +5     
- Misses      70923    70989      +66     
+ Partials     2510     2509       -1     

Flag	Coverage Δ		*Carryforward flag
differential	20.02% <3.75%> (?)
total	25.03% <ø> (+<0.01%)	⬆️	Carriedforward from 93e4e7d

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

[shmpwk]

LGTM!