feat(obstacle avoidance planner): fix out curve, make calculation cost low, make optimization stable, refactor, etc.

[takayuki5168]

Related Issue(required)

#366

Description(required)

This is a PR of the feature branch to improve obstacle avoidance planner, including

• fix out curve
  • previous: the base link (back wheel center) tries to follow the reference trajectory
  • current: the geometrical center or front wheel center (can be designated with rosparam) tries to follow the reference trajectory
• make calculation cost low
  • remove redundant implementation
  • remove publishing unnecessary topics (can be designated with rosparam)
  • Fixing out curve issue since solvability gets higher.
  • use sparse matrix
  • use Linf norm for MPT's soft constraints instead of L2 norm
  • make size of obstacle clearance map small
  • connect optimized points and remain points with spline interpolation instead of non-robust EB
  • etc..
• make optimization stable (by dealing with the fixed length well)
  • make fixing distance not so long, and remove them from optimization (previously, fixing points are also optimization's variable)
  • tune optimization
  • etc..
• refactor
  • organize main functions in node.cpp, and add flowchart in README
  • refactor utils functions
  • refactor mpt_optimizer.cpp
  • etc..
• etc

Review Procedure(required)

try many cases with simple psim in odaiba/kashiwanoha

Related PR(optional)

Pre-Review Checklist for the PR Author

PR Author should check the checkboxes below when creating the PR.

• Read commit-guidelines
• Assign PR to reviewer

If you are adding new package following items are required:

• Documentation with description of the package is available
• A sample launch file and parameter file are available if the package contains executable nodes

Checklist for the PR Reviewer

Reviewers should check the checkboxes below before approval.

• Commits are properly organized and messages are according to the guideline
• PR title describes the changes

Post-Review Checklist for the PR Author

PR Author should check the checkboxes below before merging.

• All open points are addressed and tracked via issues or tickets

CI Checks

• Build and test for PR / build-and-test-pr: Required to pass before the merge.
• Build and test for PR / clang-tidy-pr: NOT required to pass before the merge. It is up to the reviewer(s). Found false positives? See the [guidelines][clang-tidy-guidelines].
• Check spelling: NOT required to pass before the merge. It is up to the reviewer(s). See here if you want to add some words to the spell check dictionary.

[github-actions]

Documentation URL: https://autowarefoundation.github.io/autoware.universe/pr-233/
Modified URLs:

• https://autowarefoundation.github.io/autoware.universe/pr-233/planning/obstacle_avoidance_planner/
• https://autowarefoundation.github.io/autoware.universe/pr-233/planning/obstacle_avoidance_planner/obstacle_avoidance_planner-design.ja/

[codecov]

Codecov Report

Merging #233 (7f8a8b4) into main (6745976) will decrease coverage by 10.93%.
The diff coverage is 0.00%.

@@            Coverage Diff             @@
##             main    #233       +/-   ##
==========================================
- Coverage   10.93%   0.00%   -10.94%     
==========================================
  Files         705      16      -689     
  Lines       49751    2792    -46959     
  Branches     7073       0     -7073     
==========================================
- Hits         5440       0     -5440     
+ Misses      39949    2792    -37157     
+ Partials     4362       0     -4362     

Impacted Files	Coverage Δ
...lude/obstacle_avoidance_planner/common_structs.hpp	0.00% <0.00%> (ø)
...e/obstacle_avoidance_planner/costmap_generator.hpp	0.00% <0.00%> (ø)
...e/obstacle_avoidance_planner/eb_path_optimizer.hpp	0.00% <0.00%> (ø)
...clude/obstacle_avoidance_planner/mpt_optimizer.hpp	0.00% <0.00%> (ø)
...lanner/include/obstacle_avoidance_planner/node.hpp	0.00% <0.00%> (ø)
...anner/include/obstacle_avoidance_planner/utils.hpp	0.00% <0.00%> (ø)
...vehicle_model/vehicle_model_bicycle_kinematics.hpp	0.00% <ø> (ø)
...stacle_avoidance_planner/src/costmap_generator.cpp	0.00% <0.00%> (ø)
...anning/obstacle_avoidance_planner/src/cv_utils.cpp	0.00% <0.00%> (ø)
...acle_avoidance_planner/src/debug_visualization.cpp	0.00% <0.00%> (ø)
... and 705 more

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 6745976...7f8a8b4. Read the comment docs.

[TakaHoribe]

@takayuki5168 Some questions in bold.

• make calculation cost low
  • Fixing out curve results in low calculation cost since solvability gets higher.
    • want more description
  • use sparse matrix
    • Is it to enable the warm start?
  • use Linf norm for MPT's soft constraints instead of L2 norm
    • Is there any bad side effects?
  • make size of obstacle clearance map small
    • want more description
  • connect optimized points and remain points with spline interpolation instead of non-robust EB
    • So, in the previous, EB (for smoothing) -> MPT -> EB (for coupling), but now EB (for smoothing) -> MPT -> spline (for coupling). Am I right?
• make optimization stable (by dealing with the fixed length well)
  • make fixing distance not so long, and remove them from optimization (previously, fixing points are also optimization's variable)
    • how many points are used?

[TakaHoribe]

what do you mean?

[TakaHoribe]

typo: commenting this out.

[TakaHoribe]

@takayuki5168 Two suggestions:

There are so many parameters. So

• Separate params to basic and advanced parameters. Flags or easy-to-understand params are in the basic, and parameters that require the technical knowledge for users to understand is in advanced.
• Use auto-calculation logic for vehicle-related parameters. For example, the number of circles for the collision check should depend on the vehicle length and width. (These parameters can be set by parameter as an advanced option as well.)

[TakaHoribe]

Move the following vehicle_circles related parameters to the below of this file as an advanced parameters.
Write like "default is false. Please set the advanced parameters under drivability_check.vehicle_circle namespace when you set this to true.

[TakaHoribe]

EB-related parameters can be moved to an advanced parameter zone as well.

[TakaHoribe]

typo: commenting this out.

[TakaHoribe]

Move to some parameters to the advanced patameters. Some of them can stay here, for example, max_steer_deg.

[TakaHoribe]

This is a vehicle-related parameter. Take an automatic calculation logic from the vehicle_length for this parameter, and use it by default. The logic could be optimization_center_offset = vehicle_info.wheel_base * 0.8.
Please write a rough description here like below.

If this parameter is commented out, the parameter is set as below by default.
The logic could be `optimization_center_offset = vehicle_info.wheel_base * 0.8`
The 0.8 scale is adopted as it performed the best.

[TakaHoribe]

This value can be kept as this it.

[TakaHoribe]

The num_for_radius should be set based on the vehicle length parameters. Please use some auto-calculation logic and use it by default.
Also, move these parameters to the advanced parameters.

[TakaHoribe]

some clearance parameters can stay here.
All weight parameters can move to the advanced param.

[yukkysaito]

pls change merge branch to main

[takayuki5168]

@TakaHoribe Give me LGTM~

[TakaHoribe]

L
G
T
M
!!!