Can not give a new goal behind in the same lane

[beyzanurkaya]

Checklist

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

Description

if you give EGO vehicle an initial pose and a goal pose in a lane with the same lane_id, but there is no lane with a different lane_id between the initial and goal pose, a regular route is created. However, if you give EGO vehicle an initial pose and a goal pose in a lane with the same lane_id, but this time there is a lane with a different lane_id between the initial pose and the goal pose, a strange route occurs. Moreover, it is possible for EGO to reach its goal pose from the point where it was in the second case.

Expected behavior

Actually it is possible to reach the goal pose given -according to the lane directions- of EGO from the point where it is. Therefore, a route should have been created as follows:

Actual behavior

There is no realistic route that the vehicle can follow. A route is created as follows:

Steps to reproduce

1. Download 1715.txt and 1715-2.txt

2. Download sample-map in here

3. Change file extention from txt to yaml

4. Change pcd and lanelet2 maps file path in yaml files

5. run ros2 launch scenario_test_runner scenario_test_runner.launch.py sensor_model:=sample_sensor_kit vehicle_model:=sample_vehicle \ scenario:=/scenario/path/1715.yaml \ architecture_type:=awf/universe launch_rviz:=false launch_autoware:=true

6. Add /planning/scenario_planning/lane_driving/trajectory topic in rviz

7. When you run the scenario, path occurred correctly.

8. run ros2 launch scenario_test_runner scenario_test_runner.launch.py sensor_model:=sample_sensor_kit vehicle_model:=sample_vehicle \ scenario:=/scenario/path/1715-2.yaml \ architecture_type:=awf/universe launch_rviz:=false launch_autoware:=true

9. Add /planning/scenario_planning/lane_driving/trajectory topic in rviz

10. When you run second scenario, path doesn't occurred correctly.

Versions

• ROS2
• autoware.universe

Possible causes

No response

Additional context

No response

[mitsudome-r]

This is a known limitation in the current planning stack.
@TakaHoribe Were there any plans to support a route with a loop?

[takayuki5168]

@TakaHoribe ping.

[takayuki5168]

@mitsudome-r @beyzanurkaya
I don't hear from Horibe-san, but we do not have any plan to deal with the issue.

If someone can fix the issue, we can support it.
One of the idea of implementation is

• if the goal lane is the same as the start lane,
• get all the previous lane from the goal lane
• plan route from the start lane to each goal's previous lane
• get the shortest route

[VRichardJP]

@takayuki5168
I have poked this a bit. I came up with a similar idea and modified the RouteHandler::planPathLaneletsBetweenCheckpoints() function like so:

• if goal and start lanelet are the same, or if they are neighbors (reachable through lane change), check if goal is behind start (using arc coordinates on the starting lanelet)
  • if not -> use the old code to generate the path
  • if yes -> a loop is necessary:
    • list all the lanelets following the starting lanelet or one of its neighbors
    • for each, generate a route that pass via the lanelet with getRouteVia()
    • keep route with shortest path

Even though this works at this level, there is another problem later in the DefaultPlanner::plan() function. The RouteHandler::createMapSegment() logic does not support loops either: both first and last lanelet of the path being part of both start_lanelets_ and goal_lanelets, the algorithm fails to find which lanelet comes after (and which lanelet comes before). In the case of the RouteHandler::createMapSegment() function, it seems quite difficult to work this problem around.

I am a bit puzzled by the purpose of the RouteHandler::createMapSegment() function though: in the RouteHandler::planPathLaneletsBetweenCheckpoints() function we create a route, from this route we create a path, and then in the RouteHandler::createMapSegment() we use the path to recreate the route? Wouldn't it be more simple to convert the lanelet::Route into an HADMapSemgent direcly?

[VRichardJP]

By using the lanelet::Route to generate the HADMapSegments directly, it is possible to support looped path in mission_planner. Using the sample map, I could generate the looped path below:

header:
  stamp:
    sec: 1668666548
    nanosec: 909129799
  frame_id: map
