fix(obstacle_avoidance_planner): adding missing functionality for stop margin due to out of drivable area #4194

ahmeddesokyebrahim · 2023-07-07T02:32:53Z

Description

This PR is to add missing functionality of stop margin due to out of drivable area in obstacle_avoidance_planner since refactoring done in #2796

Tests performed

Same as mentioned in original PR : #2786

Notes for reviewers

Original PR : #2786
Refactoring PR : #2796

Interface changes

N.A.

Effects on system behavior

Adding a margin for stop point when the the stop point is calculated for the footprint out of drivable area.

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 · 2023-07-11T16:01:03Z

Codecov Report

Patch coverage: 6.25% and no project coverage change.

Comparison is base (37cbd8d) 15.17% compared to head (55818f4) 15.17%.

Additional details and impacted files

@@           Coverage Diff           @@
##             main    #4194   +/-   ##
=======================================
  Coverage   15.17%   15.17%           
=======================================
  Files        1493     1493           
  Lines      102964   102970    +6     
  Branches    31611    31613    +2     
=======================================
+ Hits        15623    15624    +1     
- Misses      70353    70358    +5     
  Partials    16988    16988

Flag	Coverage Δ		*Carryforward flag
differential	`39.64% <6.25%> (?)`
total	`15.17% <ø> (+<0.01%)`	⬆️	Carriedforward from 37cbd8d

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

Impacted Files	Coverage Δ
control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp	`0.28% <ø> (+<0.01%)`	⬆️
...nner/utils/lane_change/lane_change_module_data.hpp	`0.00% <ø> (ø)`
...h_planner/src/scene_module/lane_change/manager.cpp	`7.14% <ø> (+0.09%)`	⬆️
...th_planner/src/scene_module/lane_change/normal.cpp	`4.56% <ø> (+0.01%)`	⬆️
...lanner/include/obstacle_avoidance_planner/node.hpp	`40.00% <ø> (ø)`
planning/obstacle_avoidance_planner/src/node.cpp	`38.82% <6.25%> (-1.51%)`	⬇️

☔ View full report in Codecov by Sentry.
📢 Do you have feedback about the report comment? Let us know in this issue.

planning/obstacle_avoidance_planner/src/node.cpp

takayuki5168 · 2023-07-12T02:48:54Z

planning/obstacle_avoidance_planner/src/node.cpp

+    if (op_target_point) {
+      const auto target_point = op_target_point.get();
+      // confirm that target point doesn't overlap with the stop point outside drivable area
+      const auto dist =
+        tier4_autoware_utils::calcDistance2d(optimized_traj_points.at(stop_idx), target_point);
+      const double overlap_threshold = 1e-3;
+      if (dist > overlap_threshold) {
+        stop_idx = motion_utils::findNearestSegmentIndex(optimized_traj_points, target_point);
+      }
+    }


Could you check if you can replace this code with the following function.

autoware.universe/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp

Lines 1196 to 1199 in 6bf539b

template <class T>

inline boost::optional<size_t> insertStopPoint(

const size_t src_segment_idx, const double distance_to_stop_point, T & points_with_twist,

const double overlap_threshold = 1e-3)

I think functionality can be achieved by insertStopPoint(), but I do believe that it is not what we intended to do.
The main idea here is to try to find an already existing point on the current trajectory that is the nearest to the margin from stop point out of drivable area, then insert zero velocity starting from this existing point.
On the other hand, insertStopPoint() will make it stop yes, but that most probably would yield to a change to the current trajectory by adding this new point as it uses insertTargetPoint() .

@ahmeddesokyebrahim
insertStopPoint calls insertTargetPoint, and the function also checks if a close point to the target point already exists in the trajectory.
By using this function, the code change on this PR will be much smaller which I prefer.
Let me know if you do not agree with mine.

Btw, can I merge the following PR first?
If merging this PR first will have trouble for you, I'll wait.
#4112

got your point, thanks for the clarification.
Then I will try it with your proposal.
And go ahead for sure with merging your PR firstly.

@takayuki5168 : Please have a look with current implementation using insertStopPoint().

Thank you so much. I added some comments.

takayuki5168 · 2023-07-18T16:25:48Z

planning/obstacle_avoidance_planner/src/node.cpp

