feat(behavior_path_planner): add yaw_dev threshold angle #2126

beyzanurkaya · 2022-10-20T12:35:55Z

Signed-off-by: beyza bnk@leodrive.ai

Description

Before PR:

vokoscreen-2022-10-20_12-39-56.mp4

After PR:

vokoscreen-2022-10-21_12-27-20.mp4

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.

codecov · 2022-10-20T13:25:23Z

Codecov Report

Base: 11.09% // Head: 11.08% // Decreases project coverage by -0.00% ⚠️

Coverage data is based on head (7922bc3) compared to base (333044b).
Patch coverage: 0.00% of modified lines in pull request are covered.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2126      +/-   ##
==========================================
- Coverage   11.09%   11.08%   -0.01%     
==========================================
  Files        1202     1202              
  Lines       86170    86251      +81     
  Branches    20680    20735      +55     
==========================================
+ Hits         9558     9559       +1     
- Misses      66661    66734      +73     
- Partials     9951     9958       +7

Flag	Coverage Δ		*Carryforward flag
differential	`3.58% <0.00%> (?)`
total	`11.06% <0.00%> (ø)`		Carriedforward from 333044b

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

Impacted Files	Coverage Δ
...or_path_planner/src/behavior_path_planner_node.cpp	`0.34% <0.00%> (+0.14%)`	⬆️

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.

kosuke55 · 2022-10-24T09:35:35Z

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

+    const auto closest_path_point_idx =
+      motion_utils::findNearestIndex(path->points, planner_data->self_pose->pose.position);


It seems better to consider an overlapping path.
https://github.com/autowarefoundation/autoware.universe/blob/main/common/motion_utils/README.md#nearest-index-for-the-ego

Right. Please use this function.

autoware.universe/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

Line 183 in 778f8c0

size_t findEgoIndex(const std::vector<T> & points) const

takayuki5168 · 2022-10-25T01:39:33Z

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

+    const auto closest_path_point_idx =
+      motion_utils::findNearestIndex(path->points, planner_data->self_pose->pose.position);


Right. Please use this function.

autoware.universe/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

Line 183 in 778f8c0

size_t findEgoIndex(const std::vector<T> & points) const

takayuki5168 · 2022-10-25T01:45:10Z

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

+    if (std::abs(yaw_dev) > M_PI / 2.0) {
+      RCLCPP_WARN(
+        get_logger(), "Path orientation is invalid. Path yaw deviation: %f (deg)",
+        yaw_dev * 180 / M_PI);
+      if (!planner_data_->prev_output_path->points.empty()) {
+        path = planner_data_->prev_output_path;
+      } else {
+        return;
+      }
+    } else {
+      // update planner data
+      planner_data_->prev_output_path = path;
+    }


Please use "early return".

takayuki5168 · 2022-10-25T04:19:07Z

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

