From 7d268799fefe6d326f906315293be1ea150af41f Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 4 Apr 2024 23:23:04 +0900 Subject: [PATCH] feat(perception_online_evaluator): ignore reversal of orientation from yaw_rate calculation (#6748) Signed-off-by: kosuke55 --- .../src/metrics_calculator.cpp | 13 +++++++++---- .../src/perception_online_evaluator_node.cpp | 1 + 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp index 20d8c6d570489..4b882bb8b2e68 100644 --- a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp +++ b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp @@ -381,12 +381,17 @@ MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & clas if (time_diff < 0.01) { continue; } - const auto current_yaw = + const double current_yaw = tf2::getYaw(object.kinematics.initial_pose_with_covariance.pose.orientation); - const auto previous_yaw = + const double previous_yaw = tf2::getYaw(previous_object.kinematics.initial_pose_with_covariance.pose.orientation); - const auto yaw_rate = - std::abs(tier4_autoware_utils::normalizeRadian(current_yaw - previous_yaw) / time_diff); + const double yaw_diff = + std::abs(tier4_autoware_utils::normalizeRadian(current_yaw - previous_yaw)); + // if yaw_diff is close to PI, reversal of orientation is likely occurring, so ignore it + if (std::abs(M_PI - yaw_diff) < 0.1) { + continue; + } + const auto yaw_rate = yaw_diff / time_diff; stat.add(yaw_rate); } metric_stat_map["yaw_rate_" + convertLabelToString(label)] = stat; diff --git a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp index f3a08bf42797a..ceb304894ad8c 100644 --- a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp @@ -214,6 +214,7 @@ rcl_interfaces::msg::SetParametersResult PerceptionOnlineEvaluatorNode::onParame auto & p = parameters_; updateParam(parameters, "smoothing_window_size", p->smoothing_window_size); + updateParam(parameters, "stopped_velocity_threshold", p->stopped_velocity_threshold); // update metrics {