From b95d27b5f21d53393115a795363cdd5aead1a14e Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Fri, 20 Oct 2023 09:24:30 +0900 Subject: [PATCH] temp Signed-off-by: Muhammad Zulfaqar Azmi --- .../behavior_path_planner/utils/utils.hpp | 8 + .../src/utils/avoidance/utils.cpp | 299 +++++++++--------- .../behavior_path_planner/src/utils/utils.cpp | 232 ++++++++++---- 3 files changed, 335 insertions(+), 204 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 cf4e58a2e9079..30dad7cd08f40 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 @@ -223,6 +223,14 @@ std::vector calcBound( const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, const bool is_left); +lanelet::ConstPoints3d calculateIntersectionPoints( + const lanelet::ConstPoints3d & drivable_lane_bound, const lanelet::ConstPolygon3d & parking_lot, + bool front_point_within_parking_lot, bool back_point_within_parking_lot, bool is_left = true); + +lanelet::ConstPoints3d getIntersectionPointsForBoundWithParkingSpace( + const lanelet::ConstPoints3d & drivable_lane_bound, const lanelet::ConstPolygon3d & parking_lot, + bool is_left = true); + lanelet::ConstPoints3d getBoundWithParkingSpace( 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 5b3fc23ff9844..c6f32d59fb71c 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -22,6 +22,7 @@ #include #include +#include #include #include #include @@ -1064,149 +1065,163 @@ void filterTargetObjects( overhang_basic_pose, parameters->use_hatched_road_markings, parameters->use_intersection_areas); - // const auto convertToGeometryPose = [](const auto & polygon) { - // std::vector converted_to_geom_poses(polygon.size()); - - // for (size_t i = 0; i < polygon.size(); ++i) { - // converted_to_geom_poses[i].position = - // lanelet::utils::conversion::toGeomMsgPt(polygon[i]); - // } - - // motion_utils::insertOrientation(converted_to_geom_poses, true); - - // return converted_to_geom_poses; - // }; - - // if (parking_lot) { - // if (freespace_areas.find(parking_lot->id()) == freespace_areas.end()) { - // const auto converted = convertToGeometryPose(*parking_lot); - // freespace_areas.insert({parking_lot->id(), converted}); - // } - - // o.to_road_shoulder_distance = std::invoke([&]() -> double { - // std::vector freespace_pt_poses{}; - // if (!parking_lot) { - // return o.to_road_shoulder_distance; - // } - // const auto converted_target_line = convertToGeometryPose(target_line); - // freespace_pt_poses.reserve(parking_lot->size()); - // for (const auto & geom_pt : freespace_areas.at(parking_lot->id())) { - // const auto point_on_right = - // (motion_utils::calcLateralOffset(converted_target_line, geom_pt.position) < - // 0.0); - - // if (isOnRight(o) != point_on_right) { - // freespace_pt_poses.push_back(geom_pt); - // } - // } - // // find nearest index - // const size_t nearest_idx = - // findNearestSegmentIndex(freespace_pt_poses, o.overhang_pose.position); - - // double nearest_freespace_segment = [&]() { - // const auto & pose1 = freespace_pt_poses[nearest_idx]; - // geometry_msgs::msg::Pose pose2; - // if (nearest_idx == freespace_pt_poses.size() - 1) { - // pose2 = freespace_pt_poses[nearest_idx]; - // std::cerr << "something wrong with the computation\n"; - // } else { - // pose2 = freespace_pt_poses[nearest_idx + 1]; - // } - // const auto segment = std::vector{pose1, pose2}; - // return std::abs(motion_utils::calcLateralOffset(segment, - // o.overhang_pose.position)); - // }(); - // size_t best_i = nearest_idx; - // // for(size_t i = 0; i < freespace_pt_poses.size() - 1; ++i){ - - // // if (lateral < nearest_freespace_segment){ - // // nearest_freespace_segment = lateral; - // // best_i = i; - // // } - // // } - // std::cerr << "nearest_freespace_segment " << nearest_freespace_segment << '\n'; - - // const auto converttoPoint3d = - // [](const geometry_msgs::msg::Pose & pose) -> lanelet::Point3d { - // // Create a new Point3d using the ID and the coordinates of the ConstPoint3d - // const auto ll_pt = lanelet::utils::conversion::toLaneletPoint(pose.position); - // lanelet::Point3d point(ll_pt.id(), ll_pt.x(), ll_pt.y(), ll_pt.z()); - // // Return the new point - // return point; - // }; - - // if (nearest_freespace_segment > o.to_road_shoulder_distance) { - // lanelet::LineString3d freespace_segment_temp; - // freespace_segment_temp.push_back(converttoPoint3d(freespace_pt_poses[best_i])); - // freespace_segment_temp.push_back(converttoPoint3d(freespace_pt_poses[best_i + - // 1])); freespace_segment = freespace_segment_temp; return - // nearest_freespace_segment; - // } - - // return o.to_road_shoulder_distance; - // }); - - // std::cerr << o.to_road_shoulder_distance << '\n'; - // // comment out for now. will use this code to check for points are correctly assigned - // to - // // left and to right. - // const auto [left_ids, right_ids] = - // std::invoke([&]() -> std::pair, std::vector> { - // std::vector left{}; - // std::vector right{}; - // if (parking_lot) { - // left.reserve(parking_lot->size()); - // right.reserve(parking_lot->size()); - // for (const auto & poly_pt : *parking_lot) { - // const auto geom_pt1 = lanelet::utils::conversion::toGeomMsgPt(poly_pt); - // const auto lateral = motion_utils::calcLateralOffset( - // convertToGeometryPose(overhang_lanelet.centerline()), geom_pt1); - - // if (lateral > 0) { - // left.push_back(poly_pt.id()); - // } else { - // right.push_back(poly_pt.id()); - // } - // } - // } - - // return {left, right}; - // }); - - // if (isOnRight(o)) { - // std::cerr << "line should be at left\t"; - // for (const auto left_id : left_ids) { - // std::cerr << left_id << '\t'; - // } - // } else { - // std::cerr << "line should be at right\t"; - // for (const auto right_id : right_ids) { - // std::cerr << right_id << '\t'; - // } - // } - // std::cerr << '\n'; - // // [[maybe_unused]] const auto intersect_with_target_line = - // // [&target_line](const Point & polygon_pt1, const Point & polygon_pt2) { - // // for (size_t i = 0; i < target_line.size(); ++i) { - // // const auto & geom_pt1 = - // lanelet::utils::conversion::toGeomMsgPt(target_line[i]); - // // const auto & geom_pt2 = - // // lanelet::utils::conversion::toGeomMsgPt(target_line[(i + 1) % - // // target_line.size()]); - - // // if (tier4_autoware_utils::intersect(polygon_pt1, polygon_pt2, geom_pt1, - // // geom_pt2)) { - // // return true; - // // } - // // } - // // return false; - // // }; - // } + [[maybe_unused]] const auto parking_lot = + std::invoke([&]() -> std::optional { + const auto lanelet_map_ptr = rh->getLaneletMapPtr(); + const auto all_parking_lot = lanelet::utils::query::getAllParkingLots(lanelet_map_ptr); + const auto current_position = lanelet::utils::to2D( + lanelet::utils::conversion::toLaneletPoint(o.overhang_pose.position)); + lanelet::ConstPolygon3d linked_parking_lot; + const auto found_parking_lot = lanelet::utils::query::getLinkedParkingLot( + current_position, all_parking_lot, &linked_parking_lot); + + if (found_parking_lot) { + return linked_parking_lot; + } + return std::nullopt; + }); + + const auto convertToGeometryPose = [](const auto & polygon) { + std::vector converted_to_geom_poses(polygon.size()); + + for (size_t i = 0; i < polygon.size(); ++i) { + converted_to_geom_poses[i].position = + lanelet::utils::conversion::toGeomMsgPt(polygon[i]); + } + + motion_utils::insertOrientation(converted_to_geom_poses, true); + + return converted_to_geom_poses; + }; + + if (parking_lot) { + if (freespace_areas.find(parking_lot->id()) == freespace_areas.end()) { + const auto converted = convertToGeometryPose(*parking_lot); + freespace_areas.insert({parking_lot->id(), converted}); + } + + o.to_road_shoulder_distance = std::invoke([&]() -> double { + std::vector freespace_pt_poses{}; + if (!parking_lot) { + return o.to_road_shoulder_distance; + } + const auto converted_target_line = convertToGeometryPose(target_line); + freespace_pt_poses.reserve(parking_lot->size()); + for (const auto & geom_pt : freespace_areas.at(parking_lot->id())) { + const auto point_on_right = + (motion_utils::calcLateralOffset(converted_target_line, geom_pt.position) < 0.0); + + if (isOnRight(o) != point_on_right) { + freespace_pt_poses.push_back(geom_pt); + } + } + // find nearest index + const size_t nearest_idx = + findNearestSegmentIndex(freespace_pt_poses, o.overhang_pose.position); + + double nearest_freespace_segment = [&]() { + const auto & pose1 = freespace_pt_poses[nearest_idx]; + geometry_msgs::msg::Pose pose2; + if (nearest_idx == freespace_pt_poses.size() - 1) { + pose2 = freespace_pt_poses[nearest_idx]; + std::cerr << "something wrong with the computation\n"; + } else { + pose2 = freespace_pt_poses[nearest_idx + 1]; + } + const auto segment = std::vector{pose1, pose2}; + return std::abs(motion_utils::calcLateralOffset(segment, o.overhang_pose.position)); + }(); + size_t best_i = nearest_idx; + // for(size_t i = 0; i < freespace_pt_poses.size() - 1; ++i){ + + // if (lateral < nearest_freespace_segment){ + // nearest_freespace_segment = lateral; + // best_i = i; + // } + // } + std::cerr << "nearest_freespace_segment " << nearest_freespace_segment << '\n'; + + const auto converttoPoint3d = + [](const geometry_msgs::msg::Pose & pose) -> lanelet::Point3d { + // Create a new Point3d using the ID and the coordinates of the ConstPoint3d + const auto ll_pt = lanelet::utils::conversion::toLaneletPoint(pose.position); + lanelet::Point3d point(ll_pt.id(), ll_pt.x(), ll_pt.y(), ll_pt.z()); + // Return the new point + return point; + }; + + if (nearest_freespace_segment > o.to_road_shoulder_distance) { + lanelet::LineString3d freespace_segment_temp; + freespace_segment_temp.push_back(converttoPoint3d(freespace_pt_poses[best_i])); + freespace_segment_temp.push_back(converttoPoint3d(freespace_pt_poses[best_i + 1])); + freespace_segment = freespace_segment_temp; + return nearest_freespace_segment; + } + + return o.to_road_shoulder_distance; + }); + + std::cerr << o.to_road_shoulder_distance << '\n'; + // comment out for now. will use this code to check for points are correctly assigned + // to + // left and to right. + const auto [left_ids, right_ids] = + std::invoke([&]() -> std::pair, std::vector> { + std::vector left{}; + std::vector right{}; + if (parking_lot) { + left.reserve(parking_lot->size()); + right.reserve(parking_lot->size()); + for (const auto & poly_pt : *parking_lot) { + const auto geom_pt1 = lanelet::utils::conversion::toGeomMsgPt(poly_pt); + const auto lateral = motion_utils::calcLateralOffset( + convertToGeometryPose(overhang_lanelet.centerline()), geom_pt1); + + if (lateral > 0) { + left.push_back(poly_pt.id()); + } else { + right.push_back(poly_pt.id()); + } + } + } + + return {left, right}; + }); + + if (isOnRight(o)) { + std::cerr << "line should be at left\t"; + for (const auto left_id : left_ids) { + std::cerr << left_id << '\t'; + } + } else { + std::cerr << "line should be at right\t"; + for (const auto right_id : right_ids) { + std::cerr << right_id << '\t'; + } + } + std::cerr << '\n'; + // [[maybe_unused]] const auto intersect_with_target_line = + // [&target_line](const Point & polygon_pt1, const Point & polygon_pt2) { + // for (size_t i = 0; i < target_line.size(); ++i) { + // const auto & geom_pt1 = + // lanelet::utils::conversion::toGeomMsgPt(target_line[i]); + // const auto & geom_pt2 = + // lanelet::utils::conversion::toGeomMsgPt(target_line[(i + 1) % + // target_line.size()]); + + // if (tier4_autoware_utils::intersect(polygon_pt1, polygon_pt2, geom_pt1, + // geom_pt2)) { + // return true; + // } + // } + // return false; + // }; + } + } + debug.bounds.push_back(target_line); + if (!freespace_segment.empty()) { + debug.bounds.push_back(freespace_segment); } - // debug.bounds.push_back(target_line); - // if (!freespace_segment.empty()) { - // debug.bounds.push_back(freespace_segment); - // } } // calculate avoid_margin dynamically diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index a449607fb3f00..eb2d99411c4eb 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1843,77 +1843,180 @@ std::vector calcBound( return motion_utils::removeOverlapPoints(to_ros_point(bound_points)); } - -lanelet::ConstPoints3d getBoundWithParkingSpace( - const std::vector & original_bound, - const std::shared_ptr & route_handler, - const std::vector & drivable_lanes, const bool is_left) +lanelet::ConstPoints3d getIntersectionPointsForBoundWithParkingSpace( + const lanelet::ConstPoints3d & drivable_lane_bound, const lanelet::ConstPolygon3d & parking_lot, + bool is_left) { - if(!is_left){ - return original_bound; + if (drivable_lane_bound.size() < 2 || parking_lot.size() < 2) { + return {}; } - 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_point = + lanelet::utils::conversion::toGeomMsgPt(drivable_lane_bound.front()); + const auto lane_back_point = lanelet::utils::conversion::toGeomMsgPt(drivable_lane_bound.back()); + const auto bound_front_point = lanelet::utils::conversion::toGeomMsgPt(parking_lot.front()); + const auto bound_back_point = lanelet::utils::conversion::toGeomMsgPt(parking_lot.back()); - 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()); + if (!tier4_autoware_utils::intersect( + lane_front_point, lane_back_point, bound_front_point, bound_back_point)) { + return {}; + } - const auto rough_intersection = - tier4_autoware_utils::intersect(lane_front_pt, lane_back_pt, bound_front_pt, bound_back_pt); + const auto front_point_within_parking_lot = + boost::geometry::within(drivable_lane_bound.front(), parking_lot); + const auto back_point_within_parking_lot = + boost::geometry::within(drivable_lane_bound.back(), parking_lot); - // just check once if there exist intersection. reduce the need to iterate too much. - if (!rough_intersection) { - return {}; + return calculateIntersectionPoints( + drivable_lane_bound, parking_lot, front_point_within_parking_lot, back_point_within_parking_lot, + is_left); +} + +lanelet::ConstPoints3d calculateIntersectionPoints( + const lanelet::ConstPoints3d & drivable_lane_bound, const lanelet::ConstPolygon3d & parking_lot, + bool front_point_within_parking_lot, bool back_point_within_parking_lot, bool is_left) +{ + lanelet::ConstPoints3d intersect_insertion_bound; + + const auto convertToGeometryPose = [](const auto & polygon) { + std::vector converted_to_geom_poses(polygon.size()); + + for (size_t i = 0; i < polygon.size(); ++i) { + converted_to_geom_poses[i].position = lanelet::utils::conversion::toGeomMsgPt(polygon[i]); } - const auto front_point_within_parking_lot = boost::geometry::within(drivable_lane_bound.front(), parking_lot); + motion_utils::insertOrientation(converted_to_geom_poses, true); - 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(); + return converted_to_geom_poses; + }; + + const auto [left_points_from_parking_lot, right_points_from_parking_lot] = + std::invoke([&]() -> std::pair { + const auto converted_drivable_lane_bound = convertToGeometryPose(drivable_lane_bound); + + lanelet::ConstPoints3d left_points_from_parking_lot; + lanelet::ConstPoints3d right_points_from_parking_lot; + + left_points_from_parking_lot.reserve(parking_lot.size()); + right_points_from_parking_lot.reserve(parking_lot.size()); + + for (const auto & pt : parking_lot) { + const auto converted_pt = lanelet::utils::conversion::toGeomMsgPt(pt); + const auto is_on_left_of_bound = + motion_utils::calcLateralOffset(converted_drivable_lane_bound, converted_pt) >= 0; + + if (is_on_left_of_bound) { + left_points_from_parking_lot.push_back(pt); + } else { + right_points_from_parking_lot.push_back(pt); + } + } + return {left_points_from_parking_lot, right_points_from_parking_lot}; + }); + + if (is_left && front_point_within_parking_lot && back_point_within_parking_lot) { + return left_points_from_parking_lot; + } + + if (!is_left && front_point_within_parking_lot && back_point_within_parking_lot) { + return right_points_from_parking_lot; + } + + lanelet::ConstPoints3d new_left_bound; + lanelet::ConstPoints3d new_right_bound; + + if (is_left && !front_point_within_parking_lot && back_point_within_parking_lot) { + new_left_bound.push_back(drivable_lane_bound.front()); + const auto bound_pt1 = lanelet::utils::conversion::toGeomMsgPt(left_points_from_parking_lot.front()); + const auto bound_pt2 = + lanelet::utils::conversion::toGeomMsgPt(right_points_from_parking_lot.back()); + for (size_t i = 0; i < drivable_lane_bound.size() - 1; ++i) { + const size_t next_i = i + 1; 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){ + if (!intersection_point) { + new_left_bound.push_back(drivable_lane_bound.at(i + 1)); 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()); + const auto intersection_point_lanelet_point = + lanelet::utils::conversion::toLaneletPoint(*intersection_point); + new_left_bound.push_back(intersection_point_lanelet_point); + break; + + } + new_left_bound.insert(new_left_bound.end(), left_points_from_parking_lot.begin(), left_points_from_parking_lot.end()); + return new_left_bound; + } - // 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; + for (size_t i = 0; i < drivable_lane_bound.size() - 1; ++i) { + size_t next_i = i + 1; + 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 < left_points_from_parking_lot.size(); ++j) { + size_t next_j = (j + 1) % left_points_from_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)); + + if (is_left && !front_point_within_parking_lot && back_point_within_parking_lot) { + } + + 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(), + left_points_from_parking_lot.begin() + static_cast(next_j), + left_points_from_parking_lot.end()); + + // If the range wrapped around, insert the remaining elements from the beginning of + // left_points_from_parking_lot + if (next_j > 0) { + intersect_insertion_bound.insert( + intersect_insertion_bound.end(), left_points_from_parking_lot.begin(), + left_points_from_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 {}; - }; + } + + return intersect_insertion_bound; +} + +lanelet::ConstPoints3d getBoundWithParkingSpace( + const std::vector & original_bound, + const std::shared_ptr & route_handler, + const std::vector & drivable_lanes, const bool is_left) +{ + if (!is_left) { + return original_bound; + } lanelet::ConstPoints3d new_bound_point; const auto lanelet_map_ptr = route_handler->getLaneletMapPtr(); @@ -1925,11 +2028,12 @@ lanelet::ConstPoints3d getBoundWithParkingSpace( 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 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_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(); }); @@ -1952,25 +2056,29 @@ lanelet::ConstPoints3d getBoundWithParkingSpace( original_bound.begin(), original_bound.end(), [&](const auto & p) { return p.id() == check_lane.leftBound3d().back().id(); }); - lanelet::ConstPoints3d tmp_bound {}; + lanelet::ConstPoints3d tmp_bound{}; tmp_bound.insert(tmp_bound.end(), shared_bound_start_itr, shared_bound_end_itr); - const auto intersection_exist = found_intersection(tmp_bound, linked_parking_lot); - if(bound_exist && intersection_exist.empty()){ + const auto intersection_exist = + getIntersectionPointsForBoundWithParkingSpace(tmp_bound, linked_parking_lot); + if (bound_exist && intersection_exist.empty()) { continue; } - if (!bound_exist && intersection_exist.empty()){ - new_bound_point.insert(new_bound_point.end(), linked_parking_lot.begin(), linked_parking_lot.end()); + if (!bound_exist && intersection_exist.empty()) { + new_bound_point.insert( + new_bound_point.end(), linked_parking_lot.begin(), linked_parking_lot.end()); } - if(!bound_exist && !intersection_exist.empty()){ - new_bound_point.insert(new_bound_point.end(), intersection_exist.begin(), intersection_exist.end()); + 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()); + new_bound_point.insert( + new_bound_point.end(), intersection_exist.begin(), intersection_exist.end()); } }