fix(avoidance): unexpected road shoulder distance calculation result

[satoshi-ota]

Description

TIER IV INTERNAL LINK

Related PR: #6781

The module searches most overhanging point by following steps:

1. pick a point from envelope polygon contour (P1).
2. create new point from P1 by using calcOffsetPose (P2).
3. search intersection point between P1-P2 and drivable bound.
4. calculate distance between intersection point and P1.
5. apply step1~4 for all polygon points.
6. sort point by distance calculated in step4.
7. pick the point whose distance is smallest from sorted points as most overhanging point.

const auto p2 = calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? -1.0 : 1.0), 0.0).position;

Previously, the point (P2) is calculated with constant offset value. However, I found sometimes it couldn't find intersection point for some polygon points when the distance between P1 and drivable bound was larger than 1.0m.

In following case, it couldn't find intersection point for bottom left/right points on white polygon. As a result, it got wrong point as the most overhanging point and created infeasible path.

In this PR, I fixed the logic for step2 to decide offset value based on polygon width so that it always can find intersection point for all polygon points.

const auto envelope_polygon_width =
        boost::geometry::area(object.envelope_poly) / object.length;

      {
        const auto p2 =
          calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? -0.5 : 0.5) * envelope_polygon_width, 0.0)
            .position;
...

Tests performed

Overhang distance is calculated expectedly. And it doesn't create infeasible path.

Effects on system behavior

Nothing.

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.

After all checkboxes are checked, anyone who has write access can merge the PR.

[shmpwk]

LGTM