From e2aee50530eed5f0f0b6f4e2f6d5ea91571cdded Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Sun, 10 Mar 2024 20:58:35 +0900 Subject: [PATCH] feat(tier4_metrics_rviz_plugin): add selected metrics view tab (#6577) --- .../tier4_metrics_rviz_plugin/CMakeLists.txt | 2 +- .../metrics_visualize_panel.hpp | 36 ++++- .../src/metrics_visualize_panel.cpp | 139 ++++++++++++++++-- 3 files changed, 164 insertions(+), 13 deletions(-) rename evaluator/tier4_metrics_rviz_plugin/{src => include}/metrics_visualize_panel.hpp (84%) diff --git a/evaluator/tier4_metrics_rviz_plugin/CMakeLists.txt b/evaluator/tier4_metrics_rviz_plugin/CMakeLists.txt index 4c1e8cf58c262..8475b596e6d6b 100644 --- a/evaluator/tier4_metrics_rviz_plugin/CMakeLists.txt +++ b/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} diff --git a/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.hpp b/evaluator/tier4_metrics_rviz_plugin/include/metrics_visualize_panel.hpp similarity index 84% rename from evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.hpp rename to evaluator/tier4_metrics_rviz_plugin/include/metrics_visualize_panel.hpp index db55eddaea844..3d66099988d8b 100644 --- a/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.hpp +++ b/evaluator/tier4_metrics_rviz_plugin/include/metrics_visualize_panel.hpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include #endif @@ -34,11 +35,13 @@ #include +#include #include #include #include #include #include + namespace rviz_plugins { @@ -148,6 +151,8 @@ struct Metric QTableWidget * getTable() const { return table; } + std::unordered_map getLabels() const { return labels; } + private: static std::optional 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::SharedPtr> subscriptions_; + + // Topics from which metrics are collected std::vector 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_; - // >> + // "Specific Metrics" tab components + QComboBox * specific_metric_selector_; + QChartView * specific_metric_chart_view_; + QTableWidget * specific_metric_table_; + + // Selected metrics data + std::optional> selected_metrics_; + + // Cache for received messages by topics + std::unordered_map current_msg_map_; + + // Mapping from topics to metrics widgets (tables and charts) std::unordered_map< std::string, std::unordered_map>> topic_widgets_map_; + // Synchronization std::mutex mutex_; + + // Stored metrics data std::unordered_map metrics_; + // Utility functions for managing widget visibility based on topics void updateWidgetVisibility(const std::string & target_topic, const bool show); void showCurrentTopicWidgets(); void hideInactiveTopicWidgets(); diff --git a/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp b/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp index 72edca55d5cdd..b92a9a7ace53d 100644 --- a/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp +++ b/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 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(tab_widget_->widget(1)); + auto * specific_metrics_layout = dynamic_cast(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 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(tab_widget_->widget(0)); + QGridLayout * all_metrics_layout = dynamic_cast(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