diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 93d9d348698c1..2372a7bf3671f 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -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; @@ -1258,26 +1268,6 @@ 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? @@ -1285,122 +1275,32 @@ void filterTargetObjects( * - 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; } }