Skip to content

Commit

Permalink
edit using segment
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 27, 2023
1 parent fdc96a0 commit 40b5c7b
Show file tree
Hide file tree
Showing 2 changed files with 145 additions and 75 deletions.
16 changes: 16 additions & 0 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 @@ -29,11 +29,13 @@
#include <autoware_auto_planning_msgs/msg/path.hpp>
#include <autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp>
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
#include <geometry_msgs/msg/detail/pose__struct.hpp>
#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/pose_array.hpp>

#include <lanelet2_core/Forward.h>
#include <lanelet2_core/primitives/Point.h>
#include <tf2/utils.h>

#ifdef ROS_DISTRO_GALACTIC
Expand Down Expand Up @@ -101,6 +103,20 @@ struct PolygonPoint
};
};

struct Segment
{
Point start;
Point end;
lanelet::ConstPoint3d start_pt;
lanelet::ConstPoint3d end_pt;
bool start_is_outside = true;

[[nodiscard]] std::optional<Point> is_intersect(const Segment & segment) const
{
return tier4_autoware_utils::intersect(this->start, this->end, segment.start, segment.end);
}
};

struct FrenetPoint
{
double length{0.0}; // longitudinal
Expand Down
204 changes: 129 additions & 75 deletions planning/behavior_path_planner/src/utils/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,18 +24,25 @@
#include <lanelet2_extension/utility/query.hpp>
#include <lanelet2_extension/utility/utilities.hpp>
#include <motion_utils/resample/resample.hpp>
#include <motion_utils/trajectory/trajectory.hpp>
#include <route_handler/route_handler.hpp>
#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
#include <tier4_autoware_utils/geometry/geometry.hpp>
#include <tier4_autoware_utils/math/unit_conversion.hpp>

#include <boost/geometry/algorithms/detail/covered_by/interface.hpp>
#include <boost/geometry/algorithms/detail/within/interface.hpp>
#include <boost/geometry/algorithms/is_valid.hpp>

#include <lanelet2_core/Forward.h>
#include <lanelet2_core/geometry/Point.h>
#include <lanelet2_core/geometry/Polygon.h>
#include <lanelet2_core/primitives/LineString.h>
#include <lanelet2_core/primitives/Point.h>
#include <lanelet2_routing/RoutingGraphContainer.h>

#include <algorithm>
#include <cstddef>
#include <limits>
#include <memory>
#include <string>
Expand Down Expand Up @@ -2046,6 +2053,29 @@ lanelet::ConstPoints3d calculateIntersectionPoints(
return intersect_insertion_bound;
}

bool intersectionBoundWithPolygon(
const lanelet::ConstPoints3d & bound_points, const lanelet::ConstPolygon3d & parking_lot)
{
if (bound_points.size() < 2 || parking_lot.size() < 2) {
return false;
}
const auto polygon_points = convertToGeometryPose(parking_lot, true);

for (size_t bound_idx = 0; bound_idx < bound_points.size() - 1; ++bound_idx) {
const auto bound_pt1 = lanelet::utils::conversion::toGeomMsgPt(bound_points[bound_idx]);
const auto bound_pt2 = lanelet::utils::conversion::toGeomMsgPt(bound_points[bound_idx + 1]);
for (size_t poly_idx = 0; poly_idx < polygon_points.size(); ++poly_idx) {
size_t next_poly_idx = (poly_idx + 1) % polygon_points.size();
const auto poly_pt1 = polygon_points[poly_idx].position;
const auto poly_pt2 = polygon_points[next_poly_idx].position;
if (tier4_autoware_utils::intersect(bound_pt1, bound_pt2, poly_pt1, poly_pt2)) {
return true;
}
}
}
return false;
}

bool parkingLotRegistered(
const lanelet::ConstPolygons3d & parking_lot_list,
const lanelet::ConstPolygon3d & current_parking_lot)
Expand All @@ -2067,6 +2097,10 @@ lanelet::ConstPoints3d getBoundWithParkingSpace(
return original_bound;
}

if (original_bound.size() < 2) {
return original_bound;
}

const auto lanelet_map_ptr = route_handler->getLaneletMapPtr();
const auto all_parking_lot = lanelet::utils::query::getAllParkingLots(lanelet_map_ptr);

Expand All @@ -2084,91 +2118,111 @@ lanelet::ConstPoints3d getBoundWithParkingSpace(
continue;
}

if (!intersectionBoundWithPolygon(original_bound, linked_parking_lot)) {
continue;
}

parking_lot_list.push_back(linked_parking_lot);
}
namespace bg = boost::geometry;

