Skip to content

Commit

Permalink
using boost
Browse files Browse the repository at this point in the history 
Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
  • Loading branch information
@zulfaqar-azmi-t4
zulfaqar-azmi-t4 committed Oct 26, 2023
1 parent e9d1f69 commit 1b25f4a
Showing 1 changed file with 98 additions and 48 deletions.
146 changes: 98 additions & 48 deletions planning/behavior_path_planner/src/utils/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1902,7 +1902,6 @@ lanelet::ConstPoints3d getIntersectionPointsForBoundWithParkingSpace(
});

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;
}
Expand Down Expand Up @@ -1981,7 +1980,9 @@ lanelet::ConstPoints3d calculateIntersectionPoints(

const auto intersection_point_lanelet_point =
lanelet::utils::conversion::toLaneletPoint(*intersection_point);
new_right_bound.insert(new_right_bound.end(), right_points_from_parking_lot.begin(), right_points_from_parking_lot.end());
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());
Expand Down Expand Up @@ -2045,6 +2046,18 @@ lanelet::ConstPoints3d calculateIntersectionPoints(
return intersect_insertion_bound;
}

bool parkingLotRegistered(
const lanelet::ConstPolygons3d & parking_lot_list,
const lanelet::ConstPolygon3d & current_parking_lot)
{
const auto is_already_in_list =
[&current_parking_lot](const lanelet::ConstPolygon3d & parking_lot) {
return parking_lot.id() == current_parking_lot.id();
};

return std::any_of(parking_lot_list.begin(), parking_lot_list.end(), is_already_in_list);
}

lanelet::ConstPoints3d getBoundWithParkingSpace(
const std::vector<lanelet::ConstPoint3d> & original_bound,
const std::shared_ptr<RouteHandler> & route_handler,
Expand All @@ -2054,69 +2067,106 @@ lanelet::ConstPoints3d getBoundWithParkingSpace(
return original_bound;
}

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{};

lanelet::ConstPolygon3d linked_parking_lot;
lanelet::ConstPolygons3d parking_lot_list;
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(); });
if (parkingLotRegistered(parking_lot_list, linked_parking_lot)) {
continue;
}

// check if intersect point exist
const auto lane_bound = check_lane.leftBound();
parking_lot_list.push_back(linked_parking_lot);
}
namespace bg = boost::geometry;

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(); });
const auto to_linestring =
[](const auto & points) {
tier4_autoware_utils::LineString3d linestring;
linestring.reserve(points.size());

lanelet::ConstPoints3d tmp_bound{};
tmp_bound.insert(tmp_bound.end(), shared_bound_start_itr, shared_bound_end_itr);
for(const auto & point:points){
bg::append(linestring, tier4_autoware_utils::Point3d(point.x(), point.y(), point.z()));
}

const auto intersection_exist =
getIntersectionPointsForBoundWithParkingSpace(tmp_bound, linked_parking_lot);
if (bound_exist && intersection_exist.empty()) {
continue;
}
return linestring;
};

auto union_bound = to_linestring(original_bound);

for(const auto & polygon_parking_lot : parking_lot_list){
auto linestring_parking_lot = to_linestring(polygon_parking_lot);
const auto union_results =
}

// lanelet::Ids parking_lots_ids{};
// for (const auto & drivable_lane : drivable_lanes) {
// 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;
// }

if (!bound_exist && intersection_exist.empty()) {
new_bound_point.insert(
new_bound_point.end(), linked_parking_lot.begin(), linked_parking_lot.end());
}
// 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(); });

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());
}
// // check if intersect point exist
// const auto lane_bound = check_lane.leftBound();

if (bound_exist && !intersection_exist.empty()) {
new_bound_point.insert(
new_bound_point.end(), intersection_exist.begin(), intersection_exist.end());
}
}
// 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 =
// 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(), 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;
}
Expand Down

0 comments on commit 1b25f4a

Please sign in to comment.