feat(behavior_path_planner): expand the drivable area based on the vehicle footprint

[maxime-clem]

Description

This PR adds the option to expand the drivable area based on the vehicle footprint.
Extra options are also added to prevent expanding the drivable area into the predicted paths of dynamic objects, or beyond some map linestrings with specific types.

Related links

#2182
https://github.com/orgs/autowarefoundation/discussions/3013
https://github.com/orgs/autowarefoundation/discussions/2851

Tests performed

Simple test in the planning simulator

Performances

Flamegraphs

For a busy scene with many dynamic objects, the expansion of the drivable area has a significant impact on the performance of the `behavior_path_planner` (right side of the flamegraph).

Most of the time taken to generate the drivable area is now spent on the expansion.

Notes for reviewers

To test this PR, please also checkout the corresponding launcher branch: https://github.com/autowarefoundation/autoware_launch/tree/behavior_path_planner/drivable-area-expander

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.

[codecov]

Codecov Report

Base: 11.62% // Head: 11.53% // Decreases project coverage by -0.10% ⚠️

Coverage data is based on head (745a085) compared to base (14979d0).
Patch coverage: 34.41% of modified lines in pull request are covered.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2609      +/-   ##
==========================================
- Coverage   11.62%   11.53%   -0.10%     
==========================================
  Files        1321     1331      +10     
  Lines       92140    95182    +3042     
  Branches    24443    26382    +1939     
==========================================
+ Hits        10714    10979     +265     
- Misses      70244    72635    +2391     
- Partials    11182    11568     +386     

Flag	Coverage Δ		*Carryforward flag
differential	13.92% <34.41%> (?)
total	11.62% <ø> (ø)		Carriedforward from 657aadb

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

Impacted Files	Coverage Δ
...ner/include/behavior_path_planner/data_manager.hpp	0.00% <ø> (ø)
...lanner/util/drivable_area_expansion/parameters.hpp	0.00% <0.00%> (ø)
...or_path_planner/src/behavior_path_planner_node.cpp	0.24% <0.00%> (+0.08%)	⬆️
planning/behavior_path_planner/src/utilities.cpp	6.90% <0.00%> (-1.09%)	⬇️
...ner/src/util/drivable_area_expansion/map_utils.cpp	22.22% <22.22%> (ø)
...ner/src/util/drivable_area_expansion/expansion.cpp	29.31% <29.31%> (ø)
...path_planner/test/test_drivable_area_expansion.cpp	34.00% <34.00%> (ø)
...er/src/util/drivable_area_expansion/footprints.cpp	37.50% <37.50%> (ø)
...rivable_area_expansion/drivable_area_expansion.cpp	55.38% <55.38%> (ø)
...r/util/drivable_area_expansion/path_projection.hpp	65.30% <65.30%> (ø)
... and 22 more

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.

[mehmetdogru]

@maxime-clem

I was testing the PR with gtu campus map & isuzu bus and I encountered something that I want to share. The case:

• there are path footprints go beyond the right bound however no space to expand it because of the road_border
• there is a neighbor lane and enough space for mpt to plan a trajectory on the left but no path footprints go beyond the left bound
• There is not enough space between the right and left bounds (area) for mpt to plan a trajectory inside it.

---

lanelet structure & road borders:

drivable_area bounds:

path_with_lane_id footprints (not accurate):

mpt footprints:

---

As the current algorithm, a bound is expanded only if the path footprints go beyond it until the road_border. In a case like this one maybe there should be a way to expand the left bound. As an idea maybe if the conditions below are met, the opposite bound can be expanded:

• If path footprints exceed a bound but it can not be expanded at all or can not be expanded enough
• Determine the related section (including the curve)
• If the related path section has enough curvature so we wouldn't do this on a straight lane
• If there is enough space over the opposite bound

At the end it would be something like this:

What do you think about this idea?

I know that as a workaround the related lane shape can be annotated wider on the lanelet2 map and we wouldn't have this problem but I don't think HAD maps should be vehicle specific.

[maxime-clem]

What do you think about this idea?

@mehmetdogru Thank you for the feedback. I understand the problem and I believe your idea is good.

I think I can simplify the current implementation to integrate your idea as follows:

• Calculate the ego path footprint by considering the ego footprint at each path point (same as current implementation).
• If the ego path footprint goes outside of the drivable area then we extend the drivable area (new).
  • Create an extended drivable area using some buffer distance on the left/right bound (new but similar to the static drivable area extension).
    • The buffer distance can be a parameter or can be calculated based on how much the ego path footprint goes out of the drivable area.
    • Option to only extend the drivable area between the first and last points that go out of the drivable area, with new parameters like you described to also extend ahead/behind these points.
  • [OPTIONAL] Cut the extended drivable area using uncrossable lines and dynamic objects' predicted paths (same as current implementation).
  • Add the extended drivable area to the original drivable area (same as current implementation).

In summary, I would simplify the current implementation to only use the ego path footprint to check if we need to extend the drivable area or not. Then I would use a simpler way to extend the drivable area (buffer distance on the left/right bounds).
I believe this will solve the problem your reported, fix some issues with the current implementation, and also improve performances.

[maxime-clem]

@mehmetdogru
I have not been able to implement the simplification I had in mind so I will continue using the current polygon based implementation for now.
In the latest commit (59f9b68), I add an option to "compensate" when the drivable area cannot be expanded due to a blocking linestring. "Compensate" means that we will create an additional footprint shifted in the opposite direction of the blocking linestring. I think this achieves what you need, although the current implementation with polygons does not work well due to the low path resolution. I added an option to use circle footprints which helps with that, but it still needs improvements.

[mehmetdogru]

I have not been able to implement the simplification I had in mind so I will continue using the current polygon based implementation for now. In the latest commit (59f9b68), I add an option to "compensate" when the drivable area cannot be expanded due to a blocking linestring. "Compensate" means that we will create an additional footprint shifted in the opposite direction of the blocking linestring. I think this achieves what you need, although the current implementation with polygons does not work well due to the low path resolution. I added an option to use circle footprints which helps with that, but it still needs improvements.

@maxime-clem

Thank you very much for the implementation. I test it out. There are couple of things I would like share (I can share more details if necessary):

• Main issue is that the calculations are very heavy and it takes up to 700 ms per cycle, is it also like this on your end? (related to pc specs etc.) filterFootprint func takes %95 percent of the time spent on expansion.
• For the same reason maybe expansion for da can be done just for mpt traj length (not for all the path length)
• Related to the compensation, I am not sure extra_compensate_shift works as intended. Even though I increase the value could't see any difference. If we use the circle_footprints when we drive on the straight lane and our actual footprints are not out of the drivable area, expansion is gonna be done anyways.

I believe what we should have at the end of the day is that enough drivable area when we are turning, and no expansion when we drive straight (no path footprint out). Roughly something like the one I shared before:

I created a scenario for this turn as a reference if you would like to test this PR as well. If you are gonna test this turn with Isuzu bus please make sure that:

• you have the changes in autoware to have Isuzu vehicle & sensor model.
• you have isuzu mpt configurations
• when you are launching scenario_test_runner specify isuzu_vehicle and isuzu_sensor as vehicle_model and sensor_model

[takayuki5168]

LGTM

@satoshi-ota I think you should take care of this PR. Could you approve if the PR looks good.

[mitsudome-r]

@mehmetdogru won't be able to review for 3 weeks.

We can merge this if code-owner approves the PR, and create issue if @mehmetdogru notice any issue.
@takayuki5168 Could you approve the PR if you are fine with merging?