fix(mission_planner): consider overlap lanelets when enable_correct_goal_pose param is true #5533

beyzanurkaya · 2023-11-09T09:13:22Z

Description

Fixes:

mission_planner fails when enable_correct_goal_pose param is true #5530

Tests performed

fix-overlap-goal-lanes.mp4

wip tier4 internal scenario test
evaluator_description: fix/consider-overlap-lanelet
2023/11/30 https://evaluation.tier4.jp/evaluation/reports/b059c77e-39d1-5d8c-8367-127e79619b10/?project_id=prd_jt

Not applicable.

Effects on system behavior

Not applicable.

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 · 2023-11-09T14:59:50Z

Codecov Report

Attention: Patch coverage is 12.90323% with 27 lines in your changes are missing coverage. Please review.

Project coverage is 14.84%. Comparing base (8948859) to head (f805b36).
Report is 2 commits behind head on main.

❗ Current head f805b36 differs from pull request most recent head 047170a. Consider uploading reports for the commit 047170a to get more accurate results

Files	Patch %	Lines
...n_planner/src/lanelet2_plugins/default_planner.cpp	11.76%	4 Missing and 11 partials ⚠️
planning/route_handler/src/route_handler.cpp	14.28%	5 Missing and 7 partials ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #5533      +/-   ##
==========================================
- Coverage   14.94%   14.84%   -0.10%     
==========================================
  Files        1943     1838     -105     
  Lines      133953   126717    -7236     
  Branches    39841    37982    -1859     
==========================================
- Hits        20020    18817    -1203     
+ Misses      91656    86652    -5004     
+ Partials    22277    21248    -1029

Flag	Coverage Δ		*Carryforward flag
differential	`14.85% <12.90%> (?)`
total	`14.85% <ø> (-0.10%)`	⬇️	Carriedforward from 78eea31

*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.

kosuke55 · 2023-11-28T03:30:28Z

@beyzanurkaya is it ok to update this branch?

beyzanurkaya · 2023-11-28T07:57:45Z

@kosuke55 sure, I rebased the branch onto main.

mehmetdogru · 2023-11-28T08:21:24Z

@kosuke55 @beyzanurkaya

I am working on a PR also covers/solves this issue. So when I am done, I would like to review this PR as well before we possibly merge it.

kosuke55 · 2023-11-29T15:36:54Z

@beyzanurkaya @mehmetdogru
thanks!
I ran tier4 internal scenario test just now, I will review this after checking the result is no problem

kosuke55 · 2023-11-30T15:35:08Z

some scenarios related shoulder_lanes fails, but I did not check closely 🙏

kosuke55 · 2023-12-11T08:44:01Z

planning/mission_planner/src/lanelet2_plugins/default_planner.cpp

+  for (const auto & gl_llt : goal_lanelets) {
+    if (
+      param_.check_footprint_inside_lanes &&
+      !check_goal_footprint(gl_llt, combined_prev_lanelet, polygon_footprint, next_lane_length) &&
+      !is_in_parking_lot(
+        lanelet::utils::query::getAllParkingLots(lanelet_map_ptr_),
+        lanelet::utils::conversion::toLaneletPoint(goal.position))) {
+      RCLCPP_WARN(logger, "Goal's footprint exceeds lane!");
+      is_goal_valid = false;
+    } else {
+      is_goal_valid = true;
+    }

-  if (is_in_lane(closest_lanelet, goal_lanelet_pt)) {
-    const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet, goal.position);
-    const auto goal_yaw = tf2::getYaw(goal.orientation);
-    const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw);
+    if (is_in_lane(gl_llt, goal_lanelet_pt)) {
+      const auto lane_yaw = lanelet::utils::getLaneletAngle(gl_llt, goal.position);
+      const auto goal_yaw = tf2::getYaw(goal.orientation);
+      const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw);

-    const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg);
-    if (std::abs(angle_diff) < th_angle) {
-      return true;
+      const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg);
+      if (std::abs(angle_diff) < th_angle) {
+        return true;
+      }


If goal's footprint exceeds lane but the process below may return true, is it ok?
It is like the footprint check is ignored.

It is ok for us because we don't have any motivation to check footprint.

@kosuke55 I'm asking just to be sure, is your suggestion to completely remove the footprint check instead of an edit that will eliminate the logical issue here?

@kosuke55 friendly ping

@beyzanurkaya
Sorry for the delay.

I am not sure if we should remove the footprint check, but the results of it sometimes appear to be unused.
This PR appears to return true even if is_goal_valid is false if one point of goal_lanelet_pt is in the goal lane and the angle is threshold only.
In what scene would you use the result of the footprint check?
(i.e., in what scene return is_goal_valid;)

Would the conditional branch look like this?

○: true
× : false

If goal's footprint exceeds lane but the process below may return true, is it ok?
It is like the footprint check is ignored.

I mentioned about 2,3 pattern.

In what scene would you use the result of the footprint check?
(i.e., in what scene return is_goal_valid;)

I think this scnene is 3 or 5.
5 will ignore the angle check.

stale · 2024-02-10T06:06:58Z

This pull request has been automatically marked as stale because it has not had recent activity.

xmfcx · 2024-03-05T20:14:22Z

@beyzanurkaya could you resolve the conflicts and update this PR?

Do you have any blockers or questions on this PR?

xmfcx · 2024-03-05T20:18:12Z

