feat(metrics_visualize_panel): add new plugin to show planning metrics (

#6154)

* feat(metrix_visualize_panel): add new plugin to show planning metrix

Signed-off-by: satoshi-ota <[email protected]>

* fix(tier4_metrics_rviz_plugin): fix small issues

Signed-off-by: satoshi-ota <[email protected]>

* chore: add maintainer

Signed-off-by: satoshi-ota <[email protected]>

* docs(tier4_metrics_rviz_plugin): fix docs

Signed-off-by: satoshi-ota <[email protected]>

---------

Signed-off-by: satoshi-ota <[email protected]>

evaluator/tier4_metrics_rviz_plugin/CMakeLists.txt

@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 3.14)
+project(tier4_metrics_rviz_plugin)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(Qt5 REQUIRED Core Widgets Charts)
+set(QT_WIDGETS_LIB Qt5::Widgets)
+set(QT_CHARTS_LIB Qt5::Charts)
+set(CMAKE_AUTOMOC ON)
+set(CMAKE_INCLUDE_CURRENT_DIR ON)
+add_definitions(-DQT_NO_KEYWORDS)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/metrics_visualize_panel.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ${QT_WIDGETS_LIB}
+  ${QT_CHARTS_LIB}
+)
+
+target_compile_options(${PROJECT_NAME} PUBLIC -Wno-error=deprecated-copy -Wno-error=pedantic)
+# Export the plugin to be imported by rviz2
+pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  icons
+  plugins
+)

evaluator/tier4_metrics_rviz_plugin/README.md

@@ -0,0 +1,16 @@
+# tier4_metrics_rviz_plugin
+
+## Purpose
+
+This plugin panel to visualize `planning_evaluator` output.
+
+## Inputs / Outputs
+
+| Name                                     | Type                                    | Description                           |
+| ---------------------------------------- | --------------------------------------- | ------------------------------------- |
+| `/diagnostic/planning_evaluator/metrics` | `diagnostic_msgs::msg::DiagnosticArray` | Subscribe `planning_evaluator` output |
+
+## HowToUse
+
+1. Start rviz and select panels/Add new panel.
+2. Select MetricsVisualizePanel and press OK.

evaluator/tier4_metrics_rviz_plugin/package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>tier4_metrics_rviz_plugin</name>
+  <version>0.0.0</version>
+  <description>The tier4_metrics_rviz_plugin package</description>
+  <maintainer email="[email protected]">Satoshi OTA</maintainer>
+  <maintainer email="[email protected]">Kyoichi Sugahara</maintainer>
+  <maintainer email="[email protected]">Maxime CLEMENT</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>diagnostic_msgs</depend>
+  <depend>libqt5-core</depend>
+  <depend>libqt5-gui</depend>
+  <depend>libqt5-widgets</depend>
+  <depend>qtbase5-dev</depend>
+  <depend>rclcpp</depend>
+  <depend>rviz_common</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+    <rviz plugin="${prefix}/plugins/plugin_description.xml"/>
+  </export>
+</package>

evaluator/tier4_metrics_rviz_plugin/plugins/plugin_description.xml

@@ -0,0 +1,5 @@
+<library path="tier4_metrics_rviz_plugin">
+  <class type="rviz_plugins::MetricsVisualizePanel" base_class_type="rviz_common::Panel">
+    <description>MetricsVisualizePanel</description>
+  </class>
+</library>

evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp

