From 47ce8dd068ac41d4052d6ba562df2479f07b4d47 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Tue, 26 Sep 2023 14:40:23 +0900 Subject: [PATCH] feat(behavior_path_planner): lane change collided polygon intersect lanes Signed-off-by: Muhammad Zulfaqar Azmi --- .../utils/lane_change/utils.hpp | 3 ++ .../path_safety_checker/safety_check.hpp | 2 +- .../avoidance/avoidance_module.cpp | 2 +- .../goal_planner/goal_planner_module.cpp | 2 +- .../src/scene_module/lane_change/normal.cpp | 36 +++++++++++++------ .../start_planner/start_planner_module.cpp | 2 +- .../src/utils/lane_change/utils.cpp | 12 +++++++ .../path_safety_checker/safety_check.cpp | 15 +++++--- 8 files changed, 55 insertions(+), 19 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index a4c2c2e695752..b66ec4c481038 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -185,5 +185,8 @@ std::optional createPolygon( ExtendedPredictedObject transform( const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters, const LaneChangeParameters & lane_change_parameters); + +bool isCollidedPolygonsInLanelet( + const std::vector & collided_polygons, const lanelet::ConstLanelets & lanes); } // namespace behavior_path_planner::utils::lane_change #endif // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index f9fbf70b2113b..c8bcc5de976dd 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -103,7 +103,7 @@ boost::optional getInterpolatedPoseWithVeloci * @param debug The debug information for collision checking. * @return true if distance is safe. */ -bool checkCollision( +std::optional> checkCollision( const PathWithLaneId & planned_path, const std::vector & predicted_ego_path, const ExtendedPredictedObject & target_object, diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 83789818c456b..b740fb86ce555 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -1889,7 +1889,7 @@ bool AvoidanceModule::isSafePath( : ego_predicted_path_for_rear_object; for (const auto & obj_path : obj_predicted_paths) { - if (!utils::path_safety_checker::checkCollision( + if (utils::path_safety_checker::checkCollision( shifted_path.path, ego_predicted_path, object, obj_path, p, parameters_->rss_params, hysteresis_factor, current_debug_data.second)) { marker_utils::updateCollisionCheckDebugMap( 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 1f5a823386774..0a767e48607a1 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 @@ -1587,7 +1587,7 @@ bool GoalPlannerModule::isSafePath() const // If a collision is detected, mark the object as unsafe and break the loop for (const auto & obj_path : obj_predicted_paths) { - if (!utils::path_safety_checker::checkCollision( + if (utils::path_safety_checker::checkCollision( pull_over_path, ego_predicted_path, object, obj_path, planner_data_->parameters, safety_check_params_->rss_params, hysteresis_factor, current_debug_data.second)) { marker_utils::updateCollisionCheckDebugMap( diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 984a45e7b3ecf..8e92f98bb08dd 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -1443,18 +1443,32 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( obj, lane_change_parameters_->use_all_predicted_path); for (const auto & obj_path : obj_predicted_paths) { - if (!utils::path_safety_checker::checkCollision( - path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0, - current_debug_data.second)) { - path_safety_status.is_safe = false; - marker_utils::updateCollisionCheckDebugMap( - debug_data, current_debug_data, path_safety_status.is_safe); - const auto & obj_pose = obj.initial_pose.pose; - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape); - path_safety_status.is_object_coming_from_rear |= - !utils::path_safety_checker::isTargetObjectFront( - path, current_pose, common_parameters.vehicle_info, obj_polygon); + const auto have_collision = utils::path_safety_checker::checkCollision( + path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0, + current_debug_data.second); + + if (!have_collision) { + continue; } + + const auto & collided_polygons = *have_collision; + const auto collision_in_current_lanes = utils::lane_change::isCollidedPolygonsInLanelet( + collided_polygons, lane_change_path.info.current_lanes); + const auto collision_in_target_lanes = utils::lane_change::isCollidedPolygonsInLanelet( + collided_polygons, lane_change_path.info.target_lanes); + + if (!collision_in_current_lanes && !collision_in_target_lanes) { + continue; + } + + path_safety_status.is_safe = false; + marker_utils::updateCollisionCheckDebugMap( + debug_data, current_debug_data, path_safety_status.is_safe); + const auto & obj_pose = obj.initial_pose.pose; + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape); + path_safety_status.is_object_coming_from_rear |= + !utils::path_safety_checker::isTargetObjectFront( + path, current_pose, common_parameters.vehicle_info, obj_polygon); } marker_utils::updateCollisionCheckDebugMap( debug_data, current_debug_data, path_safety_status.is_safe); 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 5247a24579212..513af1b0db995 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 @@ -1030,7 +1030,7 @@ bool StartPlannerModule::isSafePath() const // If a collision is detected, mark the object as unsafe and break the loop for (const auto & obj_path : obj_predicted_paths) { - if (!utils::path_safety_checker::checkCollision( + if (utils::path_safety_checker::checkCollision( pull_out_path, ego_predicted_path, object, obj_path, planner_data_->parameters, safety_check_params_->rss_params, hysteresis_factor, current_debug_data.second)) { marker_utils::updateCollisionCheckDebugMap( diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index e4fbf41e5af00..102d7683f00ad 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -1103,4 +1103,16 @@ ExtendedPredictedObject transform( return extended_object; } + +bool isCollidedPolygonsInLanelet( + const std::vector & collided_polygons, const lanelet::ConstLanelets & lanes) +{ + const auto lanes_polygon = createPolygon(lanes, 0.0, std::numeric_limits::max()); + + const auto is_in_lanes = [&](const auto & collided_polygon) { + return lanes_polygon && boost::geometry::intersects(lanes_polygon.value(), collided_polygon); + }; + + return std::any_of(collided_polygons.begin(), collided_polygons.end(), is_in_lanes); +} } // namespace behavior_path_planner::utils::lane_change diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 9fb7d0acdf432..764ee966a564e 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -257,7 +257,7 @@ boost::optional getInterpolatedPoseWithVeloci return PoseWithVelocityAndPolygonStamped{current_time, pose, velocity, ego_polygon}; } -bool checkCollision( +std::optional> checkCollision( [[maybe_unused]] const PathWithLaneId & planned_path, const std::vector & predicted_ego_path, const ExtendedPredictedObject & target_object, @@ -271,6 +271,8 @@ bool checkCollision( debug.current_obj_pose = target_object.initial_pose.pose; } + std::vector collided_polygons{}; + collided_polygons.reserve(target_object_path.path.size()); for (const auto & obj_pose_with_poly : target_object_path.path) { const auto & current_time = obj_pose_with_poly.time; @@ -302,7 +304,8 @@ bool checkCollision( // check overlap if (boost::geometry::overlaps(ego_polygon, obj_polygon)) { debug.unsafe_reason = "overlap_polygon"; - return false; + collided_polygons.push_back(obj_polygon); + continue; } // compute which one is at the front of the other @@ -341,11 +344,15 @@ bool checkCollision( // check overlap with extended polygon if (boost::geometry::overlaps(extended_ego_polygon, extended_obj_polygon)) { debug.unsafe_reason = "overlap_extended_polygon"; - return false; + collided_polygons.push_back(obj_polygon); } } - return true; + if (!collided_polygons.empty()) { + return collided_polygons; + } + + return std::nullopt; } bool checkCollisionWithExtraStoppingMargin(