mission_planner unable to set goal when goal is on lanelet that is included in route in other direction

[sebekx]

Checklist

• [X] I've read the contribution guidelines.
• [X] I've searched other issues and no duplicate issues were found.
• [X] I'm convinced that this is not my fault but a bug.

Description

In situation when there are two lanelets one on another but in different direction and those lanelets contains the same lanestrings but in opposite direction and we drive through one lanelet then turn around at the roundabout and set goal on the other lanelet in opposite direction.

so we have:

• lanelet (1) bounds (left: 2, right 1)
• lanelet (2) bounds (left: 1, right 2)

Mission planner reports error: "Goal's footprint exceeds lane!". which is not true, but there is a problem with DefaultPlanner::is_goal_valid function. I've found that there is function that combine lanelets with shoulder and there is condition that check exactly that case:

https://github.com/sebekx/autoware.universe/blob/fd032e86926cbd191f8fdca3b154f46cadad3fd8/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp#L88

else if (
      // if not exist, add left bound of lanelet to lefts
      // if the **left** of this lanelet does not match any of the **right** bounds of `lanelets`,
      // then its left bound constitutes the left boundary of the entire merged lanelet
      std::count(right_bound_ids.begin(), right_bound_ids.end(), llt.leftBound().id()) < 1) {
      add_bound(llt.leftBound(), lefts);
    }

add_bound is called only when there is no such bound in opposite direction. Could you please explain what cases it addresses? When i removed this condition i was able to set that trajectory.

Expected behavior

Mission planner correctly generates routes in such case

Actual behavior

Mission planner is not able to generate route in such case due to error "Goal's footprint exceeds lane!"

Steps to reproduce

1. Prepare map with lanelet and roundabout, as shown above.
2. Set ego vehicle before lanelet
3. Set goal position just after roundabout in opposite direction to force route through first lanelat, roundabout and stops on second lanelet.

Versions

No response

Possible causes

Function that checks if vehicle is within lanelet checks combined_prev_lanelet:

bool DefaultPlanner::check_goal_footprint_inside_lanes(
  const lanelet::ConstLanelet & current_lanelet,
  const lanelet::ConstLanelet & combined_prev_lanelet,
  const tier4_autoware_utils::Polygon2d & goal_footprint, double & next_lane_length,
  const double search_margin)
{
  // check if goal footprint is in current lane
  if (boost::geometry::within(goal_footprint, combined_prev_lanelet.polygon2d().basicPolygon())) {
    return true;
  }

however this lanelet is generated by:

lanelet::ConstLanelet combine_lanelets_with_shoulder(
  const lanelet::ConstLanelets & lanelets, const lanelet::ConstLanelets & shoulder_lanelets)

which contains conditions that prevents before adding such lanes:

} else if (
      // if not exist, add right bound of lanelet to rights
      // if the **right** of this lanelet does not match any of the **left** bounds of `lanelets`,
      // then its right bound constitutes the right boundary of the entire merged lanelet
      std::count(left_bound_ids.begin(), left_bound_ids.end(), llt.rightBound().id()) < 1) {
      add_bound(llt.rightBound(), rights);
    }

Additional context

No response

[NorahXiong]

@sebekx Could you upload your map for testing?

[ahmeddesokyebrahim]

@sebekx Would you please share the map you used to have it easier to reproduce the same behavior ?

[stale[bot]]

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