fix(behavior_path_planner): boundary's nearest search #3716

takayuki5168 · 2023-05-15T10:18:05Z

Description

In the following case, the boundary's nearest index search is calculated wrong.
I fixed this issue by calculating the minimum distance from each segment on bound and the target point (ego pose in this case).

This issue happens when avoiding objects with the motion planner by extracting objects from the drivable area.

Tests performed

planning simulator

Effects on system behavior

No change 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.

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.

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

takayuki5168 · 2023-05-15T10:30:21Z

planning/behavior_path_planner/src/utils/utils.cpp

+  for (const bool is_object_right : {true, false}) {
    const auto & polygons = is_object_right ? right_polygons : left_polygons;


This is just a small refactoring

purewater0901 · 2023-05-15T10:49:57Z

planning/behavior_path_planner/src/utils/utils.cpp

+  const double squared_segment_length = tier4_autoware_utils::calcSquaredDistance2d(a, b);
+  if (0 <= dot_val && dot_val <= squared_segment_length) {
+    const double numerator = std::abs((p.x - a.x) * (a.y - b.y) - (p.y - a.y) * (a.x - b.x));
+    const double denominator = std::sqrt(std::pow(a.x - b.x, 2) + std::pow(a.y - b.y, 2));


Can we use the squared length of the segment?

Yes, I implemented according to these articles
https://tjkendev.github.io/procon-library/python/geometry/segment_line_point_distance.html
https://qiita.com/kkdd/items/b3c5e06798e59fe2768e

In the first one, if condition uses squared segment length.

codecov · 2023-05-15T11:18:19Z

Codecov Report

Patch coverage has no change and project coverage change: +0.10 🎉

Comparison is base (c2d4bc8) 14.55% compared to head (13b8e43) 14.65%.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3716      +/-   ##
==========================================
+ Coverage   14.55%   14.65%   +0.10%     
==========================================
  Files        1409     1409              
  Lines       99298   100126     +828     
  Branches    29378    29987     +609     
==========================================
+ Hits        14457    14678     +221     
- Misses      69258    69708     +450     
- Partials    15583    15740     +157

Flag	Coverage Δ		*Carryforward flag
differential	`13.31% <0.00%> (?)`
total	`14.55% <ø> (+<0.01%)`	⬆️	Carriedforward from c2d4bc8

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

Impacted Files	Coverage Δ
planning/behavior_path_planner/src/utils/utils.cpp	`19.39% <0.00%> (+2.38%)`	⬆️

... and 3 files 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.

…tion#3716) * fix(behavior_path_planner): boundary's nearest search Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* feat(dynamic_avoidance): apply LPF to shift length, and positive relative velocity (autowarefoundation#4047) Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * feat(dynamic_avoidance): suppress flickering of dynamic avoidance launching (autowarefoundation#4048) * feat(dynamic_avoidance): suppress flickering of dynamic avoidance launching Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update config Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * feat(dynamic_avoidance): ignore cut-out vehicle Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update cut out decision policy Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix(obstacle_avoidance_planner): smooth path connection after optimization (autowarefoundation#3754) * fix(obstacle_avoidance_planner): smooth path connection after optimization Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix(obstacle_avoidance_planner): fix optimization constraint for narrow-road driving (autowarefoundation#3701) Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix(lane_change): copy drivable area info (autowarefoundation#3738) Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * feat(map_based_prediction): suppress lane change decision flickering (autowarefoundation#3788) Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * feat(map_based_prediction): disable time delay compensation (autowarefoundation#3806) * disable time delay compensation Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * fix unused parameter Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * fix lane change decision not working problem due to empty future_lanenet in unconnected lanelets Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> --------- Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * fix(behavior_path_planner): boundary's nearest search (autowarefoundation#3716) * fix(behavior_path_planner): boundary's nearest search Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix(behavior_path_planner): extract backward obstacles correctly (autowarefoundation#3719) Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix(behavior_path_planner): extract obstacles from drivable area without intersection (autowarefoundation#3741) * fix(behavior_path_planner): extract obstacles from drivable area without intersection Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update map_based_prediction method Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix(avoidance): fix invalid cherry pick Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: Yoshi Ri <yoshiyoshidetteiu@gmail.com> Co-authored-by: satoshi-ota <satoshi.ota928@gmail.com>

fix(behavior_path_planner): boundary's nearest search

ed070a4

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

github-actions bot added the component:planning Route planning, decision-making, and navigation. (auto-assigned) label May 15, 2023

takayuki5168 marked this pull request as ready for review May 15, 2023 10:27

takayuki5168 requested review from rej55, kosuke55, kyoichi-sugahara, satoshi-ota, shmpwk, taikitanaka3, TakaHoribe, tkimura4, purewater0901, zulfaqar-azmi-t4 and a team as code owners May 15, 2023 10:27

fix

13b8e43

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

takayuki5168 commented May 15, 2023

View reviewed changes

purewater0901 reviewed May 15, 2023

View reviewed changes

purewater0901 approved these changes May 15, 2023

View reviewed changes

takayuki5168 merged commit 8f4b385 into autowarefoundation:main May 15, 2023

takayuki5168 deleted the fix/bound-nearest-search branch May 15, 2023 11:53

tier4-autoware-public-bot bot mentioned this pull request May 16, 2023

chore: sync upstream tier4/autoware.universe#480

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

fix(behavior_path_planner): boundary's nearest search #3716

fix(behavior_path_planner): boundary's nearest search #3716

takayuki5168 commented May 15, 2023 •

edited

Loading

takayuki5168 May 15, 2023

purewater0901 May 15, 2023

takayuki5168 May 15, 2023

codecov bot commented May 15, 2023 •

edited

Loading

		for (const bool is_object_right : {true, false}) {
		const auto & polygons = is_object_right ? right_polygons : left_polygons;

fix(behavior_path_planner): boundary's nearest search #3716

fix(behavior_path_planner): boundary's nearest search #3716

Conversation

takayuki5168 commented May 15, 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

takayuki5168 May 15, 2023

Choose a reason for hiding this comment

purewater0901 May 15, 2023

Choose a reason for hiding this comment

takayuki5168 May 15, 2023

Choose a reason for hiding this comment

codecov bot commented May 15, 2023 • edited Loading

Codecov Report

takayuki5168 commented May 15, 2023 •

edited

Loading

codecov bot commented May 15, 2023 •

edited

Loading