fix(avoidance): fix invalid road bound distance calculation

[satoshi-ota]

Background

The avoidance module calculates distance to drivable bound by using tier4_autoware_utils::intersection with following step.

1. check object offset direction based on current path. (fig: on left)
2. calculate overhang point (=p1). (fig: black point)
3. calculate offset point (=p2) based on overhang points by using tier4_autoware_utils::calcOffsetPose.
4. calculate intersect point (=p3) between line p1-p2 and drivable bound.
5. calculate distance (=D) p1 to p3.

In most case (fig: nominal case), we can find intersect by following logic. But, in edge case, we can't.

        const auto p2 =
          calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? 100.0 : -100.0), 0.0).position;
        const auto opt_intersect =
          tier4_autoware_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i));

Then, there is following logic as well to calculate D'.

        const auto p2 =
          calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? -100.0 : 100.0), 0.0).position;
        const auto opt_intersect =
          tier4_autoware_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i));

Description

BUG: The module couldn't calculate distance to drivable bound properly. As a result, the module didn't create avoidance path even when there was enough space to avoid.

This issue caused by unexpected intersetion point calculation in following logic. And, I decreased the hard coded value to 1m in order not to get unexpected intersection point.

        const auto p2 =
          calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? -100.0 : 100.0), 0.0).position;
        const auto opt_intersect =
          tier4_autoware_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i));

Tests performed

simplescreenrecorder-2024-04-09_16.34.49.mp4

• PASS TIER IV INTERNAL SCENARIOS

Effects on system behavior

Fix bug.

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!!

My note

• If there are several intersected points, the closer one is chosen.

  autoware.universe/planning/behavior_path_avoidance_module/src/utils.cpp

  Lines 889 to 891 in 0f4cb74

  	std::sort(intersects.begin(), intersects.end(), [](const auto & a, const auto & b) {
  	return std::get<0>(a) < std::get<0>(b);
  	});

• I understand that the symmetrical lanelet, obstacle position would work as we expected.