Skip to content

Commit

Permalink
fix(avoidance): use nearest overhang point
Browse files Browse the repository at this point in the history
Signed-off-by: satoshi-ota <[email protected]>
  • Loading branch information
satoshi-ota committed Feb 7, 2024
1 parent b14cf2d commit 6acafbc
Show file tree
Hide file tree
Showing 7 changed files with 55 additions and 73 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -265,8 +265,8 @@ ObjectData AvoidanceByLaneChange::createObjectData(
: Direction::RIGHT;

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

// Check whether the the ego should avoid the object.
const auto & vehicle_width = planner_data_->parameters.vehicle_width;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -336,12 +336,8 @@ struct ObjectData // avoidance target
{
ObjectData() = default;

ObjectData(PredictedObject obj, double lat, double lon, double len, double overhang)
: object(std::move(obj)),
to_centerline(lat),
longitudinal(lon),
length(len),
overhang_dist(overhang)
ObjectData(PredictedObject obj, double lat, double lon, double len)
: object(std::move(obj)), to_centerline(lat), longitudinal(lon), length(len)
{
}

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

// lateral distance to the closest footprint, in Frenet coordinate
double overhang_dist{0.0};

// lateral shiftable ratio
double shiftable_ratio{0.0};

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

// the position of the overhang
Pose overhang_pose;

// envelope polygon
Polygon2d envelope_poly{};

Expand Down Expand Up @@ -425,14 +415,17 @@ struct ObjectData // avoidance target
// object direction.
Direction direction{Direction::NONE};

// overhang points (sort by distance)
std::vector<std::pair<double, Point>> overhang_points{};

// unavoidable reason
std::string reason{};

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

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

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -176,7 +176,8 @@ class AvoidanceHelper
{
using utils::avoidance::calcShiftLength;

const auto shift_length = calcShiftLength(is_on_right, object.overhang_dist, margin);
const auto shift_length =
calcShiftLength(is_on_right, object.overhang_points.front().first, margin);

Check warning on line 180 in planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp#L180

Added line #L180 was not covered by tests
return is_on_right ? std::min(shift_length, getLeftShiftBound())
: std::max(shift_length, getRightShiftBound());
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -56,8 +56,8 @@ double lerpShiftLengthOnArc(double arc, const AvoidLine & al);
void fillLongitudinalAndLengthByClosestEnvelopeFootprint(
const PathWithLaneId & path, const Point & ego_pos, ObjectData & obj);

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

void setEndData(
AvoidLine & al, const double length, const geometry_msgs::msg::Pose & end, const size_t end_idx,
Expand Down
8 changes: 4 additions & 4 deletions planning/behavior_path_avoidance_module/src/debug.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -113,7 +113,7 @@ MarkerArray createToDrivableBoundDistance(const ObjectDataArray & objects, std::
MarkerArray msg;

for (const auto & object : objects) {
if (!object.nearest_bound_point.has_value()) {
if (!object.narrowest_place.has_value()) {
continue;
}

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

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

marker.pose.position = object.nearest_bound_point.value();
marker.pose.position = object.narrowest_place.value().second;

Check warning on line 136 in planning/behavior_path_avoidance_module/src/debug.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_module/src/debug.cpp#L136

Added line #L136 was not covered by tests
std::ostringstream string_stream;
string_stream << object.to_road_shoulder_distance << "[m]";
marker.text = string_stream.str();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -235,7 +235,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
const double LAT_DIST_BUFFER = desire_shift_length > 0.0 ? 1e-3 : -1e-3;

const auto infeasible =
std::abs(feasible_shift_length - object.overhang_dist) - LAT_DIST_BUFFER <
std::abs(feasible_shift_length - object.overhang_points.front().first) - LAT_DIST_BUFFER <
0.5 * data_->parameters.vehicle_width + object_parameter.safety_buffer_lateral;
if (infeasible) {
RCLCPP_DEBUG(rclcpp::get_logger(""), "feasible shift length is not enough to avoid. ");
Expand Down
88 changes: 38 additions & 50 deletions planning/behavior_path_avoidance_module/src/utils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -823,8 +823,8 @@ bool isNoNeedAvoidanceBehavior(
return false;
}

const auto shift_length =
calcShiftLength(isOnRight(object), object.overhang_dist, object.avoid_margin.value());
const auto shift_length = calcShiftLength(
isOnRight(object), object.overhang_points.front().first, object.avoid_margin.value());

Check warning on line 827 in planning/behavior_path_avoidance_module/src/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_module/src/utils.cpp#L826-L827

Added lines #L826 - L827 were not covered by tests
if (!isShiftNecessary(isOnRight(object), shift_length)) {
object.reason = "NotNeedAvoidance";
return true;
Expand Down Expand Up @@ -885,40 +885,41 @@ double getRoadShoulderDistance(
return 0.0;
}

const auto centerline_pose =
lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position);
const auto & p1_object = object.overhang_pose.position;
const auto p_tmp =
geometry_msgs::build<Pose>().position(p1_object).orientation(centerline_pose.orientation);
const auto p2_object =
calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? 100.0 : -100.0), 0.0).position;
std::vector<std::pair<Point, Point>> intersects;
for (const auto & p1 : object.overhang_points) {
const auto centerline_pose =
lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position);
const auto p_tmp =
geometry_msgs::build<Pose>().position(p1.second).orientation(centerline_pose.orientation);

Check warning on line 893 in planning/behavior_path_avoidance_module/src/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_module/src/utils.cpp#L893

Added line #L893 was not covered by tests
const auto p2 = calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? 100.0 : -100.0), 0.0).position;

// TODO(Satoshi OTA): check if the basic point is on right or left of bound.
const auto bound = isOnRight(object) ? data.left_bound : data.right_bound;
// TODO(Satoshi OTA): check if the basic point is on right or left of bound.
const auto bound = isOnRight(object) ? data.left_bound : data.right_bound;

std::vector<Point> intersects;
for (size_t i = 1; i < bound.size(); i++) {
const auto opt_intersect =
tier4_autoware_utils::intersect(p1_object, p2_object, bound.at(i - 1), bound.at(i));
for (size_t i = 1; i < bound.size(); i++) {
const auto opt_intersect =
tier4_autoware_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i));

if (!opt_intersect) {
continue;
}
if (!opt_intersect) {
continue;

Check warning on line 904 in planning/behavior_path_avoidance_module/src/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_module/src/utils.cpp#L904

Added line #L904 was not covered by tests
}

intersects.push_back(opt_intersect.value());
intersects.emplace_back(p1.second, opt_intersect.value());
}
}

std::sort(intersects.begin(), intersects.end(), [](const auto & a, const auto & b) {
return calcDistance2d(a.first, a.second) < calcDistance2d(b.first, b.second);

Check warning on line 912 in planning/behavior_path_avoidance_module/src/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_module/src/utils.cpp#L911-L912

Added lines #L911 - L912 were not covered by tests
});

if (intersects.empty()) {
return 0.0;
}

std::sort(intersects.begin(), intersects.end(), [&p1_object](const auto & a, const auto & b) {
return calcDistance2d(p1_object, a) < calcDistance2d(p1_object, b);
});
object.narrowest_place = intersects.front();

object.nearest_bound_point = intersects.front();

return calcDistance2d(p1_object, object.nearest_bound_point.value());
return calcDistance2d(
object.narrowest_place.value().first, object.narrowest_place.value().second);

Check warning on line 922 in planning/behavior_path_avoidance_module/src/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Method

getRoadShoulderDistance has a cyclomatic complexity of 9, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

Check warning on line 922 in planning/behavior_path_avoidance_module/src/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_module/src/utils.cpp#L922

Added line #L922 was not covered by tests
}
} // namespace filtering_utils

