feat(avoidance): make it possible to use freespace areas in avoidance module #6001

satoshi-ota · 2023-12-31T00:42:57Z

Description

Previously, since avoidance module didn't consider freespace areas (parking log) as drivable space, sometimes the ego couldn't avoid objects. (In this case, the module used only lanelets as drivable space.)

In this PR, I implement new feature to use freespace to avoid objects.

Please review this PR at first.
autowarefoundation/autoware_launch#818

simplescreenrecorder-2024-01-24_10.17.55.mp4

Tests performed

PASS TIER IV INTERNAL SCENARIOS ALL PASS

Effects on system behavior

Improve avoidance flexibility.

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.

kosuke55 · 2024-01-24T09:08:35Z

...ning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp

+  if (original_bound_itr->id() != original_bound_rev_itr->id()) {
+    const auto polygon_bound =
+      extract_bound_from_polygon(original_bound_itr->id(), original_bound_rev_itr->id());
+
+    expanded_bound.insert(expanded_bound.end(), original_bound.begin(), original_bound_itr);
+    expanded_bound.insert(expanded_bound.end(), polygon_bound.begin(), polygon_bound.end());
+    expanded_bound.insert(
+      expanded_bound.end(), std::next(original_bound_rev_itr).base(), original_bound.end());
+
+  } else if (boost::geometry::within(
+               to2D(original_bound.front().basicPoint()), to2D(polygon).basicPolygon())) {
+    auto polygon_bound =
+      extract_bound_from_polygon(other_side_bound_itr->id(), original_bound_itr->id());
+    std::reverse(polygon_bound.begin(), polygon_bound.end());
+    auto bound_edge = get_bound_edge(polygon_bound, true);
+    std::reverse(bound_edge.begin(), bound_edge.end());
+
+    expanded_bound.insert(expanded_bound.end(), bound_edge.begin(), bound_edge.end());
+    expanded_bound.insert(expanded_bound.end(), original_bound_itr, original_bound.end());
+
+  } else if (boost::geometry::within(
+               to2D(original_bound.back().basicPoint()), to2D(polygon).basicPolygon())) {
+    const auto polygon_bound =
+      extract_bound_from_polygon(original_bound_itr->id(), other_side_bound_itr->id());
+    const auto bound_edge = get_bound_edge(polygon_bound, false);
+
+    expanded_bound.insert(expanded_bound.end(), original_bound.begin(), original_bound_itr);
+    expanded_bound.insert(expanded_bound.end(), bound_edge.begin(), bound_edge.end());
+
+  } else {
+    expanded_bound = original_bound;
+  }


better to add explanation to each patter

I believe it greatly helps code readability and reduce complexity if you use enums to describe what is happening and then use a switch for each case:

something like

`enum Classification { CASE_A, CASE_B, CASE_C, CASE_D };

// lambda function
const auto classifyBound = /[ & /] () -> Classification
{
if (original_bound_itr->id() != original_bound_rev_itr->id()) return Classification::CASE_A;
if (boost::geometry::within(to2D(original_bound.front().basicPoint())))
return Classification::CASE_B;
if (boost::geometry::within(to2D(original_bound.back().basicPoint())))
return Classification::CASE_C;
return Classification::CASE_D;
};

cacse = classifyBound()

switch(case){

...
}
`

If you change the names of the cases for a descriptive name and then use a switch I believe it would be more descriptive without having to write any comments

I fixed implementation for readability in 28badef.

kosuke55 · 2024-01-24T09:10:03Z

...ning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp

+  const auto post_process = [&](const auto & bound, const auto skip) {
+    return skip
+             ? bound
+             : postProcess(
+                 bound, path, planner_data, drivable_lanes, enable_expanding_hatched_road_markings,
+                 enable_expanding_intersection_areas, is_left, is_driving_forward);
+  };


this is just wrapper of postProcess function with skip. seems not need make this lambda

kosuke55 · 2024-01-24T09:16:37Z

...ning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp

