16
16
17
17
#include " behavior_path_avoidance_module/data_structs.hpp"
18
18
#include " behavior_path_avoidance_module/utils.hpp"
19
+ #include " behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp"
19
20
#include " behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp"
20
21
#include " behavior_path_planner_common/utils/path_utils.hpp"
21
22
#include " behavior_path_planner_common/utils/traffic_light_utils.hpp"
@@ -816,8 +817,8 @@ bool isNoNeedAvoidanceBehavior(
816
817
return false ;
817
818
}
818
819
819
- const auto shift_length =
820
- calcShiftLength ( isOnRight (object), object.overhang_dist , object.avoid_margin .value ());
820
+ const auto shift_length = calcShiftLength (
821
+ isOnRight (object), object.overhang_points . front (). first , object.avoid_margin .value ());
821
822
if (!isShiftNecessary (isOnRight (object), shift_length)) {
822
823
object.reason = " NotNeedAvoidance" ;
823
824
return true ;
@@ -878,45 +879,41 @@ double getRoadShoulderDistance(
878
879
return 0.0 ;
879
880
}
880
881
881
- const auto centerline_pose =
882
- lanelet::utils::getClosestCenterPose (object.overhang_lanelet , object_pose.position );
883
- const auto & p1_object = object.overhang_pose .position ;
884
- const auto p_tmp =
885
- geometry_msgs::build<Pose>().position (p1_object).orientation (centerline_pose.orientation );
886
- const auto p2_object =
887
- calcOffsetPose (p_tmp, 0.0 , (isOnRight (object) ? 100.0 : -100.0 ), 0.0 ).position ;
888
-
889
- // TODO(Satoshi OTA): check if the basic point is on right or left of bound.
890
- const auto bound = isOnRight (object) ? data.left_bound : data.right_bound ;
891
-
892
- std::vector<Point > intersects;
893
- for (size_t i = 1 ; i < bound.size (); i++) {
894
- const auto p1_bound =
895
- geometry_msgs::build<Point >().x (bound[i - 1 ].x ()).y (bound[i - 1 ].y ()).z (bound[i - 1 ].z ());
896
- const auto p2_bound =
897
- geometry_msgs::build<Point >().x (bound[i].x ()).y (bound[i].y ()).z (bound[i].z ());
898
-
899
- const auto opt_intersect =
900
- tier4_autoware_utils::intersect (p1_object, p2_object, p1_bound, p2_bound);
901
-
902
- if (!opt_intersect) {
903
- continue ;
904
- }
882
+ std::vector<std::pair<Point , Point >> intersects;
883
+ for (const auto & p1 : object.overhang_points ) {
884
+ const auto centerline_pose =
885
+ lanelet::utils::getClosestCenterPose (object.overhang_lanelet , object_pose.position );
886
+ const auto p_tmp =
887
+ geometry_msgs::build<Pose>().position (p1.second ).orientation (centerline_pose.orientation );
888
+ const auto p2 = calcOffsetPose (p_tmp, 0.0 , (isOnRight (object) ? 100.0 : -100.0 ), 0.0 ).position ;
889
+
890
+ // TODO(Satoshi OTA): check if the basic point is on right or left of bound.
891
+ const auto bound = isOnRight (object) ? data.left_bound : data.right_bound ;
892
+
893
+ for (size_t i = 1 ; i < bound.size (); i++) {
894
+ const auto opt_intersect =
895
+ tier4_autoware_utils::intersect (p1.second , p2, bound.at (i - 1 ), bound.at (i));
905
896
906
- intersects.push_back (opt_intersect.value ());
897
+ if (!opt_intersect) {
898
+ continue ;
899
+ }
900
+
901
+ intersects.emplace_back (p1.second , opt_intersect.value ());
902
+ }
907
903
}
908
904
905
+ std::sort (intersects.begin (), intersects.end (), [](const auto & a, const auto & b) {
906
+ return calcDistance2d (a.first , a.second ) < calcDistance2d (b.first , b.second );
907
+ });
908
+
909
909
if (intersects.empty ()) {
910
910
return 0.0 ;
911
911
}
912
912
913
- std::sort (intersects.begin (), intersects.end (), [&p1_object](const auto & a, const auto & b) {
914
- return calcDistance2d (p1_object, a) < calcDistance2d (p1_object, b);
915
- });
916
-
917
- object.nearest_bound_point = intersects.front ();
913
+ object.narrowest_place = intersects.front ();
918
914
919
- return calcDistance2d (p1_object, object.nearest_bound_point .value ());
915
+ return calcDistance2d (
916
+ object.narrowest_place .value ().first , object.narrowest_place .value ().second );
920
917
}
921
918
} // namespace filtering_utils
922
919
@@ -1072,34 +1069,22 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint(
1072
1069
return ;
1073
1070
}
1074
1071
1075
- double calcEnvelopeOverhangDistance (
1076
- const ObjectData & object_data, const PathWithLaneId & path, Point & overhang_pose )
1072
+ std::vector<std::pair< double , Point >> calcEnvelopeOverhangDistance (
1073
+ const ObjectData & object_data, const PathWithLaneId & path)
1077
1074
{
1078
- double largest_overhang = isOnRight (object_data) ? - 100.0 : 100.0 ; // large number
1075
+ std::vector<std::pair< double , Point >> overhang_points{};
1079
1076
1080
1077
for (const auto & p : object_data.envelope_poly .outer ()) {
1081
1078
const auto point = tier4_autoware_utils::createPoint (p.x (), p.y (), 0.0 );
1082
1079
// TODO(someone): search around first position where the ego should avoid the object.
1083
1080
const auto idx = findNearestIndex (path.points , point);
1084
1081
const auto lateral = calcLateralDeviation (getPose (path.points .at (idx)), point);
1085
-
1086
- const auto & overhang_pose_on_right = [&overhang_pose, &largest_overhang, &point, &lateral]() {
1087
- if (lateral > largest_overhang) {
1088
- overhang_pose = point;
1089
- }
1090
- return std::max (largest_overhang, lateral);
1091
- };
1092
-
1093
- const auto & overhang_pose_on_left = [&overhang_pose, &largest_overhang, &point, &lateral]() {
1094
- if (lateral < largest_overhang) {
1095
- overhang_pose = point;
1096
- }
1097
- return std::min (largest_overhang, lateral);
1098
- };
1099
-
1100
- largest_overhang = isOnRight (object_data) ? overhang_pose_on_right () : overhang_pose_on_left ();
1082
+ overhang_points.emplace_back (lateral, point);
1101
1083
}
1102
- return largest_overhang;
1084
+ std::sort (overhang_points.begin (), overhang_points.end (), [&](const auto & a, const auto & b) {
1085
+ return isOnRight (object_data) ? b.first < a.first : a.first < b.first ;
1086
+ });
1087
+ return overhang_points;
1103
1088
}
1104
1089
1105
1090
void setEndData (
@@ -1182,24 +1167,21 @@ std::vector<DrivableAreaInfo::Obstacle> generateObstaclePolygonsForDrivableArea(
1182
1167
{
1183
1168
std::vector<DrivableAreaInfo::Obstacle> obstacles_for_drivable_area;
1184
1169
1185
- if (objects.empty () || !parameters-> enable_bound_clipping ) {
1170
+ if (objects.empty ()) {
1186
1171
return obstacles_for_drivable_area;
1187
1172
}
1188
1173
1189
1174
for (const auto & object : objects) {
1190
- // If avoidance is executed by both behavior and motion, only non-avoidable object will be
1191
- // extracted from the drivable area.
1192
- if (!parameters->disable_path_update ) {
1193
- if (object.is_avoidable ) {
1194
- continue ;
1195
- }
1196
- }
1197
-
1198
1175
// check if avoid marin is calculated
1199
1176
if (!object.avoid_margin .has_value ()) {
1200
1177
continue ;
1201
1178
}
1202
1179
1180
+ // check original polygon
1181
+ if (object.envelope_poly .outer ().empty ()) {
1182
+ continue ;
1183
+ }
1184
+
1203
1185
const auto object_type = utils::getHighestProbLabel (object.object .classification );
1204
1186
const auto object_parameter = parameters->object_parameters .at (object_type);
1205
1187
@@ -1447,10 +1429,10 @@ void fillAvoidanceNecessity(
1447
1429
0.5 * vehicle_width + object_parameter.safety_buffer_lateral * object_data.distance_factor ;
1448
1430
1449
1431
const auto check_necessity = [&](const auto hysteresis_factor) {
1450
- return (isOnRight (object_data) &&
1451
- std::abs (object_data. overhang_dist ) < safety_margin * hysteresis_factor) ||
1432
+ return (isOnRight (object_data) && std::abs (object_data. overhang_points . front (). first ) <
1433
+ safety_margin * hysteresis_factor) ||
1452
1434
(!isOnRight (object_data) &&
1453
- object_data.overhang_dist < safety_margin * hysteresis_factor);
1435
+ object_data.overhang_points . front (). first < safety_margin * hysteresis_factor);
1454
1436
};
1455
1437
1456
1438
const auto id = object_data.object .object_id ;
@@ -1613,6 +1595,40 @@ void compensateDetectionLost(
1613
1595
}
1614
1596
}
1615
1597
1598
+ void updateRoadShoulderDistance (
1599
+ AvoidancePlanningData & data, const std::shared_ptr<const PlannerData> & planner_data,
1600
+ const std::shared_ptr<AvoidanceParameters> & parameters)
1601
+ {
1602
+ ObjectDataArray clip_objects;
1603
+ std::for_each (data.other_objects .begin (), data.other_objects .end (), [&](const auto & object) {
1604
+ if (!filtering_utils::isMovingObject (object, parameters)) {
1605
+ clip_objects.push_back (object);
1606
+ }
1607
+ });
1608
+ for (auto & o : clip_objects) {
1609
+ const auto & vehicle_width = planner_data->parameters .vehicle_width ;
1610
+ const auto object_type = utils::getHighestProbLabel (o.object .classification );
1611
+ const auto object_parameter = parameters->object_parameters .at (object_type);
1612
+
1613
+ o.avoid_margin = object_parameter.safety_buffer_lateral + 0.5 * vehicle_width;
1614
+ }
1615
+ const auto extract_obstacles = generateObstaclePolygonsForDrivableArea (
1616
+ clip_objects, parameters, planner_data->parameters .vehicle_width / 2.0 );
1617
+
1618
+ auto tmp_path = data.reference_path ;
1619
+ tmp_path.left_bound = data.left_bound ;
1620
+ tmp_path.right_bound = data.right_bound ;
1621
+ utils::extractObstaclesFromDrivableArea (tmp_path, extract_obstacles);
1622
+
1623
+ data.left_bound = tmp_path.left_bound ;
1624
+ data.right_bound = tmp_path.right_bound ;
1625
+
1626
+ for (auto & o : data.target_objects ) {
1627
+ o.to_road_shoulder_distance = filtering_utils::getRoadShoulderDistance (o, data, planner_data);
1628
+ o.avoid_margin = filtering_utils::getAvoidMargin (o, planner_data, parameters);
1629
+ }
1630
+ }
1631
+
1616
1632
void filterTargetObjects (
1617
1633
ObjectDataArray & objects, AvoidancePlanningData & data, const double forward_detection_range,
1618
1634
const std::shared_ptr<const PlannerData> & planner_data,
@@ -1636,8 +1652,7 @@ void filterTargetObjects(
1636
1652
}
1637
1653
1638
1654
// Find the footprint point closest to the path, set to object_data.overhang_distance.
1639
- o.overhang_dist =
1640
- calcEnvelopeOverhangDistance (o, data.reference_path , o.overhang_pose .position );
1655
+ o.overhang_points = utils::avoidance::calcEnvelopeOverhangDistance (o, data.reference_path );
1641
1656
o.to_road_shoulder_distance = filtering_utils::getRoadShoulderDistance (o, data, planner_data);
1642
1657
o.avoid_margin = filtering_utils::getAvoidMargin (o, planner_data, parameters);
1643
1658
@@ -1844,7 +1859,10 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
1844
1859
PredictedObjects ret{};
1845
1860
std::for_each (objects.begin (), objects.end (), [&p, &ret, ¶meters](const auto & object) {
1846
1861
if (filtering_utils::isSafetyCheckTargetObjectType (object.object , parameters)) {
1847
- ret.objects .push_back (object.object );
1862
+ // check only moving objects
1863
+ if (filtering_utils::isMovingObject (object, parameters)) {
1864
+ ret.objects .push_back (object.object );
1865
+ }
1848
1866
}
1849
1867
});
1850
1868
return ret;
0 commit comments