Trajectory planning for long vehicles not working properly

[WJaworskiRobotec]

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

While testing of ITRI Bus on ITRI campus in AWSIM, I've discovered that the planner does not work properly for planning of turns for long vehicles. As can be noticed on the screenshot below, the bus is trying to "cut" the right turn by driving on the sidewalk. I know that there were seperate issues/discussions about expanding drivable area, but in this case we should think about different trajectory planning algorithm considering the dimensions of the vehicle. In reality, long vehicles like buses drive more to the outer part of the road, to avoid driving on the sidewalk.

The same behavior was observed in the planning_simulator, so it's not related to AWSIM:

Used vehicle model :
https://github.com/autowarefoundation/cbus_vehicle_launch

Location of the observed problem:

EDIT:
After more investigation, it looks like the problem can be solved by modifying drivable area (https://github.com/orgs/autowarefoundation/discussions/2851). Anyway, I believe that to make it possible for longer vehicles to behave properly, in some cases the drivable area should be even moved away from inner turn radius (in current logic the negative value of bound_offset)

Expected behavior

Bus properly driving on the entire map of ITRI campus.

Actual behavior

Bus gets stuck on the right turns.

Steps to reproduce

1. Run planning simulator with ITRI map and cbus_vehicle
2. Set the goal poses to follow demo route 2 defined for ITRI project : link

Versions

No response

Possible causes

No response

Additional context

No response

[yukkysaito]

I think the current solution was to extend the drivable area to the opposite lane.
@satoshi-ota Can you explain the details?

P.S.
It is explained here by @maxime-clem

[satoshi-ota]

Hi @WJaworskiRobotec
As @yukkysaito said, we have to expand the drivable for large vehicles, such as buses and trailers, to make turns at small intersections. In fact, the discussions is in progress here about the feature to dynamically expand the drivable area, but this has not yet been implemented.

On the other hand, the feature to expand the drivable area statically already exists. The feature expands the width of the drivable area in module behavior_path_planner (e.g. LaneFollowing) by a specified amount, which can be changed with the following parameters.

autoware.universe/planning/behavior_path_planner/config/drivable_area_expansion.yaml

Lines 1 to 20 in f1efcc2

	/**:
	ros__parameters:
	avoidance:
	drivable_area_right_bound_offset: 0.5
	drivable_area_left_bound_offset: 0.5
	lane_change:
	drivable_area_right_bound_offset: 0.0
	drivable_area_left_bound_offset: 0.0
	lane_following:
	drivable_area_right_bound_offset: 0.5
	drivable_area_left_bound_offset: 0.5
	pull_out:
	drivable_area_right_bound_offset: 0.0
	drivable_area_left_bound_offset: 0.0
	pull_over:
	drivable_area_right_bound_offset: 0.0
	drivable_area_left_bound_offset: 0.0
	side_shift:
	drivable_area_right_bound_offset: 0.0
	drivable_area_left_bound_offset: 0.0

FYI:

• feat(behavior_path_planner): params to expand drivable area in each module #1973
• behavior_path_planner do not consistently expand the drivable area #1854

[WJaworskiRobotec]

Paths to cover as the first step are defined here : autowarefoundation/autoware-projects#45

[mehmetdogru]

@WJaworskiRobotec

Here are my inputs/thoughts about the issue if this temp solution is used for this turn:

---

1. (out of context) First of all when I use this vehicle model and itri_bus_sensor_kit I got the results as below. It seems like sensor kit-base_link-3D mesh are not properly aligned. If it is needed @kaancolak can give more detailed info about this and offer his help. For now I won't be using the 3D vehicle model to show the results.

• Idk if I alone encountered this but the 3D vehicle model shown in rviz seems to have an offset wrt base_link.
  When I visualize the footprint of the vehicle /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint calculated with given vehicle parameters this is the result:

model_base_link_offset.mp4

@WJaworskiRobotec If these are not my mistake I guess an issue should be created about it.

---

2. To add road_border tag here is a way of how it can be done with vector_map_builder:

• Simply click on a linestring and choose road_border as type.
• Optionally you can select multiple linestrings by holding SHIFT key and clicking on the ones whose type you wanna change then type road_border.

annotating_road_borders.mp4

---

3. When we start the autoware with drivable_area_expansion parameters (lets say 1.5 m for all -right&left-) still you wont see that it is not expanded on the road_border sides.
   The reason is that; when using the expandLanelets function you can specify the types you would like to skip. And it is road_border type as default. But only in avoidance_module it is given. So you have to add road_border type in lane_following module as an argument as well for this demo as it is done in avoidance_module.

@maxime-clem Is this done intentionally? If not shall we create a PR where all the modules use road_border arg as default to skip the expansion?

Edit: This issue solved with #2288. Should be fine after this PR is merged.

---

4. Then we will have and expanded lanelet only on sides we want it to be:

side_out.mp4

In the video you will see that side of the vehicle is outside of drivable_area, yet it doesn't stop. It is because in obstacle_avoidance_module only the corners are checked if they are out or not. This issue is known and worked on in #1999 and will be merged soon hopefully.

However it is possible to handle the turns better since there is lots of space available yet we are not able to turn safely. It can be accomplished by enabling the parameter kinematics.optimization_center_offset and assigning 0 to it. Which it will make it possible that base_link will be optimization center and yields more realistic optimization results. Here is more info about it. To me for long vehicles such as buses, optimization center should be base_link. Idk also as default if we should change it or not @mitsudome-r @maxime-clem? On isuzu bus we use base_link as optimization center.

Results after changing the optimization center:

optimization_center.mp4

[takayuki5168]

@mehmetdogru

To me for long vehicles such as buses, optimization center should be base_link

I agree with this idea.
The default tuning of kinematics.optimization_center_offset is for a normal car. Therefore, I think it's good to set 0 for buses.
If you have further question about tuning of obstacle_avoidance_planner package, feel free to ask me.

[maxime-clem]

@maxime-clem Is this done intentionally? If not shall we create a PR where all the modules use road_border arg as default to skip the expansion?

You are right and this is something I overlooked in my previous PR. I will prepare a new PR to fix this.
EDIT: draft PR is here #2288