Skip to content

Commit

Permalink
freespace grabber
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 19, 2023
1 parent 64d8080 commit 7d5e623
Show file tree
Hide file tree
Showing 3 changed files with 298 additions and 2 deletions.
5 changes: 5 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,11 @@ 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 getBoundWithParkingSpace(
const std::vector<lanelet::ConstPoint3d> & original_bound,
const std::shared_ptr<RouteHandler> & route_handler,
const std::vector<DrivableLanes> & drivable_lanes, const bool is_left);

std::vector<lanelet::ConstPoint3d> getBoundWithHatchedRoadMarkings(
const std::vector<lanelet::ConstPoint3d> & original_bound,
const std::shared_ptr<RouteHandler> & route_handler);
Expand Down
155 changes: 153 additions & 2 deletions planning/behavior_path_planner/src/utils/avoidance/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,11 +21,15 @@
#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"

#include <autoware_auto_tf2/tf2_autoware_auto_msgs.hpp>
#include <lanelet2_extension/utility/message_conversion.hpp>
#include <lanelet2_extension/utility/utilities.hpp>
#include <motion_utils/trajectory/interpolation.hpp>
#include <motion_utils/trajectory/trajectory.hpp>
#include <tier4_autoware_utils/geometry/geometry.hpp>
#include <tier4_autoware_utils/ros/uuid_helper.hpp>

#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 <boost/geometry.hpp>
Expand All @@ -39,10 +43,12 @@
#include <lanelet2_routing/RoutingGraphContainer.h>

#include <algorithm>
#include <cstddef>
#include <limits>
#include <memory>
#include <set>
#include <string>
#include <utility>
#include <vector>

namespace behavior_path_planner::utils::avoidance
Expand Down Expand Up @@ -913,7 +919,7 @@ void compensateDetectionLost(
}

void filterTargetObjects(
ObjectDataArray & objects, AvoidancePlanningData & data, DebugData & debug,
ObjectDataArray & objects, AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug,
const std::shared_ptr<const PlannerData> & planner_data,
const std::shared_ptr<AvoidanceParameters> & parameters)
{
Expand All @@ -932,6 +938,7 @@ void filterTargetObjects(
const auto & ego_pos = planner_data->self_odometry->pose.pose.position;
const auto & vehicle_width = planner_data->parameters.vehicle_width;
const rclcpp::Time now = rclcpp::Clock(RCL_ROS_TIME).now();
std::unordered_map<lanelet::Id, std::vector<geometry_msgs::msg::Pose>> freespace_areas;

// for goal
const auto ego_idx = planner_data->findEgoIndex(path_points);
Expand Down Expand Up @@ -1024,6 +1031,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 @@ -1048,14 +1057,156 @@ 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(
rh, target_line, o.to_road_shoulder_distance, overhang_lanelet, o.overhang_pose.position,
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;
// // };
// }
}
// debug.bounds.push_back(target_line);
// if (!freespace_segment.empty()) {
// debug.bounds.push_back(freespace_segment);
// }
}

// calculate avoid_margin dynamically
Expand Down
140 changes: 140 additions & 0 deletions planning/behavior_path_planner/src/utils/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@

#include "behavior_path_planner/utils/utils.hpp"

#include "behavior_path_planner/data_manager.hpp"
#include "behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp"
#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp"
#include "motion_utils/trajectory/path_with_lane_id.hpp"
Expand All @@ -23,13 +24,15 @@
#include <lanelet2_extension/utility/query.hpp>
#include <lanelet2_extension/utility/utilities.hpp>
#include <motion_utils/resample/resample.hpp>
#include <route_handler/route_handler.hpp>
#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
#include <tier4_autoware_utils/math/unit_conversion.hpp>

#include <boost/geometry/algorithms/is_valid.hpp>

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

#include <algorithm>
Expand Down Expand Up @@ -1834,9 +1837,146 @@ std::vector<geometry_msgs::msg::Point> calcBound(
getBoundWithIntersectionAreas(bound_points, route_handler, drivable_lanes, is_left);
}

{
bound_points = getBoundWithParkingSpace(bound_points, route_handler, drivable_lanes, is_left);
}

return motion_utils::removeOverlapPoints(to_ros_point(bound_points));
}

lanelet::ConstPoints3d getBoundWithParkingSpace(
const std::vector<lanelet::ConstPoint3d> & original_bound,
const std::shared_ptr<RouteHandler> & route_handler,
const std::vector<DrivableLanes> & drivable_lanes, const bool is_left)
{
if(!is_left){
return original_bound;
}

const auto found_intersection =
[](
const lanelet::ConstPoints3d & drivable_lane_bound,
const lanelet::ConstPolygon3d & parking_lot) -> lanelet::ConstPoints3d {
if (drivable_lane_bound.size() < 2 || parking_lot.size() < 2) {
return {};
}

const auto lane_front_pt = lanelet::utils::conversion::toGeomMsgPt(drivable_lane_bound.front());
const auto lane_back_pt = lanelet::utils::conversion::toGeomMsgPt(drivable_lane_bound.back());
const auto bound_front_pt = lanelet::utils::conversion::toGeomMsgPt(parking_lot.front());
const auto bound_back_pt = lanelet::utils::conversion::toGeomMsgPt(parking_lot.back());

const auto rough_intersection =
tier4_autoware_utils::intersect(lane_front_pt, lane_back_pt, bound_front_pt, bound_back_pt);

// just check once if there exist intersection. reduce the need to iterate too much.
if (!rough_intersection) {
return {};
}

const auto front_point_within_parking_lot = boost::geometry::within(drivable_lane_bound.front(), parking_lot);

lanelet::ConstPoints3d intersect_insertion_bound;
for (size_t i = 0; i < drivable_lane_bound.size(); ++i) {
size_t next_i = (i + 1) % drivable_lane_bound.size();
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 < parking_lot.size(); ++j) {
size_t next_j = (j + 1) % 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(), parking_lot.begin() + static_cast<int>(next_j), parking_lot.end());

// If the range wrapped around, insert the remaining elements from the beginning of parking_lot
if (next_j > 0) {
intersect_insertion_bound.insert(intersect_insertion_bound.end(), parking_lot.begin(), 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;
}
}
}
return {};
};

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

// check if intersect point exist
const auto lane_bound = check_lane.leftBound();

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 = found_intersection(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;
}

void makeBoundLongitudinallyMonotonic(
PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data,
const bool is_bound_left)
Expand Down

0 comments on commit 7d5e623

Please sign in to comment.