perf(safety_check): use light weight object position check logic (aut…

…owarefoundation#4762) perf(safety_check): use light weight object position check Signed-off-by: satoshi-ota <[email protected]>
tier4 · Sep 11, 2023 · 3beb77c · 3beb77c
1 parent 37577fa
commit 3beb77c
Show file tree

Hide file tree

Showing 3 changed files with 26 additions and 5 deletions.
diff --git a/...ior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/...ior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp
@@ -52,6 +52,9 @@ using vehicle_info_util::VehicleInfo;
 
 namespace bg = boost::geometry;
 
+bool isTargetObjectFront(
+  const geometry_msgs::msg::Pose & ego_pose, const Polygon2d & obj_polygon,
+  const vehicle_info_util::VehicleInfo & vehicle_info);
 bool isTargetObjectFront(
   const PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose,
   const vehicle_info_util::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon);

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
@@ -1924,8 +1924,8 @@ bool AvoidanceModule::isSafePath(
     const auto obj_polygon =
       tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape);
 
-    const auto is_object_front = utils::path_safety_checker::isTargetObjectFront(
-      shifted_path.path, getEgoPose(), p.vehicle_info, obj_polygon);
+    const auto is_object_front =
+      utils::path_safety_checker::isTargetObjectFront(getEgoPose(), obj_polygon, p.vehicle_info);
 
     const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj(
       object, parameters_->check_all_predicted_path);

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
@@ -30,6 +30,25 @@ void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point &
   bg::append(polygon.outer(), point);
 }
 
+bool isTargetObjectFront(
+  const geometry_msgs::msg::Pose & ego_pose, const Polygon2d & obj_polygon,
+  const vehicle_info_util::VehicleInfo & vehicle_info)
+{
+  const double base_to_front = vehicle_info.max_longitudinal_offset_m;
+  const auto ego_offset_pose =
+    tier4_autoware_utils::calcOffsetPose(ego_pose, base_to_front, 0.0, 0.0);
+
+  // check all edges in the polygon
+  for (const auto & obj_edge : obj_polygon.outer()) {
+    const auto obj_point = tier4_autoware_utils::createPoint(obj_edge.x(), obj_edge.y(), 0.0);
+    if (tier4_autoware_utils::calcLongitudinalDeviation(ego_offset_pose, obj_point) > 0.0) {
+      return true;
+    }
+  }
+
+  return false;
+}
+
 bool isTargetObjectFront(
   const PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose,
   const vehicle_info_util::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon)
@@ -214,7 +233,7 @@ boost::optional<PoseWithVelocityAndPolygonStamped> getInterpolatedPoseWithVeloci
 }
 
 bool checkCollision(
-  const PathWithLaneId & planned_path,
+  [[maybe_unused]] const PathWithLaneId & planned_path,
   const std::vector<PoseWithVelocityStamped> & predicted_ego_path,
   const ExtendedPredictedObject & target_object,
   const PredictedPathWithPolygon & target_object_path,
@@ -259,8 +278,7 @@ bool checkCollision(
     }
 
     // compute which one is at the front of the other
-    const bool is_object_front =
-      isTargetObjectFront(planned_path, ego_pose, ego_vehicle_info, obj_polygon);
+    const bool is_object_front = isTargetObjectFront(ego_pose, obj_polygon, ego_vehicle_info);
     const auto & [front_object_velocity, rear_object_velocity] =
       is_object_front ? std::make_pair(object_velocity, ego_velocity)
                       : std::make_pair(ego_velocity, object_velocity);