From 861539e12e22b77cfb9f84deb1f65747442e785d Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Wed, 28 Dec 2022 18:40:06 +0900 Subject: [PATCH] feat(behavior_path_planner): separateObjectsByLanelets Signed-off-by: kosuke55 --- .../behavior_path_planner/utilities.hpp | 19 ++++- .../pull_out/geometric_pull_out.cpp | 4 +- .../scene_module/pull_out/shift_pull_out.cpp | 4 +- .../scene_module/pull_over/goal_searcher.cpp | 4 +- .../behavior_path_planner/src/utilities.cpp | 77 +++++++++++++++++-- 5 files changed, 92 insertions(+), 16 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index c1f9f05587a93..0595ab5e1e36e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -43,10 +43,10 @@ #else #include #endif - #include #include #include +#include #include namespace behavior_path_planner::util @@ -88,7 +88,7 @@ struct ProjectedDistancePoint double distance{0.0}; }; -template > +template > ProjectedDistancePoint pointToSegment( const Point2d & reference_point, const Point2d & point_from_ego, const Point2d & point_from_object); @@ -266,7 +266,20 @@ std::vector filterObjectIndicesByLanelets( std::vector filterObjectIndicesByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); -PredictedObjects filterObjectsByLanelets( +/** + * @brief Separate index of the obstacles into two part based on whether the object is within + * lanelet. + * @return Indices of objects pair. first objects are in the lanelet, and second others are out of + * lanelet. + */ +std::pair, std::vector> separateObjectIndicesByLanelets( + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); + +/** + * @brief Separate the objects into two part based on whether the object is within lanelet. + * @return Objects pair. first objects are in the lanelet, and second others are out of lanelet. + */ +std::pair separateObjectsByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); std::vector filterObjectsIndicesByPath( diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp index 76ccead98268e..a080eefdf64f7 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp @@ -52,8 +52,8 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p // collision check with objects in shoulder lanes const auto arc_path = planner_.getArcPath(); - const auto shoulder_lane_objects = - util::filterObjectsByLanelets(*(planner_data_->dynamic_object), shoulder_lanes); + const auto [shoulder_lane_objects, others] = + util::separateObjectsByLanelets(*(planner_data_->dynamic_object), shoulder_lanes); if (util::checkCollisionBetweenPathFootprintsAndObjects( vehicle_footprint_, arc_path, shoulder_lane_objects, parameters_.collision_check_margin)) { return {}; diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp index d16d69839ba36..385bbe6486edd 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp @@ -56,8 +56,8 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) } // extract objects in shoulder lane for collision check - const auto shoulder_lane_objects = - util::filterObjectsByLanelets(*dynamic_objects, shoulder_lanes); + const auto [shoulder_lane_objects, others] = + util::separateObjectsByLanelets(*dynamic_objects, shoulder_lanes); // get safe path for (auto & pull_out_path : pull_out_paths) { diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp index a9724f018172c..f8ef93edc55a5 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp @@ -62,8 +62,8 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) route_handler->getCenterLinePath(pull_over_lanes, s_start, s_end), parameters_.goal_search_interval); - const auto shoulder_lane_objects = - util::filterObjectsByLanelets(*(planner_data_->dynamic_object), pull_over_lanes); + const auto [shoulder_lane_objects, others] = + util::separateObjectsByLanelets(*(planner_data_->dynamic_object), pull_over_lanes); std::vector original_search_poses{}; // for search area visualizing size_t goal_id = 0; diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index b899439215a02..2127e560db5e0 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -598,16 +598,79 @@ std::vector filterObjectIndicesByLanelets( return indices; } -PredictedObjects filterObjectsByLanelets( +std::pair, std::vector> separateObjectIndicesByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) { - PredictedObjects filtered_objects; - const auto indices = filterObjectIndicesByLanelets(objects, target_lanelets); - filtered_objects.objects.reserve(indices.size()); - for (const size_t i : indices) { - filtered_objects.objects.push_back(objects.objects.at(i)); + if (target_lanelets.empty()) { + return {}; + } + + std::vector target_indices; + std::vector other_indices; + + for (size_t i = 0; i < objects.objects.size(); i++) { + // create object polygon + const auto & obj = objects.objects.at(i); + // create object polygon + Polygon2d obj_polygon; + if (!util::calcObjectPolygon(obj, &obj_polygon)) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("utilities"), + "Failed to calcObjectPolygon...!!!"); + continue; + } + + bool is_filtered_object = false; + + for (const auto & llt : target_lanelets) { + // create lanelet polygon + const auto polygon2d = llt.polygon2d().basicPolygon(); + if (polygon2d.empty()) { + // no lanelet polygon + continue; + } + Polygon2d lanelet_polygon; + for (const auto & lanelet_point : polygon2d) { + lanelet_polygon.outer().emplace_back(lanelet_point.x(), lanelet_point.y()); + } + lanelet_polygon.outer().push_back(lanelet_polygon.outer().front()); + // check the object does not intersect the lanelet + if (!boost::geometry::disjoint(lanelet_polygon, obj_polygon)) { + target_indices.push_back(i); + is_filtered_object = true; + break; + } + } + + if (!is_filtered_object) { + other_indices.push_back(i); + } } - return filtered_objects; + + return std::make_pair(target_indices, other_indices); +} + +std::pair separateObjectsByLanelets( + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) +{ + PredictedObjects target_objects; + PredictedObjects other_objects; + + const auto [target_indices, other_indices] = + separateObjectIndicesByLanelets(objects, target_lanelets); + + target_objects.objects.reserve(target_indices.size()); + other_objects.objects.reserve(other_indices.size()); + + for (const size_t i : target_indices) { + target_objects.objects.push_back(objects.objects.at(i)); + } + + for (const size_t i : other_indices) { + other_objects.objects.push_back(objects.objects.at(i)); + } + + return std::make_pair(target_objects, other_objects); } bool calcObjectPolygon(const PredictedObject & object, Polygon2d * object_polygon)