+      auto stop_idx = *first_outside_idx;
+      const auto dist = tier4_autoware_utils::calcDistance2d(
+        optimized_traj_points.at(0), optimized_traj_points.at(stop_idx));
+      const auto dist_with_margin = dist - vehicle_stop_margin_outside_drivable_area_;
+      const auto first_outside_idx_with_margin =
+        motion_utils::insertStopPoint(dist_with_margin, optimized_traj_points);
+      if (first_outside_idx_with_margin) {
+        stop_idx = *first_outside_idx_with_margin;
+      }
+      publishVirtualWall(optimized_traj_points.at(stop_idx).pose);
+      for (size_t i = stop_idx; i < optimized_traj_points.size(); ++i) {


Could you make variables private like this.

const auto stop_idx = [&]() { const auto dist = tier4_autoware_utils::calcDistance2d( optimized_traj_points.at(0), optimized_traj_points.at(*first_outside_idx)); const auto dist_with_margin = dist - vehicle_stop_margin_outside_drivable_area_; const auto first_outside_idx_with_margin = motion_utils::insertStopPoint(dist_with_margin, optimized_traj_points); if (first_outside_idx_with_margin) { return *first_outside_idx_with_margin; } return *first_outside_idx; }(); publishVirtualWall(optimized_traj_points.at(stop_idx).pose); for (size_t i = stop_idx; i < optimized_traj_points.size(); ++i) {

Also, even if enable_outside_drivable_area_stop_ is false, we wanna call publishVirtualWall.
So the following would be better.

if (first_outside_idx) { const auto stop_idx = ...; publishVirtualWall(optimized_traj_points.at(stop_idx).pose); if (enable_outside_drivable_area_stop_) { for (size_t i = stop_idx; i < optimized_traj_points.size(); ++i) { ... } } }

Thank you so much @takayuki5168 for the valuable comments 👍 .
I have updated the code, please have a look.

takayuki5168 · 2023-07-19T00:12:50Z

planning/obstacle_avoidance_planner/src/node.cpp

+        motion_utils::insertStopPoint(dist_with_margin, optimized_traj_points);
+      if (first_outside_idx_with_margin) {
+        return *first_outside_idx_with_margin;
+      } else {


(Just comment. It's okay not to apply the change.)
This else can be removed. This is called early return.

I have removed it. Thanks again for the comments!!

takayuki5168

Thank you for the change. I approve.

…p margin due to out of drivable area Signed-off-by: AhmedEbrahim <[email protected]>

…when footprint is going out of drivable area Signed-off-by: AhmedEbrahim <[email protected]>

Signed-off-by: AhmedEbrahim <[email protected]>

…rent logic Signed-off-by: AhmedEbrahim <[email protected]>

…e index with margin cannot be calculated Signed-off-by: AhmedEbrahim <[email protected]>

…alculation Signed-off-by: AhmedEbrahim <[email protected]>

… early return Signed-off-by: AhmedEbrahim <[email protected]>

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

ahmeddesokyebrahim self-assigned this Jul 7, 2023

ahmeddesokyebrahim mentioned this pull request Jul 7, 2023

fix(obstacle_avoidance_planner): adding missing functionality for stop margin due to out of drivable area autowarefoundation/autoware_launch#438

Merged

4 tasks

ahmeddesokyebrahim force-pushed the autoware/universe/4190-add-missing-func-oap-stop-margin branch 2 times, most recently from 6047a30 to d9b3f08 Compare July 11, 2023 15:10

ahmeddesokyebrahim marked this pull request as ready for review July 11, 2023 15:26

ahmeddesokyebrahim requested review from kosuke55, TakaHoribe and takayuki5168 as code owners July 11, 2023 15:26

ahmeddesokyebrahim requested a review from mehmetdogru July 11, 2023 15:26

takayuki5168 reviewed Jul 12, 2023

View reviewed changes

ahmeddesokyebrahim force-pushed the autoware/universe/4190-add-missing-func-oap-stop-margin branch 2 times, most recently from 1f5c297 to ae62120 Compare July 14, 2023 14:57

ahmeddesokyebrahim requested a review from takayuki5168 July 14, 2023 16:18

takayuki5168 reviewed Jul 18, 2023

View reviewed changes

ahmeddesokyebrahim force-pushed the autoware/universe/4190-add-missing-func-oap-stop-margin branch from f4ec531 to b09ee48 Compare July 18, 2023 20:55

takayuki5168 reviewed Jul 19, 2023

View reviewed changes

takayuki5168 approved these changes Jul 19, 2023

View reviewed changes

ahmeddesokyebrahim and others added 9 commits July 19, 2023 11:15

fix(obstacle_avoidance_planner): adding missing functionality for sto…

905b1fa

…p margin due to out of drivable area Signed-off-by: AhmedEbrahim <[email protected]>

style(pre-commit): autofix

8324ed9

fix(obstacle_avoidance_planner): fixing logic for adding stop margin …

44595b5

…when footprint is going out of drivable area Signed-off-by: AhmedEbrahim <[email protected]>

fix(obstacle_stop_planner): fixing review comment

9134e1c

Signed-off-by: AhmedEbrahim <[email protected]>

fix(obstacle_stop_planner): fixing merge conflicts

be4bef1

Signed-off-by: AhmedEbrahim <[email protected]>

fix(obstacle_avoidance_planner): using insertStopPoint instead of cur…

ae8d8a8

…rent logic Signed-off-by: AhmedEbrahim <[email protected]>

fix(obsacle_stop_planner): maintain using of first_outside_idx in cas…

d778ec4

…e index with margin cannot be calculated Signed-off-by: AhmedEbrahim <[email protected]>

fix(obstacle_avoidance_planner): using lambda function for stop_idx c…

4bf9d17

…alculation Signed-off-by: AhmedEbrahim <[email protected]>

fix(obstacle_avoidance_planner): removing unneeded else statement for…

55818f4

… early return Signed-off-by: AhmedEbrahim <[email protected]>

ahmeddesokyebrahim force-pushed the autoware/universe/4190-add-missing-func-oap-stop-margin branch from b09ee48 to 55818f4 Compare July 19, 2023 08:24

ahmeddesokyebrahim merged commit 8c257b9 into autowarefoundation:main Jul 19, 2023

tier4-autoware-public-bot bot mentioned this pull request Jul 19, 2023

chore: sync upstream tier4/autoware.universe#670

Merged

takayuki5168 mentioned this pull request Jul 20, 2023

chore(autoware_launch): zero margin for outside the drivable area autowarefoundation/autoware_launch#468

Merged

4 tasks

ahmeddesokyebrahim deleted the autoware/universe/4190-add-missing-func-oap-stop-margin branch July 24, 2023 07:49

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

fix(obstacle_avoidance_planner): adding missing functionality for stop margin due to out of drivable area #4194

fix(obstacle_avoidance_planner): adding missing functionality for stop margin due to out of drivable area #4194

ahmeddesokyebrahim commented Jul 7, 2023 •

edited

Loading

codecov bot commented Jul 11, 2023 •

edited

Loading

takayuki5168 Jul 12, 2023

ahmeddesokyebrahim Jul 13, 2023

takayuki5168 Jul 14, 2023

ahmeddesokyebrahim Jul 14, 2023

ahmeddesokyebrahim Jul 14, 2023

takayuki5168 Jul 18, 2023

takayuki5168 Jul 18, 2023

takayuki5168 Jul 18, 2023

ahmeddesokyebrahim Jul 18, 2023

takayuki5168 Jul 19, 2023

ahmeddesokyebrahim Jul 19, 2023

takayuki5168 left a comment

	template <class T>
	inline boost::optional<size_t> insertStopPoint(
	const size_t src_segment_idx, const double distance_to_stop_point, T & points_with_twist,
	const double overlap_threshold = 1e-3)

fix(obstacle_avoidance_planner): adding missing functionality for stop margin due to out of drivable area #4194

fix(obstacle_avoidance_planner): adding missing functionality for stop margin due to out of drivable area #4194

Conversation

ahmeddesokyebrahim commented Jul 7, 2023 • edited Loading

Description

Related links

Tests performed

Notes for reviewers

Interface changes

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

codecov bot commented Jul 11, 2023 • edited Loading

Codecov Report

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

takayuki5168 left a comment

Choose a reason for hiding this comment

ahmeddesokyebrahim commented Jul 7, 2023 •

edited

Loading

codecov bot commented Jul 11, 2023 •

edited

Loading