From 2da561f4959fc3185c0525ad76c05aa7f4605e1a Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 17 Aug 2023 17:10:55 +0900 Subject: [PATCH] feat(intersection): consider object velocity direction (#4651) * feat(intersection): consider object velocity direction Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../src/scene_intersection.cpp | 11 ++++++++--- .../src/util.cpp | 6 ++++-- 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index dcfa3b0fce71f..6bfe1c83d370a 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -800,8 +800,11 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( util::isOverTargetIndex(*path, closest_idx, current_pose, pass_judge_line_idx); const bool is_over_default_stop_line = util::isOverTargetIndex(*path, closest_idx, current_pose, default_stop_line_idx); - const double vel = std::fabs(planner_data_->current_velocity->twist.linear.x); - const bool keep_detection = (vel < planner_param_.collision_detection.keep_detection_vel_thr); + const double vel_norm = std::hypot( + planner_data_->current_velocity->twist.linear.x, + planner_data_->current_velocity->twist.linear.y); + const bool keep_detection = + (vel_norm < planner_param_.collision_detection.keep_detection_vel_thr); // if ego is over the pass judge line and not stopped if (is_peeking_) { // do nothing @@ -871,7 +874,9 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( target_objects.objects.begin(), target_objects.objects.end(), std::back_inserter(parked_attention_objects), [thresh = planner_param_.occlusion.ignore_parked_vehicle_speed_threshold](const auto & object) { - return std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x) <= thresh; + return std::hypot( + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y) <= thresh; }); const bool is_occlusion_cleared = (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !tl_arrow_solid_on) diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 178df99ba7b02..f0343891c6051 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -923,8 +923,10 @@ bool checkStuckVehicleInIntersection( if (!isTargetStuckVehicleType(object)) { continue; // not target vehicle type } - const auto obj_v = std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x); - if (obj_v > stuck_vehicle_vel_thr) { + const auto obj_v_norm = std::hypot( + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); + if (obj_v_norm > stuck_vehicle_vel_thr) { continue; // not stop vehicle }