feat(perception_online_evaluator): imporve yaw rate metrics considering flip (#6881)

* feat(perception_online_evaluator): imporve yaw rate metrics considering flip

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* fix test

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

---------

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Author: kosuke55

evaluator/perception_online_evaluator/src/metrics_calculator.cpp

@@ -442,13 +442,12 @@ MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & clas
         tf2::getYaw(object.kinematics.initial_pose_with_covariance.pose.orientation);
       const double previous_yaw =
         tf2::getYaw(previous_object.kinematics.initial_pose_with_covariance.pose.orientation);
+      // Calculate the absolute difference between current_yaw and previous_yaw
       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;
+      // The yaw difference is flipped if the angle is larger than 90 degrees
+      const double yaw_diff_flip_fixed = std::min(yaw_diff, M_PI - yaw_diff);
+      const double yaw_rate = yaw_diff_flip_fixed / time_diff;
       stat.add(yaw_rate);
     }
     metric_stat_map["yaw_rate_" + convertLabelToString(label)] = stat;

evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp

@@ -924,6 +924,8 @@ TEST_F(EvalTest, testYawRate_5)
   setTargetMetric("yaw_rate_CAR");
 
   const double yaw_rate = 5.0;
+  const double yaw_rate_flip_fixed =
+    std::min(yaw_rate, (M_PI - yaw_rate * time_step_) / time_step_);
 
   for (double time = 0; time <= time_delay_ + 0.01; time += time_step_) {
     const auto objects = rotateObjects(
@@ -936,7 +938,7 @@ TEST_F(EvalTest, testYawRate_5)
     const auto objects = rotateObjects(
       makeStraightPredictedObjects(time, ObjectClassification::CAR, 0.0), yaw_rate * time);
     publishEgoTF(time);
-    EXPECT_NEAR(publishObjectsAndGetMetric(objects), yaw_rate, epsilon);
+    EXPECT_NEAR(publishObjectsAndGetMetric(objects), yaw_rate_flip_fixed, epsilon);
   }
 }
 
@@ -946,6 +948,8 @@ TEST_F(EvalTest, testYawRate_minus_5)
   setTargetMetric("yaw_rate_CAR");
 
   const double yaw_rate = 5.0;
+  const double yaw_rate_flip_fixed =
+    std::min(yaw_rate, (M_PI - yaw_rate * time_step_) / time_step_);
 
   for (double time = 0; time <= time_delay_ + 0.01; time += time_step_) {
     const auto objects = rotateObjects(
@@ -958,7 +962,7 @@ TEST_F(EvalTest, testYawRate_minus_5)
     const auto objects = rotateObjects(
       makeStraightPredictedObjects(time, ObjectClassification::CAR, 0.0), -yaw_rate * time);
     publishEgoTF(time);
-    EXPECT_NEAR(publishObjectsAndGetMetric(objects), yaw_rate, epsilon);
+    EXPECT_NEAR(publishObjectsAndGetMetric(objects), yaw_rate_flip_fixed, epsilon);
   }
 }
 // ==========================================================================================