Refactor mission planning to support when is goal behind start on the same lane

[VRichardJP]

Checklist

• I've read the contribution guidelines.
• I've searched other issues and no duplicate issues were found.
• I've agreed with the maintainers that I can plan this task.

Description

As pointed in #2291 and #1715, the mission_planner and behavior_path_planner modules cannot generate neither a route nor a path when the goal pose is behind the start pose on the same lanelet.

This limitation comes from the way paths are designed in Autoware modules. Basically, all planning modules decribe paths as lists of lanelets (ConstLanelets or std::vector<ConstLanelet>). This only works as long as Autoware does not need to go through the same lanelet twice (which happens when the goal pose is behind the start pose).

To support the latter case, path definition should be refined. For example, a path could be:

• a list of lanelets (ConstLanelets)
• a start point on the first lanelet (if any)
• a goal point on the last lanelet (if any)

With this new definition, the first and last lanelet could be the same, yet the starting "segment" and finishing "segment" could be different. In the case the goal point is behind the start point, the first segment would be from the start pose to the end of the lanelet, while the last segment would be from the beginning of the lanelet to the goal pose.

Purpose

• add support for goal behind start scenario

Possible approaches

1. In route_handler, introduce a new LaneletPath class, composed of:

• ConstLanelets lanelets: the list of lanelets composing the path
• double start_arc_length: the arc length of the start point on the first lanelet (if any)
• double goal_arc_length: the arc length of the goal point on the last lanelet (if any)

2. Any planning function using ConstLanelets as path should use the new type instead. ConstLanelets could still be used for non-path purposes (e.g. to list neighbor lanes). As a consequence, the following modules would need to be heavily reworked:

• mission_planning
• route_handler
• behavior_path_planner

3. Introduce Helper functions to implement route related logic (e.g. extend a path forward)

4. Clean old code

Definition of done

• new mission_planner supports all existing use cases + goal behind start on the same lanelet
• new behavior_path_planenr supports all existing use case + goal behind start on the same lanelet
• all route related logic is handled directly by route_handler module

Obviously, a set of OSM maps is needed to make sure Autoware still behaves the same in the supported cases.

[shulanbushangshu]

@VRichardJP As you say,the nodes including mission_planning,route_handler ,behavior_path_planner must be changed.But I plan the goal route by the previous lanes of the start lane as the #1715 said in the mission_planning(The loop checking is necessary).
In the node of behavior_path_planner,I set the flag of loop plan by the start pose and target pose and change the functions including "getLaneletSequenceAfter()","setRoute()" which are used to generate the path to deal with the same lane checking in the route_handler.

The simulation is as below:

[VRichardJP]

@shulanbushangshu
I would be curious to see the changes you have made to make it work.
On a first try, I modified mostly the route_handler and simply introduced a pose argument to all the getLaneletSequence() like functions.

This is enough for the route to be planned correctly, but then behavior_path_planner modules are not able to generate the path to the goal pose.

I tried to do a similar thing and fix behavior_path_planner modules to handle the case "goal behind start". I works, but it makes the code quite fragile: it is very easy not to pay attention to the special case "goal behind start" and break it.

To avoid the extra cognitive burden for planning maintainers, I propose to add a specific type for lanelet paths, which would handle such tricky case by design

[shulanbushangshu]

@VRichardJP As you say,then behavior_path_planner modules are not able to generate the path to the goal pose.So I add the additional codes to deal with the problem in function "modifyPathForSmoothGoalConnection". In the modifyPathForSmoothGoalConnection,I change the goal_lane_id and refined_goal by judging the target pose.

[VRichardJP]

@xmfcx you can assign this to me. I am working on this at the moment (it just take some time ^^)

[VRichardJP]

@shulanbushangshu It's infortunate we both worked on the same thing at the same time :(
I checked your change and I came up with a similar idea in my initial implementation. However I realized it did not work in more complex situations, for example when there is a lane change in the route. Hence my (maybe over-engineered) solution.

[shulanbushangshu]

@VRichardJP ,Thank for your check.I will confirm it and continue to pay attention to this issue.

[xmfcx]

This PR is waiting for autowarefoundation/autoware#2983 to be implemented, which could take about a month. We will take action once it is implemented.

[stale]

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

[stale]

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