Autoware generate backward trajectory when placed on overlapping lanelets

[VRichardJP]

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

Normal trajectory:

Backward trajectory:

Almost normal trajectory (straight lanelet should be preferred):

It seems like the backward trajectory is generated when the vehicle is barely inside the curved lanelet. Based on the vehicle orientation, I would expect the trajectory to include the straight lanelet, but Autoware seems to think the vehicle is on the curved lanelet instead.

Expected behavior

Autoware does not drive backward in the middle of an intersection

Actual behavior

random backward trajectory is generated

Steps to reproduce

Can be reproduced with the sample map:

Just place the vehicle like the image above and set the goal in front.

Versions

No response

Possible causes

No response

Additional context

No response

[kyoichi-sugahara]

@VRichardJP It is possible that this problem is caused by lane departure check for the path between current ego pose to shift end pose. And there is no lane departure between backed pose and shift end pose. If this is the reason, can you disable the parameter of check_shift_path_lane_departure? the link is here https://github.com/tier4/autoware_launch/blob/14d144c3e909dda2c989ee42bed8b549c281d4c2/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml#L16

[VRichardJP]

~@kyoichi-sugahara The parameter is false by default on my setup (almost latest autoware foundation main). I tested with the parameter enabled just in case, it does not not change anything.~

Beside the backward trajectory which is really weird, I think there is a problem with the way RouteHandler::planPathLaneletsBetweenCheckpoints is implemented:

1. find all lanelets containing ego pose. If the vehicle is on no lanelet, it looks for the closest lanelet instead (which feels a bit dangerous).
2. find closest lanelet to goal pose.
3. for each starting lanelet candidate, check if ego orientation is acceptable and try to generate a route the goal lanelet. Finally keep the route with the shortest length

In the backward trajectory case, there are 2 candidates for the starting lanelet (the straight and curved ones), and because the curved lanelet is shorter than the curved one it is selected as the starting lanelet. I see 2 problems here:

• Choosing a route over another because one lanelet is shorter than another one is a bit dubious. Length before ego pose should not matter.
• Even if the path was shorter, the curved is absolutely not appropriate here: the vehicle does not even fit in. So during step 1, I think the curved lanelet could be selected as a "closest" lanelet candidate if there was no other appropriate candidate, but it should not be considered as a lanelet containing ego.

[VRichardJP]

@kyoichi-sugahara I didn't look at the correct codebase sorry. The check_shift_path_lane_departure parameter is indeed enabled in my case. Disabling the parameter seems to fix the backward trajectory problem:

[kyoichi-sugahara]

@VRichardJP good! I was about to visualize the lanes which are used for lane departure check by merging this PR :smile: https://github.com/autowarefoundation/autoware.universe/pull/6110#pullrequestreview-1856024789 If you want to visualize, please make use of that information. And I feel this lane also shold be included in the lane departure check :thinking: Anyway thank you for the report!

[danielsanchezaran]

@VRichardJP I am currently working on a fix based on the yaw difference between ego and lanelet to choose the correct starting lanelet: https://github.com/autowarefoundation/autoware.universe/pull/6550

[danielsanchezaran]

@VRichardJP I think the merged PR: https://github.com/autowarefoundation/autoware.universe/pull/6550 has solved this issue, could you please confirm?

[VRichardJP]

@danielsanchezaran I think it it ok now

Thank you !