Dynamically Expanding the Drivable Area

[maxime-clem]

This discussion is to share ideas for dynamically expanding the drivable area when necessary.

Background

The Path message published by the behavior_path_planner contains the drivable area, i.e., a region that the ego footprint must not exit.

The drivable area is represented by a nav_msgs/OccupancyGrid. Currently, cells of the occupancy grid can take 2 values: drivable and not drivable.

This drivable area usually corresponds to the lanelets of the route followed by ego. Only in some situations (e.g., lane changes) does the drivable area extend into other lanes.

Planners like the obstacle_avoidance_planner must then guarantee that the vehicle footprint along the planned trajectory does not overlaps with any "not drivable" cells.

Trajectory footprint inside the drivable area	Trajectory footprint outside the drivable area

Motivating use case and issues

A recent issue motivated this discussion: autowarefoundation/autoware.universe#1759

When a road or curve is too narrow or the vehicle too big, the drivable area can be "impossible" to drive. Current planners will make the vehicle stop before the narrow passage and ego cannot progress further along its route.

Temporary solution

The behavior_path_planner has parameters which enable expanding the drivable area with fixed left and right extra length.
A sharp right turn can thus be made drivable by extending the area to left.

Default drivable area	Expanded drivable area

Issues

There are 2 issues with the temporary solution stated above:

• The parameters must be set at launch time and cannot be changed during runtime.
  • This means that the drivable area will always be expanded, even if sometimes the lane is wide enough to drive.
• If a dynamic object is coming towards ego in the other lane, the planners will not detect the need to stop.
  • Currently, dynamic obstacles are only checked in the current lane.
  • Static obstacles are still avoided even if in another lane.

Possible solutions

I write here some ideas for being able to expand the drivable area only when necessary.
These ideas do not tackle the issue with dynamic obstacles which must be resolved differently.

Dynamic drivable area expansion

A function can be added to dynamically change the parameters to expand the drivable area.
This function could be part of the behavior_path_planner or could be a new node.

The new function would detect when the trajectory stops because of an impossible drivable area.
In such situation, the new function would start increasing the left or right extra length until the trajectory no longer stops.

Pros: not too heavy to implement.
Cons: reactive approach.

Proposal 1 (11/15): AWF-202211-DynamicDrivableArea.pdf

Soft drivable area

The format of the drivable area can be changed to include more than the 2 current values "drivable" and "not drivable". For example, a value "undesired" can be added such that:

• If the planner cannot generate a trajectory that strictly stays inside the "drivable" cells, a trajectory that overlaps some "undesired" cells is acceptable.
• It is still forbidden to overlap "not drivable" cells.

Pros: more expressive, allows for smarter planning.
Cons: requires heavy changes.

[zulfaqar-azmi-t4]

The dynamic drivable area expansion seems more easier to implement, and I would suggest to append the whole side lane instead of length-based. It will make the code more easier to implement and less complexity(?).

[WJaworskiRobotec]

@maxime-clem

I believe that we should expand the drivable area at the initialization phase of Autoware. It should be mostly expanded on the curves of lanelet and it should depend on the shape of lanelet and the parameters of the selected Ego vehicle stored in vehicle_info_param.yaml:

ros__parameters:
wheel_radius: 0.383 # The radius of the wheel, primarily used for dead reckoning.
wheel_width: 0.235 # The lateral width of a wheel tire, primarily used for dead reckoning.
wheel_base: 2.79 # between front wheel center and rear wheel center
wheel_tread: 1.64 # between left wheel center and right wheel center
front_overhang: 1.0 # between front wheel center and vehicle front
rear_overhang: 1.1 # between rear wheel center and vehicle rear
left_overhang: 0.128 # between left wheel center and vehicle left
right_overhang: 0.128 # between right wheel center and vehicle right
vehicle_height: 2.5
max_steer_angle: 0.70 # [rad]

Based on these parameters we should be able to calculate the lateral length of vehicle footprint in each place of the lanelet. If my idea is not clear I can prepare some drawings.

I'm not sure if it's already considered somewhere in the planner, but definitely should be also considered in the drivable area, especially for the long vehicles (like bus)

[maxime-clem]

I think I understand your point.

• We can calculate if the road is too narrow simply from the lanelet map and the vehicle dimension.
• We can expand the drivable area only in places where the road is too narrow.

