feat(obstacle_avoidance_planner): deal with infeasible drivable area

[takayuki5168]

Description

When obstacles are extracted from the drivable area for static avoidance or dynamic avoidance by the motion planner, in some cases, the drivable area width is too narrow and it's infeasible to drive inside the drivable area as shown in the following figures.

In this case, without the PR, the trajectory optimization failed due to infeasible drivability.
To deal with this case, the obstacle avoidance planner expands the drivable area if it's infeasible to drive.

This feature of keeping the feasible drivable area width should be considered when the drivable area is created in the behavior_path_planner.
The reason I put this feature into not the behavior_path_planner but the obstacle_avoidance_planner is to add less implementation for this feature.
The obstacle_avoidance_planner already calculates the drivable area's width, so it's quite easy to add this feature there.

NOTE:
This feature does not adversely affect driving on narrow roads. (e.g. The drivable area will not be expanded to the outside the road.)

Without this PR
Failed to optimize the avoiding trajectory.
The output on the terminal is [osqp_interface]: [MPT] Optimization failed due to solved inaccurate

With this PR
Succeeded in optimizing the avoiding trajectory by modifying the drivable area internally in obstacle_avoidance_planner.

Tests performed

planning simulator

Effects on system behavior

Nothing

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.

[takayuki5168]

@TakaHoribe
I created a feature PR. For me, it's fine not to go deep into the code change for approval.

[codecov]

Codecov Report

Patch coverage: 5.63% and project coverage change: -0.01 ⚠️

Comparison is base (3766d5a) 14.09% compared to head (7034e09) 14.08%.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3579      +/-   ##
==========================================
- Coverage   14.09%   14.08%   -0.01%     
==========================================
  Files        1396     1396              
  Lines       98109    98180      +71     
  Branches    29150    29194      +44     
==========================================
+ Hits        13826    13828       +2     
- Misses      69550    69607      +57     
- Partials    14733    14745      +12     

Flag	Coverage Δ		*Carryforward flag
differential	32.09% <5.63%> (?)
total	14.09% <ø> (-0.01%)	⬇️	Carriedforward from 00a1b66

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

Impacted Files	Coverage Δ
...clude/obstacle_avoidance_planner/mpt_optimizer.hpp	85.71% <ø> (ø)
...g/obstacle_avoidance_planner/src/mpt_optimizer.cpp	39.80% <5.63%> (-3.25%)	⬇️

... and 1 file with indirect coverage changes

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