From acb07233a66aa961806dab47b7f43481032ffd7c Mon Sep 17 00:00:00 2001
From: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Date: Tue, 26 Sep 2023 18:17:20 +0900
Subject: [PATCH] rearrange code

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
---
 .../path_safety_checker/safety_check.hpp      |  9 +++---
 .../path_safety_checker/safety_check.cpp      | 29 +++++++++----------
 2 files changed, 18 insertions(+), 20 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 f3e413ce8b368..9cc4afe22bdce 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
@@ -88,7 +88,6 @@ boost::optional<PoseWithVelocityStamped> calcInterpolatedPoseWithVelocity(
 boost::optional<PoseWithVelocityAndPolygonStamped> getInterpolatedPoseWithVelocityAndPolygonStamped(
   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.
@@ -101,9 +100,9 @@ boost::optional<PoseWithVelocityAndPolygonStamped> getInterpolatedPoseWithVeloci
  * @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.
+ * @return true if distance is safe.
  */
-std::vector<Polygon2d> getCollidedPolygons(
+bool checkCollision(
   const PathWithLaneId & planned_path,
   const std::vector<PoseWithVelocityStamped> & predicted_ego_path,
   const ExtendedPredictedObject & target_object,
@@ -123,9 +122,9 @@ std::vector<Polygon2d> getCollidedPolygons(
  * @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 true if distance is safe.
+ * @return a list of polygon when collision is expected.
  */
-bool checkCollision(
+std::vector<Polygon2d> getCollidedPolygons(
   const PathWithLaneId & planned_path,
   const std::vector<PoseWithVelocityStamped> & predicted_ego_path,
   const ExtendedPredictedObject & target_object,
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 19fc868924eb1..4e671f9f4b599 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,6 +257,20 @@ boost::optional<PoseWithVelocityAndPolygonStamped> getInterpolatedPoseWithVeloci
   return PoseWithVelocityAndPolygonStamped{current_time, pose, velocity, ego_polygon};
 }
 
+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();
+}
+
 std::vector<Polygon2d> getCollidedPolygons(
   [[maybe_unused]] const PathWithLaneId & planned_path,
   const std::vector<PoseWithVelocityStamped> & predicted_ego_path,
@@ -350,21 +364,6 @@ std::vector<Polygon2d> getCollidedPolygons(
 
   return collided_polygons;
 }
-
-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(
   const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects,
   const double base_to_front, const double base_to_rear, const double width,