feat(avoidance): improve canceling judgement

Signed-off-by: satoshi-ota <[email protected]>
tier4 · Sep 11, 2023 · 03c987c · 03c987c
1 parent 71277da
commit 03c987c
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 6 deletions.
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
@@ -1929,11 +1929,15 @@ bool AvoidanceModule::isSafePath(
     const auto is_object_front =
       utils::path_safety_checker::isTargetObjectFront(getEgoPose(), obj_polygon, p.vehicle_info);
 
+    const auto is_object_incoming =
+      std::abs(calcYawDeviation(getEgoPose(), object.initial_pose.pose)) > M_PI_2;
+
     const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj(
       object, parameters_->check_all_predicted_path);
 
-    const auto & ego_predicted_path =
-      is_object_front ? ego_predicted_path_for_front_object : ego_predicted_path_for_rear_object;
+    const auto & ego_predicted_path = is_object_front && !is_object_incoming
+                                        ? ego_predicted_path_for_front_object
+                                        : ego_predicted_path_for_rear_object;
 
     for (const auto & obj_path : obj_predicted_paths) {
       CollisionCheckDebug collision{};

diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp
@@ -143,11 +143,9 @@ AvoidanceModuleManager::AvoidanceModuleManager(
     p.check_current_lane = get_parameter<bool>(node, ns + "check_current_lane");
     p.check_shift_side_lane = get_parameter<bool>(node, ns + "check_shift_side_lane");
     p.check_other_side_lane = get_parameter<bool>(node, ns + "check_other_side_lane");
-    p.check_unavoidable_object =
-      get_parameter<bool>(node, ns + "check_unavoidable_object");
+    p.check_unavoidable_object = get_parameter<bool>(node, ns + "check_unavoidable_object");
     p.check_other_object = get_parameter<bool>(node, ns + "check_other_object");
-    p.check_all_predicted_path =
-      get_parameter<bool>(node, ns + "check_all_predicted_path");
+    p.check_all_predicted_path = get_parameter<bool>(node, ns + "check_all_predicted_path");
     p.time_horizon_for_front_object =
       get_parameter<double>(node, ns + "time_horizon_for_front_object");
     p.time_horizon_for_rear_object =