Merge branch 'main' into feat/adapi-diagnostics

Author: isamu-takagi

common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp

@@ -15,13 +15,17 @@
 #ifndef TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_
 #define TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_
 
+#include <utility>
+
 namespace tier4_autoware_utils
 {
 
 float sin(float radian);
 
 float cos(float radian);
 
+std::pair<float, float> sin_and_cos(float radian);
+
 }  // namespace tier4_autoware_utils
 
 #endif  // TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_

common/tier4_autoware_utils/src/math/trigonometry.cpp

@@ -49,4 +49,27 @@ float cos(float radian)
   return sin(radian + static_cast<float>(tier4_autoware_utils::pi) / 2.f);
 }
 
+std::pair<float, float> sin_and_cos(float radian)
+{
+  constexpr float tmp =
+    (180.f / static_cast<float>(tier4_autoware_utils::pi)) * (discrete_arcs_num_360 / 360.f);
+  const float degree = radian * tmp;
+  size_t idx =
+    (static_cast<int>(std::round(degree)) % discrete_arcs_num_360 + discrete_arcs_num_360) %
+    discrete_arcs_num_360;
+
+  if (idx < discrete_arcs_num_90) {
+    return {g_sin_table[idx], g_sin_table[discrete_arcs_num_90 - idx]};
+  } else if (discrete_arcs_num_90 <= idx && idx < 2 * discrete_arcs_num_90) {
+    idx = 2 * discrete_arcs_num_90 - idx;
+    return {g_sin_table[idx], -g_sin_table[discrete_arcs_num_90 - idx]};
+  } else if (2 * discrete_arcs_num_90 <= idx && idx < 3 * discrete_arcs_num_90) {
+    idx = idx - 2 * discrete_arcs_num_90;
+    return {-g_sin_table[idx], -g_sin_table[discrete_arcs_num_90 - idx]};
+  } else {  // 3 * discrete_arcs_num_90 <= idx && idx < 4 * discrete_arcs_num_90
+    idx = 4 * discrete_arcs_num_90 - idx;
+    return {-g_sin_table[idx], g_sin_table[discrete_arcs_num_90 - idx]};
+  }
+}
+
 }  // namespace tier4_autoware_utils

common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp

@@ -40,3 +40,13 @@ TEST(trigonometry, cos)
         tier4_autoware_utils::cos(x * static_cast<float>(i))) < 10e-5);
   }
 }
+
+TEST(trigonometry, sin_and_cos)
+{
+  float x = 4.f * tier4_autoware_utils::pi / 128.f;
+  for (int i = 0; i < 128; i++) {
+    const auto sin_and_cos = tier4_autoware_utils::sin_and_cos(x * static_cast<float>(i));
+    EXPECT_TRUE(std::abs(std::sin(x * static_cast<float>(i)) - sin_and_cos.first) < 10e-7);
+    EXPECT_TRUE(std::abs(std::cos(x * static_cast<float>(i)) - sin_and_cos.second) < 10e-7);
+  }
+}

evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp

@@ -20,7 +20,6 @@
 #include <iostream>
 #include <string>
 #include <unordered_map>
-#include <variant>
 #include <vector>
 
 namespace perception_diagnostics
@@ -37,11 +36,7 @@ enum class Metric {
   SIZE,
 };
 
-// Each metric has a different return type. (statistic or just a one value etc).
-// To handle them all in the MetricsCalculator::calculate function, define MetricsMap as a variant
 using MetricStatMap = std::unordered_map<std::string, Stat<double>>;
