perf(static_obstacle_avoidance): improve logic to reduce computationa…

…l cost (#8432)

* perf(safety_check): check within first

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* perf(static_obstacle_avoidance): remove duplicated process

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* perf(static_obstacle_avoidance): remove heavy process

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp

@@ -61,32 +61,30 @@ bool isCentroidWithinLanelet(
   const PredictedObject & object, const lanelet::ConstLanelet & lanelet, const double yaw_threshold)
 {
   const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose;
-  const auto closest_pose = lanelet::utils::getClosestCenterPose(lanelet, object_pose.position);
-  if (
-    std::abs(autoware::universe_utils::calcYawDeviation(closest_pose, object_pose)) >
-    yaw_threshold) {
+  if (!boost::geometry::within(
+        lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pose.position))
+          .basicPoint(),
+        lanelet.polygon2d().basicPolygon())) {
     return false;
   }
 
-  return boost::geometry::within(
-    lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pose.position))
-      .basicPoint(),
-    lanelet.polygon2d().basicPolygon());
+  const auto closest_pose = lanelet::utils::getClosestCenterPose(lanelet, object_pose.position);
+  return std::abs(autoware::universe_utils::calcYawDeviation(closest_pose, object_pose)) <
+         yaw_threshold;
 }
 
 bool isPolygonOverlapLanelet(
   const PredictedObject & object, const lanelet::ConstLanelet & lanelet, const double yaw_threshold)
 {
-  const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose;
-  const auto closest_pose = lanelet::utils::getClosestCenterPose(lanelet, object_pose.position);
-  if (
-    std::abs(autoware::universe_utils::calcYawDeviation(closest_pose, object_pose)) >
-    yaw_threshold) {
+  const auto lanelet_polygon = utils::toPolygon2d(lanelet);
+  if (!isPolygonOverlapLanelet(object, lanelet_polygon)) {
     return false;
   }
 
-  const auto lanelet_polygon = utils::toPolygon2d(lanelet);
-  return isPolygonOverlapLanelet(object, lanelet_polygon);
+  const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose;
+  const auto closest_pose = lanelet::utils::getClosestCenterPose(lanelet, object_pose.position);
+  return std::abs(autoware::universe_utils::calcYawDeviation(closest_pose, object_pose)) <
+         yaw_threshold;
 }
 
 bool isPolygonOverlapLanelet(

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp

@@ -987,11 +987,6 @@ BehaviorModuleOutput StaticObstacleAvoidanceModule::plan()
     spline_shift_path = helper_->getPreviousSplineShiftPath();
   }
 
-  // post processing
-  {
-    postProcess();  // remove old shift points
-  }
-
   BehaviorModuleOutput output;
 
   const auto is_ignore_signal = [this](const UUID & uuid) {

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp

@@ -2282,10 +2282,9 @@ std::pair<PredictedObjects, PredictedObjects> separateObjectsByPath(
   Pose p_reference_ego_front = reference_path.points.front().point.pose;
   Pose p_spline_ego_front = spline_path.points.front().point.pose;
   double next_longitudinal_distance = parameters->resample_interval_for_output;
+  const auto offset = arc_length_array.at(ego_idx);
   for (size_t i = 0; i < points_size; ++i) {
-    const auto distance_from_ego =
-      autoware::motion_utils::calcSignedArcLength(reference_path.points, ego_idx, i);
-    if (distance_from_ego > object_check_forward_distance) {
+    if (arc_length_array.at(i) > object_check_forward_distance + offset) {
       break;
     }