Skip to content

Commit 214c9b7

Browse files
ktro2828ktro2828
authored
refactor(perception_online_evaluator): use tier4_metric_msgs instead of diagnostic_msgs (#9485)
1 parent 2de4765 commit 214c9b7

File tree

5 files changed

+83
-77
lines changed

5 files changed

+83
-77
lines changed

‎evaluator/perception_online_evaluator/README.md

+5-5
Original file line numberDiff line numberDiff line change
@@ -155,11 +155,11 @@ where:
155155

156156
## Inputs / Outputs
157157

158-
| Name | Type | Description |
159-
| ----------------- | ------------------------------------------------- | ------------------------------------------------- |
160-
| `~/input/objects` | `autoware_perception_msgs::msg::PredictedObjects` | The predicted objects to evaluate. |
161-
| `~/metrics` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostic information about perception accuracy. |
162-
| `~/markers` | `visualization_msgs::msg::MarkerArray` | Visual markers for debugging and visualization. |
158+
| Name | Type | Description |
159+
| ----------------- | ------------------------------------------------- | ----------------------------------------------- |
160+
| `~/input/objects` | `autoware_perception_msgs::msg::PredictedObjects` | The predicted objects to evaluate. |
161+
| `~/metrics` | `tier4_metric_msgs::msg::MetricArray` | Metric information about perception accuracy. |
162+
| `~/markers` | `visualization_msgs::msg::MarkerArray` | Visual markers for debugging and visualization. |
163163

164164
## Parameters
165165

‎evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp

+26-8
Original file line numberDiff line numberDiff line change
@@ -23,9 +23,10 @@
2323
#include "tf2_ros/transform_listener.h"
2424

2525
#include "autoware_perception_msgs/msg/predicted_objects.hpp"
26-
#include "diagnostic_msgs/msg/diagnostic_array.hpp"
2726
#include "nav_msgs/msg/odometry.hpp"
2827
#include "visualization_msgs/msg/marker_array.hpp"
28+
#include <tier4_metric_msgs/msg/metric.hpp>
29+
#include <tier4_metric_msgs/msg/metric_array.hpp>
2930

3031
#include <array>
3132
#include <deque>
@@ -38,8 +39,6 @@ namespace perception_diagnostics
3839
using autoware::universe_utils::Accumulator;
3940
using autoware_perception_msgs::msg::ObjectClassification;
4041
using autoware_perception_msgs::msg::PredictedObjects;
41-
using diagnostic_msgs::msg::DiagnosticArray;
42-
using diagnostic_msgs::msg::DiagnosticStatus;
4342
using nav_msgs::msg::Odometry;
4443
using TFMessage = tf2_msgs::msg::TFMessage;
4544

@@ -60,15 +59,34 @@ class PerceptionOnlineEvaluatorNode : public rclcpp::Node
6059
*/
6160
void onObjects(const PredictedObjects::ConstSharedPtr objects_msg);
6261

63-
DiagnosticStatus generateDiagnosticStatus(
64-
const std::string metric, const Accumulator<double> & metric_stat) const;
65-
DiagnosticStatus generateDiagnosticStatus(
66-
const std::string & metric, const double metric_value) const;
62+
/**
63+
* @brief Convert metric statistic to `tier4_metric_msgs::msg::Metric` and append to
64+
* `tier4_metric_msgs::msg::MetricArray`.
65+
*
66+
* @param metric Metric name.
67+
* @param metric_stat Metric statistic.
68+
* @param metrics_msg Metrics value container.
69+
*/
70+
void toMetricMsg(
71+
const std::string & metric, const Accumulator<double> & metric_stat,
72+
tier4_metric_msgs::msg::MetricArray & metrics_msg) const;
73+
74+
/**
75+
* @brief Convert metric value to `tier4_metric_msgs::msg::Metric` and append to
76+
* `tier4_metric_msgs::msg::MetricArray
77+
*
78+
* @param metric Metric name.
79+
* @param metric_stat Metric value.
80+
* @param metrics_msg Metrics value container.
81+
*/
82+
void toMetricMsg(
83+
const std::string & metric, const double metric_value,
84+
tier4_metric_msgs::msg::MetricArray & metrics_msg) const;
6785

6886
private:
6987
// Subscribers and publishers
7088
rclcpp::Subscription<PredictedObjects>::SharedPtr objects_sub_;
71-
rclcpp::Publisher<DiagnosticArray>::SharedPtr metrics_pub_;
89+
rclcpp::Publisher<tier4_metric_msgs::msg::MetricArray>::SharedPtr metrics_pub_;
7290
rclcpp::Publisher<MarkerArray>::SharedPtr pub_marker_;
7391

7492
// TF

‎evaluator/perception_online_evaluator/package.xml

+1-1
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,6 @@
2323
<depend>autoware_perception_msgs</depend>
2424
<depend>autoware_universe_utils</depend>
2525
<depend>autoware_vehicle_info_utils</depend>
26-
<depend>diagnostic_msgs</depend>
2726
<depend>eigen</depend>
2827
<depend>geometry_msgs</depend>
2928
<depend>glog</depend>
@@ -33,6 +32,7 @@
3332
<depend>rclcpp_components</depend>
3433
<depend>tf2</depend>
3534
<depend>tf2_ros</depend>
35+
<depend>tier4_metric_msgs</depend>
3636

3737
<test_depend>ament_cmake_ros</test_depend>
3838
<test_depend>ament_index_cpp</test_depend>

‎evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp

+34-38
Original file line numberDiff line numberDiff line change
@@ -50,7 +50,7 @@ PerceptionOnlineEvaluatorNode::PerceptionOnlineEvaluatorNode(
5050

5151
objects_sub_ = create_subscription<PredictedObjects>(
5252
"~/input/objects", 1, std::bind(&PerceptionOnlineEvaluatorNode::onObjects, this, _1));
53-
metrics_pub_ = create_publisher<DiagnosticArray>("~/metrics", 1);
53+
metrics_pub_ = create_publisher<tier4_metric_msgs::msg::MetricArray>("~/metrics", 1);
5454
pub_marker_ = create_publisher<MarkerArray>("~/markers", 10);
5555

5656
tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
@@ -65,7 +65,8 @@ PerceptionOnlineEvaluatorNode::PerceptionOnlineEvaluatorNode(
6565

6666
void PerceptionOnlineEvaluatorNode::publishMetrics()
6767
{
68-
DiagnosticArray metrics_msg;
68+
// DiagnosticArray metrics_msg;
69+
tier4_metric_msgs::msg::MetricArray metrics_msg;
6970

7071
// calculate metrics
7172
for (const Metric & metric : parameters_->metrics) {
@@ -80,60 +81,55 @@ void PerceptionOnlineEvaluatorNode::publishMetrics()
8081
for (const auto & [metric, value] : arg) {
8182
if constexpr (std::is_same_v<T, MetricStatMap>) {
8283
if (value.count() > 0) {
83-
metrics_msg.status.emplace_back(generateDiagnosticStatus(metric, value));
84+
toMetricMsg(metric, value, metrics_msg);
8485
}
8586
} else if constexpr (std::is_same_v<T, MetricValueMap>) {
86-
metrics_msg.status.emplace_back(generateDiagnosticStatus(metric, value));
87+
toMetricMsg(metric, value, metrics_msg);
8788
}
8889
}
8990
},
9091
metric_result.value());
9192
}
9293

9394
// publish metrics
94-
if (!metrics_msg.status.empty()) {
95-
metrics_msg.header.stamp = now();
95+
if (!metrics_msg.metric_array.empty()) {
96+
metrics_msg.stamp = now();
9697
metrics_pub_->publish(metrics_msg);
9798
}
9899
publishDebugMarker();
99100
}
100101

101-
DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus(
102-
const std::string metric, const Accumulator<double> & metric_stat) const
102+
void PerceptionOnlineEvaluatorNode::toMetricMsg(
103+
const std::string & metric, const Accumulator<double> & metric_stat,
104+
tier4_metric_msgs::msg::MetricArray & metrics_msg) const
103105
{
104-
DiagnosticStatus status;
105-
106-
status.level = status.OK;
107-
status.name = metric;
108-
109-
diagnostic_msgs::msg::KeyValue key_value;
110-
key_value.key = "min";
111-
key_value.value = std::to_string(metric_stat.min());
112-
status.values.push_back(key_value);
113-
key_value.key = "max";
114-
key_value.value = std::to_string(metric_stat.max());
115-
status.values.push_back(key_value);
116-
key_value.key = "mean";
117-
key_value.value = std::to_string(metric_stat.mean());
118-
status.values.push_back(key_value);
119-
120-
return status;
106+
// min value
107+
metrics_msg.metric_array.emplace_back(tier4_metric_msgs::build<tier4_metric_msgs::msg::Metric>()
108+
.name(metric + "/min")
109+
.unit("")
110+
.value(std::to_string(metric_stat.min())));
111+
112+
// max value
113+
metrics_msg.metric_array.emplace_back(tier4_metric_msgs::build<tier4_metric_msgs::msg::Metric>()
114+
.name(metric + "/max")
115+
.unit("")
116+
.value(std::to_string(metric_stat.max())));
117+
118+
// mean value
119+
metrics_msg.metric_array.emplace_back(tier4_metric_msgs::build<tier4_metric_msgs::msg::Metric>()
120+
.name(metric + "/mean")
121+
.unit("")
122+
.value(std::to_string(metric_stat.mean())));
121123
}
122124

123-
DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus(
124-
const std::string & metric, const double value) const
125+
void PerceptionOnlineEvaluatorNode::toMetricMsg(
126+
const std::string & metric, const double metric_value,
127+
tier4_metric_msgs::msg::MetricArray & metrics_msg) const
125128
{
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;
129+
metrics_msg.metric_array.emplace_back(tier4_metric_msgs::build<tier4_metric_msgs::msg::Metric>()
130+
.name(metric + "/metric_value")
131+
.unit("")
132+
.value(std::to_string(metric_value)));
137133
}
138134

139135
void PerceptionOnlineEvaluatorNode::onObjects(const PredictedObjects::ConstSharedPtr objects_msg)

‎evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp

+17-25
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,7 @@
2121

2222
#include <autoware_perception_msgs/msg/object_classification.hpp>
2323
#include <autoware_perception_msgs/msg/predicted_objects.hpp>
24-
#include <diagnostic_msgs/msg/diagnostic_array.hpp>
24+
#include <tier4_metric_msgs/msg/metric_array.hpp>
2525
#include <visualization_msgs/msg/marker_array.hpp>
2626

2727
#include "boost/lexical_cast.hpp"
@@ -37,7 +37,6 @@
3737
using EvalNode = perception_diagnostics::PerceptionOnlineEvaluatorNode;
3838
using PredictedObjects = autoware_perception_msgs::msg::PredictedObjects;
3939
using PredictedObject = autoware_perception_msgs::msg::PredictedObject;
40-
using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray;
4140
using MarkerArray = visualization_msgs::msg::MarkerArray;
4241
using ObjectClassification = autoware_perception_msgs::msg::ObjectClassification;
4342
using nav_msgs::msg::Odometry;
@@ -125,36 +124,29 @@ class EvalTest : public ::testing::Test
125124
tf_pub_->publish(tf_msg);
126125
}
127126

128-
void setTargetMetric(perception_diagnostics::Metric metric)
127+
void setTargetMetric(const perception_diagnostics::Metric & metric)
129128
{
130129
const auto metric_str = perception_diagnostics::metric_to_str.at(metric);
131130
setTargetMetric(metric_str);
132131
}
133132

134-
void setTargetMetric(std::string metric_str)
133+
void setTargetMetric(const std::string & metric_str)
135134
{
136-
const auto is_target_metric = [metric_str](const auto & status) {
137-
return status.name == metric_str;
138-
};
139-
metric_sub_ = rclcpp::create_subscription<DiagnosticArray>(
135+
metric_sub_ = rclcpp::create_subscription<tier4_metric_msgs::msg::MetricArray>(
140136
eval_node, "/perception_online_evaluator/metrics", 1,
141-
[=](const DiagnosticArray::ConstSharedPtr msg) {
142-
const auto it = std::find_if(msg->status.begin(), msg->status.end(), is_target_metric);
143-
if (it != msg->status.end()) {
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-
}
137+
[this, metric_str](const tier4_metric_msgs::msg::MetricArray::ConstSharedPtr msg) {
138+
// extract a metric whose name includes metrics_str
139+
const auto it = std::find_if(
140+
msg->metric_array.begin(), msg->metric_array.end(), [&metric_str](const auto & metric) {
141+
return metric.name == metric_str + "/metric_value" ||
142+
metric.name == metric_str + "/mean";
143+
});
144+
145+
if (it != msg->metric_array.end()) {
146+
metric_value_ = boost::lexical_cast<double>(it->value);
157147
metric_updated_ = true;
148+
} else {
149+
metric_updated_ = false;
158150
}
159151
});
160152
}
@@ -316,7 +308,7 @@ class EvalTest : public ::testing::Test
316308

317309
// Pub/Sub
318310
rclcpp::Publisher<PredictedObjects>::SharedPtr objects_pub_;
319-
rclcpp::Subscription<DiagnosticArray>::SharedPtr metric_sub_;
311+
rclcpp::Subscription<tier4_metric_msgs::msg::MetricArray>::SharedPtr metric_sub_;
320312
rclcpp::Subscription<MarkerArray>::SharedPtr marker_sub_;
321313
rclcpp::Publisher<TFMessage>::SharedPtr tf_pub_;
322314
bool has_received_marker_{false};

0 commit comments

Comments
 (0)