fix(behavior_velocity_planner): support u-turn for detection_area, vtl and stop line #1700

takayuki5168 · 2022-08-25T23:57:58Z

Description

support u-turn and crossing path for detection_area, vtl and stop line

issue
#1581

Tests performed

Notes for reviewers

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 · 2022-08-26T01:12:01Z

Codecov Report

Merging #1700 (eddfecd) into main (231b7ed) will decrease coverage by 0.24%.
The diff coverage is 14.82%.

@@            Coverage Diff             @@
##             main    #1700      +/-   ##
==========================================
- Coverage   10.24%   10.00%   -0.25%     
==========================================
  Files        1202     1204       +2     
  Lines       86463    89844    +3381     
  Branches    19893    22010    +2117     
==========================================
+ Hits         8861     8986     +125     
- Misses      68383    71323    +2940     
- Partials     9219     9535     +316

Flag	Coverage Δ		*Carryforward flag
differential	`10.94% <16.46%> (?)`
total	`10.23% <7.82%> (ø)`		Carriedforward from 36cec0b

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

Impacted Files	Coverage Δ
...ils/test/src/trajectory/test_path_with_lane_id.cpp	`43.28% <0.00%> (ø)`
...include/behavior_velocity_planner/planner_data.hpp	`0.00% <ø> (ø)`
..._planner/include/scene_module/blind_spot/scene.hpp	`0.00% <ø> (ø)`
...ene_module/occlusion_spot/occlusion_spot_utils.hpp	`57.14% <ø> (ø)`
...er/include/scene_module/scene_module_interface.hpp	`0.00% <0.00%> (ø)`
...anner/include/scene_module/traffic_light/scene.hpp	`0.00% <ø> (ø)`
...clude/scene_module/virtual_traffic_light/scene.hpp	`0.00% <ø> (ø)`
...y_planner/include/utilization/trajectory_utils.hpp	`0.00% <0.00%> (ø)`
...vior_velocity_planner/include/utilization/util.hpp	`0.00% <0.00%> (ø)`
planning/behavior_velocity_planner/src/node.cpp	`0.62% <0.00%> (+0.19%)`	⬆️
... and 62 more

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

planning/behavior_velocity_planner/include/utilization/util.hpp

taikitanaka3 · 2022-08-26T06:03:28Z

I will Approve if this PR passes these 3 case

self crossing
U turn
turn

…vtl, stop_line Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

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

taikitanaka3

LGTM
I checked "detection area" / "stop line" / "virtual traffic light" works as expected below case
https://user-images.githubusercontent.com/65527974/187327490-a11ab00d-e84c-47e2-bfdb-2edb76a48cb2.mp4
https://user-images.githubusercontent.com/65527974/187327498-126772f5-fd09-434a-b8a6-b3d4d0b0ddf1.mp4
https://user-images.githubusercontent.com/65527974/187355643-e1d22bac-4c2d-418f-a886-40219713d38a.mp4

satoshi-ota · 2022-08-31T02:46:55Z

planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp

  /**
   * @brief : calculate signed arc length consider stop margin from stop line
   *
   * |----------------------------|
   * s---ego----------x--|--------g
   */
+  const size_t stop_line_seg_idx = planning_utils::calcSegmentIndexFromPointIndex(
+    path->points, stop_pose.position, stop_point_idx);
+  const double stop_line_margin = base_link2front + planner_param_.stop_margin;


I think stop_line_margin should not contain base_link2front because the offset has already been considered in stop_pose calculation in arc_lane_utils::createTargetPoint().

planner_param_.stop_margin is also considered in that util function ?

satoshi-ota · 2022-08-31T02:50:53Z

planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp

-      path->points, src_point_with_search_range_index, dst_point_with_search_range_index) -
+    motion_utils::calcSignedArcLength(
+      path->points, planner_data_->current_pose.pose.position, current_seg_idx, stop_pose.position,
+      stop_line_seg_idx) -
    stop_line_margin;


We should remove stop_line_margin from this calculation ?

