diff --git a/evaluator/perception_online_evaluator/README.md b/evaluator/perception_online_evaluator/README.md index 7df375ac236d0..17e7e1a2b5652 100644 --- a/evaluator/perception_online_evaluator/README.md +++ b/evaluator/perception_online_evaluator/README.md @@ -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 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 ea560a48f928b..61c1ba40134df 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 @@ -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 +#include #include #include @@ -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; @@ -60,15 +59,34 @@ class PerceptionOnlineEvaluatorNode : public rclcpp::Node */ void onObjects(const PredictedObjects::ConstSharedPtr objects_msg); - DiagnosticStatus generateDiagnosticStatus( - const std::string metric, const Accumulator & 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 & 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::SharedPtr objects_sub_; - rclcpp::Publisher::SharedPtr metrics_pub_; + rclcpp::Publisher::SharedPtr metrics_pub_; rclcpp::Publisher::SharedPtr pub_marker_; // TF diff --git a/evaluator/perception_online_evaluator/package.xml b/evaluator/perception_online_evaluator/package.xml index 654586a76453d..1e7c0a7d128e6 100644 --- a/evaluator/perception_online_evaluator/package.xml +++ b/evaluator/perception_online_evaluator/package.xml @@ -23,7 +23,6 @@ autoware_perception_msgs autoware_universe_utils autoware_vehicle_info_utils - diagnostic_msgs eigen geometry_msgs glog @@ -33,6 +32,7 @@ rclcpp_components tf2 tf2_ros + tier4_metric_msgs ament_cmake_ros ament_index_cpp 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 abbdbd382498a..7d8344c24819c 100644 --- a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp @@ -50,7 +50,7 @@ PerceptionOnlineEvaluatorNode::PerceptionOnlineEvaluatorNode( objects_sub_ = create_subscription( "~/input/objects", 1, std::bind(&PerceptionOnlineEvaluatorNode::onObjects, this, _1)); - metrics_pub_ = create_publisher("~/metrics", 1); + metrics_pub_ = create_publisher("~/metrics", 1); pub_marker_ = create_publisher("~/markers", 10); tf_buffer_ = std::make_unique(this->get_clock()); @@ -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) { @@ -80,10 +81,10 @@ void PerceptionOnlineEvaluatorNode::publishMetrics() for (const auto & [metric, value] : arg) { if constexpr (std::is_same_v) { if (value.count() > 0) { - metrics_msg.status.emplace_back(generateDiagnosticStatus(metric, value)); + toMetricMsg(metric, value, metrics_msg); } } else if constexpr (std::is_same_v) { - metrics_msg.status.emplace_back(generateDiagnosticStatus(metric, value)); + toMetricMsg(metric, value, metrics_msg); } } }, @@ -91,49 +92,44 @@ void PerceptionOnlineEvaluatorNode::publishMetrics() } // 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 & metric_stat) const +void PerceptionOnlineEvaluatorNode::toMetricMsg( + const std::string & metric, const Accumulator & 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() + .name(metric + "/min") + .unit("") + .value(std::to_string(metric_stat.min()))); + + // max value + metrics_msg.metric_array.emplace_back(tier4_metric_msgs::build() + .name(metric + "/max") + .unit("") + .value(std::to_string(metric_stat.max()))); + + // mean value + metrics_msg.metric_array.emplace_back(tier4_metric_msgs::build() + .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() + .name(metric + "/metric_value") + .unit("") + .value(std::to_string(metric_value))); } void PerceptionOnlineEvaluatorNode::onObjects(const PredictedObjects::ConstSharedPtr objects_msg) 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 e2ab22be2b61b..5b23f6d37a1d4 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 @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include "boost/lexical_cast.hpp" @@ -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; @@ -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( + metric_sub_ = rclcpp::create_subscription( 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(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); - } - } + [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(it->value); metric_updated_ = true; + } else { + metric_updated_ = false; } }); } @@ -316,7 +308,7 @@ class EvalTest : public ::testing::Test // Pub/Sub rclcpp::Publisher::SharedPtr objects_pub_; - rclcpp::Subscription::SharedPtr metric_sub_; + rclcpp::Subscription::SharedPtr metric_sub_; rclcpp::Subscription::SharedPtr marker_sub_; rclcpp::Publisher::SharedPtr tf_pub_; bool has_received_marker_{false};