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 79da02b9b65c6..72edca55d5cdd 100644 --- a/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp +++ b/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp @@ -30,6 +30,12 @@ MetricsVisualizePanel::MetricsVisualizePanel(QWidget * parent) : rviz_common::Panel(parent), grid_(new QGridLayout()) { setLayout(grid_); + topic_selector_ = new QComboBox(); + for (const auto & topic : topics_) { + topic_selector_->addItem(QString::fromStdString(topic)); + } + grid_->addWidget(topic_selector_, 0, 0, 1, -1); + connect(topic_selector_, SIGNAL(currentIndexChanged(int)), this, SLOT(onTopicChanged())); } void MetricsVisualizePanel::onInitialize() @@ -38,14 +44,53 @@ void MetricsVisualizePanel::onInitialize() raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - sub_ = raw_node_->create_subscription( - "/diagnostic/planning_evaluator/metrics", rclcpp::QoS{1}, - std::bind(&MetricsVisualizePanel::onMetrics, this, _1)); + for (const auto & topic_name : topics_) { + const auto callback = [this, topic_name](const DiagnosticArray::ConstSharedPtr msg) { + this->onMetrics(msg, topic_name); + }; + const auto subscription = + raw_node_->create_subscription(topic_name, rclcpp::QoS{1}, callback); + subscriptions_[topic_name] = subscription; + } const auto period = std::chrono::milliseconds(static_cast(1e3 / 10)); timer_ = raw_node_->create_wall_timer(period, [&]() { onTimer(); }); } +void MetricsVisualizePanel::updateWidgetVisibility( + const std::string & target_topic, const bool show) +{ + for (const auto & [topic_name, metric_widgets_pair] : topic_widgets_map_) { + const bool is_target_topic = (topic_name == target_topic); + if ((!is_target_topic && show) || (is_target_topic && !show)) { + continue; + } + for (const auto & [metric, widgets] : metric_widgets_pair) { + widgets.first->setVisible(show); + widgets.second->setVisible(show); + } + } +} + +void MetricsVisualizePanel::showCurrentTopicWidgets() +{ + const std::string current_topic = topic_selector_->currentText().toStdString(); + updateWidgetVisibility(current_topic, true); +} + +void MetricsVisualizePanel::hideInactiveTopicWidgets() +{ + const std::string current_topic = topic_selector_->currentText().toStdString(); + updateWidgetVisibility(current_topic, false); +} + +void MetricsVisualizePanel::onTopicChanged() +{ + std::lock_guard message_lock(mutex_); + hideInactiveTopicWidgets(); + showCurrentTopicWidgets(); +} + void MetricsVisualizePanel::onTimer() { std::lock_guard message_lock(mutex_); @@ -56,24 +101,38 @@ void MetricsVisualizePanel::onTimer() } } -void MetricsVisualizePanel::onMetrics(const DiagnosticArray::ConstSharedPtr msg) +void MetricsVisualizePanel::onMetrics( + const DiagnosticArray::ConstSharedPtr & msg, const std::string & topic_name) { std::lock_guard 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); + for (const auto & status : msg->status) { + const size_t num_current_metrics = topic_widgets_map_[topic_name].size(); if (metrics_.count(status.name) == 0) { - auto metric = Metric(status); + const 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); + + // 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 + 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); + + // Also add the widgets to the graph_widgets_ vector for easy removal later + topic_widgets_map_[topic_name][status.name] = std::make_pair(tableWidget, chartViewWidget); } metrics_.at(status.name).updateData(time, status); diff --git a/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.hpp b/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.hpp index a2aa674f017bc..db55eddaea844 100644 --- a/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.hpp +++ b/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.hpp @@ -19,6 +19,7 @@ #ifndef Q_MOC_RUN #include #include +#include #include #include #include @@ -36,7 +37,8 @@ #include #include #include - +#include +#include namespace rviz_plugins { @@ -186,19 +188,32 @@ class MetricsVisualizePanel : public rviz_common::Panel void onInitialize() override; private Q_SLOTS: + void onTopicChanged(); private: rclcpp::Node::SharedPtr raw_node_; rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Subscription::SharedPtr sub_; + std::unordered_map::SharedPtr> subscriptions_; + std::vector topics_ = { + "/diagnostic/planning_evaluator/metrics", "/diagnostic/perception_online_evaluator/metrics"}; void onTimer(); - void onMetrics(const DiagnosticArray::ConstSharedPtr msg); + void onMetrics(const DiagnosticArray::ConstSharedPtr & msg, const std::string & topic_name); QGridLayout * grid_; + QComboBox * topic_selector_; + + // >> + std::unordered_map< + std::string, std::unordered_map>> + topic_widgets_map_; std::mutex mutex_; std::unordered_map metrics_; + + void updateWidgetVisibility(const std::string & target_topic, const bool show); + void showCurrentTopicWidgets(); + void hideInactiveTopicWidgets(); }; } // namespace rviz_plugins