Skip to content

Commit

Permalink
fixup! feat(avoidance): suppress unnecessary avoidance path
Browse files Browse the repository at this point in the history
  • Loading branch information
@satoshi-ota
satoshi-ota committed Oct 19, 2023
1 parent 5d25eea commit 45a24b9
Showing 1 changed file with 29 additions and 129 deletions.
158 changes: 29 additions & 129 deletions planning/behavior_path_planner/src/utils/avoidance/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -303,6 +303,16 @@ bool isParkedOnRoadShoulder(
using lanelet::geometry::toArcCoordinates;
using lanelet::utils::to2D;

// ============================================ <- most_left_lanelet.leftBound()
// y road shoulder
// ^ ------------------------------------------
// | x +
// +---> --- object closest lanelet --- o ----- <- object_closest_lanelet.centerline()
//
// --------------------------------------------
// +: object position
// o: nearest point on centerline

lanelet::BasicPoint2d object_centroid(object.centroid.x(), object.centroid.y());

const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose;
Expand Down Expand Up @@ -1258,149 +1268,39 @@ void filterTargetObjects(
continue;
}

{
std::string turn_direction = overhang_lanelet.attributeOr("turn_direction", "else");
if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") {
const auto closest_pose =
lanelet::utils::getClosestCenterPose(overhang_lanelet, object_pose.position);
const auto yaw_deviation = std::abs(calcYawDeviation(closest_pose, object_pose));
const auto deviation =
tier4_autoware_utils::rad2deg(calcYawDeviation(closest_pose, object_pose));
constexpr double THRESHOLD = M_PI_2 / 9.0;
std::cout << "yaw[deg]:" << deviation << " [rad]:" << yaw_deviation << std::endl;
if (yaw_deviation < THRESHOLD || yaw_deviation > M_PI - THRESHOLD) {
o.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT;
data.other_objects.push_back(o);
continue;
}
}
}

lanelet::BasicPoint2d object_centroid(o.centroid.x(), o.centroid.y());

/**
* Is not object in adjacent lane?
* - Yes -> Is parking object?
* - Yes -> the object is avoidance target.
* - No -> ignore this object.
* - No -> the object is avoidance target no matter whether it is parking object or not.
*/
lanelet::BasicPoint2d object_centroid(o.centroid.x(), o.centroid.y());
const auto is_in_ego_lane =
within(object_centroid, overhang_lanelet.polygon2d().basicPolygon());
if (is_in_ego_lane) {
/**
* TODO(Satoshi Ota) use intersection area
* under the assumption that there is no parking vehicle inside intersection,
* ignore all objects that is in the ego lane as not parking objects.
*/
// std::string turn_direction = overhang_lanelet.attributeOr("turn_direction", "else");
// if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight")
// {
// o.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT;
// data.other_objects.push_back(o);
// continue;
// }

// const auto centerline_pose =
// lanelet::utils::getClosestCenterPose(overhang_lanelet, object_pose.position);
// lanelet::BasicPoint3d centerline_point(
// centerline_pose.position.x, centerline_pose.position.y, centerline_pose.position.z);

// ============================================ <- most_left_lanelet.leftBound()
// y road shoulder
// ^ ------------------------------------------
// | x +
// +---> --- object closest lanelet --- o ----- <- object_closest_lanelet.centerline()
//
// --------------------------------------------
// +: object position
// o: nearest point on centerline

// bool is_left_side_parked_vehicle = false;
// if (!isOnRight(o)) {
// auto [object_shiftable_distance, sub_type] = [&]() {
// const auto most_left_road_lanelet = rh->getMostLeftLanelet(overhang_lanelet);
// const auto most_left_lanelet_candidates =
// rh->getLaneletMapPtr()->laneletLayer.findUsages(most_left_road_lanelet.leftBound());

// lanelet::ConstLanelet most_left_lanelet = most_left_road_lanelet;
// const lanelet::Attribute sub_type =
// most_left_lanelet.attribute(lanelet::AttributeName::Subtype);

// for (const auto & ll : most_left_lanelet_candidates) {
// const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype);
// if (sub_type.value() == "road_shoulder") {
// most_left_lanelet = ll;
// }
// }

// const auto center_to_left_boundary = distance2d(
// to2D(most_left_lanelet.leftBound().basicLineString()), to2D(centerline_point));

// return std::make_pair(
// center_to_left_boundary - 0.5 * o.object.shape.dimensions.y, sub_type);
// }();

// if (sub_type.value() != "road_shoulder") {
// object_shiftable_distance += parameters->object_check_min_road_shoulder_width;
// }

// const auto arc_coordinates =
// toArcCoordinates(to2D(overhang_lanelet.centerline().basicLineString()),
// object_centroid);
// o.shiftable_ratio = arc_coordinates.distance / object_shiftable_distance;

// is_left_side_parked_vehicle = o.shiftable_ratio >
// parameters->object_check_shiftable_ratio;
// }

// bool is_right_side_parked_vehicle = false;
// if (isOnRight(o)) {
// auto [object_shiftable_distance, sub_type] = [&]() {
// const auto most_right_road_lanelet = rh->getMostRightLanelet(overhang_lanelet);
// const auto most_right_lanelet_candidates =
// rh->getLaneletMapPtr()->laneletLayer.findUsages(most_right_road_lanelet.rightBound());

// lanelet::ConstLanelet most_right_lanelet = most_right_road_lanelet;
// const lanelet::Attribute sub_type =
// most_right_lanelet.attribute(lanelet::AttributeName::Subtype);

// for (const auto & ll : most_right_lanelet_candidates) {
// const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype);
// if (sub_type.value() == "road_shoulder") {
// most_right_lanelet = ll;
// }
// }

// const auto center_to_right_boundary = distance2d(
// to2D(most_right_lanelet.rightBound().basicLineString()), to2D(centerline_point));

// return std::make_pair(
// center_to_right_boundary - 0.5 * o.object.shape.dimensions.y, sub_type);
// }();

// if (sub_type.value() != "road_shoulder") {
// object_shiftable_distance += parameters->object_check_min_road_shoulder_width;
// }

// const auto arc_coordinates =
// toArcCoordinates(to2D(overhang_lanelet.centerline().basicLineString()),
// object_centroid);
// o.shiftable_ratio = -1.0 * arc_coordinates.distance / object_shiftable_distance;

// is_right_side_parked_vehicle = o.shiftable_ratio >
// parameters->object_check_shiftable_ratio;
// }

// if (!is_left_side_parked_vehicle && !is_right_side_parked_vehicle) {
// o.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT;
// data.other_objects.push_back(o);
// continue;
// }
if (isParkedOnRoadShoulder(o, rh, parameters)) {
o.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT;
data.other_objects.push_back(o);
continue;
} else {
o.last_seen = now;
o.avoid_margin = avoid_margin;
data.target_objects.push_back(o);
continue;
}
}

std::string turn_direction = overhang_lanelet.attributeOr("turn_direction", "else");
if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") {
constexpr double THRESHOLD = M_PI_2 / 9.0;
const auto closest_pose =
lanelet::utils::getClosestCenterPose(overhang_lanelet, object_pose.position);
const auto yaw_deviation = std::abs(calcYawDeviation(closest_pose, object_pose));
if (yaw_deviation < THRESHOLD || yaw_deviation > M_PI - THRESHOLD) {
o.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT;
data.other_objects.push_back(o);
continue;
}
}

Expand Down

0 comments on commit 45a24b9

Please sign in to comment.