feat(perception_online_evaluator): ignore reversal of orientation fro…

…m yaw_rate calculation (#6748) Signed-off-by: kosuke55 <[email protected]>
autowarefoundation · Apr 4, 2024 · 7d26879 · 7d26879
1 parent f088fb4
commit 7d26879
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 4 deletions.
diff --git 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
@@ -214,6 +214,7 @@ rcl_interfaces::msg::SetParametersResult PerceptionOnlineEvaluatorNode::onParame
   auto & p = parameters_;
 
   updateParam<size_t>(parameters, "smoothing_window_size", p->smoothing_window_size);
+  updateParam<double>(parameters, "stopped_velocity_threshold", p->stopped_velocity_threshold);
 
   // update metrics
   {