const auto to_linestring =
[](const auto & points) {
tier4_autoware_utils::LineString3d linestring;
linestring.reserve(points.size());

for(const auto & point:points){
bg::append(linestring, tier4_autoware_utils::Point3d(point.x(), point.y(), point.z()));
}

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

// 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();
if (parking_lot_list.empty()) {
return original_bound;
}

// 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_segments = [](const auto & points, bool loop = false) {
std::vector<Segment> segments;
segments.reserve(points.size());

// lanelet::ConstPoints3d tmp_bound{};
// tmp_bound.insert(tmp_bound.end(), shared_bound_start_itr, shared_bound_end_itr);
for (size_t i = 0; i < points.size(); i++) {
auto next_i = (i + 1) % points.size();
if (loop && next_i == 0) {
break;
}
Segment segment;
segment.start = lanelet::utils::conversion::toGeomMsgPt(points[i]);
segment.end = lanelet::utils::conversion::toGeomMsgPt(points[next_i]);
segment.start_pt = points[i];
segment.end_pt = points[next_i];
segments.push_back(segment);
}

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

// if (!bound_exist && intersection_exist.empty()) {
// new_bound_point.insert(
// new_bound_point.end(), linked_parking_lot.begin(), linked_parking_lot.end());
// }
const auto to_lanelet_points = [](const std::vector<Segment> & segments) {
lanelet::ConstPoints3d points;
if (segments.empty()) {
return points;
}

// 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());
// }
points.reserve(segments.size() + 1);
for (const auto & segment : segments) {
points.push_back(segment.start_pt);
}
points.push_back(segments.back().end_pt);
return points;
};
auto bound_segments = to_segments(original_bound);

const auto bound_front_covered_by_first_polygon =
boost::geometry::covered_by(original_bound.front(), parking_lot_list.front());
const auto bound_back_covered_by_first_polygon =
boost::geometry::covered_by(original_bound.back(), parking_lot_list.front());
const auto bound_back_covered_by_last_polygon =
(parking_lot_list.size() == 1)
? bound_back_covered_by_first_polygon
: boost::geometry::covered_by(original_bound.back(), parking_lot_list.back());

auto parking_lot_segments = to_segments(parking_lot_list.front(), true);

if (!bound_front_covered_by_first_polygon && !bound_back_covered_by_last_polygon) {
std::vector<Segment> new_segment;
new_segment.push_back(bound_segments.front());
for (const auto & bound_segment : bound_segments) {
for (const auto & parking_lot_segment : parking_lot_segments) {
auto is_intersect = bound_segment.is_intersect(parking_lot_segment);
if (!is_intersect) {
continue;
}

// if (bound_exist && !intersection_exist.empty()) {
// new_bound_point.insert(
// new_bound_point.end(), intersection_exist.begin(), intersection_exist.end());
// }
// }
std::vector<Point> bound_pt{bound_segment.start, bound_segment.end};
const auto lateral_dist =
motion_utils::calcLateralOffset(bound_pt, parking_lot_segment.start);
auto point_to_append =
(lateral_dist > 0 && is_left) ? parking_lot_segment.start : parking_lot_segment.end;
auto ll_pt_to_append =
(lateral_dist > 0 && is_left) ? parking_lot_segment.start_pt : parking_lot_segment.end_pt;

auto intersect_ll_pt = lanelet::utils::conversion::toLaneletPoint(*is_intersect);
Segment intersect_segment;
if (new_segment.back().start_is_outside) {
new_segment.back().end = *is_intersect;
new_segment.back().end_pt = intersect_ll_pt;
intersect_segment.start = *is_intersect;
intersect_segment.start_pt = intersect_ll_pt;
intersect_segment.end = point_to_append;
intersect_segment.end_pt = ll_pt_to_append;
} else {
intersect_segment.start = *is_intersect;
intersect_segment.start_pt = intersect_ll_pt;
intersect_segment.end = new_segment.back().end;
intersect_segment.end_pt = new_segment.back().end_pt;
new_segment.back().start = point_to_append;
new_segment.back().start_pt = ll_pt_to_append;
new_segment.back().end = *is_intersect;
new_segment.back().end_pt = intersect_ll_pt;
}
new_segment.push_back(intersect_segment);
if (!new_segment.back().start_is_outside) {
break;
}
new_segment.back().start_is_outside = !new_segment.back().start_is_outside;
}
}
return to_lanelet_points(new_segment);
}

return new_bound_point;
return {};
}

void makeBoundLongitudinallyMonotonic(
Expand Down

0 comments on commit 40b5c7b

Please sign in to comment.