use wrapper instead of std::optional

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp

@@ -89,6 +89,28 @@ boost::optional<PoseWithVelocityAndPolygonStamped> getInterpolatedPoseWithVeloci
   const std::vector<PoseWithVelocityStamped> & 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<Polygon2d> getCollidedPolygons(
+  const PathWithLaneId & planned_path,
+  const std::vector<PoseWithVelocityStamped> & 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<PoseWithVelocityAndPolygonStamped> getInterpolatedPoseWithVeloci
  * @param debug The debug information for collision checking.
  * @return true if distance is safe.
  */
-std::optional<std::vector<Polygon2d>> checkCollision(
+bool checkCollision(
   const PathWithLaneId & planned_path,
   const std::vector<PoseWithVelocityStamped> & predicted_ego_path,
   const ExtendedPredictedObject & target_object,

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(

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(

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(

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(

planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp

@@ -257,7 +257,7 @@ boost::optional<PoseWithVelocityAndPolygonStamped> getInterpolatedPoseWithVeloci
   return PoseWithVelocityAndPolygonStamped{current_time, pose, velocity, ego_polygon};
 }
 
-std::optional<std::vector<Polygon2d>> checkCollision(
+std::vector<Polygon2d> getCollidedPolygons(
   [[maybe_unused]] const PathWithLaneId & planned_path,
   const std::vector<PoseWithVelocityStamped> & predicted_ego_path,
   const ExtendedPredictedObject & target_object,
@@ -348,11 +348,21 @@ std::optional<std::vector<Polygon2d>> checkCollision(
     }
   }
 
-  if (!collided_polygons.empty()) {
-    return collided_polygons;
-  }
+  return collided_polygons;
+}
 
-  return std::nullopt;
+bool checkCollision(
+  const PathWithLaneId & planned_path,
+  const std::vector<PoseWithVelocityStamped> & 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(