Skip to content

Commit

Permalink
refactor(perception_online_evaluator): use tier4_metric_msgs instead …
Browse files Browse the repository at this point in the history 
…of diagnostic_msgs (#9485)
  • Loading branch information
@ktro2828
ktro2828 authored Dec 2, 2024
1 parent 2de4765 commit 214c9b7
Show file tree
Hide file tree
Showing 5 changed files with 83 additions and 77 deletions.
10 changes: 5 additions & 5 deletions evaluator/perception_online_evaluator/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -155,11 +155,11 @@ where:

## Inputs / Outputs

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

## Parameters

Expand Down
34 changes: 26 additions & 8 deletions ...online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,9 +23,10 @@
#include "tf2_ros/transform_listener.h"

#include "autoware_perception_msgs/msg/predicted_objects.hpp"
#include "diagnostic_msgs/msg/diagnostic_array.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "visualization_msgs/msg/marker_array.hpp"
#include <tier4_metric_msgs/msg/metric.hpp>
#include <tier4_metric_msgs/msg/metric_array.hpp>

#include <array>
#include <deque>
Expand All @@ -38,8 +39,6 @@ namespace perception_diagnostics
using autoware::universe_utils::Accumulator;
using autoware_perception_msgs::msg::ObjectClassification;
using autoware_perception_msgs::msg::PredictedObjects;
using diagnostic_msgs::msg::DiagnosticArray;
using diagnostic_msgs::msg::DiagnosticStatus;
using nav_msgs::msg::Odometry;
using TFMessage = tf2_msgs::msg::TFMessage;

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

DiagnosticStatus generateDiagnosticStatus(
const std::string metric, const Accumulator<double> & metric_stat) const;
DiagnosticStatus generateDiagnosticStatus(
const std::string & metric, const double metric_value) const;
/**
* @brief Convert metric statistic to `tier4_metric_msgs::msg::Metric` and append to
* `tier4_metric_msgs::msg::MetricArray`.
*
* @param metric Metric name.
* @param metric_stat Metric statistic.
* @param metrics_msg Metrics value container.
*/
void toMetricMsg(
const std::string & metric, const Accumulator<double> & metric_stat,
tier4_metric_msgs::msg::MetricArray & metrics_msg) const;

/**
* @brief Convert metric value to `tier4_metric_msgs::msg::Metric` and append to
* `tier4_metric_msgs::msg::MetricArray
*
* @param metric Metric name.
* @param metric_stat Metric value.
* @param metrics_msg Metrics value container.
*/
void toMetricMsg(
const std::string & metric, const double metric_value,
tier4_metric_msgs::msg::MetricArray & metrics_msg) const;

private:
// Subscribers and publishers
rclcpp::Subscription<PredictedObjects>::SharedPtr objects_sub_;
rclcpp::Publisher<DiagnosticArray>::SharedPtr metrics_pub_;
rclcpp::Publisher<tier4_metric_msgs::msg::MetricArray>::SharedPtr metrics_pub_;
rclcpp::Publisher<MarkerArray>::SharedPtr pub_marker_;

// TF
Expand Down
2 changes: 1 addition & 1 deletion evaluator/perception_online_evaluator/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,6 @@
<depend>autoware_perception_msgs</depend>
<depend>autoware_universe_utils</depend>
<depend>autoware_vehicle_info_utils</depend>
<depend>diagnostic_msgs</depend>
<depend>eigen</depend>
<depend>geometry_msgs</depend>
<depend>glog</depend>
Expand All @@ -33,6 +32,7 @@
<depend>rclcpp_components</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tier4_metric_msgs</depend>

<test_depend>ament_cmake_ros</test_depend>
<test_depend>ament_index_cpp</test_depend>
Expand Down
72 changes: 34 additions & 38 deletions evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@ PerceptionOnlineEvaluatorNode::PerceptionOnlineEvaluatorNode(

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

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

void PerceptionOnlineEvaluatorNode::publishMetrics()
{
DiagnosticArray metrics_msg;
// DiagnosticArray metrics_msg;
tier4_metric_msgs::msg::MetricArray metrics_msg;

// calculate metrics
for (const Metric & metric : parameters_->metrics) {
Expand All @@ -80,60 +81,55 @@ void PerceptionOnlineEvaluatorNode::publishMetrics()
for (const auto & [metric, value] : arg) {
if constexpr (std::is_same_v<T, MetricStatMap>) {
if (value.count() > 0) {
metrics_msg.status.emplace_back(generateDiagnosticStatus(metric, value));
toMetricMsg(metric, value, metrics_msg);
}
} else if constexpr (std::is_same_v<T, MetricValueMap>) {
metrics_msg.status.emplace_back(generateDiagnosticStatus(metric, value));
toMetricMsg(metric, value, metrics_msg);
}
}
},
metric_result.value());
}

// publish metrics
if (!metrics_msg.status.empty()) {
metrics_msg.header.stamp = now();
if (!metrics_msg.metric_array.empty()) {
metrics_msg.stamp = now();
metrics_pub_->publish(metrics_msg);
}
publishDebugMarker();
}

DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus(
const std::string metric, const Accumulator<double> & metric_stat) const
void PerceptionOnlineEvaluatorNode::toMetricMsg(
const std::string & metric, const Accumulator<double> & metric_stat,
tier4_metric_msgs::msg::MetricArray & metrics_msg) const
{
DiagnosticStatus status;

status.level = status.OK;
status.name = metric;

diagnostic_msgs::msg::KeyValue key_value;
key_value.key = "min";
key_value.value = std::to_string(metric_stat.min());
status.values.push_back(key_value);
key_value.key = "max";
key_value.value = std::to_string(metric_stat.max());
status.values.push_back(key_value);
key_value.key = "mean";
key_value.value = std::to_string(metric_stat.mean());
status.values.push_back(key_value);

return status;
// min value
metrics_msg.metric_array.emplace_back(tier4_metric_msgs::build<tier4_metric_msgs::msg::Metric>()
.name(metric + "/min")
.unit("")
.value(std::to_string(metric_stat.min())));

// max value
metrics_msg.metric_array.emplace_back(tier4_metric_msgs::build<tier4_metric_msgs::msg::Metric>()
.name(metric + "/max")
.unit("")
.value(std::to_string(metric_stat.max())));

// mean value
metrics_msg.metric_array.emplace_back(tier4_metric_msgs::build<tier4_metric_msgs::msg::Metric>()
.name(metric + "/mean")
.unit("")
.value(std::to_string(metric_stat.mean())));
}

DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus(
const std::string & metric, const double value) const
void PerceptionOnlineEvaluatorNode::toMetricMsg(
const std::string & metric, const double metric_value,
tier4_metric_msgs::msg::MetricArray & metrics_msg) 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;
metrics_msg.metric_array.emplace_back(tier4_metric_msgs::build<tier4_metric_msgs::msg::Metric>()
.name(metric + "/metric_value")
.unit("")
.value(std::to_string(metric_value)));
}

