fix(obstacle_avoidance_planner): add guard for failed optimized trajectory

[takayuki5168]

Signed-off-by: Takayuki Murooka takayuki5168@gmail.com

Description

fix for https://tier4.atlassian.net/browse/T4PB-23420

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.