I would suggest to first squash, then rebase to make it easier to resolve the conflicts. You can contact me if you need help.

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

stale · 2024-07-06T13:48:20Z

This pull request has been automatically marked as stale because it has not had recent activity.

beyzanurkaya requested review from rej55, kosuke55, soblin, takayuki5168, purewater0901, zulfaqar-azmi-t4, isamu-takagi, mitsudome-r and TakaHoribe as code owners November 9, 2023 09:13

beyzanurkaya self-assigned this Nov 9, 2023

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

beyzanurkaya added type:bug Software flaws or errors. tag:run-build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) labels Nov 9, 2023

mehmetdogru self-requested a review November 9, 2023 12:18

kosuke55 mentioned this pull request Nov 22, 2023

mission_planner fails when enable_correct_goal_pose param is true #5530

Open

3 tasks

kosuke55 self-assigned this Nov 22, 2023

beyzanurkaya force-pushed the fix/consider-overlap-lanelet branch from d3bd760 to 256369a Compare November 28, 2023 07:56

beyzanurkaya force-pushed the fix/consider-overlap-lanelet branch 2 times, most recently from 256369a to 0a0342c Compare November 28, 2023 10:17

kosuke55 force-pushed the fix/consider-overlap-lanelet branch from 0a0342c to 7237442 Compare November 29, 2023 15:32

kosuke55 force-pushed the fix/consider-overlap-lanelet branch 2 times, most recently from 208003b to 4590451 Compare December 11, 2023 02:37

kosuke55 mentioned this pull request Dec 11, 2023

fix(goal_planner): fix checkOriginalGoalIsInShoulder #5836

Merged

4 tasks

kosuke55 reviewed Dec 11, 2023

View reviewed changes

kosuke55 closed this in #5836 Dec 12, 2023

kosuke55 reopened this Dec 12, 2023

stale bot added status:stale Inactive or outdated issues. (auto-assigned) and removed status:stale Inactive or outdated issues. (auto-assigned) labels Feb 10, 2024

beyzanurkaya force-pushed the fix/consider-overlap-lanelet branch from 4590451 to f805b36 Compare February 14, 2024 12:56

beyzanurkaya force-pushed the fix/consider-overlap-lanelet branch 2 times, most recently from f51def1 to 84a591e Compare April 2, 2024 10:57

consider all overlap lanelets as a goal lanelet

1c3540a

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

beyzanurkaya force-pushed the fix/consider-overlap-lanelet branch from 84a591e to 1c3540a Compare April 2, 2024 12:39

style(pre-commit): autofix

047170a

beyzanurkaya marked this pull request as draft May 7, 2024 09:04

stale bot added the status:stale Inactive or outdated issues. (auto-assigned) label Jul 6, 2024

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

fix(mission_planner): consider overlap lanelets when enable_correct_goal_pose param is true #5533

fix(mission_planner): consider overlap lanelets when enable_correct_goal_pose param is true #5533

beyzanurkaya commented Nov 9, 2023 •

edited by xmfcx

Loading

codecov bot commented Nov 9, 2023 •

edited

Loading

kosuke55 commented Nov 28, 2023

beyzanurkaya commented Nov 28, 2023

mehmetdogru commented Nov 28, 2023

kosuke55 commented Nov 29, 2023

kosuke55 commented Nov 30, 2023

kosuke55 Dec 11, 2023

kosuke55 Dec 11, 2023

beyzanurkaya Feb 14, 2024

beyzanurkaya Feb 22, 2024

kosuke55 Feb 28, 2024 •

edited

Loading

kosuke55 Feb 28, 2024 •

edited

Loading

stale bot commented Feb 10, 2024

xmfcx commented Mar 5, 2024

xmfcx commented Mar 5, 2024

stale bot commented Jul 6, 2024

fix(mission_planner): consider overlap lanelets when enable_correct_goal_pose param is true #5533

Are you sure you want to change the base?

fix(mission_planner): consider overlap lanelets when enable_correct_goal_pose param is true #5533

Conversation

beyzanurkaya commented Nov 9, 2023 • edited by xmfcx 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

codecov bot commented Nov 9, 2023 • edited Loading

Codecov Report

kosuke55 commented Nov 28, 2023

beyzanurkaya commented Nov 28, 2023

mehmetdogru commented Nov 28, 2023

kosuke55 commented Nov 29, 2023

kosuke55 commented Nov 30, 2023

kosuke55 Dec 11, 2023

Choose a reason for hiding this comment

kosuke55 Dec 11, 2023

Choose a reason for hiding this comment

beyzanurkaya Feb 14, 2024

Choose a reason for hiding this comment

beyzanurkaya Feb 22, 2024

Choose a reason for hiding this comment

kosuke55 Feb 28, 2024 • edited Loading

Choose a reason for hiding this comment

kosuke55 Feb 28, 2024 • edited Loading

Choose a reason for hiding this comment

stale bot commented Feb 10, 2024

xmfcx commented Mar 5, 2024

xmfcx commented Mar 5, 2024

stale bot commented Jul 6, 2024

beyzanurkaya commented Nov 9, 2023 •

edited by xmfcx

Loading

codecov bot commented Nov 9, 2023 •

edited

Loading

kosuke55 Feb 28, 2024 •

edited

Loading

kosuke55 Feb 28, 2024 •

edited

Loading