From 1e05b180b36841355884f532828795f61e1a61fc Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Tue, 17 Oct 2023 19:55:42 +0900 Subject: [PATCH] some drivable area appears Signed-off-by: Muhammad Zulfaqar Azmi --- .../behavior_path_planner/utils/utils.hpp | 5 + .../src/utils/avoidance/utils.cpp | 2 +- .../behavior_path_planner/src/utils/utils.cpp | 147 ++++++++++++++---- 3 files changed, 126 insertions(+), 28 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 3e03a09d3adf8..0ae8bcccdf423 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 @@ -231,6 +231,11 @@ std::vector calcBound( const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, const bool is_left); +lanelet::ConstPoints3d getBoundWithParkingSpace( + const std::vector & original_bound, + const std::shared_ptr & route_handler, + const std::vector & drivable_lanes, const bool is_left); + std::vector getBoundWithHatchedRoadMarkings( const std::vector & original_bound, const std::shared_ptr & route_handler); diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index daa7cc55dd55f..93e178cc06be7 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -921,7 +921,7 @@ void compensateDetectionLost( } void filterTargetObjects( - ObjectDataArray & objects, AvoidancePlanningData & data, DebugData & debug, + ObjectDataArray & objects, AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug, const std::shared_ptr & planner_data, const std::shared_ptr & parameters) { diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 7f1aa41573045..f7480b46337aa 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1838,51 +1838,144 @@ std::vector calcBound( getBoundWithIntersectionAreas(bound_points, route_handler, drivable_lanes, is_left); } + { + bound_points = getBoundWithParkingSpace(bound_points, route_handler, drivable_lanes, is_left); + } + return motion_utils::removeOverlapPoints(to_ros_point(bound_points)); } -std::vector getBoundWithParkingSpace( +lanelet::ConstPoints3d getBoundWithParkingSpace( const std::vector & original_bound, - [[maybe_unused]]const std::shared_ptr & route_handler, + const std::shared_ptr & route_handler, const std::vector & drivable_lanes, const bool is_left) { + if(!is_left){ + return original_bound; + } + + const auto found_intersection = + []( + const lanelet::ConstPoints3d & drivable_lane_bound, + const lanelet::ConstPolygon3d & parking_lot) -> lanelet::ConstPoints3d { + if (drivable_lane_bound.size() < 2 || parking_lot.size() < 2) { + return {}; + } + + const auto lane_front_pt = lanelet::utils::conversion::toGeomMsgPt(drivable_lane_bound.front()); + const auto lane_back_pt = lanelet::utils::conversion::toGeomMsgPt(drivable_lane_bound.back()); + const auto bound_front_pt = lanelet::utils::conversion::toGeomMsgPt(parking_lot.front()); + const auto bound_back_pt = lanelet::utils::conversion::toGeomMsgPt(parking_lot.back()); + + const auto rough_intersection = + tier4_autoware_utils::intersect(lane_front_pt, lane_back_pt, bound_front_pt, bound_back_pt); + + // just check once if there exist intersection. reduce the need to iterate too much. + if (!rough_intersection) { + return {}; + } + + const auto front_point_within_parking_lot = boost::geometry::within(drivable_lane_bound.front(), parking_lot); + + lanelet::ConstPoints3d intersect_insertion_bound; + for (size_t i = 0; i < drivable_lane_bound.size(); ++i) { + size_t next_i = (i + 1) % drivable_lane_bound.size(); + const auto lane_pt1 = lanelet::utils::conversion::toGeomMsgPt(drivable_lane_bound.at(i)); + const auto lane_pt2 = lanelet::utils::conversion::toGeomMsgPt(drivable_lane_bound.at(next_i)); + for (size_t j = 0; j < parking_lot.size(); ++j) { + size_t next_j = (j + 1) % parking_lot.size(); + const auto bound_pt1 = lanelet::utils::conversion::toGeomMsgPt(drivable_lane_bound.at(j)); + const auto bound_pt2 = lanelet::utils::conversion::toGeomMsgPt(drivable_lane_bound.at(next_j)); + const auto intersection_point = + tier4_autoware_utils::intersect(lane_pt1, lane_pt2, bound_pt1, bound_pt2); + + if(!intersection_point){ + continue; + } + + const auto intersection_point_lanelet_point = lanelet::utils::conversion::toLaneletPoint(*intersection_point); + if (intersection_point && !front_point_within_parking_lot) { + intersect_insertion_bound.insert(intersect_insertion_bound.end(), drivable_lane_bound.begin(), drivable_lane_bound.begin() + static_cast(i)); + // Insert elements starting from next_i to the end of parking_lot + intersect_insertion_bound.push_back(intersection_point_lanelet_point); + intersect_insertion_bound.insert(intersect_insertion_bound.end(), parking_lot.begin() + static_cast(next_j), parking_lot.end()); + + // If the range wrapped around, insert the remaining elements from the beginning of parking_lot + if (next_j > 0) { + intersect_insertion_bound.insert(intersect_insertion_bound.end(), parking_lot.begin(), parking_lot.begin() + static_cast(next_j)); + } + return intersect_insertion_bound; + } + if (intersection_point && front_point_within_parking_lot){ + intersect_insertion_bound.push_back(intersection_point_lanelet_point); + intersect_insertion_bound.insert(intersect_insertion_bound.end(), drivable_lane_bound.begin() + next_i, drivable_lane_bound.end()); + return intersect_insertion_bound; + } + } + } + return {}; + }; + + lanelet::ConstPoints3d new_bound_point; const auto lanelet_map_ptr = route_handler->getLaneletMapPtr(); const auto all_parking_lot = lanelet::utils::query::getAllParkingLots(lanelet_map_ptr); - std::vector parking_lot_ids; + lanelet::Ids parking_lots_ids{}; + for (const auto & drivable_lane : drivable_lanes) { + const auto & check_lane = is_left ? drivable_lane.left_lane : drivable_lane.right_lane; + 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; + } - lanelet::ConstPolygons3d linked_parking_lots; + 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(); }); - for (const auto & lanes : drivable_lanes) { - if (is_left) { - const auto & left_lane = lanes.left_lane; + // check if intersect point exist + const auto lane_bound = check_lane.leftBound(); - lanelet::ConstPolygon3d linked_parking_lot; - const auto found_parking_lot = - lanelet::utils::query::getLinkedParkingLot(left_lane, all_parking_lot, &linked_parking_lot); + 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(); }); - if (!found_parking_lot) { - continue; - } + lanelet::ConstPoints3d tmp_bound {}; + tmp_bound.insert(tmp_bound.end(), shared_bound_start_itr, shared_bound_end_itr); - if (!parking_lot_ids.empty()) { - const auto not_registered = std::none_of( - parking_lot_ids.begin(), parking_lot_ids.end(), - [&](const auto id) { return id == linked_parking_lot.id(); }); + const auto intersection_exist = found_intersection(tmp_bound, linked_parking_lot); + if(bound_exist && intersection_exist.empty()){ + continue; + } - if (not_registered) { - linked_parking_lots.push_back(linked_parking_lot); - parking_lot_ids.push_back(linked_parking_lot.id()); - } - } else { - parking_lot_ids.push_back(linked_parking_lot.id()); - linked_parking_lots.push_back(linked_parking_lot); - } + if (!bound_exist && intersection_exist.empty()){ + new_bound_point.insert(new_bound_point.end(), linked_parking_lot.begin(), linked_parking_lot.end()); } - } - std::vector new_bound_point; + 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()); + } + if (bound_exist && !intersection_exist.empty()) { + new_bound_point.insert(new_bound_point.end(), intersection_exist.begin(), intersection_exist.end()); + } + } + return new_bound_point; } void makeBoundLongitudinallyMonotonic(