void PerceptionOnlineEvaluatorNode::onObjects(const PredictedObjects::ConstSharedPtr objects_msg)
Expand Down
42 changes: 17 additions & 25 deletions evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@

#include <autoware_perception_msgs/msg/object_classification.hpp>
#include <autoware_perception_msgs/msg/predicted_objects.hpp>
#include <diagnostic_msgs/msg/diagnostic_array.hpp>
#include <tier4_metric_msgs/msg/metric_array.hpp>
#include <visualization_msgs/msg/marker_array.hpp>

#include "boost/lexical_cast.hpp"
Expand All @@ -37,7 +37,6 @@
using EvalNode = perception_diagnostics::PerceptionOnlineEvaluatorNode;
using PredictedObjects = autoware_perception_msgs::msg::PredictedObjects;
using PredictedObject = autoware_perception_msgs::msg::PredictedObject;
using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray;
using MarkerArray = visualization_msgs::msg::MarkerArray;
using ObjectClassification = autoware_perception_msgs::msg::ObjectClassification;
using nav_msgs::msg::Odometry;
Expand Down Expand Up @@ -125,36 +124,29 @@ class EvalTest : public ::testing::Test
tf_pub_->publish(tf_msg);
}

void setTargetMetric(perception_diagnostics::Metric metric)
void setTargetMetric(const perception_diagnostics::Metric & metric)
{
const auto metric_str = perception_diagnostics::metric_to_str.at(metric);
setTargetMetric(metric_str);
}

void setTargetMetric(std::string metric_str)
void setTargetMetric(const std::string & metric_str)
{
const auto is_target_metric = [metric_str](const auto & status) {
return status.name == metric_str;
};
metric_sub_ = rclcpp::create_subscription<DiagnosticArray>(
metric_sub_ = rclcpp::create_subscription<tier4_metric_msgs::msg::MetricArray>(
eval_node, "/perception_online_evaluator/metrics", 1,
[=](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);
}
}
[this, metric_str](const tier4_metric_msgs::msg::MetricArray::ConstSharedPtr msg) {
// extract a metric whose name includes metrics_str
const auto it = std::find_if(
msg->metric_array.begin(), msg->metric_array.end(), [&metric_str](const auto & metric) {
return metric.name == metric_str + "/metric_value" ||
metric.name == metric_str + "/mean";
});

if (it != msg->metric_array.end()) {
metric_value_ = boost::lexical_cast<double>(it->value);
metric_updated_ = true;
} else {
metric_updated_ = false;
}
});
}
Expand Down Expand Up @@ -316,7 +308,7 @@ class EvalTest : public ::testing::Test

// Pub/Sub
rclcpp::Publisher<PredictedObjects>::SharedPtr objects_pub_;
rclcpp::Subscription<DiagnosticArray>::SharedPtr metric_sub_;
rclcpp::Subscription<tier4_metric_msgs::msg::MetricArray>::SharedPtr metric_sub_;
rclcpp::Subscription<MarkerArray>::SharedPtr marker_sub_;
rclcpp::Publisher<TFMessage>::SharedPtr tf_pub_;
bool has_received_marker_{false};
Expand Down

0 comments on commit 214c9b7

Please sign in to comment.