From 53769e5993d7c26c7bc3e4af4ed195c986cbb6e5 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Fri, 20 Oct 2023 20:09:23 +0900 Subject: [PATCH] seems ok Signed-off-by: Zulfaqar Azmi --- .../behavior_path_planner/utils/utils.hpp | 6 +- .../behavior_path_planner/src/utils/utils.cpp | 225 ++++++++++-------- 2 files changed, 134 insertions(+), 97 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 30dad7cd08f40..7e90f04b6f32d 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 @@ -224,8 +224,10 @@ std::vector calcBound( 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); + const lanelet::ConstPoints3d & drivable_lane_bound, + const lanelet::ConstPoints3d & left_points_from_parking_lot, + const lanelet::ConstPoints3d & right_points_from_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, diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 54e163434d864..461796aeaedf9 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -43,6 +43,25 @@ namespace { +template +std::vector convertToGeometryPose( + const Primitive & polygon, bool skip_orientation = false) +{ + using Poses = std::vector; + Poses converted_to_geom_poses; + converted_to_geom_poses.resize(polygon.size()); + + for (size_t i = 0; i < polygon.size(); ++i) { + converted_to_geom_poses[i].position = lanelet::utils::conversion::toGeomMsgPt(polygon[i]); + } + + if (skip_orientation) { + motion_utils::insertOrientation(converted_to_geom_poses, true); + } + + return converted_to_geom_poses; +}; + double calcInterpolatedZ( const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const geometry_msgs::msg::Point target_pos, const size_t seg_idx) @@ -1838,6 +1857,7 @@ std::vector calcBound( } { + std::cerr << "finding bound points for parking space\n"; bound_points = getBoundWithParkingSpace(bound_points, route_handler, drivable_lanes, is_left); } @@ -1851,45 +1871,11 @@ lanelet::ConstPoints3d getIntersectionPointsForBoundWithParkingSpace( 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()); - - if (!tier4_autoware_utils::intersect( - lane_front_point, lane_back_point, bound_front_point, bound_back_point)) { - return {}; - } - 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); - 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]); - } - - motion_utils::insertOrientation(converted_to_geom_poses, true); - - 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); @@ -1914,20 +1900,37 @@ lanelet::ConstPoints3d calculateIntersectionPoints( 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 (front_point_within_parking_lot && back_point_within_parking_lot) { + + std::cerr << "points in points out\n"; + return is_left ? left_points_from_parking_lot : right_points_from_parking_lot; } - if (!is_left && front_point_within_parking_lot && back_point_within_parking_lot) { - return right_points_from_parking_lot; + std::cerr << "have intersection, need find the intersection " << '\n'; + + return calculateIntersectionPoints( + drivable_lane_bound, left_points_from_parking_lot, right_points_from_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::ConstPoints3d & left_points_from_parking_lot, + const lanelet::ConstPoints3d & right_points_from_parking_lot, bool front_point_within_parking_lot, + bool back_point_within_parking_lot, bool is_left) +{ + if (left_points_from_parking_lot.size() < 2 || right_points_from_parking_lot.size() < 2) { + std::cerr << "Points less\n"; + return is_left ? left_points_from_parking_lot : right_points_from_parking_lot; } + lanelet::ConstPoints3d intersect_insertion_bound; + std::cerr << "First check\n"; 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_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) { @@ -1935,36 +1938,38 @@ lanelet::ConstPoints3d calculateIntersectionPoints( 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)); - const auto intersection_point = - tier4_autoware_utils::intersect(lane_pt1, lane_pt2, bound_pt1, bound_pt2); + const auto intersection_point = + tier4_autoware_utils::intersect(lane_pt1, lane_pt2, bound_pt1, bound_pt2); - if (!intersection_point) { - new_left_bound.push_back(drivable_lane_bound.at(i + 1)); - continue; - } + 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); - new_left_bound.push_back(intersection_point_lanelet_point); - break; - + 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()); + new_left_bound.insert( + new_left_bound.end(), left_points_from_parking_lot.begin(), + left_points_from_parking_lot.end()); + std::cerr << "returning\n"; return new_left_bound; } + std::cerr << "Second check\n"; - 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) { - } + if (!is_left && front_point_within_parking_lot && !back_point_within_parking_lot) { + lanelet::ConstPoints3d new_right_bound; + new_right_bound.push_back(drivable_lane_bound.front()); + const auto bound_pt1 = + lanelet::utils::conversion::toGeomMsgPt(left_points_from_parking_lot.back()); + const auto bound_pt2 = + lanelet::utils::conversion::toGeomMsgPt(right_points_from_parking_lot.front()); + 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)); const auto intersection_point = tier4_autoware_utils::intersect(lane_pt1, lane_pt2, bound_pt1, bound_pt2); @@ -1975,37 +1980,67 @@ lanelet::ConstPoints3d calculateIntersectionPoints( 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; - } + new_right_bound.insert(new_right_bound.end(), right_points_from_parking_lot.begin(), right_points_from_parking_lot.end()); + new_right_bound.push_back(intersection_point_lanelet_point); + new_right_bound.insert( + new_right_bound.end(), drivable_lane_bound.begin() + next_i, drivable_lane_bound.end()); + break; } - } - + std::cerr << "returning\n"; + return new_right_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)); + + // 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; + // } + // } + // } + + std::cerr << "No effects\n"; return intersect_insertion_bound; }