…l and stop line (autowarefoundation#1700) * fix(behavior_velocity_planner): deal with u-turn for detection_area, vtl, stop_line Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * enable behavior velocity test and fix bug Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix bug Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix collision check Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * add test Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * remove unnecessary debug output Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

autowarefoundation#1700) Signed-off-by: Yuma Nihei <yuma.nihei@tier4.jp>

takayuki5168 force-pushed the feat/behvior-vel-nearest-ego branch from ca2abe8 to cf813cb Compare August 26, 2022 00:56

takayuki5168 changed the title ~~fix(behavior_velocity_planner): use common ego nearest search~~ fix(behavior_velocity_planner): support u-turn for detection_area, vtl and stop line Aug 26, 2022

takayuki5168 force-pushed the feat/behvior-vel-nearest-ego branch from b7e7df8 to 2e5c149 Compare August 26, 2022 02:33

takayuki5168 requested review from kosuke55, taikitanaka3 and purewater0901 August 26, 2022 02:41

taikitanaka3 reviewed Aug 26, 2022

View reviewed changes

planning/behavior_velocity_planner/include/utilization/util.hpp Show resolved Hide resolved

takayuki5168 marked this pull request as ready for review August 26, 2022 05:12

takayuki5168 force-pushed the feat/behvior-vel-nearest-ego branch from f368184 to da8ff70 Compare August 29, 2022 00:33

takayuki5168 added 4 commits August 29, 2022 10:05

fix(behavior_velocity_planner): deal with u-turn for detection_area, …

59b58e2

…vtl, stop_line Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

fix

159424f

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

enable behavior velocity test and fix bug

04b4f40

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

fix bug

071a1ce

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

takayuki5168 force-pushed the feat/behvior-vel-nearest-ego branch from da8ff70 to 071a1ce Compare August 29, 2022 02:31

takayuki5168 added 2 commits August 29, 2022 12:04

fix

ab1ce26

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

fix

6078c91

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

takayuki5168 force-pushed the feat/behvior-vel-nearest-ego branch from 634fb25 to 6078c91 Compare August 29, 2022 23:55

takayuki5168 added 3 commits August 30, 2022 13:36

fix collision check

f82a316

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

add test

36cec0b

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

remove unnecessary debug output

eddfecd

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

taikitanaka3 approved these changes Aug 30, 2022

View reviewed changes

takayuki5168 merged commit ae833f3 into autowarefoundation:main Aug 30, 2022

takayuki5168 deleted the feat/behvior-vel-nearest-ego branch August 30, 2022 07:02

tier4-autoware-public-bot bot mentioned this pull request Aug 31, 2022

chore: sync upstream tier4/autoware.universe#100

Merged

satoshi-ota reviewed Aug 31, 2022

View reviewed changes

takayuki5168 mentioned this pull request Oct 20, 2022

Deal with U-turn and crossing path #1581

Open

3 tasks

yn-mrse added a commit to tier4/autoware.universe that referenced this pull request Jun 23, 2023

fix(behavior_velocity_planner): support u-turn stop_line (partial apply

9f75dd0

autowarefoundation#1700) Signed-off-by: Yuma Nihei <yuma.nihei@tier4.jp>

yn-mrse added a commit to tier4/autoware.universe that referenced this pull request Jun 23, 2023

fix(behavior_velocity_planner): support u-turn stop_line (partial-apply

4f40efc

autowarefoundation#1700) Signed-off-by: Yuma Nihei <yuma.nihei@tier4.jp>

yn-mrse added a commit to tier4/autoware.universe that referenced this pull request Jun 23, 2023

fix(behavior_velocity_planner): support u-turn stop_line (partial-apply

57f1dbc

autowarefoundation#1700) Signed-off-by: Yuma Nihei <yuma.nihei@tier4.jp>

This was referenced Jun 23, 2023

fix(behavior_velocity_planner): support u-turn for stop line (partial-apply #1700) tier4/autoware.universe#614

Closed

feat: keep stopping in hold stop margin distance tier4/autoware.universe#595

Closed

yn-mrse added a commit to tier4/autoware.universe that referenced this pull request Jun 23, 2023

fix(behavior_velocity_planner): support u-turn stop_line (partial-apply

d0dc338

autowarefoundation#1700) Signed-off-by: Yuma Nihei <yuma.nihei@tier4.jp>

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

fix(behavior_velocity_planner): support u-turn for detection_area, vtl and stop line #1700

fix(behavior_velocity_planner): support u-turn for detection_area, vtl and stop line #1700

takayuki5168 commented Aug 25, 2022 •

edited

Loading

codecov bot commented Aug 26, 2022 •

edited

Loading

taikitanaka3 commented Aug 26, 2022

taikitanaka3 left a comment

satoshi-ota Aug 31, 2022 •

edited

Loading

satoshi-ota Aug 31, 2022

satoshi-ota Aug 31, 2022

fix(behavior_velocity_planner): support u-turn for detection_area, vtl and stop line #1700

fix(behavior_velocity_planner): support u-turn for detection_area, vtl and stop line #1700

Conversation

takayuki5168 commented Aug 25, 2022 • edited Loading

Description

Related links

Tests performed

Notes for reviewers

Pre-review checklist for the PR author

In-review checklist for the PR reviewers

Post-review checklist for the PR author

codecov bot commented Aug 26, 2022 • edited Loading

Codecov Report

taikitanaka3 commented Aug 26, 2022

taikitanaka3 left a comment

Choose a reason for hiding this comment

satoshi-ota Aug 31, 2022 • edited Loading

Choose a reason for hiding this comment

satoshi-ota Aug 31, 2022

Choose a reason for hiding this comment

satoshi-ota Aug 31, 2022

Choose a reason for hiding this comment

takayuki5168 commented Aug 25, 2022 •

edited

Loading

codecov bot commented Aug 26, 2022 •

edited

Loading

satoshi-ota Aug 31, 2022 •

edited

Loading