@@ -0,0 +1,86 @@
+//  Copyright 2024 TIER IV, Inc. All rights reserved.
+//
+//  Licensed under the Apache License, Version 2.0 (the "License");
+//  you may not use this file except in compliance with the License.
+//  You may obtain a copy of the License at
+//
+//      http://www.apache.org/licenses/LICENSE-2.0
+//
+//  Unless required by applicable law or agreed to in writing, software
+//  distributed under the License is distributed on an "AS IS" BASIS,
+//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+//  See the License for the specific language governing permissions and
+//  limitations under the License.
+//
+
+#include "metrics_visualize_panel.hpp"
+
+#include <rviz_common/display_context.hpp>
+
+#include <X11/Xlib.h>
+
+#include <algorithm>
+#include <optional>
+#include <string>
+#include <vector>
+
+namespace rviz_plugins
+{
+MetricsVisualizePanel::MetricsVisualizePanel(QWidget * parent)
+: rviz_common::Panel(parent), grid_(new QGridLayout())
+{
+  setLayout(grid_);
+}
+
+void MetricsVisualizePanel::onInitialize()
+{
+  using std::placeholders::_1;
+
+  raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
+
+  sub_ = raw_node_->create_subscription<DiagnosticArray>(
+    "/diagnostic/planning_evaluator/metrics", rclcpp::QoS{1},
+    std::bind(&MetricsVisualizePanel::onMetrics, this, _1));
+
+  const auto period = std::chrono::milliseconds(static_cast<int64_t>(1e3 / 10));
+  timer_ = raw_node_->create_wall_timer(period, [&]() { onTimer(); });
+}
+
+void MetricsVisualizePanel::onTimer()
+{
+  std::lock_guard<std::mutex> message_lock(mutex_);
+
+  for (auto & [name, metric] : metrics_) {
+    metric.updateGraph();
+    metric.updateTable();
+  }
+}
+
+void MetricsVisualizePanel::onMetrics(const DiagnosticArray::ConstSharedPtr msg)
+{
+  std::lock_guard<std::mutex> message_lock(mutex_);
+
+  const auto time = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9;
+
+  constexpr size_t GRAPH_COL_SIZE = 5;
+  for (size_t i = 0; i < msg->status.size(); ++i) {
+    const auto & status = msg->status.at(i);
+
+    if (metrics_.count(status.name) == 0) {
+      auto metric = Metric(status);
+      metrics_.emplace(status.name, metric);
+      grid_->addWidget(metric.getTable(), i / GRAPH_COL_SIZE * 2, i % GRAPH_COL_SIZE);
+      grid_->setRowStretch(i / GRAPH_COL_SIZE * 2, false);
+      grid_->addWidget(metric.getChartView(), i / GRAPH_COL_SIZE * 2 + 1, i % GRAPH_COL_SIZE);
+      grid_->setRowStretch(i / GRAPH_COL_SIZE * 2 + 1, true);
+      grid_->setColumnStretch(i % GRAPH_COL_SIZE, true);
+    }
+
+    metrics_.at(status.name).updateData(time, status);
+  }
+}
+
+}  // namespace rviz_plugins
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(rviz_plugins::MetricsVisualizePanel, rviz_common::Panel)

evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.hpp