-using MetricValueMap = std::unordered_map<std::string, double>;
-using MetricsMap = std::variant<MetricStatMap, MetricValueMap>;
 
 struct PredictedPathDeviationMetrics
 {

evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp

@@ -96,7 +96,7 @@ class MetricsCalculator
    * @param [in] metric Metric enum value
    * @return map of string describing the requested metric and the calculated value
    */
-  std::optional<MetricsMap> calculate(const Metric & metric) const;
+  std::optional<MetricStatMap> calculate(const Metric & metric) const;
 
   /**
    * @brief set the dynamic objects used to calculate obstacle metrics
@@ -143,7 +143,7 @@ class MetricsCalculator
   PredictedPathDeviationMetrics calcPredictedPathDeviationMetrics(
     const PredictedObjects & objects, const double time_horizon) const;
   MetricStatMap calcYawRateMetrics(const ClassObjectsMap & class_objects_map) const;
-  MetricValueMap calcObjectsCountMetrics() const;
+  MetricStatMap calcObjectsCountMetrics() const;
 
   bool hasPassedTime(const rclcpp::Time stamp) const;
   bool hasPassedTime(const std::string uuid, const rclcpp::Time stamp) const;

evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp

@@ -61,8 +61,6 @@ class PerceptionOnlineEvaluatorNode : public rclcpp::Node
 
   DiagnosticStatus generateDiagnosticStatus(
     const std::string metric, const Stat<double> & metric_stat) const;
-  DiagnosticStatus generateDiagnosticStatus(
-    const std::string metric, const double metric_value) const;
 
 private:
   // Subscribers and publishers

evaluator/perception_online_evaluator/src/metrics_calculator.cpp

@@ -26,7 +26,7 @@ namespace perception_diagnostics
 using object_recognition_utils::convertLabelToString;
 using tier4_autoware_utils::inverseTransformPoint;
 
-std::optional<MetricsMap> MetricsCalculator::calculate(const Metric & metric) const
+std::optional<MetricStatMap> MetricsCalculator::calculate(const Metric & metric) const
 {
   // clang-format off
   const bool use_past_objects = metric == Metric::lateral_deviation        ||
@@ -455,14 +455,15 @@ MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & clas
   return metric_stat_map;
 }
 
-MetricValueMap MetricsCalculator::calcObjectsCountMetrics() const
+MetricStatMap MetricsCalculator::calcObjectsCountMetrics() const
 {
-  MetricValueMap metric_stat_map;
+  MetricStatMap metric_stat_map;
   // calculate the average number of objects in the detection area in all past frames
   const auto overall_average_count = detection_counter_.getOverallAverageCount();
   for (const auto & [label, range_and_count] : overall_average_count) {
     for (const auto & [range, count] : range_and_count) {
-      metric_stat_map["average_objects_count_" + convertLabelToString(label) + "_" + range] = count;
+      metric_stat_map["average_objects_count_" + convertLabelToString(label) + "_" + range].add(
+        count);
     }
   }
   // calculate the average number of objects in the detection area in the past
@@ -471,16 +472,17 @@ MetricValueMap MetricsCalculator::calcObjectsCountMetrics() const
     detection_counter_.getAverageCount(parameters_->objects_count_window_seconds);
   for (const auto & [label, range_and_count] : average_count) {
     for (const auto & [range, count] : range_and_count) {
-      metric_stat_map
-        ["interval_average_objects_count_" + convertLabelToString(label) + "_" + range] = count;
+      metric_stat_map["interval_average_objects_count_" + convertLabelToString(label) + "_" + range]
+        .add(count);
     }
   }
 
   // calculate the total number of objects in the detection area
   const auto total_count = detection_counter_.getTotalCount();
   for (const auto & [label, range_and_count] : total_count) {
     for (const auto & [range, count] : range_and_count) {
-      metric_stat_map["total_objects_count_" + convertLabelToString(label) + "_" + range] = count;
+      metric_stat_map["total_objects_count_" + convertLabelToString(label) + "_" + range].add(
+        count);
     }
   }
 

evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp

@@ -69,25 +69,16 @@ void PerceptionOnlineEvaluatorNode::publishMetrics()
 
   // calculate metrics
   for (const Metric & metric : parameters_->metrics) {
-    const auto metric_result = metrics_calculator_.calculate(Metric(metric));
-    if (!metric_result.has_value()) {
+    const auto metric_stat_map = metrics_calculator_.calculate(Metric(metric));
+    if (!metric_stat_map.has_value()) {
       continue;
     }
 
-    std::visit(
-      [&metrics_msg, this](auto && arg) {
-        using T = std::decay_t<decltype(arg)>;
-        for (const auto & [metric, value] : arg) {
-          if constexpr (std::is_same_v<T, MetricStatMap>) {
-            if (value.count() > 0) {
-              metrics_msg.status.push_back(generateDiagnosticStatus(metric, value));
-            }
-          } else if constexpr (std::is_same_v<T, MetricValueMap>) {
-            metrics_msg.status.push_back(generateDiagnosticStatus(metric, value));
-          }
-        }
-      },
-      metric_result.value());
+    for (const auto & [metric, stat] : metric_stat_map.value()) {
+      if (stat.count() > 0) {
+        metrics_msg.status.push_back(generateDiagnosticStatus(metric, stat));
+      }
+    }
   }
 
   // publish metrics
@@ -120,22 +111,6 @@ DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus(
   return status;
 }
 
-DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus(
-  const std::string metric, const double value) const
-{
-  DiagnosticStatus status;
-
-  status.level = status.OK;
-  status.name = metric;
-
-  diagnostic_msgs::msg::KeyValue key_value;
-  key_value.key = "metric_value";
-  key_value.value = std::to_string(value);
-  status.values.push_back(key_value);
-
-  return status;
-}
-
 void PerceptionOnlineEvaluatorNode::onObjects(const PredictedObjects::ConstSharedPtr objects_msg)
 {
   metrics_calculator_.setPredictedObjects(*objects_msg, *tf_buffer_);

evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp

@@ -141,19 +141,7 @@ class EvalTest : public ::testing::Test
       [=](const DiagnosticArray::ConstSharedPtr msg) {
         const auto it = std::find_if(msg->status.begin(), msg->status.end(), is_target_metric);
         if (it != msg->status.end()) {
-          const auto mean_it = std::find_if(
-            it->values.begin(), it->values.end(),
-            [](const auto & key_value) { return key_value.key == "mean"; });
-          if (mean_it != it->values.end()) {
-            metric_value_ = boost::lexical_cast<double>(mean_it->value);
-          } else {
-            const auto metric_value_it = std::find_if(
-              it->values.begin(), it->values.end(),
-              [](const auto & key_value) { return key_value.key == "metric_value"; });
-            if (metric_value_it != it->values.end()) {
-              metric_value_ = boost::lexical_cast<double>(metric_value_it->value);
-            }
-          }
+          metric_value_ = boost::lexical_cast<double>(it->values[2].value);
           metric_updated_ = true;
         }
       });