Skip to content

Commit fe30833

Browse files
authored
feat(perception_online_evaluator): add metric_value not only stat (#7100)
Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
1 parent 6d77841 commit fe30833

File tree

6 files changed

+61
-19
lines changed

6 files changed

+61
-19
lines changed

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

+5
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020
#include <iostream>
2121
#include <string>
2222
#include <unordered_map>
23+
#include <variant>
2324
#include <vector>
2425

2526
namespace perception_diagnostics
@@ -36,7 +37,11 @@ enum class Metric {
3637
SIZE,
3738
};
3839

40+
// Each metric has a different return type. (statistic or just a one value etc).
41+
// To handle them all in the MetricsCalculator::calculate function, define MetricsMap as a variant
3942
using MetricStatMap = std::unordered_map<std::string, Stat<double>>;
43+
using MetricValueMap = std::unordered_map<std::string, double>;
44+
using MetricsMap = std::variant<MetricStatMap, MetricValueMap>;
4045

4146
struct PredictedPathDeviationMetrics
4247
{

evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -96,7 +96,7 @@ class MetricsCalculator
9696
* @param [in] metric Metric enum value
9797
* @return map of string describing the requested metric and the calculated value
9898
*/
99-
std::optional<MetricStatMap> calculate(const Metric & metric) const;
99+
std::optional<MetricsMap> calculate(const Metric & metric) const;
100100

101101
/**
102102
* @brief set the dynamic objects used to calculate obstacle metrics
@@ -143,7 +143,7 @@ class MetricsCalculator
143143
PredictedPathDeviationMetrics calcPredictedPathDeviationMetrics(
144144
const PredictedObjects & objects, const double time_horizon) const;
145145
MetricStatMap calcYawRateMetrics(const ClassObjectsMap & class_objects_map) const;
146-
MetricStatMap calcObjectsCountMetrics() const;
146+
MetricValueMap calcObjectsCountMetrics() const;
147147

148148
bool hasPassedTime(const rclcpp::Time stamp) const;
149149
bool hasPassedTime(const std::string uuid, const rclcpp::Time stamp) const;

evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp

+2
Original file line numberDiff line numberDiff line change
@@ -61,6 +61,8 @@ class PerceptionOnlineEvaluatorNode : public rclcpp::Node
6161

6262
DiagnosticStatus generateDiagnosticStatus(
6363
const std::string metric, const Stat<double> & metric_stat) const;
64+
DiagnosticStatus generateDiagnosticStatus(
65+
const std::string metric, const double metric_value) const;
6466

6567
private:
6668
// Subscribers and publishers

evaluator/perception_online_evaluator/src/metrics_calculator.cpp

+7-9
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,7 @@ namespace perception_diagnostics
2626
using object_recognition_utils::convertLabelToString;
2727
using tier4_autoware_utils::inverseTransformPoint;
2828

29-
std::optional<MetricStatMap> MetricsCalculator::calculate(const Metric & metric) const
29+
std::optional<MetricsMap> MetricsCalculator::calculate(const Metric & metric) const
3030
{
3131
// clang-format off
3232
const bool use_past_objects = metric == Metric::lateral_deviation ||
@@ -455,15 +455,14 @@ MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & clas
455455
return metric_stat_map;
456456
}
457457

458-
MetricStatMap MetricsCalculator::calcObjectsCountMetrics() const
458+
MetricValueMap MetricsCalculator::calcObjectsCountMetrics() const
459459
{
460-
MetricStatMap metric_stat_map;
460+
MetricValueMap metric_stat_map;
461461
// calculate the average number of objects in the detection area in all past frames
462462
const auto overall_average_count = detection_counter_.getOverallAverageCount();
463463
for (const auto & [label, range_and_count] : overall_average_count) {
464464
for (const auto & [range, count] : range_and_count) {
465-
metric_stat_map["average_objects_count_" + convertLabelToString(label) + "_" + range].add(
466-
count);
465+
metric_stat_map["average_objects_count_" + convertLabelToString(label) + "_" + range] = count;
467466
}
468467
}
469468
// calculate the average number of objects in the detection area in the past
@@ -472,17 +471,16 @@ MetricStatMap MetricsCalculator::calcObjectsCountMetrics() const
472471
detection_counter_.getAverageCount(parameters_->objects_count_window_seconds);
473472
for (const auto & [label, range_and_count] : average_count) {
474473
for (const auto & [range, count] : range_and_count) {
475-
metric_stat_map["interval_average_objects_count_" + convertLabelToString(label) + "_" + range]
476-
.add(count);
474+
metric_stat_map
475+
["interval_average_objects_count_" + convertLabelToString(label) + "_" + range] = count;
477476
}
478477
}
479478

480479
// calculate the total number of objects in the detection area
481480
const auto total_count = detection_counter_.getTotalCount();
482481
for (const auto & [label, range_and_count] : total_count) {
483482
for (const auto & [range, count] : range_and_count) {
484-
metric_stat_map["total_objects_count_" + convertLabelToString(label) + "_" + range].add(
485-
count);
483+
metric_stat_map["total_objects_count_" + convertLabelToString(label) + "_" + range] = count;
486484
}
487485
}
488486

evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp

+32-7
Original file line numberDiff line numberDiff line change
@@ -69,16 +69,25 @@ void PerceptionOnlineEvaluatorNode::publishMetrics()
6969

7070
// calculate metrics
7171
for (const Metric & metric : parameters_->metrics) {
72-
const auto metric_stat_map = metrics_calculator_.calculate(Metric(metric));
73-
if (!metric_stat_map.has_value()) {
72+
const auto metric_result = metrics_calculator_.calculate(Metric(metric));
73+
if (!metric_result.has_value()) {
7474
continue;
7575
}
7676

77-
for (const auto & [metric, stat] : metric_stat_map.value()) {
78-
if (stat.count() > 0) {
79-
metrics_msg.status.push_back(generateDiagnosticStatus(metric, stat));
80-
}
81-
}
77+
std::visit(
78+
[&metrics_msg, this](auto && arg) {
79+
using T = std::decay_t<decltype(arg)>;
80+
for (const auto & [metric, value] : arg) {
81+
if constexpr (std::is_same_v<T, MetricStatMap>) {
82+
if (value.count() > 0) {
83+
metrics_msg.status.push_back(generateDiagnosticStatus(metric, value));
84+
}
85+
} else if constexpr (std::is_same_v<T, MetricValueMap>) {
86+
metrics_msg.status.push_back(generateDiagnosticStatus(metric, value));
87+
}
88+
}
89+
},
90+
metric_result.value());
8291
}
8392

8493
// publish metrics
@@ -111,6 +120,22 @@ DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus(
111120
return status;
112121
}
113122

123+
DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus(
124+
const std::string metric, const double value) const
125+
{
126+
DiagnosticStatus status;
127+
128+
status.level = status.OK;
129+
status.name = metric;
130+
131+
diagnostic_msgs::msg::KeyValue key_value;
132+
key_value.key = "metric_value";
133+
key_value.value = std::to_string(value);
134+
status.values.push_back(key_value);
135+
136+
return status;
137+
}
138+
114139
void PerceptionOnlineEvaluatorNode::onObjects(const PredictedObjects::ConstSharedPtr objects_msg)
115140
{
116141
metrics_calculator_.setPredictedObjects(*objects_msg, *tf_buffer_);

evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp

+13-1
Original file line numberDiff line numberDiff line change
@@ -141,7 +141,19 @@ class EvalTest : public ::testing::Test
141141
[=](const DiagnosticArray::ConstSharedPtr msg) {
142142
const auto it = std::find_if(msg->status.begin(), msg->status.end(), is_target_metric);
143143
if (it != msg->status.end()) {
144-
metric_value_ = boost::lexical_cast<double>(it->values[2].value);
144+
const auto mean_it = std::find_if(
145+
it->values.begin(), it->values.end(),
146+
[](const auto & key_value) { return key_value.key == "mean"; });
147+
if (mean_it != it->values.end()) {
148+
metric_value_ = boost::lexical_cast<double>(mean_it->value);
149+
} else {
150+
const auto metric_value_it = std::find_if(
151+
it->values.begin(), it->values.end(),
152+
[](const auto & key_value) { return key_value.key == "metric_value"; });
153+
if (metric_value_it != it->values.end()) {
154+
metric_value_ = boost::lexical_cast<double>(metric_value_it->value);
155+
}
156+
}
145157
metric_updated_ = true;
146158
}
147159
});

0 commit comments

Comments
 (0)