I agree this logic works well as long as there are no obstacles. Obstacles are not known in advance and can make a road more narrow than expected (e.g., a vehicle is parked on the side of the road). In this case, we need a dynamic way to expand the drivable area.

Please let me know if I misunderstood something.

[WJaworskiRobotec]

Yes, I think you understand correctly.

You are in general right about the obstacles. If I start thinking about the situations on the road that requires going off the currently driven lane, I'm having another idea:

• Depending on the situation (parked vehicle, other obstacle, narrow turn etc.) the planned trajectory can differ a lot
  but:
• It should never go outside of the road surface.

Wouldn't it be easier if we consider the entire road surface as drivable and the appropriate path is just handled by a planner ?

I apologize If it's not a valid suggestion, I'm not really familiar with the planning part of Autoware, I've just noticed the problem in simulation based tests of ITRI and trying to help :)

[maxime-clem]

Thank you for participating in the discussion. It is very valuable.
I totally agree with your idea and I think it is similar to the "Soft drivable area" I proposed.
The planner should prioritize driving in its current lane, but it should also be allowed to drive in other lanes if necessary.
This can be achieved by (1) changing the drivable area to have multiple areas with various costs, and (2) modifying the planners to consider an objective function over the drivable area.
I personally prefer this solution and when I have time I will prepare a more detailed proposal.

[tleyden]

Currently, dynamic obstacles are only checked in the current lane.

Out of curiosity, how does this work? Are the dynamic obstacles detected via lidar, but ignored if their position is determined to be outside of the current lane?

[maxime-clem]

They are only ignored by the obstacle_avoidance_planner which generates the trajectory around obstacles in the ego lane.
Other planners will detect the dynamic obstacle on the planned trajectory and force a 0 velocity before the collision.
There are known issues with this logic as shown in the video posted in this discussion: https://github.com/orgs/autowarefoundation/discussions/2915

[WJaworskiRobotec]

@maxime-clem
I've discovered that the planning definitely does not work properly for long vehicles : autowarefoundation/autoware.universe#2182 .
I believe that this issue with expanding the drivable area is critical for BusODD scenarios.

[TakaHoribe]

@maxime-clem @WJaworskiRobotec
I suggest going with the "dynamic drivable area (DA) expansion" to solve the issue autowarefoundation/autoware.universe#2182 because of its simplicity.
The "Soft DA" idea looks also good to me, but it has a significant impact on the whole system. We should rethink the overall design of the DA if we are going to do this. It would take a long time.

As @WJaworskiRobotec said, the dynamic DA Expansion can be determined statically from lane structure and vehicle geometry. (Although the DA generation itself is dynamic, so it will be dynamic anyway.)

My specific suggestions are:

• Check the footprint of the vehicle along the centerline defined on the map and if it goes outside the lane boundary, make the area a candidate for expansion. (A margin for the footprint may be provided.)
• If the candidate extension area is really drivable, add the area to the DA. (e.g. the DA shouldn't have a sidewalk even if it is a candidate.) Maybe we can take AND for the areas.

This operation can be implemented in the behavior_path_planner which currently makes the DA. The resultant DA is "driving lane (original)" + ("footprint along the centerline" and "drivable").

The minor problem is that at the time the DA is generated, the path vehicle follows has not been determined. So, strictly speaking, extending the DA based on an accurate decision is impossible. However, this is a minor issue that can be put off this time.

[yukkysaito]

The minor problem is that at the time the DA is generated, the path vehicle follows has not been determined. So, strictly speaking, extending the DA based on an accurate decision is impossible. However, this is a minor issue that can be put off this time.

If DA is not enough when the behavior path planner generates the rough path, does it mean that DA is extended?

[maxime-clem]

I think this idea is the best for the short term.

If DA is not enough when the behavior path planner generates the rough path, does it mean that DA is extended?

With this idea, the DA is generated and extended by the behavior_path_planner.
I think the point made by TakaHoribe is that the DA + path calculated by the behavior_path_planner creates a problem that can be impossible to solve by the motion planner (obstacle_avoidance_planner).

I wrote my understanding of the problem and a proposal of this idea here:
AWF-202211-DynamicDrivableArea.pdf

I will present it at the Software Working Group today and, if people like the idea, I will implement it before the end of the year.