Skip to content

Commit

Permalink
seems ok
Browse files Browse the repository at this point in the history 
Signed-off-by: Zulfaqar Azmi <[email protected]>
  • Loading branch information
@zulfaqar-azmi-t4
zulfaqar-azmi-t4 committed Oct 25, 2023
1 parent acb7fd0 commit 53769e5
Show file tree
Hide file tree
Showing 2 changed files with 134 additions and 97 deletions.
6 changes: 4 additions & 2 deletions planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -224,8 +224,10 @@ std::vector<geometry_msgs::msg::Point> 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,
Expand Down
225 changes: 130 additions & 95 deletions planning/behavior_path_planner/src/utils/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,25 @@

namespace
{
template <typename Primitive>
std::vector<geometry_msgs::msg::Pose> convertToGeometryPose(
const Primitive & polygon, bool skip_orientation = false)
{
using Poses = std::vector<geometry_msgs::msg::Pose>;
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)
Expand Down Expand Up @@ -1838,6 +1857,7 @@ std::vector<geometry_msgs::msg::Point> calcBound(
}

{
std::cerr << "finding bound points for parking space\n";
bound_points = getBoundWithParkingSpace(bound_points, route_handler, drivable_lanes, is_left);
}

Expand All @@ -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<geometry_msgs::msg::Pose> 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<lanelet::ConstPoints3d, lanelet::ConstPoints3d> {
const auto converted_drivable_lane_bound = convertToGeometryPose(drivable_lane_bound);
Expand All @@ -1914,57 +1900,76 @@ 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) {
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);
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);
Expand All @@ -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<int>(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<int>(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<int>(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<int>(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<int>(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<int>(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;
}

Expand Down

0 comments on commit 53769e5

Please sign in to comment.