+    if (std::abs(yaw_dev) > M_PI / 2.0) {
+      RCLCPP_WARN(
+        get_logger(), "Path orientation is invalid. Path yaw deviation: %f (deg)",
+        yaw_dev * 180 / M_PI);
+      if (!planner_data_->prev_output_path->points.empty()) {
+        path = planner_data_->prev_output_path;
+      } else {
+        return;
+      }
+    } else {


Sorry to convert the PR to draft.

Using previously planned path resolves the issue, but ideally using previous path should be avoided since the current planning with the current dynamic situation is not applied to the planning result.

This issue is known one. During lane_following in behavior_path_planner, the nearest lanelet to ego is searched and the path will start from the lanelet. Therefore, if some functions like drivable area expansion moves the ego pose CoG to neighbour lanelets, the path will suddenly start from the neighbour lanelet.

Rather than hiding an issue with previous path, we should fix this known issue fundamentally.

Signed-off-by: beyza <bnk@leodrive.ai>

xmfcx · 2022-11-15T15:08:48Z

Currently @mehmetdogru and @beyzanurkaya are looking for alternative solutions for this issue.

mehmetdogru · 2022-12-22T06:28:04Z

Since the approach followed in this PR is not a proper one, I will be closing the PR. I will create another PR which fixes the related issue.

beyzanurkaya changed the title ~~add yaw_dev threshold angle~~ feat(behavior_pat_planner): add yaw_dev threshold angle Oct 20, 2022

beyzanurkaya changed the title ~~feat(behavior_pat_planner): add yaw_dev threshold angle~~ feat(behavior_path_planner): add yaw_dev threshold angle Oct 20, 2022

beyzanurkaya force-pushed the 2125-generating-invalid-path-orientation branch from 673a576 to 7f1d7c3 Compare October 20, 2022 12:55

beyzanurkaya force-pushed the 2125-generating-invalid-path-orientation branch from 56362c2 to 778f8c0 Compare October 21, 2022 08:18

mehmetdogru marked this pull request as ready for review October 24, 2022 09:23

mehmetdogru requested review from rej55, kosuke55, satoshi-ota, shmpwk, taikitanaka3, TakaHoribe, takayuki5168, tkimura4, purewater0901 and zulfaqar-azmi-t4 as code owners October 24, 2022 09:23

kosuke55 reviewed Oct 24, 2022

View reviewed changes

takayuki5168 reviewed Oct 25, 2022

View reviewed changes

takayuki5168 marked this pull request as draft October 25, 2022 02:13

takayuki5168 reviewed Oct 25, 2022

View reviewed changes

beyzanurkaya self-assigned this Nov 3, 2022

beyza and others added 4 commits November 3, 2022 13:55

add yaw_dev threshold angle

845b8d3

Signed-off-by: beyza <bnk@leodrive.ai>

ci(pre-commit): autofix

09585ee

used early return

a56764f

Signed-off-by: beyza <bnk@leodrive.ai>

change finding nearest_index function

7922bc3

Signed-off-by: beyza <bnk@leodrive.ai>

beyzanurkaya force-pushed the 2125-generating-invalid-path-orientation branch from 778f8c0 to 7922bc3 Compare November 3, 2022 11:15

mehmetdogru closed this Dec 22, 2022

mehmetdogru deleted the 2125-generating-invalid-path-orientation branch December 22, 2022 06:29

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

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

feat(behavior_path_planner): add yaw_dev threshold angle #2126

feat(behavior_path_planner): add yaw_dev threshold angle #2126

beyzanurkaya commented Oct 20, 2022 •

edited by mehmetdogru

Loading

codecov bot commented Oct 20, 2022 •

edited

Loading

kosuke55 Oct 24, 2022 •

edited

Loading

takayuki5168 Oct 25, 2022

takayuki5168 Oct 25, 2022

takayuki5168 Oct 25, 2022

takayuki5168 Oct 25, 2022

xmfcx commented Nov 15, 2022

mehmetdogru commented Dec 22, 2022

		const auto closest_path_point_idx =
		motion_utils::findNearestIndex(path->points, planner_data->self_pose->pose.position);

feat(behavior_path_planner): add yaw_dev threshold angle #2126

feat(behavior_path_planner): add yaw_dev threshold angle #2126

Conversation

beyzanurkaya commented Oct 20, 2022 • edited by mehmetdogru Loading

Description

Pre-review checklist for the PR author

In-review checklist for the PR reviewers

Post-review checklist for the PR author

codecov bot commented Oct 20, 2022 • edited Loading

Codecov Report

kosuke55 Oct 24, 2022 • edited Loading

Choose a reason for hiding this comment

takayuki5168 Oct 25, 2022

Choose a reason for hiding this comment

takayuki5168 Oct 25, 2022

Choose a reason for hiding this comment

takayuki5168 Oct 25, 2022

Choose a reason for hiding this comment

takayuki5168 Oct 25, 2022

Choose a reason for hiding this comment

xmfcx commented Nov 15, 2022

mehmetdogru commented Dec 22, 2022

beyzanurkaya commented Oct 20, 2022 •

edited by mehmetdogru

Loading

codecov bot commented Oct 20, 2022 •

edited

Loading

kosuke55 Oct 24, 2022 •

edited

Loading