From 40b5c7b138207e3157189e94b1f8316f1f2fe2ec Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Fri, 27 Oct 2023 19:47:00 +0900 Subject: [PATCH] edit using segment Signed-off-by: Zulfaqar Azmi --- .../behavior_path_planner/utils/utils.hpp | 16 ++ .../behavior_path_planner/src/utils/utils.cpp | 204 +++++++++++------- 2 files changed, 145 insertions(+), 75 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index 48e1266215ed8..06f648d8aab6c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -29,11 +29,13 @@ #include #include #include +#include #include #include #include #include +#include #include #ifdef ROS_DISTRO_GALACTIC @@ -101,6 +103,20 @@ struct PolygonPoint }; }; +struct Segment +{ + Point start; + Point end; + lanelet::ConstPoint3d start_pt; + lanelet::ConstPoint3d end_pt; + bool start_is_outside = true; + + [[nodiscard]] std::optional is_intersect(const Segment & segment) const + { + return tier4_autoware_utils::intersect(this->start, this->end, segment.start, segment.end); + } +}; + struct FrenetPoint { double length{0.0}; // longitudinal diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 2de457c5c18a2..53d53f9df8421 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -24,18 +24,25 @@ #include #include #include +#include #include #include +#include #include +#include +#include #include +#include #include #include +#include #include #include #include +#include #include #include #include @@ -2046,6 +2053,29 @@ lanelet::ConstPoints3d calculateIntersectionPoints( return intersect_insertion_bound; } +bool intersectionBoundWithPolygon( + const lanelet::ConstPoints3d & bound_points, const lanelet::ConstPolygon3d & parking_lot) +{ + if (bound_points.size() < 2 || parking_lot.size() < 2) { + return false; + } + const auto polygon_points = convertToGeometryPose(parking_lot, true); + + for (size_t bound_idx = 0; bound_idx < bound_points.size() - 1; ++bound_idx) { + const auto bound_pt1 = lanelet::utils::conversion::toGeomMsgPt(bound_points[bound_idx]); + const auto bound_pt2 = lanelet::utils::conversion::toGeomMsgPt(bound_points[bound_idx + 1]); + for (size_t poly_idx = 0; poly_idx < polygon_points.size(); ++poly_idx) { + size_t next_poly_idx = (poly_idx + 1) % polygon_points.size(); + const auto poly_pt1 = polygon_points[poly_idx].position; + const auto poly_pt2 = polygon_points[next_poly_idx].position; + if (tier4_autoware_utils::intersect(bound_pt1, bound_pt2, poly_pt1, poly_pt2)) { + return true; + } + } + } + return false; +} + bool parkingLotRegistered( const lanelet::ConstPolygons3d & parking_lot_list, const lanelet::ConstPolygon3d & current_parking_lot) @@ -2067,6 +2097,10 @@ lanelet::ConstPoints3d getBoundWithParkingSpace( return original_bound; } + if (original_bound.size() < 2) { + return original_bound; + } + const auto lanelet_map_ptr = route_handler->getLaneletMapPtr(); const auto all_parking_lot = lanelet::utils::query::getAllParkingLots(lanelet_map_ptr); @@ -2084,91 +2118,111 @@ lanelet::ConstPoints3d getBoundWithParkingSpace( continue; } + if (!intersectionBoundWithPolygon(original_bound, linked_parking_lot)) { + continue; + } + parking_lot_list.push_back(linked_parking_lot); } - namespace bg = boost::geometry; - - const auto to_linestring = - [](const auto & points) { - tier4_autoware_utils::LineString3d linestring; - linestring.reserve(points.size()); - - for(const auto & point:points){ - bg::append(linestring, tier4_autoware_utils::Point3d(point.x(), point.y(), point.z())); - } - - return linestring; - }; - - auto union_bound = to_linestring(original_bound); - - for(const auto & polygon_parking_lot : parking_lot_list){ - auto linestring_parking_lot = to_linestring(polygon_parking_lot); - const auto union_results = - } - - // lanelet::Ids parking_lots_ids{}; - // for (const auto & drivable_lane : drivable_lanes) { - // lanelet::ConstPolygon3d linked_parking_lot; - // const auto found_parking_lot = - // lanelet::utils::query::getLinkedParkingLot(check_lane, all_parking_lot, - // &linked_parking_lot); - - // // if not parking lot, no drivable area extension needed, so just copy the sub-vector of - // // original bounds - // if (!found_parking_lot) { - // const auto not_shared_bound_start_itr = std::find_if( - // original_bound.begin(), original_bound.end(), - // [&](const auto & p) { return p.id() == check_lane.leftBound().front().id(); }); - // const auto not_shared_bound_end_itr = std::find_if( - // original_bound.begin(), original_bound.end(), - // [&](const auto & p) { return p.id() == check_lane.leftBound3d().back().id(); }); - // new_bound_point.insert( - // new_bound_point.end(), not_shared_bound_start_itr, not_shared_bound_end_itr); - // continue; - // } - - // const auto bound_exist = std::any_of( - // parking_lots_ids.begin(), parking_lots_ids.end(), - // [&](const lanelet::Id id) { return id == linked_parking_lot.id(); }); - // // check if intersect point exist - // const auto lane_bound = check_lane.leftBound(); + if (parking_lot_list.empty()) { + return original_bound; + } - // const auto shared_bound_start_itr = std::find_if( - // original_bound.begin(), original_bound.end(), - // [&](const auto & p) { return p.id() == check_lane.leftBound().front().id(); }); - // const auto shared_bound_end_itr = std::find_if( - // original_bound.begin(), original_bound.end(), - // [&](const auto & p) { return p.id() == check_lane.leftBound3d().back().id(); }); + const auto to_segments = [](const auto & points, bool loop = false) { + std::vector segments; + segments.reserve(points.size()); - // lanelet::ConstPoints3d tmp_bound{}; - // tmp_bound.insert(tmp_bound.end(), shared_bound_start_itr, shared_bound_end_itr); + for (size_t i = 0; i < points.size(); i++) { + auto next_i = (i + 1) % points.size(); + if (loop && next_i == 0) { + break; + } + Segment segment; + segment.start = lanelet::utils::conversion::toGeomMsgPt(points[i]); + segment.end = lanelet::utils::conversion::toGeomMsgPt(points[next_i]); + segment.start_pt = points[i]; + segment.end_pt = points[next_i]; + segments.push_back(segment); + } - // const auto intersection_exist = - // getIntersectionPointsForBoundWithParkingSpace(tmp_bound, linked_parking_lot); - // if (bound_exist && intersection_exist.empty()) { - // continue; - // } + return segments; + }; - // if (!bound_exist && intersection_exist.empty()) { - // new_bound_point.insert( - // new_bound_point.end(), linked_parking_lot.begin(), linked_parking_lot.end()); - // } + const auto to_lanelet_points = [](const std::vector & segments) { + lanelet::ConstPoints3d points; + if (segments.empty()) { + return points; + } - // if (!bound_exist && !intersection_exist.empty()) { - // new_bound_point.insert( - // new_bound_point.end(), intersection_exist.begin(), intersection_exist.end()); - // parking_lots_ids.push_back(linked_parking_lot.id()); - // } + points.reserve(segments.size() + 1); + for (const auto & segment : segments) { + points.push_back(segment.start_pt); + } + points.push_back(segments.back().end_pt); + return points; + }; + auto bound_segments = to_segments(original_bound); + + const auto bound_front_covered_by_first_polygon = + boost::geometry::covered_by(original_bound.front(), parking_lot_list.front()); + const auto bound_back_covered_by_first_polygon = + boost::geometry::covered_by(original_bound.back(), parking_lot_list.front()); + const auto bound_back_covered_by_last_polygon = + (parking_lot_list.size() == 1) + ? bound_back_covered_by_first_polygon + : boost::geometry::covered_by(original_bound.back(), parking_lot_list.back()); + + auto parking_lot_segments = to_segments(parking_lot_list.front(), true); + + if (!bound_front_covered_by_first_polygon && !bound_back_covered_by_last_polygon) { + std::vector new_segment; + new_segment.push_back(bound_segments.front()); + for (const auto & bound_segment : bound_segments) { + for (const auto & parking_lot_segment : parking_lot_segments) { + auto is_intersect = bound_segment.is_intersect(parking_lot_segment); + if (!is_intersect) { + continue; + } - // if (bound_exist && !intersection_exist.empty()) { - // new_bound_point.insert( - // new_bound_point.end(), intersection_exist.begin(), intersection_exist.end()); - // } - // } + std::vector bound_pt{bound_segment.start, bound_segment.end}; + const auto lateral_dist = + motion_utils::calcLateralOffset(bound_pt, parking_lot_segment.start); + auto point_to_append = + (lateral_dist > 0 && is_left) ? parking_lot_segment.start : parking_lot_segment.end; + auto ll_pt_to_append = + (lateral_dist > 0 && is_left) ? parking_lot_segment.start_pt : parking_lot_segment.end_pt; + + auto intersect_ll_pt = lanelet::utils::conversion::toLaneletPoint(*is_intersect); + Segment intersect_segment; + if (new_segment.back().start_is_outside) { + new_segment.back().end = *is_intersect; + new_segment.back().end_pt = intersect_ll_pt; + intersect_segment.start = *is_intersect; + intersect_segment.start_pt = intersect_ll_pt; + intersect_segment.end = point_to_append; + intersect_segment.end_pt = ll_pt_to_append; + } else { + intersect_segment.start = *is_intersect; + intersect_segment.start_pt = intersect_ll_pt; + intersect_segment.end = new_segment.back().end; + intersect_segment.end_pt = new_segment.back().end_pt; + new_segment.back().start = point_to_append; + new_segment.back().start_pt = ll_pt_to_append; + new_segment.back().end = *is_intersect; + new_segment.back().end_pt = intersect_ll_pt; + } + new_segment.push_back(intersect_segment); + if (!new_segment.back().start_is_outside) { + break; + } + new_segment.back().start_is_outside = !new_segment.back().start_is_outside; + } + } + return to_lanelet_points(new_segment); + } - return new_bound_point; + return {}; } void makeBoundLongitudinallyMonotonic(