fix(obstacle_avoidance_planner): add guard for failed optimized trajectory

[takayuki5168]

Description

commit to apply tier4#194 to main

fix for https://tier4.atlassian.net/browse/T4PB-23420 (TIER IV internal link)

When optimization failed, trajectory sometimes diverged.
In order to send this kind of trajectory to the latter module, I added a guard seeing how much deviation the trajectory has against the reference path.
If the deviation is too large, sends a previously planned path to the latter module.

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.

[codecov]

Codecov Report

Base: 10.50% // Head: 10.46% // Decreases project coverage by -0.03% ⚠️

Coverage data is based on head (412de4c) compared to base (a7270a8).
Patch coverage: 0.00% of modified lines in pull request are covered.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2398      +/-   ##
==========================================
- Coverage   10.50%   10.46%   -0.04%     
==========================================
  Files        1260     1260              
  Lines       91667    92016     +349     
  Branches    21123    21123              
==========================================
  Hits         9630     9630              
- Misses      71829    72178     +349     
  Partials    10208    10208              

Flag	Coverage Δ		*Carryforward flag
differential	0.00% <0.00%> (?)
total	10.48% <0.00%> (ø)		Carriedforward from a7270a8

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

Impacted Files	Coverage Δ
...g/obstacle_avoidance_planner/src/mpt_optimizer.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.

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