diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp index eaa07f2317940..926fbb7435f3a 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp @@ -20,7 +20,6 @@ #include #include #include -#include #include 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>; -using MetricValueMap = std::unordered_map; -using MetricsMap = std::variant; struct PredictedPathDeviationMetrics { diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp index a04819d4844ce..96b1c01ee2d16 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp +++ b/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 calculate(const Metric & metric) const; + std::optional 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; diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp index 070d938b8bd0a..e5c41ff28c4da 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp +++ b/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 & metric_stat) const; - DiagnosticStatus generateDiagnosticStatus( - const std::string metric, const double metric_value) const; private: // Subscribers and publishers diff --git a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp index cc455445ca9f8..9c1e0667e4fef 100644 --- a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp +++ b/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 MetricsCalculator::calculate(const Metric & metric) const +std::optional 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,8 +472,8 @@ 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); } } @@ -480,7 +481,8 @@ MetricValueMap MetricsCalculator::calcObjectsCountMetrics() const 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); } } 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 be9ac2332f62c..0fcdd77f4d515 100644 --- a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp +++ b/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; - for (const auto & [metric, value] : arg) { - if constexpr (std::is_same_v) { - if (value.count() > 0) { - metrics_msg.status.push_back(generateDiagnosticStatus(metric, value)); - } - } else if constexpr (std::is_same_v) { - 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_); diff --git a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp index 53ef0fb99c8e3..95d6f07cca5d9 100644 --- a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp +++ b/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(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(metric_value_it->value); - } - } + metric_value_ = boost::lexical_cast(it->values[2].value); metric_updated_ = true; } });