fix(obstacle_stop_planner): fix hunting collision point

[kosuke55]

Description

This bug was caused by #1458.
If the index of latest_stop_point_ is used as it is, it causes out of range when the trajectory suddenly changes its shape.
The index needs to be updated with the current trajectory.

[openscenario_interpreter_node-3] [component_container_mt-49] latest_stop_point_.get().index : 708
[openscenario_interpreter_node-3] [component_container_mt-49] output.size() : 695
[openscenario_interpreter_node-3] [component_container_mt-49] terminate called after throwing an instance of 'std::out_of_range'
[openscenario_interpreter_node-3] [component_container_mt-49]   what():  vector::_M_range_check: __n (which is 708) >= this->size() (which is 695)

Related links

#1458

Tests performed

psim

Notes for reviewers

This is TIER IV internal scenario test of pull_over but it reproduces the bug. (not always but sometimes)
https://evaluation.tier4.jp/evaluation/reports/0d07ff47-c173-53bb-8e6d-f5b9f43924fd/tests/8eb804b2-7244-5355-8a8c-89ac7994ff56/355badd4-2f56-57a2-b092-bd61995bfdd0?project_id=prd_jt

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

Merging #1627 (f267e69) into main (90e4e78) will decrease coverage by 0.02%.
The diff coverage is 0.00%.

@@            Coverage Diff             @@
##             main    #1627      +/-   ##
==========================================
- Coverage   10.77%   10.74%   -0.03%     
==========================================
  Files        1111     1111              
  Lines       78542    78732     +190     
  Branches    18555    18555              
==========================================
  Hits         8462     8462              
- Misses      61226    61416     +190     
  Partials     8854     8854              

Flag	Coverage Δ		*Carryforward flag
differential	0.00% <0.00%> (?)
total	10.75% <0.00%> (ø)		Carriedforward from 90e4e78

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

Impacted Files	Coverage Δ
planning/obstacle_stop_planner/src/node.cpp	0.00% <0.00%> (ø)

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

[satoshi-ota]

LGTM 🙏