Skip to content

Commit 6acafbc

Browse files
committed
fix(avoidance): use nearest overhang point
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent b14cf2d commit 6acafbc

File tree

7 files changed

+55
-73
lines changed

7 files changed

+55
-73
lines changed

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -265,8 +265,8 @@ ObjectData AvoidanceByLaneChange::createObjectData(
265265
: Direction::RIGHT;
266266

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

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

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

+6-13
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

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

+2-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,

planning/behavior_path_avoidance_module/src/debug.cpp

+4-4
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();

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

+38-50
Original file line numberDiff line numberDiff line change
@@ -823,8 +823,8 @@ bool isNoNeedAvoidanceBehavior(
823823
return false;
824824
}
825825

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());
828828
if (!isShiftNecessary(isOnRight(object), shift_length)) {
829829
object.reason = "NotNeedAvoidance";
830830
return true;
@@ -885,40 +885,41 @@ double getRoadShoulderDistance(
885885
return 0.0;
886886
}
887887

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;
895895

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;
898898

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

904-
if (!opt_intersect) {
905-
continue;
906-
}
903+
if (!opt_intersect) {
904+
continue;
905+
}
907906

908-
intersects.push_back(opt_intersect.value());
907+
intersects.emplace_back(p1.second, opt_intersect.value());
908+
}
909909
}
910910

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+
911915
if (intersects.empty()) {
912916
return 0.0;
913917
}
914918

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

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);
922923
}
923924
} // namespace filtering_utils
924925

@@ -1074,34 +1075,22 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint(
10741075
return;
10751076
}
10761077

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)
10791080
{
1080-
double largest_overhang = isOnRight(object_data) ? -100.0 : 100.0; // large number
1081+
std::vector<std::pair<double, Point>> overhang_points{};
10811082

10821083
for (const auto & p : object_data.envelope_poly.outer()) {
10831084
const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0);
10841085
// TODO(someone): search around first position where the ego should avoid the object.
10851086
const auto idx = findNearestIndex(path.points, point);
10861087
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);
11031089
}
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;
11051094
}
11061095

11071096
void setEndData(
@@ -1446,10 +1435,10 @@ void fillAvoidanceNecessity(
14461435
0.5 * vehicle_width + object_parameter.safety_buffer_lateral * object_data.distance_factor;
14471436

14481437
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) ||
14511440
(!isOnRight(object_data) &&
1452-
object_data.overhang_dist < safety_margin * hysteresis_factor);
1441+
object_data.overhang_points.front().first < safety_margin * hysteresis_factor);
14531442
};
14541443

14551444
const auto id = object_data.object.object_id;
@@ -1669,8 +1658,7 @@ void filterTargetObjects(
16691658
}
16701659

16711660
// 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);
16741662
o.to_road_shoulder_distance = filtering_utils::getRoadShoulderDistance(o, data, planner_data);
16751663
o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters);
16761664

0 commit comments

Comments
 (0)