diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp index 0a172c7f5544b..21a8cce93a1be 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp @@ -186,15 +186,19 @@ bool isVehicle(const PredictedObject & obj) bool isStuckVehicle(const PredictedObject & obj, const double min_vel) { if (!isVehicle(obj)) return false; - const auto & obj_vel = obj.kinematics.initial_twist_with_covariance.twist.linear.x; - return std::abs(obj_vel) <= min_vel; + const auto & obj_vel_norm = std::hypot( + obj.kinematics.initial_twist_with_covariance.twist.linear.x, + obj.kinematics.initial_twist_with_covariance.twist.linear.y); + return obj_vel_norm <= min_vel; } bool isMovingVehicle(const PredictedObject & obj, const double min_vel) { if (!isVehicle(obj)) return false; - const auto & obj_vel = obj.kinematics.initial_twist_with_covariance.twist.linear.x; - return std::abs(obj_vel) > min_vel; + const auto & obj_vel_norm = std::hypot( + obj.kinematics.initial_twist_with_covariance.twist.linear.x, + obj.kinematics.initial_twist_with_covariance.twist.linear.y); + return obj_vel_norm > min_vel; } std::vector extractVehicles(