Expand Down Expand Up @@ -1074,34 +1075,22 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint(
return;
}

double calcEnvelopeOverhangDistance(
const ObjectData & object_data, const PathWithLaneId & path, Point & overhang_pose)
std::vector<std::pair<double, Point>> calcEnvelopeOverhangDistance(

Check warning on line 1078 in planning/behavior_path_avoidance_module/src/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_module/src/utils.cpp#L1078

Added line #L1078 was not covered by tests
const ObjectData & object_data, const PathWithLaneId & path)
{
double largest_overhang = isOnRight(object_data) ? -100.0 : 100.0; // large number
std::vector<std::pair<double, Point>> overhang_points{};

Check warning on line 1081 in planning/behavior_path_avoidance_module/src/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_module/src/utils.cpp#L1081

Added line #L1081 was not covered by tests

for (const auto & p : object_data.envelope_poly.outer()) {
const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0);
// TODO(someone): search around first position where the ego should avoid the object.
const auto idx = findNearestIndex(path.points, point);
const auto lateral = calcLateralDeviation(getPose(path.points.at(idx)), point);

const auto & overhang_pose_on_right = [&overhang_pose, &largest_overhang, &point, &lateral]() {
if (lateral > largest_overhang) {
overhang_pose = point;
}
return std::max(largest_overhang, lateral);
};

const auto & overhang_pose_on_left = [&overhang_pose, &largest_overhang, &point, &lateral]() {
if (lateral < largest_overhang) {
overhang_pose = point;
}
return std::min(largest_overhang, lateral);
};

largest_overhang = isOnRight(object_data) ? overhang_pose_on_right() : overhang_pose_on_left();
overhang_points.emplace_back(lateral, point);
}
return largest_overhang;
std::sort(overhang_points.begin(), overhang_points.end(), [&](const auto & a, const auto & b) {

Check warning on line 1090 in planning/behavior_path_avoidance_module/src/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_module/src/utils.cpp#L1090

Added line #L1090 was not covered by tests
return isOnRight(object_data) ? b.first < a.first : a.first < b.first;
});
return overhang_points;

Check warning on line 1093 in planning/behavior_path_avoidance_module/src/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_module/src/utils.cpp#L1093

Added line #L1093 was not covered by tests
}

void setEndData(
Expand Down Expand Up @@ -1446,10 +1435,10 @@ void fillAvoidanceNecessity(
0.5 * vehicle_width + object_parameter.safety_buffer_lateral * object_data.distance_factor;

const auto check_necessity = [&](const auto hysteresis_factor) {
return (isOnRight(object_data) &&
std::abs(object_data.overhang_dist) < safety_margin * hysteresis_factor) ||
return (isOnRight(object_data) && std::abs(object_data.overhang_points.front().first) <
safety_margin * hysteresis_factor) ||
(!isOnRight(object_data) &&
object_data.overhang_dist < safety_margin * hysteresis_factor);
object_data.overhang_points.front().first < safety_margin * hysteresis_factor);
};

const auto id = object_data.object.object_id;
Expand Down Expand Up @@ -1669,8 +1658,7 @@ void filterTargetObjects(
}

// Find the footprint point closest to the path, set to object_data.overhang_distance.
o.overhang_dist =
calcEnvelopeOverhangDistance(o, data.reference_path, o.overhang_pose.position);
o.overhang_points = utils::avoidance::calcEnvelopeOverhangDistance(o, data.reference_path);
o.to_road_shoulder_distance = filtering_utils::getRoadShoulderDistance(o, data, planner_data);
o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters);

Expand Down

0 comments on commit 6acafbc

Please sign in to comment.