From d5b99a3ff76c50b788e8fe3c07bc7b119ce6f316 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Tue, 26 Sep 2023 18:15:03 +0900 Subject: [PATCH] use wrapper instead of std::optional Signed-off-by: Muhammad Zulfaqar Azmi --- .../path_safety_checker/safety_check.hpp | 24 ++++++++++++++++++- .../avoidance/avoidance_module.cpp | 2 +- .../goal_planner/goal_planner_module.cpp | 2 +- .../src/scene_module/lane_change/normal.cpp | 5 ++-- .../start_planner/start_planner_module.cpp | 2 +- .../path_safety_checker/safety_check.cpp | 20 ++++++++++++---- 6 files changed, 43 insertions(+), 12 deletions(-) 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 c8bcc5de976dd..f3e413ce8b368 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 @@ -89,6 +89,28 @@ boost::optional getInterpolatedPoseWithVeloci const std::vector & pred_path, const double current_time, const VehicleInfo & ego_info); +/** + * @brief Iterate the points in the ego and target's predicted path and + * perform safety check for each of the iterated points. + * @param planned_path The predicted path of the ego vehicle. + * @param predicted_ego_path Ego vehicle's predicted path + * @param ego_current_velocity Current velocity of the ego vehicle. + * @param target_object The predicted object to check collision with. + * @param target_object_path The predicted path of the target object. + * @param common_parameters The common parameters used in behavior path planner. + * @param front_object_deceleration The deceleration of the object in the front.(used in RSS) + * @param rear_object_deceleration The deceleration of the object in the rear.(used in RSS) + * @param debug The debug information for collision checking. + * @return a list of polygon when collision is expected. + */ +std::vector getCollidedPolygons( + const PathWithLaneId & planned_path, + const std::vector & predicted_ego_path, + const ExtendedPredictedObject & target_object, + const PredictedPathWithPolygon & target_object_path, + const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, + const double hysteresis_factor, CollisionCheckDebug & debug); + /** * @brief Iterate the points in the ego and target's predicted path and * perform safety check for each of the iterated points. @@ -103,7 +125,7 @@ boost::optional getInterpolatedPoseWithVeloci * @param debug The debug information for collision checking. * @return true if distance is safe. */ -std::optional> checkCollision( +bool 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 b740fb86ce555..83789818c456b 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 0a767e48607a1..1f5a823386774 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 8e92f98bb08dd..ec7c1c76b58e2 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,15 +1443,14 @@ 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) { - const auto have_collision = utils::path_safety_checker::checkCollision( + const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons( path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0, current_debug_data.second); - if (!have_collision) { + if (collided_polygons.empty()) { 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( 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 513af1b0db995..5247a24579212 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/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 764ee966a564e..19fc868924eb1 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}; } -std::optional> checkCollision( +std::vector getCollidedPolygons( [[maybe_unused]] const PathWithLaneId & planned_path, const std::vector & predicted_ego_path, const ExtendedPredictedObject & target_object, @@ -348,11 +348,21 @@ std::optional> checkCollision( } } - if (!collided_polygons.empty()) { - return collided_polygons; - } + return collided_polygons; +} - return std::nullopt; +bool checkCollision( + const PathWithLaneId & planned_path, + const std::vector & predicted_ego_path, + const ExtendedPredictedObject & target_object, + const PredictedPathWithPolygon & target_object_path, + const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, + const double hysteresis_factor, CollisionCheckDebug & debug) +{ + const auto collided_polygons = getCollidedPolygons( + planned_path, predicted_ego_path, target_object, target_object_path, common_parameters, + rss_parameters, hysteresis_factor, debug); + return collided_polygons.empty(); } bool checkCollisionWithExtraStoppingMargin(