@@ -0,0 +1,205 @@
+//  Copyright 2024 TIER IV, Inc. All rights reserved.
+//
+//  Licensed under the Apache License, Version 2.0 (the "License");
+//  you may not use this file except in compliance with the License.
+//  You may obtain a copy of the License at
+//
+//      http://www.apache.org/licenses/LICENSE-2.0
+//
+//  Unless required by applicable law or agreed to in writing, software
+//  distributed under the License is distributed on an "AS IS" BASIS,
+//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+//  See the License for the specific language governing permissions and
+//  limitations under the License.
+//
+
+#ifndef METRICS_VISUALIZE_PANEL_HPP_
+#define METRICS_VISUALIZE_PANEL_HPP_
+
+#ifndef Q_MOC_RUN
+#include <QChartView>
+#include <QColor>
+#include <QGridLayout>
+#include <QHeaderView>
+#include <QLabel>
+#include <QLineSeries>
+#include <QPainter>
+#include <QTableWidget>
+#include <QVBoxLayout>
+#endif
+
+#include <rclcpp/rclcpp.hpp>
+#include <rviz_common/panel.hpp>
+
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+
+#include <limits>
+#include <string>
+#include <unordered_map>
+
+namespace rviz_plugins
+{
+
+using diagnostic_msgs::msg::DiagnosticArray;
+using diagnostic_msgs::msg::DiagnosticStatus;
+using diagnostic_msgs::msg::KeyValue;
+using QtCharts::QChart;
+using QtCharts::QChartView;
+using QtCharts::QLineSeries;
+
+struct Metric
+{
+public:
+  explicit Metric(const DiagnosticStatus & status) : chart(new QChartView), table(new QTableWidget)
+  {
+    init(status);
+  }
+
+  void init(const DiagnosticStatus & status)
+  {
+    QStringList header{};
+
+    {
+      auto label = new QLabel;
+      label->setAlignment(Qt::AlignCenter);
+      label->setText("metric_name");
+      labels.emplace("metric_name", label);
+
+      header.push_back(QString::fromStdString(status.name));
+    }
+
+    for (const auto & [key, value] : status.values) {
+      auto label = new QLabel;
+      label->setAlignment(Qt::AlignCenter);
+      labels.emplace(key, label);
+
+      auto plot = new QLineSeries;
+      plot->setName(QString::fromStdString(key));
+      plots.emplace(key, plot);
+      chart->chart()->addSeries(plot);
+      chart->chart()->createDefaultAxes();
+
+      header.push_back(QString::fromStdString(key));
+    }
+
+    {
+      chart->chart()->setTitle(QString::fromStdString(status.name));
+      chart->chart()->legend()->setVisible(true);
+      chart->chart()->legend()->detachFromChart();
+      chart->setRenderHint(QPainter::Antialiasing);
+    }
+
+    {
+      table->setColumnCount(status.values.size() + 1);
+      table->setHorizontalHeaderLabels(header);
+      table->verticalHeader()->hide();
+      table->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch);
+      table->setRowCount(1);
+      table->setSizeAdjustPolicy(QAbstractScrollArea::AdjustToContents);
+    }
+  }
+
+  void updateData(const double time, const DiagnosticStatus & status)
+  {
+    for (const auto & [key, value] : status.values) {
+      const double data = std::stod(value);
+      labels.at(key)->setText(QString::fromStdString(toString(data)));
+      plots.at(key)->append(time, data);
+      updateMinMax(data);
+    }
+
+    {
+      const auto area = chart->chart()->plotArea();
+      const auto rect = chart->chart()->legend()->rect();
+      chart->chart()->legend()->setGeometry(
+        QRectF(area.x(), area.y(), area.width(), rect.height()));
+      chart->chart()->axes(Qt::Horizontal).front()->setRange(time - 100.0, time);
+    }
+
+    {
+      table->setCellWidget(0, 0, labels.at("metric_name"));
+    }
+
+    for (size_t i = 0; i < status.values.size(); ++i) {
+      table->setCellWidget(0, i + 1, labels.at(status.values.at(i).key));
+    }
+  }
+
+  void updateMinMax(double data)
+  {
+    if (data < y_range_min) {
+      y_range_min = data > 0.0 ? 0.9 * data : 1.1 * data;
+      chart->chart()->axes(Qt::Vertical).front()->setMin(y_range_min);
+    }
+
+    if (data > y_range_max) {
+      y_range_max = data > 0.0 ? 1.1 * data : 0.9 * data;
+      chart->chart()->axes(Qt::Vertical).front()->setMax(y_range_max);
+    }
+  }
+
+  void updateTable() { table->update(); }
+
+  void updateGraph() { chart->update(); }
+
+  QChartView * getChartView() const { return chart; }
+
+  QTableWidget * getTable() const { return table; }
+
+private:
+  static std::optional<std::string> getValue(const DiagnosticStatus & status, std::string && key)
+  {
+    const auto itr = std::find_if(
+      status.values.begin(), status.values.end(),
+      [&](const auto & value) { return value.key == key; });
+
+    if (itr == status.values.end()) {
+      return std::nullopt;
+    }
+
+    return itr->value;
+  }
+
+  static std::string toString(const double & value)
+  {
+    std::stringstream ss;
+    ss << std::scientific << std::setprecision(2) << value;
+    return ss.str();
+  }
+
+  QChartView * chart;
+  QTableWidget * table;
+
+  std::unordered_map<std::string, QLabel *> labels;
+  std::unordered_map<std::string, QLineSeries *> plots;
+
+  double y_range_min{std::numeric_limits<double>::max()};
+  double y_range_max{std::numeric_limits<double>::lowest()};
+};
+
+class MetricsVisualizePanel : public rviz_common::Panel
+{
+  Q_OBJECT
+
+public:
+  explicit MetricsVisualizePanel(QWidget * parent = nullptr);
+  void onInitialize() override;
+
+private Q_SLOTS:
+
+private:
+  rclcpp::Node::SharedPtr raw_node_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::Subscription<DiagnosticArray>::SharedPtr sub_;
+
+  void onTimer();
+  void onMetrics(const DiagnosticArray::ConstSharedPtr msg);
+
+  QGridLayout * grid_;
+
+  std::mutex mutex_;
+  std::unordered_map<std::string, Metric> metrics_;
+};
+}  // namespace rviz_plugins
+
+#endif  // METRICS_VISUALIZE_PANEL_HPP_