VRichardJP
commented
9 months ago I think you face the same problem I face in https://github.com/autowarefoundation/autoware.universe/issues/6271#issuecomment-1920884945:
Beside the backward trajectory which is really weird, I think there is a problem with the way
RouteHandler::planPathLaneletsBetweenCheckpointsis implemented:
- 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).
- find closest lanelet to goal pose.
- 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 your case, the initial pose is located on 2 lanelets (the vehicle shape is ignored), so the route handler has to decide which one in correct.
For 3, the hard coded yaw threshold is 90 degrees (!!), so even a perpendicular lanelet like in your case may be included as candidate. Then, since the goal you have set is located on the right part of the map, the path passing by the perpendicular lanelet is likely shorter than the one passing by the correct lanelet. So eventually the route handler thinks the perpendicular lanelets is the correct one.
As I mentionned in the other issue, I think the starting lanelets (and goal lanelets?) should be classified into more categories. For example (from higher priority to lower):
- vehicle fits in lanelet, angle diff is low (e.g. <30 deg)
- vehicle does not fit in lanelet, but vehicle pose is in the lanelet, angle diff is low
- vehicle pose is outside lanelet but close (e.g. ~2m), angle diff is high (<45)
- vehicle pose is far or angle is high
I am not sure it is a good idea to generate generate a path in the last case.
Kim-mins
danielsanchezaran
idorobotics
Checklist
Description
Hi team,
I found that, when the ego vehicle is initialized at the intersection, the generated trajectory is sometimes vertical to the ego vehicle.
Here's the [rviz video] and [launch.log] file.
When I checked it several times, this does not occur every time, but sometimes.
Expected behavior
I want the generated trajectory to be aligned with the ego vehicle's heading.
Actual behavior
But the generated trajectory is not aligned.
Steps to reproduce
I ran the [planning simulation] with the prebuilt docker, version [20240201] Pointcloud map: [link] Lanelet2 map: [link]
And I set the initial pose at the intersection, and set the goal point somewhere.
Versions
I ran the planning simulation with the prebuilt docker, version [20240201]
Possible causes
It seems the heading is not considered well for some positions of the intersection.
Additional context
I also searched for the similar issues, and I guess https://github.com/autowarefoundation/autoware.universe/issues/6271 could be the similar one, that issue is regarding the
backward trajectorythough.