@@ -823,8 +823,8 @@ bool isNoNeedAvoidanceBehavior(
823
823
return false ;
824
824
}
825
825
826
- const auto shift_length =
827
- calcShiftLength ( isOnRight (object), object.overhang_dist , object.avoid_margin .value ());
826
+ const auto shift_length = calcShiftLength (
827
+ isOnRight (object), object.overhang_points . front (). first , object.avoid_margin .value ());
828
828
if (!isShiftNecessary (isOnRight (object), shift_length)) {
829
829
object.reason = " NotNeedAvoidance" ;
830
830
return true ;
@@ -885,40 +885,41 @@ double getRoadShoulderDistance(
885
885
return 0.0 ;
886
886
}
887
887
888
- const auto centerline_pose =
889
- lanelet::utils::getClosestCenterPose ( object.overhang_lanelet , object_pose. position );
890
- const auto & p1_object = object. overhang_pose . position ;
891
- const auto p_tmp =
892
- geometry_msgs::build<Pose>(). position (p1_object). orientation (centerline_pose. orientation );
893
- const auto p2_object =
894
- calcOffsetPose (p_tmp, 0.0 , (isOnRight (object) ? 100.0 : -100.0 ), 0.0 ).position ;
888
+ std::vector<std::pair< Point , Point >> intersects;
889
+ for ( const auto & p1 : object.overhang_points ) {
890
+ const auto centerline_pose =
891
+ lanelet::utils::getClosestCenterPose (object. overhang_lanelet , object_pose. position );
892
+ const auto p_tmp =
893
+ geometry_msgs::build<Pose>(). position (p1. second ). orientation (centerline_pose. orientation );
894
+ const auto p2 = calcOffsetPose (p_tmp, 0.0 , (isOnRight (object) ? 100.0 : -100.0 ), 0.0 ).position ;
895
895
896
- // TODO(Satoshi OTA): check if the basic point is on right or left of bound.
897
- const auto bound = isOnRight (object) ? data.left_bound : data.right_bound ;
896
+ // TODO(Satoshi OTA): check if the basic point is on right or left of bound.
897
+ const auto bound = isOnRight (object) ? data.left_bound : data.right_bound ;
898
898
899
- std::vector<Point > intersects;
900
- for (size_t i = 1 ; i < bound.size (); i++) {
901
- const auto opt_intersect =
902
- tier4_autoware_utils::intersect (p1_object, p2_object, bound.at (i - 1 ), bound.at (i));
899
+ for (size_t i = 1 ; i < bound.size (); i++) {
900
+ const auto opt_intersect =
901
+ tier4_autoware_utils::intersect (p1.second , p2, bound.at (i - 1 ), bound.at (i));
903
902
904
- if (!opt_intersect) {
905
- continue ;
906
- }
903
+ if (!opt_intersect) {
904
+ continue ;
905
+ }
907
906
908
- intersects.push_back (opt_intersect.value ());
907
+ intersects.emplace_back (p1.second , opt_intersect.value ());
908
+ }
909
909
}
910
910
911
+ std::sort (intersects.begin (), intersects.end (), [](const auto & a, const auto & b) {
912
+ return calcDistance2d (a.first , a.second ) < calcDistance2d (b.first , b.second );
913
+ });
914
+
911
915
if (intersects.empty ()) {
912
916
return 0.0 ;
913
917
}
914
918
915
- std::sort (intersects.begin (), intersects.end (), [&p1_object](const auto & a, const auto & b) {
916
- return calcDistance2d (p1_object, a) < calcDistance2d (p1_object, b);
917
- });
919
+ object.narrowest_place = intersects.front ();
918
920
919
- object.nearest_bound_point = intersects.front ();
920
-
921
- return calcDistance2d (p1_object, object.nearest_bound_point .value ());
921
+ return calcDistance2d (
922
+ object.narrowest_place .value ().first , object.narrowest_place .value ().second );
922
923
}
923
924
} // namespace filtering_utils
924
925
@@ -1074,34 +1075,22 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint(
1074
1075
return ;
1075
1076
}
1076
1077
1077
- double calcEnvelopeOverhangDistance (
1078
- const ObjectData & object_data, const PathWithLaneId & path, Point & overhang_pose )
1078
+ std::vector<std::pair< double , Point >> calcEnvelopeOverhangDistance (
1079
+ const ObjectData & object_data, const PathWithLaneId & path)
1079
1080
{
1080
- double largest_overhang = isOnRight (object_data) ? - 100.0 : 100.0 ; // large number
1081
+ std::vector<std::pair< double , Point >> overhang_points{};
1081
1082
1082
1083
for (const auto & p : object_data.envelope_poly .outer ()) {
1083
1084
const auto point = tier4_autoware_utils::createPoint (p.x (), p.y (), 0.0 );
1084
1085
// TODO(someone): search around first position where the ego should avoid the object.
1085
1086
const auto idx = findNearestIndex (path.points , point);
1086
1087
const auto lateral = calcLateralDeviation (getPose (path.points .at (idx)), point);
1087
-
1088
- const auto & overhang_pose_on_right = [&overhang_pose, &largest_overhang, &point, &lateral]() {
1089
- if (lateral > largest_overhang) {
1090
- overhang_pose = point;
1091
- }
1092
- return std::max (largest_overhang, lateral);
1093
- };
1094
-
1095
- const auto & overhang_pose_on_left = [&overhang_pose, &largest_overhang, &point, &lateral]() {
1096
- if (lateral < largest_overhang) {
1097
- overhang_pose = point;
1098
- }
1099
- return std::min (largest_overhang, lateral);
1100
- };
1101
-
1102
- largest_overhang = isOnRight (object_data) ? overhang_pose_on_right () : overhang_pose_on_left ();
1088
+ overhang_points.emplace_back (lateral, point);
1103
1089
}
1104
- return largest_overhang;
1090
+ std::sort (overhang_points.begin (), overhang_points.end (), [&](const auto & a, const auto & b) {
1091
+ return isOnRight (object_data) ? b.first < a.first : a.first < b.first ;
1092
+ });
1093
+ return overhang_points;
1105
1094
}
1106
1095
1107
1096
void setEndData (
@@ -1446,10 +1435,10 @@ void fillAvoidanceNecessity(
1446
1435
0.5 * vehicle_width + object_parameter.safety_buffer_lateral * object_data.distance_factor ;
1447
1436
1448
1437
const auto check_necessity = [&](const auto hysteresis_factor) {
1449
- return (isOnRight (object_data) &&
1450
- std::abs (object_data. overhang_dist ) < safety_margin * hysteresis_factor) ||
1438
+ return (isOnRight (object_data) && std::abs (object_data. overhang_points . front (). first ) <
1439
+ safety_margin * hysteresis_factor) ||
1451
1440
(!isOnRight (object_data) &&
1452
- object_data.overhang_dist < safety_margin * hysteresis_factor);
1441
+ object_data.overhang_points . front (). first < safety_margin * hysteresis_factor);
1453
1442
};
1454
1443
1455
1444
const auto id = object_data.object .object_id ;
@@ -1669,8 +1658,7 @@ void filterTargetObjects(
1669
1658
}
1670
1659
1671
1660
// Find the footprint point closest to the path, set to object_data.overhang_distance.
1672
- o.overhang_dist =
1673
- calcEnvelopeOverhangDistance (o, data.reference_path , o.overhang_pose .position );
1661
+ o.overhang_points = utils::avoidance::calcEnvelopeOverhangDistance (o, data.reference_path );
1674
1662
o.to_road_shoulder_distance = filtering_utils::getRoadShoulderDistance (o, data, planner_data);
1675
1663
o.avoid_margin = filtering_utils::getAvoidMargin (o, planner_data, parameters);
1676
1664
0 commit comments