feat(behavior_path_sampling_module): add sampling based planner

[danielsanchezaran]

Description

This feature implements an (experimental) sampling based planner into the behavior path planning architecture, and its heavily based on the sampling based planner. The behavior_path_sampling_module utilizes the Frenet coordinate space to sample/generate several candidate paths for the ego vehicle.

The candidate paths are evaluated based on hard constraints, used to prune samples that don't satisfy certain conditions, and soft constraints, that evaluate the cost or fitness of each sample. After each iteration the "best" sampled path (the one with the lowest cost) is chosen as the output of the module.

At every iteration, the previous "best" path is used as a reference to generate a new "best" path. That is accomplished by using

The image below illustrates the candidate paths (red and green lines) and the valid sampled paths (green lines only) generated by the module:

NOTE:
This feature is still in development, this module should only be used on simulations for the time being.

Uses:
In theory, with appropriate pruning and sampling, this module can be adapted to solve most behavior path problems, however, this implementation is aimed at avoiding static obstacles.

Some examples:

Avoidance with many roads to do LC:

cap-.2024-01-22-13-06-55.mp4

Narrow road with parked cars in front:

cap-.2024-01-22-14-19-09.mp4

Avoiding a parked vehicle:

cap-.2024-01-22-14-38-53.mp4

Hard avoidance scenarios:

cap-.2024-01-22-14-41-39.mp4

Related links

Requires these changes to launch: autowarefoundation/autoware_launch#810

Tests performed

Tests on Psim and real life tests on a service robot.

Notes for reviewers

This is an experimental feature, it has not been sufficiently tested on real life scenarios to be proved safe for use. It is a work in progress and should be used only in simulations until specified otherwise.

Known limitations:

Activation conditions for the module require refinement.
A dynamic parameter tuning might be necessary to force the generated path to merge back into the original lane.
Collision checking can be improved.

Interface changes

Adds a new optional module for the bpp based on path sampling in the frenet coordinate space.

Effects on system behavior

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.

[maxime-clem]

I have a bunch of small comments but overall the code looks good.

Please add some documentation with some information about the module, how it should be used, the current limitations, future works, etc.

[satoshi-ota]

🎉