Skip to content

Commit

Permalink
temp
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 20, 2023
1 parent 7d5e623 commit b95d27b
Show file tree
Hide file tree
Showing 3 changed files with 335 additions and 204 deletions.
8 changes: 8 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 @@ -223,6 +223,14 @@ std::vector<geometry_msgs::msg::Point> calcBound(
const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas,
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);

lanelet::ConstPoints3d getIntersectionPointsForBoundWithParkingSpace(
const lanelet::ConstPoints3d & drivable_lane_bound, const lanelet::ConstPolygon3d & parking_lot,
bool is_left = true);

lanelet::ConstPoints3d getBoundWithParkingSpace(
const std::vector<lanelet::ConstPoint3d> & original_bound,
const std::shared_ptr<RouteHandler> & route_handler,
Expand Down
299 changes: 157 additions & 142 deletions planning/behavior_path_planner/src/utils/avoidance/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@

#include <autoware_auto_tf2/tf2_autoware_auto_msgs.hpp>
#include <lanelet2_extension/utility/message_conversion.hpp>
#include <lanelet2_extension/utility/query.hpp>
#include <lanelet2_extension/utility/utilities.hpp>
#include <motion_utils/trajectory/interpolation.hpp>
#include <motion_utils/trajectory/trajectory.hpp>
Expand Down Expand Up @@ -1064,149 +1065,163 @@ void filterTargetObjects(
overhang_basic_pose, parameters->use_hatched_road_markings,
parameters->use_intersection_areas);

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

// 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<geometry_msgs::msg::Pose> 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<geometry_msgs::msg::Pose>{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<size_t>, std::vector<size_t>> {
// std::vector<size_t> left{};
// std::vector<size_t> 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;
// // };
// }
[[maybe_unused]] const auto parking_lot =
std::invoke([&]() -> std::optional<lanelet::ConstPolygon3d> {
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<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;
};

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<geometry_msgs::msg::Pose> 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<geometry_msgs::msg::Pose>{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<size_t>, std::vector<size_t>> {
std::vector<size_t> left{};
std::vector<size_t> 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);
}
// debug.bounds.push_back(target_line);
// if (!freespace_segment.empty()) {
// debug.bounds.push_back(freespace_segment);
// }
}

// calculate avoid_margin dynamically
Expand Down
Loading

0 comments on commit b95d27b

Please sign in to comment.