feat(tier4_metrics_rviz_plugin): add selected metrics view tab (#6577)

Author: kosuke55

evaluator/tier4_metrics_rviz_plugin/CMakeLists.txt

@@ -8,11 +8,11 @@ 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
+  include/metrics_visualize_panel.hpp
 )
 
 target_link_libraries(${PROJECT_NAME}

evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.hpp evaluator/tier4_metrics_rviz_plugin/include/metrics_visualize_panel.hpp

@@ -25,6 +25,7 @@
 #include <QLabel>
 #include <QLineSeries>
 #include <QPainter>
+#include <QPushButton>
 #include <QTableWidget>
 #include <QVBoxLayout>
 #endif
@@ -34,11 +35,13 @@
 
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
 
+#include <iostream>
 #include <limits>
 #include <string>
 #include <unordered_map>
 #include <utility>
 #include <vector>
+
 namespace rviz_plugins
 {
 
@@ -148,6 +151,8 @@ struct Metric
 
   QTableWidget * getTable() const { return table; }
 
+  std::unordered_map<std::string, QLabel *> getLabels() const { return labels; }
+
 private:
   static std::optional<std::string> getValue(const DiagnosticStatus & status, std::string && key)
   {
@@ -188,29 +193,58 @@ class MetricsVisualizePanel : public rviz_common::Panel
   void onInitialize() override;
 
 private Q_SLOTS:
+  // Slot functions triggered by UI events
   void onTopicChanged();
+  void onSpecificMetricChanged();
+  void onClearButtonClicked();
+  void onTabChanged();
 
 private:
+  // ROS 2 node and subscriptions for handling metrics data
   rclcpp::Node::SharedPtr raw_node_;
   rclcpp::TimerBase::SharedPtr timer_;
   std::unordered_map<std::string, rclcpp::Subscription<DiagnosticArray>::SharedPtr> subscriptions_;
+
+  // Topics from which metrics are collected
   std::vector<std::string> topics_ = {
     "/diagnostic/planning_evaluator/metrics", "/diagnostic/perception_online_evaluator/metrics"};
 
+  // Timer and metrics message callback
   void onTimer();
   void onMetrics(const DiagnosticArray::ConstSharedPtr & msg, const std::string & topic_name);
 
+  // Functions to update UI based on selected metrics
+  void updateViews();
+  void updateSelectedMetric(const std::string & metric_name);
+
+  // UI components
   QGridLayout * grid_;
   QComboBox * topic_selector_;
+  QTabWidget * tab_widget_;
 
-  // <topic_name, <metric_name, <table, chart>>>
+  // "Specific Metrics" tab components
+  QComboBox * specific_metric_selector_;
+  QChartView * specific_metric_chart_view_;
+  QTableWidget * specific_metric_table_;
+
+  // Selected metrics data
+  std::optional<std::pair<std::string, Metric>> selected_metrics_;
+
+  // Cache for received messages by topics
+  std::unordered_map<std::string, DiagnosticArray::ConstSharedPtr> current_msg_map_;
+
+  // Mapping from topics to metrics widgets (tables and charts)
   std::unordered_map<
     std::string, std::unordered_map<std::string, std::pair<QTableWidget *, QChartView *>>>
     topic_widgets_map_;
 
+  // Synchronization
   std::mutex mutex_;
+
+  // Stored metrics data
   std::unordered_map<std::string, Metric> metrics_;
 
+  // Utility functions for managing widget visibility based on topics
   void updateWidgetVisibility(const std::string & target_topic, const bool show);
   void showCurrentTopicWidgets();
   void hideInactiveTopicWidgets();

evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp

@@ -29,13 +29,53 @@ namespace rviz_plugins
 MetricsVisualizePanel::MetricsVisualizePanel(QWidget * parent)
 : rviz_common::Panel(parent), grid_(new QGridLayout())
 {
-  setLayout(grid_);
+  // Initialize the main tab widget
+  tab_widget_ = new QTabWidget();
+
+  // Create and configure the "All Metrics" tab
+  QWidget * all_metrics_widget = new QWidget();
+  grid_ = new QGridLayout(all_metrics_widget);  // Apply grid layout to the widget directly
+  all_metrics_widget->setLayout(grid_);
+
+  // Add topic selector combobox
   topic_selector_ = new QComboBox();
   for (const auto & topic : topics_) {
     topic_selector_->addItem(QString::fromStdString(topic));
   }
-  grid_->addWidget(topic_selector_, 0, 0, 1, -1);
+  grid_->addWidget(topic_selector_, 0, 0, 1, -1);  // Add topic selector to the grid layout
   connect(topic_selector_, SIGNAL(currentIndexChanged(int)), this, SLOT(onTopicChanged()));
+
+  tab_widget_->addTab(
+    all_metrics_widget, "All Metrics");  // Add "All Metrics" tab to the tab widget
+
+  // Create and configure the "Specific Metrics" tab
+  QWidget * specific_metrics_widget = new QWidget();
+  QVBoxLayout * specific_metrics_layout = new QVBoxLayout();
+  specific_metrics_widget->setLayout(specific_metrics_layout);
+
+  // Add specific metric selector combobox
+  specific_metric_selector_ = new QComboBox();
+  specific_metrics_layout->addWidget(specific_metric_selector_);
+  connect(
+    specific_metric_selector_, SIGNAL(currentIndexChanged(int)), this,
+    SLOT(onSpecificMetricChanged()));
+
+  // Add clear button
+  QPushButton * clear_button = new QPushButton("Clear");
+  specific_metrics_layout->addWidget(clear_button);
+  connect(clear_button, &QPushButton::clicked, this, &MetricsVisualizePanel::onClearButtonClicked);
+
+  // Add chart view for specific metrics
+  specific_metric_chart_view_ = new QChartView();
+  specific_metrics_layout->addWidget(specific_metric_chart_view_);
+
+  tab_widget_->addTab(
+    specific_metrics_widget, "Specific Metrics");  // Add "Specific Metrics" tab to the tab widget
+
+  // Set the main layout of the panel
+  QVBoxLayout * main_layout = new QVBoxLayout();
+  main_layout->addWidget(tab_widget_);
+  setLayout(main_layout);
 }
 
 void MetricsVisualizePanel::onInitialize()
@@ -91,6 +131,56 @@ void MetricsVisualizePanel::onTopicChanged()
   showCurrentTopicWidgets();
 }
 
+void MetricsVisualizePanel::updateSelectedMetric(const std::string & metric_name)
+{
+  std::lock_guard<std::mutex> message_lock(mutex_);
+
+  for (const auto & [topic, msg] : current_msg_map_) {
+    const auto time = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9;
+    for (const auto & status : msg->status) {
+      if (metric_name == status.name) {
+        selected_metrics_ = {metric_name, Metric(status)};
+        selected_metrics_->second.updateData(time, status);
+        return;
+      }
+    }
+  }
+}
+
+void MetricsVisualizePanel::updateViews()
+{
+  if (!selected_metrics_) {
+    return;
+  }
+
+  Metric & metric = selected_metrics_->second;
+  specific_metric_chart_view_->setChart(metric.getChartView()->chart());
+  auto * specific_metrics_widget = dynamic_cast<QWidget *>(tab_widget_->widget(1));
+  auto * specific_metrics_layout = dynamic_cast<QVBoxLayout *>(specific_metrics_widget->layout());
+  specific_metrics_layout->removeWidget(specific_metric_table_);
+  specific_metric_table_ = metric.getTable();
+  QSizePolicy sizePolicy(QSizePolicy::Preferred, QSizePolicy::Fixed);
+  sizePolicy.setHeightForWidth(specific_metric_table_->sizePolicy().hasHeightForWidth());
+  specific_metric_table_->setSizePolicy(sizePolicy);
+  specific_metrics_layout->insertWidget(1, specific_metric_table_);
+}
+
+void MetricsVisualizePanel::onSpecificMetricChanged()
+{
+  const auto selected_metrics_str = specific_metric_selector_->currentText().toStdString();
+  updateSelectedMetric(selected_metrics_str);
+  updateViews();
+}
+
+void MetricsVisualizePanel::onClearButtonClicked()
+{
+  if (!selected_metrics_) {
+    return;
+  }
+  updateSelectedMetric(selected_metrics_->first);
+  updateViews();
+}
+
 void MetricsVisualizePanel::onTimer()
 {
   std::lock_guard<std::mutex> message_lock(mutex_);
@@ -99,6 +189,11 @@ void MetricsVisualizePanel::onTimer()
     metric.updateGraph();
     metric.updateTable();
   }
+
+  if (selected_metrics_) {
+    selected_metrics_->second.updateGraph();
+    selected_metrics_->second.updateTable();
+  }
 }
 
 void MetricsVisualizePanel::onMetrics(
@@ -117,26 +212,48 @@ void MetricsVisualizePanel::onMetrics(
 
       // Calculate grid position
       const size_t row = num_current_metrics / GRAPH_COL_SIZE * 2 +
-                         1;  // start from 1 to leave space for the topic selector
+                         2;  // start from 2 to leave space for the topic selector and tab widget
       const size_t col = num_current_metrics % GRAPH_COL_SIZE;
 
       // Get the widgets from the metric
       const auto tableWidget = metric.getTable();
       const auto chartViewWidget = metric.getChartView();
 
-      // Add the widgets to the grid layout
-      grid_->addWidget(tableWidget, row, col);
-      grid_->setRowStretch(row, false);
-      grid_->addWidget(chartViewWidget, row + 1, col);
-      grid_->setRowStretch(row + 1, true);
-      grid_->setColumnStretch(col, true);
+      // Get the layout for the "All Metrics" tab
+      auto all_metrics_widget = dynamic_cast<QWidget *>(tab_widget_->widget(0));
+      QGridLayout * all_metrics_layout = dynamic_cast<QGridLayout *>(all_metrics_widget->layout());
 
-      // Also add the widgets to the graph_widgets_ vector for easy removal later
+      // Add the widgets to the "All Metrics" tab layout
+      all_metrics_layout->addWidget(tableWidget, row, col);
+      all_metrics_layout->setRowStretch(row, false);
+      all_metrics_layout->addWidget(chartViewWidget, row + 1, col);
+      all_metrics_layout->setRowStretch(row + 1, true);
+      all_metrics_layout->setColumnStretch(col, true);
+
+      // Also add the widgets to the topic_widgets_map_ for easy management
       topic_widgets_map_[topic_name][status.name] = std::make_pair(tableWidget, chartViewWidget);
     }
-
     metrics_.at(status.name).updateData(time, status);
+
+    // update selected metrics
+    const auto selected_metrics_str = specific_metric_selector_->currentText().toStdString();
+    if (selected_metrics_str == status.name) {
+      if (selected_metrics_) {
+        selected_metrics_->second.updateData(time, status);
+      }
+    }
   }
+
+  // Update the specific metric selector
+  QSignalBlocker blocker(specific_metric_selector_);
+  for (const auto & status : msg->status) {
+    if (specific_metric_selector_->findText(QString::fromStdString(status.name)) == -1) {
+      specific_metric_selector_->addItem(QString::fromStdString(status.name));
+    }
+  }
+
+  // save the message for metrics selector
+  current_msg_map_[topic_name] = msg;
 }
 
 }  // namespace rviz_plugins