feat(out_of_lane): ignore objects coming from behind ego

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp

@@ -2092,9 +2092,9 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
     });
   };
 
-  const auto to_predicted_objects = [&p, &parameters](const auto & objects) {
+  const auto to_predicted_objects = [&parameters](const auto & objects) {
     PredictedObjects ret{};
-    std::for_each(objects.begin(), objects.end(), [&p, &ret, &parameters](const auto & object) {
+    std::for_each(objects.begin(), objects.end(), [&ret, &parameters](const auto & object) {
       if (filtering_utils::isSafetyCheckTargetObjectType(object.object, parameters)) {
         // check only moving objects
         if (filtering_utils::isMovingObject(object, parameters) || !object.is_parked) {

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp

@@ -111,6 +111,12 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects(
       }) != object.classification.end();
     if (is_pedestrian) continue;
 
+    const auto is_coming_from_behind =
+      motion_utils::calcSignedArcLength(
+        ego_data.trajectory_points, ego_data.first_trajectory_idx,
+        object.kinematics.initial_pose_with_covariance.pose.position) < 0.0;
+    if (is_coming_from_behind) continue;
+
     auto filtered_object = object;
     const auto is_invalid_predicted_path = [&](const auto & predicted_path) {
       const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence;

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp

@@ -36,7 +36,6 @@
 #include <map>
 #include <memory>
 #include <string>
-#include <utility>
 #include <vector>
 
 namespace autoware::motion_velocity_planner