From 7d5e62367c35f4e3b780748329f1bc382d872ca6 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Thu, 14 Sep 2023 09:08:05 +0900 Subject: [PATCH] freespace grabber Signed-off-by: Muhammad Zulfaqar Azmi --- .../behavior_path_planner/utils/utils.hpp | 5 + .../src/utils/avoidance/utils.cpp | 155 +++++++++++++++++- .../behavior_path_planner/src/utils/utils.cpp | 140 ++++++++++++++++ 3 files changed, 298 insertions(+), 2 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 f418c9c7300f7..cf4e58a2e9079 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,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 a9f40dad86ae4..5b3fc23ff9844 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -21,11 +21,15 @@ #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include +#include #include #include +#include #include #include +#include +#include #include #include @@ -39,10 +43,12 @@ #include #include +#include #include #include #include #include +#include #include namespace behavior_path_planner::utils::avoidance @@ -913,7 +919,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) { @@ -932,6 +938,7 @@ void filterTargetObjects( const auto & ego_pos = planner_data->self_odometry->pose.pose.position; const auto & vehicle_width = planner_data->parameters.vehicle_width; const rclcpp::Time now = rclcpp::Clock(RCL_ROS_TIME).now(); + std::unordered_map> freespace_areas; // for goal const auto ego_idx = planner_data->findEgoIndex(path_points); @@ -1024,6 +1031,8 @@ void filterTargetObjects( const bool get_opposite = parameters->use_opposite_lane; lanelet::ConstLineString3d target_line{}; + lanelet::ConstLineString3d freespace_segment{}; + o.to_road_shoulder_distance = std::numeric_limits::max(); const auto update_road_to_shoulder_distance = [&](const auto & target_lanelet) { const auto lines = @@ -1048,14 +1057,156 @@ void filterTargetObjects( if (rh->getNextLaneletWithinRoute(overhang_lanelet, &next_lanelet)) { update_road_to_shoulder_distance(next_lanelet); } - debug.bounds.push_back(target_line); { o.to_road_shoulder_distance = extendToRoadShoulderDistanceWithPolygon( rh, target_line, o.to_road_shoulder_distance, overhang_lanelet, o.overhang_pose.position, 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; + // // }; + // } } + // 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 6927327e24ef4..a449607fb3f00 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 "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "motion_utils/trajectory/path_with_lane_id.hpp" @@ -23,6 +24,7 @@ #include #include #include +#include #include #include @@ -30,6 +32,7 @@ #include #include +#include #include #include @@ -1834,9 +1837,146 @@ 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)); } +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; + } + + 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); + 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; + } + + 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(); + + 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(); }); + + 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()){ + 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(), 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( PathWithLaneId & path, const std::shared_ptr & planner_data, const bool is_bound_left)