start_pose:
  position:
    x: 3746.66455078125
    y: 73682.6875
    z: 19.629067044963804
  orientation:
    x: 0.0
    y: 0.0
    z: -0.9823370611829297
    w: 0.18712001022468155
goal_pose:
  position:
    x: 3761.641357421875
    y: 73690.7265625
    z: 19.627330542547767
  orientation:
    x: 0.0
    y: 0.0
    z: 0.9804611881898023
    w: -0.19671262911526827
segments:
- primitives:
  - id: 9232
    primitive_type: lane
  preferred_primitive_id: 9232
- primitives:
  - id: 11
    primitive_type: lane
  preferred_primitive_id: 11
- primitives:
  - id: 109
    primitive_type: lane
  preferred_primitive_id: 109
- primitives:
  - id: 17
    primitive_type: lane
  preferred_primitive_id: 17
- primitives:
  - id: 9297
    primitive_type: lane
  preferred_primitive_id: 9297
- primitives:
  - id: 9102
    primitive_type: lane
  preferred_primitive_id: 9102
- primitives:
  - id: 9540
    primitive_type: lane
  preferred_primitive_id: 9540
- primitives:
  - id: 9546
    primitive_type: lane
  preferred_primitive_id: 9546
- primitives:
  - id: 9178
    primitive_type: lane
  preferred_primitive_id: 9178
- primitives:
  - id: 54
    primitive_type: lane
  preferred_primitive_id: 54
- primitives:
  - id: 112
    primitive_type: lane
  preferred_primitive_id: 112
- primitives:
  - id: 19
    primitive_type: lane
  preferred_primitive_id: 19
- primitives:
  - id: 9232
    primitive_type: lane
  preferred_primitive_id: 9232

For comparison, here is what the mission_planner used to publish:

header:
  stamp:
    sec: 1668666812
    nanosec: 331233223
  frame_id: map
start_pose:
  position:
    x: 3747.22705078125
    y: 73683.4296875
    z: 0.0
  orientation:
    x: 0.0
    y: 0.0
    z: -0.9689194345204261
    w: 0.24737649324181518
goal_pose:
  position:
    x: 3766.694091796875
    y: 73693.265625
    z: 19.61630566976633
  orientation:
    x: 0.0
    y: 0.0
    z: 0.968972667622123
    w: -0.2471678971898795
segments:
- primitives:
  - id: 9232
    primitive_type: lane
  preferred_primitive_id: 9232

Despites the global path being correct now, Autoware still fails to generate the path:

Anyone knows which downstream module may not support looped path?

[VRichardJP]

I figured out the behavior_path_planner modules all have a special logic for the goal lanelet. Once the goal lanelet detection is fixed, Autoware is able to generate the path:

Summary

To support this use case, I had to modify the 2 packages:

• route_handler -> so that the mission_planner module is able to generate "looped" path. Actually, loops are still not supported, it's just that when the goal pose is behind the start pose the first and last lanelet are the same. The path itself does not loop.
• behavior_path_planner -> so that the submodules distinguish when the ego vehicle is at the beginning of the path (after the start) from when the vehicle is at the end of the path (before the goal)

The tricky part is how to make the difference between the starting lanelet and goal lanelet (when they are the same). The only thing that changes is the position of the ego vehicle. As a consequence, any function involved in building the path, or querying whether a lanelet is the start or goal lanelet must use this extra information. Concretely, I replaced most ConstLanelet types by a new struct ConstLaneletPose, which contains both lanelet and arc length data (i.e. a point on the lanelet centerline).

I have not done it yet, but if you follow the same logic it would make also more sense not to use ConstLanelets for paths (= std::vector<ConstLanelet>), but instead a new ConstLaneletPoses type, which would not only contain the ConstLanelets data, but also the arc length of the starting and finishing point on the first and last lanelet.

I can make a PR for this. Unless someone else has a better idea?

[VRichardJP]

-> #2424

[xmfcx]

I'll close this since it is now being worked on #2354