diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp index e1256fd774027..4d5b9052a4681 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp @@ -117,14 +117,16 @@ void filterObjectsByClass( * lanelet. */ std::pair, std::vector> separateObjectIndicesByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, + const std::function & condition); /** * @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); + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, + const std::function & condition); /** * @brief Get the predicted path from an object. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index c5e5aa3e50191..3e03a09d3adf8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -336,6 +336,8 @@ std::optional getSignedDistanceFromBoundary( // misc +Polygon2d toPolygon2d(const lanelet::ConstLanelet & lanelet); + std::vector getTargetLaneletPolygons( const lanelet::ConstLanelets & lanelets, const Pose & pose, const double check_length, const std::string & target_type); diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index ce88760212d90..4de8720a4a52d 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -21,6 +21,7 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" #include @@ -1300,12 +1301,17 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const return false; } + const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto lanelet_polygon = utils::toPolygon2d(lanelet); + return !boost::geometry::disjoint(lanelet_polygon, object_polygon); + }; const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, parameters_->forward_goal_search_length); const auto [pull_over_lane_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( - *(planner_data_->dynamic_object), pull_over_lanes); + *(planner_data_->dynamic_object), pull_over_lanes, condition); const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( pull_over_lane_objects, parameters_->th_moving_object_velocity); const auto common_parameters = planner_data_->parameters; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp index 6d06b26cd1e17..ce49501b78d08 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp @@ -17,6 +17,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 "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include #include @@ -140,9 +141,14 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects( { const auto p = std::dynamic_pointer_cast(avoidance_parameters_); + const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto lanelet_polygon = utils::toPolygon2d(lanelet); + return !boost::geometry::disjoint(lanelet_polygon, object_polygon); + }; const auto [object_within_target_lane, object_outside_target_lane] = utils::path_safety_checker::separateObjectsByLanelets( - *planner_data_->dynamic_object, data.current_lanelets); + *planner_data_->dynamic_object, data.current_lanelets, condition); // Assume that the maximum allocation for data.other object is the sum of // objects_within_target_lane and object_outside_target_lane. The maximum allocation for diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 092b91499252c..f19bcf30e7771 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -20,6 +20,7 @@ #include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" #include "motion_utils/trajectory/trajectory.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include #include @@ -770,9 +771,14 @@ std::vector StartPlannerModule::searchPullOutStartPoses( std::vector pull_out_start_pose{}; // filter pull out lanes stop objects + const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto lanelet_polygon = utils::toPolygon2d(lanelet); + return !boost::geometry::disjoint(lanelet_polygon, object_polygon); + }; const auto [pull_out_lane_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( - *planner_data_->dynamic_object, status_.pull_out_lanes); + *planner_data_->dynamic_object, status_.pull_out_lanes, condition); const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( pull_out_lane_objects, parameters_->th_moving_object_velocity); diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index b2b4ecd2ea42a..86886e4414838 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -564,25 +564,6 @@ double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const return v * std::cos(calcYawDeviation(p_ref, p_target)); } -bool isCentroidWithinLanelets( - const PredictedObject & object, const lanelet::ConstLanelets & target_lanelets) -{ - if (target_lanelets.empty()) { - return false; - } - - const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position; - lanelet::BasicPoint2d object_centroid(object_pos.x, object_pos.y); - - for (const auto & llt : target_lanelets) { - if (boost::geometry::within(object_centroid, llt.polygon2d().basicPolygon())) { - return true; - } - } - - return false; -} - lanelet::ConstLanelets getTargetLanelets( const std::shared_ptr & planner_data, lanelet::ConstLanelets & route_lanelets, const double left_offset, const double right_offset) @@ -1591,14 +1572,26 @@ std::vector getSafetyCheckTargetObjects( std::vector target_objects; - const auto append_target_objects = [&](const auto & check_lanes, const auto & objects) { - std::for_each(objects.begin(), objects.end(), [&](const auto & object) { - if (isCentroidWithinLanelets(object.object, check_lanes)) { - target_objects.push_back(utils::avoidance::transform(object.object, p)); - } + const auto append = [&](const auto & objects) { + std::for_each(objects.objects.begin(), objects.objects.end(), [&](const auto & object) { + target_objects.push_back(utils::avoidance::transform(object, p)); }); }; + const auto to_predicted_objects = [&p](const auto & objects) { + PredictedObjects ret{}; + std::for_each(objects.begin(), objects.end(), [&p, &ret](const auto & object) { + ret.objects.push_back(object.object); + }); + return ret; + }; + + const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { + const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position; + lanelet::BasicPoint2d object_centroid(object_pos.x, object_pos.y); + return boost::geometry::within(object_centroid, lanelet.polygon2d().basicPolygon()); + }; + const auto unavoidable_objects = [&data]() { ObjectDataArray ret; std::for_each(data.target_objects.begin(), data.target_objects.end(), [&](const auto & object) { @@ -1614,11 +1607,15 @@ std::vector getSafetyCheckTargetObjects( const auto check_lanes = getAdjacentLane(planner_data, p, true); if (p->check_other_object) { - append_target_objects(check_lanes, data.other_objects); + const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( + to_predicted_objects(data.other_objects), check_lanes, condition); + append(targets); } if (p->check_unavoidable_object) { - append_target_objects(check_lanes, unavoidable_objects); + const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( + to_predicted_objects(unavoidable_objects), check_lanes, condition); + append(targets); } } @@ -1627,11 +1624,15 @@ std::vector getSafetyCheckTargetObjects( const auto check_lanes = getAdjacentLane(planner_data, p, false); if (p->check_other_object) { - append_target_objects(check_lanes, data.other_objects); + const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( + to_predicted_objects(data.other_objects), check_lanes, condition); + append(targets); } if (p->check_unavoidable_object) { - append_target_objects(check_lanes, unavoidable_objects); + const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( + to_predicted_objects(unavoidable_objects), check_lanes, condition); + append(targets); } } @@ -1640,11 +1641,15 @@ std::vector getSafetyCheckTargetObjects( const auto check_lanes = data.current_lanelets; if (p->check_other_object) { - append_target_objects(check_lanes, data.other_objects); + const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( + to_predicted_objects(data.other_objects), check_lanes, condition); + append(targets); } if (p->check_unavoidable_object) { - append_target_objects(check_lanes, unavoidable_objects); + const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( + to_predicted_objects(unavoidable_objects), check_lanes, condition); + append(targets); } } diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index 11173154f6b3a..bd374c12427bc 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -266,13 +266,18 @@ void GoalSearcher::countObjectsToAvoid( void GoalSearcher::update(GoalCandidates & goal_candidates) const { + const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto lanelet_polygon = utils::toPolygon2d(lanelet); + return !boost::geometry::disjoint(lanelet_polygon, object_polygon); + }; const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( *(planner_data_->dynamic_object), parameters_.th_moving_object_velocity); const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length); const auto [pull_over_lane_stop_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets(stop_objects, pull_over_lanes); + utils::path_safety_checker::separateObjectsByLanelets(stop_objects, pull_over_lanes, condition); if (parameters_.prioritize_goals_before_objects) { countObjectsToAvoid(goal_candidates, pull_over_lane_stop_objects); diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp index ff54ce2d2dcc4..e63d35f7bc925 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp @@ -128,7 +128,8 @@ void filterObjectsByClass( } std::pair, std::vector> separateObjectIndicesByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, + const std::function & condition) { if (target_lanelets.empty()) { return {}; @@ -138,25 +139,9 @@ std::pair, std::vector> separateObjectIndicesByLanel 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 - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj); 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)) { + if (condition(objects.objects.at(i), llt)) { target_indices.push_back(i); is_filtered_object = true; break; @@ -172,13 +157,14 @@ std::pair, std::vector> separateObjectIndicesByLanel } std::pair separateObjectsByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, + const std::function & condition) { PredictedObjects target_objects; PredictedObjects other_objects; const auto [target_indices, other_indices] = - separateObjectIndicesByLanelets(objects, target_lanelets); + separateObjectIndicesByLanelets(objects, target_lanelets, condition); target_objects.objects.reserve(target_indices.size()); other_objects.objects.reserve(other_indices.size()); diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp index bc071e2a2a288..f4bfb4d681440 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp @@ -18,6 +18,7 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -62,10 +63,15 @@ boost::optional GeometricPullOut::plan(const Pose & start_pose, con // collision check with stop objects in pull out lanes const auto arc_path = planner_.getArcPath(); + const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto lanelet_polygon = utils::toPolygon2d(lanelet); + return !boost::geometry::disjoint(lanelet_polygon, object_polygon); + }; const auto & stop_objects = utils::path_safety_checker::filterObjectsByVelocity( *(planner_data_->dynamic_object), parameters_.th_moving_object_velocity); const auto [pull_out_lane_stop_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets(stop_objects, pull_out_lanes); + utils::path_safety_checker::separateObjectsByLanelets(stop_objects, pull_out_lanes, condition); if (utils::checkCollisionBetweenPathFootprintsAndObjects( vehicle_footprint_, arc_path, pull_out_lane_stop_objects, diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp index b82157cc17e41..02c6d41bdbf08 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp @@ -20,6 +20,7 @@ #include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" #include "motion_utils/trajectory/path_with_lane_id.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -64,8 +65,14 @@ boost::optional ShiftPullOut::plan(const Pose & start_pose, const P } // extract stop objects in pull out lane for collision check + const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto lanelet_polygon = utils::toPolygon2d(lanelet); + return !boost::geometry::disjoint(lanelet_polygon, object_polygon); + }; const auto [pull_out_lane_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets(*dynamic_objects, pull_out_lanes); + utils::path_safety_checker::separateObjectsByLanelets( + *dynamic_objects, pull_out_lanes, condition); const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( pull_out_lane_objects, parameters_.th_moving_object_velocity); diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 7305405b50410..03063b5a9e2fe 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -2675,6 +2675,19 @@ double getArcLengthToTargetLanelet( std::min(arc_front.length - arc_pose.length, arc_back.length - arc_pose.length), 0.0); } +Polygon2d toPolygon2d(const lanelet::ConstLanelet & lanelet) +{ + Polygon2d polygon; + for (const auto & p : lanelet.polygon2d().basicPolygon()) { + polygon.outer().emplace_back(p.x(), p.y()); + } + polygon.outer().push_back(polygon.outer().front()); + + return tier4_autoware_utils::isClockwise(polygon) + ? polygon + : tier4_autoware_utils::inverseClockwise(polygon); +} + std::vector getTargetLaneletPolygons( const lanelet::PolygonLayer & map_polygons, lanelet::ConstLanelets & lanelets, const Pose & pose, const double check_length, const std::string & target_type)