Collision path when the part of obstacle crosses the centerline

[shulanbushangshu]

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

When large obstacle is present and part of the obstacle crosses the centerline of the lane, the calculated maximum offset distance decreases rather than increases.
In this case, the planned path would collide with the obstacle.

Expected behavior

The calculated offset distance will continue to grow, and the planned path will not collide with obstacles.
The calculation of offset distance must be changed to deal with the obstacle (the part of obstacle crossing the centerline of the lane）

Actual behavior

The calculated maximum offset distance decreases rather than increases in this case.
The planned path collides with large obstacle.

Steps to reproduce

1.Start autoware planning simulator using default;
2. Set initial position and gobal position;
3.Set a larger obstacle(like a truck) which intersects the lanes (part of the obstacle crosses the centerline of the lane)between initial position and gobal position.

Versions

1.OS: Ubuntun 20.04
2.ROS2: Galactic
3.Autoware: Autoware.universe:main

Possible causes

The calculation of offset distance can not deal with obstacle that partially cross the centerline. The calculated maximum offset distance decreases in this case.

Additional context

No response

[TakaHoribe]

Following the document, the avoidance path should be calculated considering the vehicle size. Needs to be fixed.

https://autowarefoundation.github.io/autoware.universe/main/planning/behavior_path_planner/behavior_path_planner_avoidance-design/

@zulfaqar-azmi-t4 Could you see this issue?

[taikitanaka3]

@shulanbushangshu cc: @TakaHoribe
I made rviz dummy bus publisher but I didn't see this issue.
#903

[shulanbushangshu]

@taikitanaka3
I have seen the #903. You can place the obstacle closer so that the part of obstacle can cross the center line.
Because the default parameter does not avoid obstacles within one meter (the distance from the center of the obstacle to the center line of the lane), I placed a large obstacle, and part of the obstacle crossed the center line.

[taikitanaka3]

@shulanbushangshu yes I could reproduce the problem thank you.

[taikitanaka3]

@zulfaqar-azmi-t4 @TakaHoribe
shift point should be at the right most of foot print vertax , not ego foot print center right?

but it looks fine with car

[shulanbushangshu]

I think there is something wrong with the calculation method of offset distance.
The maximum offset distance is calculated through the points on the shape of the obstacle. When some points cross the center line of the lane, the polarity of the calculated additional distance changes and the offset distance will not increase.

[taikitanaka3]

TO solve this

1. consider vehicle length
   fix(behavior_path_planner): fix ap point for long obeject #1041
   
2. calc avoid point and avoid return point from closest foot print or intersection of lanelet .
   fix(behavior_path_planner): add distance to returned point in avoidance module #915
   

[shulanbushangshu]

@taikitanaka3 @zulfaqar-azmi-t4
I think we can change the calculation method of horizontal distance. The current source code is as follows：

const auto max_shift_length =
o.to_road_shoulder_distance - road_shoulder_safety_margin - 0.5 * vehicle_width;
const auto max_left_shift_limit = &o, &max_allowable_lateral_distance, &max_shift_length,
this noexcept {
const auto left_shift_constraint = std::min(getLeftShiftBound(), max_shift_length);
return (o.to_road_shoulder_distance > max_allowable_lateral_distance) ? left_shift_constraint
: 0.0;
};

const auto max_right_shift_limit = [&o, &max_allowable_lateral_distance, &max_shift_length,
                                    this]() noexcept {
  const auto right_shift_constraint = std::max(getRightShiftBound(), -max_shift_length);
  return (o.to_road_shoulder_distance > max_allowable_lateral_distance) ? right_shift_constraint
                                                                        : 0.0;
};

const auto shift_length = isOnRight(o)
                            ? std::min(o.overhang_dist + avoid_margin, max_left_shift_limit())
                            : std::max(o.overhang_dist - avoid_margin, max_right_shift_limit());

[zulfaqar-azmi-t4]

@shulanbushangshu

The current source code has simplify the code slightly. But yes, do you have any idea's for the improvement?

[shulanbushangshu]

@zulfaqar-azmi-t4 @taikitanaka3
As shown in the following simulation diagram:

When the vehicle occupies the whole lane, the calculated maximum offset distance is too small.
This situation cannot be corrected by only compensating the longitudinal distance. Because the vehicle will still collide with obstacles at the maximum offset point.

[zulfaqar-azmi-t4]

@shulanbushangshu
I think i might know the root cause of this. basically yes, there is a bug in the current horizontal distance computation that didn't account for this kind of situation. Most probably I am able to fix them. Please wait for my PR sometime today or tomorrow.

[zulfaqar-azmi-t4]

@shulanbushangshu
The #1055 PR should solve the following issues. The original cause of this issue is due to the limitation bug when limiting the maximum shift. however, such limitation is not needed anymore, so I removed them. Looking forward to your assistance in confirming the solution.