Skip to content

Commit

Permalink
able to get correct segment for "most" cases
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 16, 2023
1 parent 3624532 commit d8112e7
Showing 1 changed file with 119 additions and 58 deletions.
177 changes: 119 additions & 58 deletions planning/behavior_path_planner/src/utils/avoidance/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#include "behavior_path_planner/utils/avoidance/utils.hpp"
#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp"
#include "behavior_path_planner/utils/path_utils.hpp"
#include "lanelet2_extension/utility/query.hpp"
#include "motion_utils/trajectory/path_with_lane_id.hpp"
#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"

Expand All @@ -32,8 +33,6 @@
#include <geometry_msgs/msg/detail/pose__struct.hpp>
#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
#include <tier4_planning_msgs/msg/avoidance_debug_factor.hpp>
#include <lanelet2_core/Forward.h>
#include <lanelet2_core/primitives/Polygon.h>

#include <boost/geometry.hpp>
#include <boost/geometry/algorithms/convex_hull.hpp>
Expand Down Expand Up @@ -1020,6 +1019,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<double>::max();
const auto update_road_to_shoulder_distance = [&](const auto & target_lanelet) {
const auto lines =
Expand All @@ -1044,7 +1045,6 @@ 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(
Expand Down Expand Up @@ -1081,70 +1081,131 @@ void filterTargetObjects(
return converted_to_geom_poses;
};

if(parking_lot){
if(freespace_areas.find(parking_lot->id()) == freespace_areas.end()){
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});
}

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());
}
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 {left, right};
return o.to_road_shoulder_distance;
});
std::cerr << "left ids:= " << '\t';
for (const auto left_id : left_ids) {
std::cerr << left_id << '\t';
}
std::cerr << '\n';
std::cerr << "right ids:= " << '\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 get_distance_to_linestring =
// [&](const lanelet::ConstPoint3d & ll_point_1, const lanelet::ConstPoint3d & ll_point_2) {
// const auto p1 = lanelet::utils::conversion::toGeomMsgPt(ll_point_1);
// const auto p2 = lanelet::utils::conversion::toGeomMsgPt(ll_point_2);

// return intersect_with_target_line(p1, p2);
// };

std::vector<double> overhang_to_freespace_ls{};
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);
}
}

Expand Down

0 comments on commit d8112e7

Please sign in to comment.