diff --git a/evaluation/tier4_metrics_rviz_plugin/CMakeLists.txt b/evaluation/tier4_metrics_rviz_plugin/CMakeLists.txt new file mode 100644 index 00000000..8475b596 --- /dev/null +++ b/evaluation/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) +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} + ${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 +) diff --git a/evaluation/tier4_metrics_rviz_plugin/README.md b/evaluation/tier4_metrics_rviz_plugin/README.md new file mode 100644 index 00000000..be941412 --- /dev/null +++ b/evaluation/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. diff --git a/evaluation/tier4_metrics_rviz_plugin/icons/classes/MetricsVisualizePanel.png b/evaluation/tier4_metrics_rviz_plugin/icons/classes/MetricsVisualizePanel.png new file mode 100644 index 00000000..6a675737 Binary files /dev/null and b/evaluation/tier4_metrics_rviz_plugin/icons/classes/MetricsVisualizePanel.png differ diff --git a/evaluation/tier4_metrics_rviz_plugin/include/metrics_visualize_panel.hpp b/evaluation/tier4_metrics_rviz_plugin/include/metrics_visualize_panel.hpp new file mode 100644 index 00000000..3d660999 --- /dev/null +++ b/evaluation/tier4_metrics_rviz_plugin/include/metrics_visualize_panel.hpp @@ -0,0 +1,254 @@ +// 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#endif + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +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(QString::fromStdString(status.name)); + labels.emplace("metric_name", label); + + header.push_back("metric_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; } + + std::unordered_map getLabels() const { return labels; } + +private: + static std::optional 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 labels; + std::unordered_map plots; + + double y_range_min{std::numeric_limits::max()}; + double y_range_max{std::numeric_limits::lowest()}; +}; + +class MetricsVisualizePanel : public rviz_common::Panel +{ + Q_OBJECT + +public: + explicit MetricsVisualizePanel(QWidget * parent = nullptr); + 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(); +}; +} // namespace rviz_plugins + +#endif // METRICS_VISUALIZE_PANEL_HPP_ diff --git a/evaluation/tier4_metrics_rviz_plugin/package.xml b/evaluation/tier4_metrics_rviz_plugin/package.xml new file mode 100644 index 00000000..d06382bc --- /dev/null +++ b/evaluation/tier4_metrics_rviz_plugin/package.xml @@ -0,0 +1,33 @@ + + + + tier4_metrics_rviz_plugin + 0.0.0 + The tier4_metrics_rviz_plugin package + Satoshi OTA + Kyoichi Sugahara + Maxime CLEMENT + Kosuke Takeuchi + Fumiya Watanabe + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + diagnostic_msgs + libqt5-charts-dev + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rclcpp + rviz_common + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/evaluation/tier4_metrics_rviz_plugin/plugins/plugin_description.xml b/evaluation/tier4_metrics_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 00000000..5aca5bd7 --- /dev/null +++ b/evaluation/tier4_metrics_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,5 @@ + + + MetricsVisualizePanel + + diff --git a/evaluation/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp b/evaluation/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp new file mode 100644 index 00000000..b92a9a7a --- /dev/null +++ b/evaluation/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp @@ -0,0 +1,262 @@ +// 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 + +#include + +#include +#include +#include +#include + +namespace rviz_plugins +{ +MetricsVisualizePanel::MetricsVisualizePanel(QWidget * parent) +: rviz_common::Panel(parent), grid_(new QGridLayout()) +{ + // 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); // 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() +{ + using std::placeholders::_1; + + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + + 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::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_); + + for (auto & [name, metric] : metrics_) { + metric.updateGraph(); + metric.updateTable(); + } + + if (selected_metrics_) { + selected_metrics_->second.updateGraph(); + selected_metrics_->second.updateTable(); + } +} + +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 (const auto & status : msg->status) { + const size_t num_current_metrics = topic_widgets_map_[topic_name].size(); + if (metrics_.count(status.name) == 0) { + const auto metric = Metric(status); + metrics_.emplace(status.name, metric); + + // Calculate grid position + const size_t row = num_current_metrics / GRAPH_COL_SIZE * 2 + + 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(); + + // 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()); + + // 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 + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::MetricsVisualizePanel, rviz_common::Panel) diff --git a/system/rqt_diagnostic_graph_monitor/CMakeLists.txt b/system/rqt_diagnostic_graph_monitor/CMakeLists.txt new file mode 100644 index 00000000..9e8bb20b --- /dev/null +++ b/system/rqt_diagnostic_graph_monitor/CMakeLists.txt @@ -0,0 +1,9 @@ +cmake_minimum_required(VERSION 3.14) +project(rqt_diagnostic_graph_monitor) + +find_package(autoware_cmake REQUIRED) +autoware_package() +ament_python_install_package(${PROJECT_NAME} PACKAGE_DIR python) +install(FILES plugin.xml DESTINATION share/${PROJECT_NAME}) +install(PROGRAMS script/rqt_diagnostic_graph_monitor DESTINATION lib/${PROJECT_NAME}) +ament_auto_package(INSTALL_TO_SHARE script) diff --git a/system/rqt_diagnostic_graph_monitor/README.md b/system/rqt_diagnostic_graph_monitor/README.md new file mode 100644 index 00000000..8dccca34 --- /dev/null +++ b/system/rqt_diagnostic_graph_monitor/README.md @@ -0,0 +1 @@ +# System diagnostic monitor diff --git a/system/rqt_diagnostic_graph_monitor/package.xml b/system/rqt_diagnostic_graph_monitor/package.xml new file mode 100644 index 00000000..60780e27 --- /dev/null +++ b/system/rqt_diagnostic_graph_monitor/package.xml @@ -0,0 +1,24 @@ + + + + rqt_diagnostic_graph_monitor + 0.1.0 + The rqt_diagnostic_graph_monitor package + Takagi, Isamu + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + python_qt_binding + rqt_gui + rqt_gui_py + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/system/rqt_diagnostic_graph_monitor/plugin.xml b/system/rqt_diagnostic_graph_monitor/plugin.xml new file mode 100644 index 00000000..6c64185e --- /dev/null +++ b/system/rqt_diagnostic_graph_monitor/plugin.xml @@ -0,0 +1,16 @@ + + + + + + + + folder + + + + utilities-system-monitor + + + + diff --git a/system/rqt_diagnostic_graph_monitor/python/__init__.py b/system/rqt_diagnostic_graph_monitor/python/__init__.py new file mode 100644 index 00000000..10f125fa --- /dev/null +++ b/system/rqt_diagnostic_graph_monitor/python/__init__.py @@ -0,0 +1,37 @@ +# Copyright 2023 The Autoware Contributors +# +# 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. + +from rqt_gui_py.plugin import Plugin + +from .module import MonitorModule +from .widget import MonitorWidget + + +class MonitorPlugin(Plugin): + def __init__(self, context): + super().__init__(context) + self.widget = MonitorWidget() + self.module = MonitorModule(context.node) + self.module.append_struct_callback(self.widget.on_graph) + context.add_widget(self.widget) + + def shutdown_plugin(self): + self.module.shutdown() + self.widget.shutdown() + + def save_settings(self, plugin_settings, instance_settings): + pass + + def restore_settings(self, plugin_settings, instance_settings): + pass diff --git a/system/rqt_diagnostic_graph_monitor/python/graph.py b/system/rqt_diagnostic_graph_monitor/python/graph.py new file mode 100644 index 00000000..cea81c12 --- /dev/null +++ b/system/rqt_diagnostic_graph_monitor/python/graph.py @@ -0,0 +1,127 @@ +# Copyright 2023 The Autoware Contributors +# +# 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. + +from diagnostic_msgs.msg import DiagnosticStatus +from rclpy.time import Time + + +class DummyStatus: + def __init__(self): + self.level = DiagnosticStatus.STALE + + +class BaseUnit: + def __init__(self, status=DummyStatus()): + self._parents = [] + self._children = [] + self._path = None + self._type = None + self._status = status + + @property + def parents(self): + return self._parents + + @property + def children(self): + return self._children + + @property + def path(self): + return self._path + + @property + def kind(self): + return self._type + + +class NodeUnit(BaseUnit): + def __init__(self, struct): + super().__init__() + self._path = struct.path + self._type = struct.type + + def update(self, status): + self._status = status + + @property + def level(self): + return self._status.level + + +class DiagUnit(BaseUnit): + def __init__(self, struct): + super().__init__() + self._path = struct.path + self._name = struct.name + self._type = "diag" + + def update(self, status): + self._status = status + + @property + def level(self): + return self._status.level + + +class UnitLink: + def __init__(self, parent: BaseUnit, child: BaseUnit): + self._parent = parent + self._child = child + parent._children.append(self) + child._parents.append(self) + + def update(self, status): + self.status = status + + @property + def parent(self): + return self._parent + + @property + def child(self): + return self._child + + +class Graph: + def __init__(self, msg): + self._struct_stamp = Time.from_msg(msg.stamp) + self._status_stamp = None + self._id = msg.id + self._nodes = [NodeUnit(struct) for struct in msg.nodes] + self._diags = [DiagUnit(struct) for struct in msg.diags] + self._units = self._nodes + self._diags + self._links = [] + for struct in msg.links: + units = self._diags if struct.is_leaf else self._nodes + nodes = self._nodes + self._links.append(UnitLink(nodes[struct.parent], units[struct.child])) + + def update(self, msg): + if msg.id == self._id: + self._status_stamp = Time.from_msg(msg.stamp) + for node, status in zip(self._nodes, msg.nodes): + node.update(status) + for diag, status in zip(self._diags, msg.diags): + diag.update(status) + for link, status in zip(self._links, msg.links): + link.update(status) + + @property + def units(self): + return self._units + + @property + def links(self): + return self._links diff --git a/system/rqt_diagnostic_graph_monitor/python/items.py b/system/rqt_diagnostic_graph_monitor/python/items.py new file mode 100644 index 00000000..96f60ef0 --- /dev/null +++ b/system/rqt_diagnostic_graph_monitor/python/items.py @@ -0,0 +1,54 @@ +# Copyright 2024 The Autoware Contributors +# +# 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. + + +from diagnostic_msgs.msg import DiagnosticStatus +from python_qt_binding import QtGui +from python_qt_binding import QtWidgets + +from .graph import BaseUnit +from .graph import UnitLink + + +class MonitorIcons: + def __init__(self): + self.disable = QtGui.QIcon.fromTheme("dialog-question") + self.unknown = QtGui.QIcon.fromTheme("system-search") + self.ok = QtGui.QIcon.fromTheme("emblem-default") + self.warn = QtGui.QIcon.fromTheme("emblem-important") + self.error = QtGui.QIcon.fromTheme("dialog-error") + self.stale = QtGui.QIcon.fromTheme("appointment-missed") + + self._levels = {} + self._levels[DiagnosticStatus.OK] = self.ok + self._levels[DiagnosticStatus.WARN] = self.warn + self._levels[DiagnosticStatus.ERROR] = self.error + self._levels[DiagnosticStatus.STALE] = self.stale + + def get(self, level): + return self._levels.get(level, self.unknown) + + +class MonitorItem: + icons = MonitorIcons() + + def __init__(self, link: UnitLink, unit: BaseUnit): + item_text = f"{unit.path} ({unit.kind})" if unit.path else f"({unit.kind})" + self.item = QtWidgets.QTreeWidgetItem([item_text]) + self.link = link + self.unit = unit + self.item.setIcon(0, self.icons.stale) + + def update(self): + self.item.setIcon(0, self.icons.get(self.unit.level)) diff --git a/system/rqt_diagnostic_graph_monitor/python/module.py b/system/rqt_diagnostic_graph_monitor/python/module.py new file mode 100644 index 00000000..74294659 --- /dev/null +++ b/system/rqt_diagnostic_graph_monitor/python/module.py @@ -0,0 +1,61 @@ +# Copyright 2023 The Autoware Contributors +# +# 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. + + +from rclpy.node import Node +from tier4_system_msgs.msg import DiagGraphStatus +from tier4_system_msgs.msg import DiagGraphStruct + +from .graph import Graph +from .utils import default_qos +from .utils import durable_qos +from .utils import foreach + + +class MonitorModule: + def __init__(self, node: Node): + self.graph = None + self.struct_callbacks = [] + self.status_callbacks = [] + self.node = node + self.sub_struct = self.subscribe_struct() + self.sub_status = self.subscribe_status() + + def append_struct_callback(self, callback): + self.struct_callbacks.append(callback) + + def append_status_callback(self, callback): + self.status_callbacks.append(callback) + + def on_struct(self, msg): + self.graph = Graph(msg) + foreach(self.struct_callbacks, lambda callback: callback(self.graph)) + + def on_status(self, msg): + self.graph.update(msg) + foreach(self.status_callbacks, lambda callback: callback(self.graph)) + + def subscribe_struct(self): + return self.node.create_subscription( + DiagGraphStruct, "/diagnostics_graph/struct", self.on_struct, durable_qos(1) + ) + + def subscribe_status(self): + return self.node.create_subscription( + DiagGraphStatus, "/diagnostics_graph/status", self.on_status, default_qos(1) + ) + + def shutdown(self): + self.node.destroy_subscription(self.sub_struct) + self.node.destroy_subscription(self.sub_status) diff --git a/system/rqt_diagnostic_graph_monitor/python/utils.py b/system/rqt_diagnostic_graph_monitor/python/utils.py new file mode 100644 index 00000000..6e66102b --- /dev/null +++ b/system/rqt_diagnostic_graph_monitor/python/utils.py @@ -0,0 +1,29 @@ +# Copyright 2024 The Autoware Contributors +# +# 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. + +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSProfile + + +def default_qos(depth): + return QoSProfile(depth=depth) + + +def durable_qos(depth): + return QoSProfile(depth=depth, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) + + +def foreach(iterable, function): + for item in iterable: + function(item) diff --git a/system/rqt_diagnostic_graph_monitor/python/widget.py b/system/rqt_diagnostic_graph_monitor/python/widget.py new file mode 100644 index 00000000..e7f022e5 --- /dev/null +++ b/system/rqt_diagnostic_graph_monitor/python/widget.py @@ -0,0 +1,85 @@ +# Copyright 2023 The Autoware Contributors +# +# 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. + +from python_qt_binding import QtCore +from python_qt_binding import QtWidgets + +from .graph import Graph +from .items import MonitorItem +from .utils import foreach + + +class MonitorWidget(QtWidgets.QSplitter): + def __init__(self): + super().__init__() + self.graph = None + self.items = [] + self.root_widget = QtWidgets.QTreeWidget() + self.tree_widget = QtWidgets.QTreeWidget() + self.root_widget.setHeaderLabels(["Top-level"]) + self.tree_widget.setHeaderLabels(["Subtrees"]) + self.addWidget(self.root_widget) + self.addWidget(self.tree_widget) + + self._timer = QtCore.QTimer() + self._timer.timeout.connect(self.on_timer) + self._timer.start(500) + + def shutdown(self): + self.clear_graph() + + def on_timer(self): + foreach(self.items, lambda item: item.update()) + + def on_graph(self, graph: Graph): + self.clear_graph() + self.build_graph(graph) + + def clear_graph(self): + self.graph = None + self.items = [] + self.root_widget.clear() + self.tree_widget.clear() + + def build_graph(self, graph: Graph): + self.graph = graph + root_units = filter(self.is_root_unit, self.graph.units) + tree_units = filter(self.is_tree_unit, self.graph.units) + root_items = [MonitorItem(None, unit) for unit in root_units] + tree_items = [MonitorItem(None, unit) for unit in tree_units] + link_items = [MonitorItem(link, link.child) for link in self.graph.links] + self.items = root_items + tree_items + link_items + + # Note: overwrite link items with root/tree items if there is more than one. + parents = {} + for items in [link_items, tree_items, root_items]: + parents.update({item.unit: item.item for item in items}) + + # Connect tree widget items. + root_widget_item = self.root_widget.invisibleRootItem() + tree_widget_item = self.tree_widget.invisibleRootItem() + for item in root_items: + root_widget_item.addChild(item.item) + for item in tree_items: + tree_widget_item.addChild(item.item) + for item in link_items: + parents[item.link.parent].addChild(item.item) + + @staticmethod + def is_root_unit(unit): + return len(unit.parents) == 0 + + @staticmethod + def is_tree_unit(unit): + return len(unit.parents) >= 2 and len(unit.children) != 0 diff --git a/system/rqt_diagnostic_graph_monitor/script/cui_diagnostic_graph_monitor b/system/rqt_diagnostic_graph_monitor/script/cui_diagnostic_graph_monitor new file mode 100755 index 00000000..8b068512 --- /dev/null +++ b/system/rqt_diagnostic_graph_monitor/script/cui_diagnostic_graph_monitor @@ -0,0 +1,14 @@ +#!/usr/bin/env python3 + +import rclpy +from rqt_diagnostic_graph_monitor.module import MonitorModule + +if __name__ == "__main__": + try: + rclpy.init() + node = rclpy.create_node("test_rqt_diagnostic_graph_monitor") + test = MonitorModule(node) + rclpy.spin(node) + rclpy.shutdown() + except KeyboardInterrupt: + print("KeyboardInterrupt") diff --git a/system/rqt_diagnostic_graph_monitor/script/rqt_diagnostic_graph_monitor b/system/rqt_diagnostic_graph_monitor/script/rqt_diagnostic_graph_monitor new file mode 100755 index 00000000..a5237a87 --- /dev/null +++ b/system/rqt_diagnostic_graph_monitor/script/rqt_diagnostic_graph_monitor @@ -0,0 +1,8 @@ +#!/usr/bin/env python3 + +import sys + +import rqt_gui.main + +rqt_main = rqt_gui.main.Main() +sys.exit(rqt_main.main(sys.argv, standalone="rqt_diagnostic_graph_monitor.MonitorPlugin"))