fix(obstacle_stop_planner): change the nearest_collision_point calculation place

[brkay54]

Description

This PR closes following issues:
#5449
#6007

The purpose of this PR is fixing logical bug.
At first for loop here, it searches collision on trajectory by looking into predicted objects. If there is a collision object, it finds the nearest point of the object (by looking both intersected points and corner points here), and it pushes predicted object, time, and nearest point into predicted_object_history_.

At the second for loop here, it searches the nearest collision object in predicted_object_history_ vector. It looks for the pre-calculated nearest point inside the trajectory footprint or not. It is the mistake.
If the nearest point we found in the first loop is an intersected point, bg::intersect might not be true in the second loop, it is a logical mistake.

In this issue (#6007) we realized if the use_predicted_objects is true, obstacle_stop_planner may react too late to obstacle.

To solve this problem, we made structural change on searchPredictedObject function. We changed the nearest_collision_point calculation place to the second loop.

Related links

Tests performed

Tested in PSim by using reaction_analyzer tool. (https://github.com/brkay54/autoware.universe/tree/feat/reaction-measure-tool/tools/reaction_analyzer)

Reaction Times before the PR:

Reaction Times after the PR:

Notes for reviewers

You can use the reaction_analyzer tool to check the reaction times of the node. Please refer README for the usage.

Interface changes

Effects on system behavior

Decreases the reaction delay of the obstacle_stop_planner when use_predicted_objects is true.

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

Attention: 54 lines in your changes are missing coverage. Please review.

Comparison is base (9fe2e15) 15.22% compared to head (d0ccd75) 15.22%.

Files	Patch %	Lines
planning/obstacle_stop_planner/src/node.cpp	0.00%	41 Missing ⚠️
...top_planner/include/obstacle_stop_planner/node.hpp	0.00%	13 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #5794      +/-   ##
==========================================
- Coverage   15.22%   15.22%   -0.01%     
==========================================
  Files        1751     1751              
  Lines      120824   120832       +8     
  Branches    36716    36717       +1     
==========================================
  Hits        18396    18396              
- Misses      81786    81794       +8     
  Partials    20642    20642              

Flag	Coverage Δ		*Carryforward flag
differential	8.75% <0.00%> (?)
total	15.23% <ø> (+<0.01%)	⬆️	Carriedforward from 9fe2e15

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

[brkay54]

Hi @satoshi-ota @shmpwk @taikitanaka3 @tkimura4, sorry to bother you, if you have some time, could you review this PR?

[satoshi-ota]

Sorry for late reply @brkay54
I'm reviewing now...

[satoshi-ota]

LGTM.

@brkay54 Thanks for your contribution. Btw, is it possible to add another maintainer from leodrive side for this package? Basically, TIER IV uses only the feature to check collision by pointcloud. So, I think it is ok to improve the feature that uses predicted objects without TIER IV maintainer.