Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(tier4_metrics_rviz_plugin): switch visualization target topic #6571

Merged
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
85 changes: 72 additions & 13 deletions evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,12 @@
: rviz_common::Panel(parent), grid_(new QGridLayout())
{
setLayout(grid_);
topic_selector_ = new QComboBox();
for (const auto & topic : topics_) {
topic_selector_->addItem(QString::fromStdString(topic));

Check warning on line 35 in evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp

View check run for this annotation

Codecov / codecov/patch

evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp#L33-L35

Added lines #L33 - L35 were not covered by tests
}
grid_->addWidget(topic_selector_, 0, 0, 1, -1);
connect(topic_selector_, SIGNAL(currentIndexChanged(int)), this, SLOT(onTopicChanged()));

Check warning on line 38 in evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp

View check run for this annotation

Codecov / codecov/patch

evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp#L37-L38

Added lines #L37 - L38 were not covered by tests
}

void MetricsVisualizePanel::onInitialize()
Expand All @@ -38,14 +44,53 @@

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));
for (const auto & topic_name : topics_) {
const auto callback = [this, topic_name](const DiagnosticArray::ConstSharedPtr msg) {
this->onMetrics(msg, topic_name);
};

Check warning on line 50 in evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp

View check run for this annotation

Codecov / codecov/patch

evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp#L47-L50

Added lines #L47 - L50 were not covered by tests
const auto subscription =
raw_node_->create_subscription<DiagnosticArray>(topic_name, rclcpp::QoS{1}, callback);

Check warning on line 52 in evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp

View check run for this annotation

Codecov / codecov/patch

evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp#L52

Added line #L52 was not covered by tests
subscriptions_[topic_name] = subscription;
}

const auto period = std::chrono::milliseconds(static_cast<int64_t>(1e3 / 10));
timer_ = raw_node_->create_wall_timer(period, [&]() { onTimer(); });
}

void MetricsVisualizePanel::updateWidgetVisibility(

Check warning on line 60 in evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp

View check run for this annotation

Codecov / codecov/patch

evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp#L60

Added line #L60 was not covered by tests
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)) {

Check warning on line 65 in evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Conditional

MetricsVisualizePanel::updateWidgetVisibility has 1 complex conditionals with 3 branches, threshold = 2. A complex conditional is an expression inside a branch (e.g. if, for, while) which consists of multiple, logical operators such as AND/OR. The more logical operators in an expression, the more severe the code smell.
continue;

Check warning on line 66 in evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp

View check run for this annotation

Codecov / codecov/patch

evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp#L63-L66

Added lines #L63 - L66 were not covered by tests
}
for (const auto & [metric, widgets] : metric_widgets_pair) {
widgets.first->setVisible(show);
widgets.second->setVisible(show);

Check warning on line 70 in evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp

View check run for this annotation

Codecov / codecov/patch

evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp#L68-L70

Added lines #L68 - L70 were not covered by tests
}
}
}

Check warning on line 73 in evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp

View check run for this annotation

Codecov / codecov/patch

evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp#L73

Added line #L73 was not covered by tests

void MetricsVisualizePanel::showCurrentTopicWidgets()

Check warning on line 75 in evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp

View check run for this annotation

Codecov / codecov/patch

evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp#L75

Added line #L75 was not covered by tests
{
const std::string current_topic = topic_selector_->currentText().toStdString();
updateWidgetVisibility(current_topic, true);
}

Check warning on line 79 in evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp

View check run for this annotation

Codecov / codecov/patch

evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp#L77-L79

Added lines #L77 - L79 were not covered by tests

void MetricsVisualizePanel::hideInactiveTopicWidgets()

Check warning on line 81 in evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp

View check run for this annotation

Codecov / codecov/patch

evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp#L81

Added line #L81 was not covered by tests
{
const std::string current_topic = topic_selector_->currentText().toStdString();
updateWidgetVisibility(current_topic, false);
}

Check warning on line 85 in evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp

View check run for this annotation

Codecov / codecov/patch

evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp#L83-L85

Added lines #L83 - L85 were not covered by tests

void MetricsVisualizePanel::onTopicChanged()

Check warning on line 87 in evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp

View check run for this annotation

Codecov / codecov/patch

evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp#L87

Added line #L87 was not covered by tests
{
std::lock_guard<std::mutex> message_lock(mutex_);
hideInactiveTopicWidgets();
showCurrentTopicWidgets();
}

Check warning on line 92 in evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp

View check run for this annotation

Codecov / codecov/patch

evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp#L89-L92

Added lines #L89 - L92 were not covered by tests

void MetricsVisualizePanel::onTimer()
{
std::lock_guard<std::mutex> message_lock(mutex_);
Expand All @@ -56,24 +101,38 @@
}
}

void MetricsVisualizePanel::onMetrics(const DiagnosticArray::ConstSharedPtr msg)
void MetricsVisualizePanel::onMetrics(

Check warning on line 104 in evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp

View check run for this annotation

Codecov / codecov/patch

evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp#L104

Added line #L104 was not covered by tests
const DiagnosticArray::ConstSharedPtr & msg, const std::string & topic_name)
{
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);

for (const auto & status : msg->status) {

Check warning on line 112 in evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp

View check run for this annotation

Codecov / codecov/patch

evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp#L112

Added line #L112 was not covered by tests
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);

Check warning on line 115 in evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp

View check run for this annotation

Codecov / codecov/patch

evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp#L115

Added line #L115 was not covered by tests
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 +

Check warning on line 119 in evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp

View check run for this annotation

Codecov / codecov/patch

evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp#L119

Added line #L119 was not covered by tests
1; // start from 1 to leave space for the topic selector
const size_t col = num_current_metrics % GRAPH_COL_SIZE;

Check warning on line 121 in evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp

View check run for this annotation

Codecov / codecov/patch

evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp#L121

Added line #L121 was not covered by tests

// 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);

Check warning on line 132 in evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp

View check run for this annotation

Codecov / codecov/patch

evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp#L128-L132

Added lines #L128 - L132 were not covered by tests

// 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);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
#ifndef Q_MOC_RUN
#include <QChartView>
#include <QColor>
#include <QComboBox>
#include <QGridLayout>
#include <QHeaderView>
#include <QLabel>
Expand All @@ -36,7 +37,8 @@
#include <limits>
#include <string>
#include <unordered_map>

#include <utility>
#include <vector>
namespace rviz_plugins
{

Expand Down Expand Up @@ -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<DiagnosticArray>::SharedPtr sub_;
std::unordered_map<std::string, rclcpp::Subscription<DiagnosticArray>::SharedPtr> subscriptions_;
std::vector<std::string> 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_;

// <topic_name, <metric_name, <table, chart>>>
std::unordered_map<
std::string, std::unordered_map<std::string, std::pair<QTableWidget *, QChartView *>>>
topic_widgets_map_;

std::mutex mutex_;
std::unordered_map<std::string, Metric> metrics_;

void updateWidgetVisibility(const std::string & target_topic, const bool show);
void showCurrentTopicWidgets();
void hideInactiveTopicWidgets();
};
} // namespace rviz_plugins

Expand Down
Loading