From 03c987c3868097bfae5eae0b24c36534327b1e8d Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Mon, 11 Sep 2023 15:24:21 +0900 Subject: [PATCH] feat(avoidance): improve canceling judgement Signed-off-by: satoshi-ota --- .../src/scene_module/avoidance/avoidance_module.cpp | 8 ++++++-- .../src/scene_module/avoidance/manager.cpp | 6 ++---- 2 files changed, 8 insertions(+), 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 index 1630ce7465c6e..40fc6ef7b53a6 100644 --- 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 index bec3ba0afd025..a9e8982a9a48a 100644 --- 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(node, ns + "check_current_lane"); p.check_shift_side_lane = get_parameter(node, ns + "check_shift_side_lane"); p.check_other_side_lane = get_parameter(node, ns + "check_other_side_lane"); - p.check_unavoidable_object = - get_parameter(node, ns + "check_unavoidable_object"); + p.check_unavoidable_object = get_parameter(node, ns + "check_unavoidable_object"); p.check_other_object = get_parameter(node, ns + "check_other_object"); - p.check_all_predicted_path = - get_parameter(node, ns + "check_all_predicted_path"); + p.check_all_predicted_path = get_parameter(node, ns + "check_all_predicted_path"); p.time_horizon_for_front_object = get_parameter(node, ns + "time_horizon_for_front_object"); p.time_horizon_for_rear_object =