From e94e02109463e948946df7ad9ce0f1330b6ed7d4 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Mon, 16 Oct 2023 18:50:25 +0900 Subject: [PATCH] temp Signed-off-by: Zulfaqar Azmi --- .../src/utils/avoidance/utils.cpp | 296 +++++++++--------- .../behavior_path_planner/src/utils/utils.cpp | 47 +++ 2 files changed, 189 insertions(+), 154 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index d91e01a3a9ca2..daa7cc55dd55f 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -1066,161 +1066,149 @@ void filterTargetObjects( overhang_basic_pose, parameters->use_hatched_road_markings, parameters->use_intersection_areas); - [[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); + // 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); + // } } // 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 cb0486b6cd01b..7f1aa41573045 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -14,6 +14,7 @@ #include "behavior_path_planner/utils/utils.hpp" +#include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp" #include "motion_utils/trajectory/path_with_lane_id.hpp" @@ -21,6 +22,7 @@ #include #include #include +#include #include #include @@ -31,6 +33,7 @@ #include #include +#include #include #include @@ -1838,6 +1841,50 @@ std::vector calcBound( return motion_utils::removeOverlapPoints(to_ros_point(bound_points)); } +std::vector getBoundWithParkingSpace( + const std::vector & original_bound, + [[maybe_unused]]const std::shared_ptr & route_handler, + const std::vector & drivable_lanes, const bool is_left) +{ + 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::ConstPolygons3d linked_parking_lots; + + for (const auto & lanes : drivable_lanes) { + if (is_left) { + const auto & left_lane = lanes.left_lane; + + lanelet::ConstPolygon3d linked_parking_lot; + const auto found_parking_lot = + lanelet::utils::query::getLinkedParkingLot(left_lane, all_parking_lot, &linked_parking_lot); + + if (!found_parking_lot) { + continue; + } + + 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(); }); + + 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); + } + } + } + + std::vector new_bound_point; + + +} + void makeBoundLongitudinallyMonotonic( PathWithLaneId & path, const std::shared_ptr & planner_data, const bool is_bound_left)