Skip to content

Commit 7cafb35

Browse files
committedFeb 8, 2024·
feat(avoidance): consider objects on shift side lane (autowarefoundation#6252)
* fix(avoidance): check safety for only moving objects Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * feat(avoidance): consider ignore object in avoid margin calculation Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(avoidance): use nearest overhang point Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> --------- Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

File tree

8 files changed

+138
-119
lines changed

8 files changed

+138
-119
lines changed
 

‎planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

+6-16
Original file line numberDiff line numberDiff line change
@@ -30,16 +30,6 @@
3030

3131
namespace behavior_path_planner
3232
{
33-
namespace
34-
{
35-
lanelet::BasicLineString3d toLineString3d(const std::vector<Point> & bound)
36-
{
37-
lanelet::BasicLineString3d ret{};
38-
std::for_each(
39-
bound.begin(), bound.end(), [&](const auto & p) { ret.emplace_back(p.x, p.y, p.z); });
40-
return ret;
41-
}
42-
} // namespace
4333
AvoidanceByLaneChange::AvoidanceByLaneChange(
4434
const std::shared_ptr<LaneChangeParameters> & parameters,
4535
std::shared_ptr<AvoidanceByLCParameters> avoidance_parameters)
@@ -172,10 +162,10 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData(
172162
// calc drivable bound
173163
const auto shorten_lanes =
174164
utils::cutOverlappedLanes(data.reference_path_rough, data.drivable_lanes);
175-
data.left_bound = toLineString3d(utils::calcBound(
176-
data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, true));
177-
data.right_bound = toLineString3d(utils::calcBound(
178-
data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, false));
165+
data.left_bound = utils::calcBound(
166+
data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, true);
167+
data.right_bound = utils::calcBound(
168+
data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, false);
179169

180170
// get related objects from dynamic_objects, and then separates them as target objects and non
181171
// target objects
@@ -275,8 +265,8 @@ ObjectData AvoidanceByLaneChange::createObjectData(
275265
: Direction::RIGHT;
276266

277267
// Find the footprint point closest to the path, set to object_data.overhang_distance.
278-
object_data.overhang_dist = utils::avoidance::calcEnvelopeOverhangDistance(
279-
object_data, data.reference_path, object_data.overhang_pose.position);
268+
object_data.overhang_points =
269+
utils::avoidance::calcEnvelopeOverhangDistance(object_data, data.reference_path);
280270

281271
// Check whether the the ego should avoid the object.
282272
const auto & vehicle_width = planner_data_->parameters.vehicle_width;

‎planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

+8-15
Original file line numberDiff line numberDiff line change
@@ -336,12 +336,8 @@ struct ObjectData // avoidance target
336336
{
337337
ObjectData() = default;
338338

339-
ObjectData(PredictedObject obj, double lat, double lon, double len, double overhang)
340-
: object(std::move(obj)),
341-
to_centerline(lat),
342-
longitudinal(lon),
343-
length(len),
344-
overhang_dist(overhang)
339+
ObjectData(PredictedObject obj, double lat, double lon, double len)
340+
: object(std::move(obj)), to_centerline(lat), longitudinal(lon), length(len)
345341
{
346342
}
347343

@@ -365,9 +361,6 @@ struct ObjectData // avoidance target
365361
// longitudinal length of vehicle, in Frenet coordinate
366362
double length{0.0};
367363

368-
// lateral distance to the closest footprint, in Frenet coordinate
369-
double overhang_dist{0.0};
370-
371364
// lateral shiftable ratio
372365
double shiftable_ratio{0.0};
373366

@@ -392,9 +385,6 @@ struct ObjectData // avoidance target
392385
// the position at the detected moment
393386
Pose init_pose;
394387

395-
// the position of the overhang
396-
Pose overhang_pose;
397-
398388
// envelope polygon
399389
Polygon2d envelope_poly{};
400390

@@ -425,14 +415,17 @@ struct ObjectData // avoidance target
425415
// object direction.
426416
Direction direction{Direction::NONE};
427417

418+
// overhang points (sort by distance)
419+
std::vector<std::pair<double, Point>> overhang_points{};
420+
428421
// unavoidable reason
429422
std::string reason{};
430423

431424
// lateral avoid margin
432425
std::optional<double> avoid_margin{std::nullopt};
433426

434427
// the nearest bound point (use in road shoulder distance calculation)
435-
std::optional<Point> nearest_bound_point{std::nullopt};
428+
std::optional<std::pair<Point, Point>> narrowest_place{std::nullopt};
436429
};
437430
using ObjectDataArray = std::vector<ObjectData>;
438431

@@ -541,9 +534,9 @@ struct AvoidancePlanningData
541534

542535
std::vector<DrivableLanes> drivable_lanes{};
543536

544-
lanelet::BasicLineString3d right_bound{};
537+
std::vector<Point> right_bound{};
545538

546-
lanelet::BasicLineString3d left_bound{};
539+
std::vector<Point> left_bound{};
547540

548541
bool safe{false};
549542

‎planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -176,7 +176,8 @@ class AvoidanceHelper
176176
{
177177
using utils::avoidance::calcShiftLength;
178178

179-
const auto shift_length = calcShiftLength(is_on_right, object.overhang_dist, margin);
179+
const auto shift_length =
180+
calcShiftLength(is_on_right, object.overhang_points.front().first, margin);
180181
return is_on_right ? std::min(shift_length, getLeftShiftBound())
181182
: std::max(shift_length, getRightShiftBound());
182183
}

‎planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp

+6-2
Original file line numberDiff line numberDiff line change
@@ -56,8 +56,8 @@ double lerpShiftLengthOnArc(double arc, const AvoidLine & al);
5656
void fillLongitudinalAndLengthByClosestEnvelopeFootprint(
5757
const PathWithLaneId & path, const Point & ego_pos, ObjectData & obj);
5858

59-
double calcEnvelopeOverhangDistance(
60-
const ObjectData & object_data, const PathWithLaneId & path, Point & overhang_pose);
59+
std::vector<std::pair<double, Point>> calcEnvelopeOverhangDistance(
60+
const ObjectData & object_data, const PathWithLaneId & path);
6161

6262
void setEndData(
6363
AvoidLine & al, const double length, const geometry_msgs::msg::Pose & end, const size_t end_idx,
@@ -127,6 +127,10 @@ void filterTargetObjects(
127127
const std::shared_ptr<const PlannerData> & planner_data,
128128
const std::shared_ptr<AvoidanceParameters> & parameters);
129129

130+
void updateRoadShoulderDistance(
131+
AvoidancePlanningData & data, const std::shared_ptr<const PlannerData> & planner_data,
132+
const std::shared_ptr<AvoidanceParameters> & parameters);
133+
130134
void fillAdditionalInfoFromPoint(const AvoidancePlanningData & data, AvoidLineArray & lines);
131135

132136
void fillAdditionalInfoFromLongitudinal(const AvoidancePlanningData & data, AvoidLine & line);

‎planning/behavior_path_avoidance_module/src/debug.cpp

+6-6
Original file line numberDiff line numberDiff line change
@@ -113,7 +113,7 @@ MarkerArray createToDrivableBoundDistance(const ObjectDataArray & objects, std::
113113
MarkerArray msg;
114114

115115
for (const auto & object : objects) {
116-
if (!object.nearest_bound_point.has_value()) {
116+
if (!object.narrowest_place.has_value()) {
117117
continue;
118118
}
119119

@@ -122,8 +122,8 @@ MarkerArray createToDrivableBoundDistance(const ObjectDataArray & objects, std::
122122
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::LINE_STRIP,
123123
createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 0.0, 0.42, 0.999));
124124

125-
marker.points.push_back(object.overhang_pose.position);
126-
marker.points.push_back(object.nearest_bound_point.value());
125+
marker.points.push_back(object.narrowest_place.value().first);
126+
marker.points.push_back(object.narrowest_place.value().second);
127127
marker.id = uuidToInt32(object.object.object_id);
128128
msg.markers.push_back(marker);
129129
}
@@ -133,7 +133,7 @@ MarkerArray createToDrivableBoundDistance(const ObjectDataArray & objects, std::
133133
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING,
134134
createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(1.0, 1.0, 0.0, 1.0));
135135

136-
marker.pose.position = object.nearest_bound_point.value();
136+
marker.pose.position = object.narrowest_place.value().second;
137137
std::ostringstream string_stream;
138138
string_stream << object.to_road_shoulder_distance << "[m]";
139139
marker.text = string_stream.str();
@@ -469,7 +469,7 @@ MarkerArray createDrivableBounds(
469469
createMarkerColor(r, g, b, 0.999));
470470

471471
for (const auto & p : data.right_bound) {
472-
marker.points.push_back(createPoint(p.x(), p.y(), p.z()));
472+
marker.points.push_back(p);
473473
}
474474

475475
msg.markers.push_back(marker);
@@ -482,7 +482,7 @@ MarkerArray createDrivableBounds(
482482
createMarkerColor(r, g, b, 0.999));
483483

484484
for (const auto & p : data.left_bound) {
485-
marker.points.push_back(createPoint(p.x(), p.y(), p.z()));
485+
marker.points.push_back(p);
486486
}
487487

488488
msg.markers.push_back(marker);

‎planning/behavior_path_avoidance_module/src/scene.cpp

+23-10
Original file line numberDiff line numberDiff line change
@@ -234,14 +234,14 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat
234234
// calc drivable bound
235235
auto tmp_path = getPreviousModuleOutput().path;
236236
const auto shorten_lanes = utils::cutOverlappedLanes(tmp_path, data.drivable_lanes);
237-
data.left_bound = toLineString3d(utils::calcBound(
237+
data.left_bound = utils::calcBound(
238238
getPreviousModuleOutput().path, planner_data_, shorten_lanes,
239239
parameters_->use_hatched_road_markings, parameters_->use_intersection_areas,
240-
parameters_->use_freespace_areas, true));
241-
data.right_bound = toLineString3d(utils::calcBound(
240+
parameters_->use_freespace_areas, true);
241+
data.right_bound = utils::calcBound(
242242
getPreviousModuleOutput().path, planner_data_, shorten_lanes,
243243
parameters_->use_hatched_road_markings, parameters_->use_intersection_areas,
244-
parameters_->use_freespace_areas, false));
244+
parameters_->use_freespace_areas, false);
245245

246246
// reference path
247247
if (isDrivingSameLane(helper_->getPreviousDrivingLanes(), data.current_lanelets)) {
@@ -294,6 +294,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects(
294294
using utils::avoidance::filterTargetObjects;
295295
using utils::avoidance::getTargetLanelets;
296296
using utils::avoidance::separateObjectsByPath;
297+
using utils::avoidance::updateRoadShoulderDistance;
297298

298299
// Separate dynamic objects based on whether they are inside or outside of the expanded lanelets.
299300
constexpr double MARGIN = 10.0;
@@ -316,6 +317,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects(
316317

317318
// Filter out the objects to determine the ones to be avoided.
318319
filterTargetObjects(objects, data, forward_detection_range, planner_data_, parameters_);
320+
updateRoadShoulderDistance(data, planner_data_, parameters_);
319321

320322
// Calculate the distance needed to safely decelerate the ego vehicle to a stop line.
321323
const auto & vehicle_width = planner_data_->parameters.vehicle_width;
@@ -930,10 +932,6 @@ BehaviorModuleOutput AvoidanceModule::plan()
930932
DrivableAreaInfo current_drivable_area_info;
931933
// generate drivable lanes
932934
current_drivable_area_info.drivable_lanes = avoid_data_.drivable_lanes;
933-
// generate obstacle polygons
934-
current_drivable_area_info.obstacles =
935-
utils::avoidance::generateObstaclePolygonsForDrivableArea(
936-
avoid_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0);
937935
// expand hatched road markings
938936
current_drivable_area_info.enable_expanding_hatched_road_markings =
939937
parameters_->use_hatched_road_markings;
@@ -942,6 +940,21 @@ BehaviorModuleOutput AvoidanceModule::plan()
942940
parameters_->use_intersection_areas;
943941
// expand freespace areas
944942
current_drivable_area_info.enable_expanding_freespace_areas = parameters_->use_freespace_areas;
943+
// generate obstacle polygons
944+
if (parameters_->enable_bound_clipping) {
945+
ObjectDataArray clip_objects;
946+
// If avoidance is executed by both behavior and motion, only non-avoidable object will be
947+
// extracted from the drivable area.
948+
std::for_each(
949+
data.target_objects.begin(), data.target_objects.end(), [&](const auto & object) {
950+
if (!object.is_avoidable) clip_objects.push_back(object);
951+
});
952+
current_drivable_area_info.obstacles =
953+
utils::avoidance::generateObstaclePolygonsForDrivableArea(
954+
clip_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0);
955+
} else {
956+
current_drivable_area_info.obstacles.clear();
957+
}
945958

946959
output.drivable_area_info = utils::combineDrivableAreaInfo(
947960
current_drivable_area_info, getPreviousModuleOutput().drivable_area_info);
@@ -1151,8 +1164,8 @@ bool AvoidanceModule::isValidShiftLine(
11511164
const size_t end_idx = shift_lines.back().end_idx;
11521165

11531166
const auto path = shifter_for_validate.getReferencePath();
1154-
const auto left_bound = lanelet::utils::to2D(avoid_data_.left_bound);
1155-
const auto right_bound = lanelet::utils::to2D(avoid_data_.right_bound);
1167+
const auto left_bound = lanelet::utils::to2D(toLineString3d(avoid_data_.left_bound));
1168+
const auto right_bound = lanelet::utils::to2D(toLineString3d(avoid_data_.right_bound));
11561169
for (size_t i = start_idx; i <= end_idx; ++i) {
11571170
const auto p = getPoint(path.points.at(i));
11581171
lanelet::BasicPoint2d basic_point{p.x, p.y};

‎planning/behavior_path_avoidance_module/src/shift_line_generator.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -235,7 +235,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
235235
const double LAT_DIST_BUFFER = desire_shift_length > 0.0 ? 1e-3 : -1e-3;
236236

237237
const auto infeasible =
238-
std::abs(feasible_shift_length - object.overhang_dist) - LAT_DIST_BUFFER <
238+
std::abs(feasible_shift_length - object.overhang_points.front().first) - LAT_DIST_BUFFER <
239239
0.5 * data_->parameters.vehicle_width + object_parameter.safety_buffer_lateral;
240240
if (infeasible) {
241241
RCLCPP_DEBUG(rclcpp::get_logger(""), "feasible shift length is not enough to avoid. ");

‎planning/behavior_path_avoidance_module/src/utils.cpp

+86-68
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@
1616

1717
#include "behavior_path_avoidance_module/data_structs.hpp"
1818
#include "behavior_path_avoidance_module/utils.hpp"
19+
#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp"
1920
#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp"
2021
#include "behavior_path_planner_common/utils/path_utils.hpp"
2122
#include "behavior_path_planner_common/utils/traffic_light_utils.hpp"
@@ -816,8 +817,8 @@ bool isNoNeedAvoidanceBehavior(
816817
return false;
817818
}
818819

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());
821822
if (!isShiftNecessary(isOnRight(object), shift_length)) {
822823
object.reason = "NotNeedAvoidance";
823824
return true;
@@ -878,45 +879,41 @@ double getRoadShoulderDistance(
878879
return 0.0;
879880
}
880881

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));
905896

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+
}
907903
}
908904

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+
909909
if (intersects.empty()) {
910910
return 0.0;
911911
}
912912

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();
918914

919-
return calcDistance2d(p1_object, object.nearest_bound_point.value());
915+
return calcDistance2d(
916+
object.narrowest_place.value().first, object.narrowest_place.value().second);
920917
}
921918
} // namespace filtering_utils
922919

@@ -1072,34 +1069,22 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint(
10721069
return;
10731070
}
10741071

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)
10771074
{
1078-
double largest_overhang = isOnRight(object_data) ? -100.0 : 100.0; // large number
1075+
std::vector<std::pair<double, Point>> overhang_points{};
10791076

10801077
for (const auto & p : object_data.envelope_poly.outer()) {
10811078
const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0);
10821079
// TODO(someone): search around first position where the ego should avoid the object.
10831080
const auto idx = findNearestIndex(path.points, point);
10841081
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);
11011083
}
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;
11031088
}
11041089

11051090
void setEndData(
@@ -1182,24 +1167,21 @@ std::vector<DrivableAreaInfo::Obstacle> generateObstaclePolygonsForDrivableArea(
11821167
{
11831168
std::vector<DrivableAreaInfo::Obstacle> obstacles_for_drivable_area;
11841169

1185-
if (objects.empty() || !parameters->enable_bound_clipping) {
1170+
if (objects.empty()) {
11861171
return obstacles_for_drivable_area;
11871172
}
11881173

11891174
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-
11981175
// check if avoid marin is calculated
11991176
if (!object.avoid_margin.has_value()) {
12001177
continue;
12011178
}
12021179

1180+
// check original polygon
1181+
if (object.envelope_poly.outer().empty()) {
1182+
continue;
1183+
}
1184+
12031185
const auto object_type = utils::getHighestProbLabel(object.object.classification);
12041186
const auto object_parameter = parameters->object_parameters.at(object_type);
12051187

@@ -1447,10 +1429,10 @@ void fillAvoidanceNecessity(
14471429
0.5 * vehicle_width + object_parameter.safety_buffer_lateral * object_data.distance_factor;
14481430

14491431
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) ||
14521434
(!isOnRight(object_data) &&
1453-
object_data.overhang_dist < safety_margin * hysteresis_factor);
1435+
object_data.overhang_points.front().first < safety_margin * hysteresis_factor);
14541436
};
14551437

14561438
const auto id = object_data.object.object_id;
@@ -1613,6 +1595,40 @@ void compensateDetectionLost(
16131595
}
16141596
}
16151597

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+
16161632
void filterTargetObjects(
16171633
ObjectDataArray & objects, AvoidancePlanningData & data, const double forward_detection_range,
16181634
const std::shared_ptr<const PlannerData> & planner_data,
@@ -1636,8 +1652,7 @@ void filterTargetObjects(
16361652
}
16371653

16381654
// 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);
16411656
o.to_road_shoulder_distance = filtering_utils::getRoadShoulderDistance(o, data, planner_data);
16421657
o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters);
16431658

@@ -1844,7 +1859,10 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
18441859
PredictedObjects ret{};
18451860
std::for_each(objects.begin(), objects.end(), [&p, &ret, &parameters](const auto & object) {
18461861
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+
}
18481866
}
18491867
});
18501868
return ret;

0 commit comments

Comments
 (0)
Please sign in to comment.