+  return std::make_pair(expanded_bound, is_driving_freespace || goal_is_in_freespace);
+}
+
+std::vector<geometry_msgs::msg::Point> postProcess(


just memo:
this is just separated from generateDrivableArea

kosuke55 · 2024-01-24T09:18:40Z

...ning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp

+  const auto get_bound_edge = [&ego_pose, &is_driving_freespace, &is_left](
+                                const auto & bound, const auto is_inside) {


what is_inside menas?

Fixed flag name is_inside -> trim_bound_behind.

kosuke55

LGTM

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

codecov · 2024-01-28T06:16:08Z

Codecov Report

Attention: 150 lines in your changes are missing coverage. Please review.

Comparison is base (02ba67f) 14.58% compared to head (0e1edea) 14.63%.
Report is 1 commits behind head on main.

Files	Patch %	Lines
...s/drivable_area_expansion/static_drivable_area.cpp	23.80%	98 Missing and 46 partials ⚠️
...nning/behavior_path_avoidance_module/src/scene.cpp	57.14%	1 Missing and 2 partials ⚠️
...ning/behavior_path_planner/src/planner_manager.cpp	0.00%	1 Missing and 1 partial ⚠️
...ehavior_path_avoidance_module/parameter_helper.hpp	0.00%	0 Missing and 1 partial ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #6001      +/-   ##
==========================================
+ Coverage   14.58%   14.63%   +0.04%     
==========================================
  Files        1871     1871              
  Lines      127569   127665      +96     
  Branches    37317    37371      +54     
==========================================
+ Hits        18608    18678      +70     
+ Misses      88042    88017      -25     
- Partials    20919    20970      +51

Flag	Coverage Δ		*Carryforward flag
differential	`12.01% <25.37%> (?)`
total	`14.57% <ø> (-0.02%)`	⬇️	Carriedforward from 02ba67f

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

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

… module (autowarefoundation#6001) * fix(static_drivable_area_expansion): check right/left bound id Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * feat(static_drivable_area): use freespace area Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * feat(avoidance): use freespace Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(AbLC): fix flag Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(planner_manager): fix flag Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * refactor(static_drivable_area_expansion): remove unused arg Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * refactor(static_drivable_area_expansion): use lambda Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(static_drivable_area_expansion): fix invalid access Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * refactor(static_drivable_area_expansion): improve readability Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(avoidance): add param Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> --------- Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> Signed-off-by: anhnv3991 <anh.nguyen.2@tier4.jp>

… module (autowarefoundation#6001) * fix(static_drivable_area_expansion): check right/left bound id Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * feat(static_drivable_area): use freespace area Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * feat(avoidance): use freespace Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(AbLC): fix flag Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(planner_manager): fix flag Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * refactor(static_drivable_area_expansion): remove unused arg Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * refactor(static_drivable_area_expansion): use lambda Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(static_drivable_area_expansion): fix invalid access Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * refactor(static_drivable_area_expansion): improve readability Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(avoidance): add param Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> --------- Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

github-actions bot added component:planning Route planning, decision-making, and navigation. (auto-assigned) component:common Common packages from the autoware-common repository. (auto-assigned) labels Dec 31, 2023

satoshi-ota force-pushed the feat/use-free-space branch from 3c46099 to 5c18b87 Compare December 31, 2023 02:35

satoshi-ota force-pushed the feat/use-free-space branch 2 times, most recently from 8cf997d to feacf2c Compare January 24, 2024 01:56

github-actions bot removed the component:common Common packages from the autoware-common repository. (auto-assigned) label Jan 24, 2024

satoshi-ota assigned kosuke55 Jan 24, 2024

satoshi-ota added the run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) label Jan 24, 2024

satoshi-ota marked this pull request as ready for review January 24, 2024 08:04

satoshi-ota requested review from danielsanchezaran, rej55, kosuke55, soblin, maxime-clem, shmpwk, TakaHoribe, takayuki5168, tkimura4, zulfaqar-azmi-t4, kyoichi-sugahara and TomohitoAndo as code owners January 24, 2024 08:04

satoshi-ota mentioned this pull request Jan 24, 2024

feat(avoidance): add new flag to use freespace in avoidance module autowarefoundation/autoware_launch#818

Merged

4 tasks

kosuke55 reviewed Jan 24, 2024

View reviewed changes

satoshi-ota force-pushed the feat/use-free-space branch from cd48dde to 28badef Compare January 25, 2024 09:09

kosuke55 approved these changes Jan 26, 2024

View reviewed changes

satoshi-ota force-pushed the feat/use-free-space branch from 28badef to 1757316 Compare January 26, 2024 07:10

satoshi-ota added 4 commits January 28, 2024 14:01

fix(static_drivable_area_expansion): check right/left bound id

7257753

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

feat(static_drivable_area): use freespace area

47426b5

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

feat(avoidance): use freespace

1ba59cb

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

fix(AbLC): fix flag

ad9de50

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

satoshi-ota added 6 commits January 28, 2024 14:01

fix(planner_manager): fix flag

c5da7c4

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

refactor(static_drivable_area_expansion): remove unused arg

edfb380

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

refactor(static_drivable_area_expansion): use lambda

eca8762

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

fix(static_drivable_area_expansion): fix invalid access

9dbcbd7

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

refactor(static_drivable_area_expansion): improve readability

1ea4dfa

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

fix(avoidance): add param

0e1edea

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

satoshi-ota force-pushed the feat/use-free-space branch from 1757316 to 0e1edea Compare January 28, 2024 05:04

satoshi-ota enabled auto-merge (squash) January 28, 2024 05:05

rej55 approved these changes Jan 29, 2024

View reviewed changes

satoshi-ota merged commit 6d6a7b0 into autowarefoundation:main Jan 29, 2024
25 of 29 checks passed

satoshi-ota deleted the feat/use-free-space branch January 29, 2024 01:47

satoshi-ota mentioned this pull request Jan 29, 2024

fix(avoidance): cherry pick new feature tier4/autoware.universe#1120

Merged

4 tasks

tier4-autoware-public-bot bot mentioned this pull request Feb 7, 2024

chore: sync upstream tier4/autoware.universe#1130

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

feat(avoidance): make it possible to use freespace areas in avoidance module #6001

feat(avoidance): make it possible to use freespace areas in avoidance module #6001

satoshi-ota commented Dec 31, 2023 •

edited

Loading

kosuke55 Jan 24, 2024

danielsanchezaran Jan 25, 2024 •

edited

Loading

satoshi-ota Jan 25, 2024

kosuke55 Jan 24, 2024

kosuke55 Jan 24, 2024

kosuke55 Jan 24, 2024

satoshi-ota Jan 25, 2024 •

edited

Loading

kosuke55 left a comment

codecov bot commented Jan 28, 2024 •

edited

Loading

		const auto get_bound_edge = [&ego_pose, &is_driving_freespace, &is_left](
		const auto & bound, const auto is_inside) {

feat(avoidance): make it possible to use freespace areas in avoidance module #6001

feat(avoidance): make it possible to use freespace areas in avoidance module #6001

Conversation

satoshi-ota commented Dec 31, 2023 • edited Loading

Description

Tests performed

Effects on system behavior

Pre-review checklist for the PR author

In-review checklist for the PR reviewers

Post-review checklist for the PR author

kosuke55 Jan 24, 2024

Choose a reason for hiding this comment

danielsanchezaran Jan 25, 2024 • edited Loading

Choose a reason for hiding this comment

satoshi-ota Jan 25, 2024

Choose a reason for hiding this comment

kosuke55 Jan 24, 2024

Choose a reason for hiding this comment

kosuke55 Jan 24, 2024

Choose a reason for hiding this comment

kosuke55 Jan 24, 2024

Choose a reason for hiding this comment

satoshi-ota Jan 25, 2024 • edited Loading

Choose a reason for hiding this comment

kosuke55 left a comment

Choose a reason for hiding this comment

codecov bot commented Jan 28, 2024 • edited Loading

Codecov Report

satoshi-ota commented Dec 31, 2023 •

edited

Loading

danielsanchezaran Jan 25, 2024 •

edited

Loading

satoshi-ota Jan 25, 2024 •

edited

Loading

codecov bot commented Jan 28, 2024 •

edited

Loading