Extending Route Lanelets Along Goal Pose Footprint

[ismetatabay]

Background:

At Mission Planner's current design, the route lanelets planned as:

• The pose of the car is assumed as the starting point.
• If checkpoint are given, lanelets between these checkpoints are calculated
• Finally, the route lanelets are calculated between start pose and goal pose (if checkpoint point are given, it also included)

---

In cases where the goal footprint exceeds the goal lanelet, this causes the vehicle to go out of the planned route lanelets when the vehicle reaches the goal.
As in the picture below, the vehicle's goal footprint exceeds the planned lanelet route:

Goal footprint exceeds goal lane	Vehicle exceeds goal lane

As Takayuki mentioned in this comment, this situation causes a problem for the obstacle avoidance planner because it can affect drivable area generation:

However, the behavior_path_planner, which generates the drivable area, does not know the correct lanelets after the goal in the case of your figure. Only mission_planner has the information of these lanelets. Therefore, the drivable area can only have lanelets until the goal_lanelet, and the lanelets after the goal cannot be added to the drivable area. Therefore, 2 still exists.

Originally posted by @takayuki5168 in autowarefoundation/autoware.universe#2088 (comment)

---

Proposal

As we discussed in this comment w/ @takayuki5168 , if the goal footprint is on the next lanelets of goal lanelet as mentioned in this PR #2088 (used checking for intersection between goal footprint and lanelets), these lanelets should also be added to the route path.

Current Design	After Our Proposal
(Note: Orange markers describes drivable area boundaries - Light Gray Markers Drivable Area)	(Note: Orange markers describes drivable area boundaries - Light Gray Markers Drivable Area)

Current Design (Route)	After Our Proposal (Route - Included Next Lanelet )
(Note: Gray path markers describes route path)	(Note: Gray path markers describes route path)

---

Discussions

1) The goal footprint is on many next lanes:

If the goal footprint of the goal pose exceeds goal lanelet and there will be a lanelet more than one:

At this situation, how we know which lanelet used to be extend route? If we add all the next lanelets of goal pose lanelet, what will the behavior path planner do in this situation?

2) Current Drivable Area Generation:

While determining the boundaries of the drivable area, route lanelets are used, the drivable area is generated as follows:
https://github.com/autowarefoundation/autoware.universe/blob/0f37fdba94984c3420e8a09c35b987199a6a3ab1/planning/behavior_path_planner/src/utilities.cpp#L1294-L1310

• By this implementation, lanelets after the goal lanelet are considered in the drivable area generation, so the obstacle_avoidance_planner can exceeds the drivable area boundaries (Note: cannot exceed drivable area, only drivable area boundaries - orange marker), as in the image below. (Drivable Area -gray marker- also exceeds boundaries)

• So, this proposal and implementation is necessary?

[ismetatabay]

@takayuki5168 Do you have any comments ? 🙂

[satoshi-ota]

@takayuki5168 ping

[takayuki5168]

@ismetatabay
Sorry to be late.

1. In my opinion, any lanelet after the goal one is acceptable to add in this case since there will be no issue depending on which lanelet to add.
   Adding all next lanelets cannot may not work with the current mission_planner and behavior_path_planner like failing to plan a route/path.

2. In behavior path planner, the next lanelet after the goal is added to fix this issue (this issue is that lanelets from the mission_planner do not cover the goal footprint).
   This is very temporary implementation, so this should be removed with your implementation in mission_planner.
   https://github.com/autowarefoundation/autoware.universe/blob/f67d7337d4975a10925d53e7967a042587b84315/planning/behavior_path_planner/src/utilities.cpp#L1176-L1199