Skip to content

Commit 5776d00

Browse files
satoshi-otasatoshi-ota
committed
chore(evaluator): move evaluators
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent ad35c13 commit 5776d00

File tree

7 files changed

+601
-0
lines changed

7 files changed

+601
-0
lines changed

‎evaluation/tier4_metrics_rviz_plugin/CMakeLists.txt

+31
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
1+
cmake_minimum_required(VERSION 3.14)
2+
project(tier4_metrics_rviz_plugin)
3+
4+
find_package(autoware_cmake REQUIRED)
5+
autoware_package()
6+
7+
find_package(Qt5 REQUIRED Core Widgets Charts)
8+
set(QT_WIDGETS_LIB Qt5::Widgets)
9+
set(QT_CHARTS_LIB Qt5::Charts)
10+
set(CMAKE_AUTOMOC ON)
11+
add_definitions(-DQT_NO_KEYWORDS)
12+
13+
ament_auto_add_library(${PROJECT_NAME} SHARED
14+
src/metrics_visualize_panel.cpp
15+
include/metrics_visualize_panel.hpp
16+
)
17+
18+
target_link_libraries(${PROJECT_NAME}
19+
${QT_WIDGETS_LIB}
20+
${QT_CHARTS_LIB}
21+
)
22+
23+
target_compile_options(${PROJECT_NAME} PUBLIC -Wno-error=deprecated-copy -Wno-error=pedantic)
24+
# Export the plugin to be imported by rviz2
25+
pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml)
26+
27+
ament_auto_package(
28+
INSTALL_TO_SHARE
29+
icons
30+
plugins
31+
)

‎evaluation/tier4_metrics_rviz_plugin/README.md

+16
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
# tier4_metrics_rviz_plugin
2+
3+
## Purpose
4+
5+
This plugin panel to visualize `planning_evaluator` output.
6+
7+
## Inputs / Outputs
8+
9+
| Name | Type | Description |
10+
| ---------------------------------------- | --------------------------------------- | ------------------------------------- |
11+
| `/diagnostic/planning_evaluator/metrics` | `diagnostic_msgs::msg::DiagnosticArray` | Subscribe `planning_evaluator` output |
12+
13+
## HowToUse
14+
15+
1. Start rviz and select panels/Add new panel.
16+
2. Select MetricsVisualizePanel and press OK.

‎evaluation/tier4_metrics_rviz_plugin/icons/classes/MetricsVisualizePanel.png

18.4 KB
Loading

‎evaluation/tier4_metrics_rviz_plugin/include/metrics_visualize_panel.hpp

+254
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,254 @@
1+
// Copyright 2024 TIER IV, Inc. All rights reserved.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
//
15+
16+
#ifndef METRICS_VISUALIZE_PANEL_HPP_
17+
#define METRICS_VISUALIZE_PANEL_HPP_
18+
19+
#ifndef Q_MOC_RUN
20+
#include <QChartView>
21+
#include <QColor>
22+
#include <QComboBox>
23+
#include <QGridLayout>
24+
#include <QHeaderView>
25+
#include <QLabel>
26+
#include <QLineSeries>
27+
#include <QPainter>
28+
#include <QPushButton>
29+
#include <QTableWidget>
30+
#include <QVBoxLayout>
31+
#endif
32+
33+
#include <rclcpp/rclcpp.hpp>
34+
#include <rviz_common/panel.hpp>
35+
36+
#include <diagnostic_msgs/msg/diagnostic_array.hpp>
37+
38+
#include <iostream>
39+
#include <limits>
40+
#include <string>
41+
#include <unordered_map>
42+
#include <utility>
43+
#include <vector>
44+
45+
namespace rviz_plugins
46+
{
47+
48+
using diagnostic_msgs::msg::DiagnosticArray;
49+
using diagnostic_msgs::msg::DiagnosticStatus;
50+
using diagnostic_msgs::msg::KeyValue;
51+
using QtCharts::QChart;
52+
using QtCharts::QChartView;
53+
using QtCharts::QLineSeries;
54+
55+
struct Metric
56+
{
57+
public:
58+
explicit Metric(const DiagnosticStatus & status) : chart(new QChartView), table(new QTableWidget)
59+
{
60+
init(status);
61+
}
62+
63+
void init(const DiagnosticStatus & status)
64+
{
65+
QStringList header{};
66+
67+
{
68+
auto label = new QLabel;
69+
label->setAlignment(Qt::AlignCenter);
70+
label->setText(QString::fromStdString(status.name));
71+
labels.emplace("metric_name", label);
72+
73+
header.push_back("metric_name");
74+
}
75+
76+
for (const auto & [key, value] : status.values) {
77+
auto label = new QLabel;
78+
label->setAlignment(Qt::AlignCenter);
79+
labels.emplace(key, label);
80+
81+
auto plot = new QLineSeries;
82+
plot->setName(QString::fromStdString(key));
83+
plots.emplace(key, plot);
84+
chart->chart()->addSeries(plot);
85+
chart->chart()->createDefaultAxes();
86+
87+
header.push_back(QString::fromStdString(key));
88+
}
89+
90+
{
91+
chart->chart()->setTitle(QString::fromStdString(status.name));
92+
chart->chart()->legend()->setVisible(true);
93+
chart->chart()->legend()->detachFromChart();
94+
chart->setRenderHint(QPainter::Antialiasing);
95+
}
96+
97+
{
98+
table->setColumnCount(status.values.size() + 1);
99+
table->setHorizontalHeaderLabels(header);
100+
table->verticalHeader()->hide();
101+
table->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch);
102+
table->setRowCount(1);
103+
table->setSizeAdjustPolicy(QAbstractScrollArea::AdjustToContents);
104+
}
105+
}
106+
107+
void updateData(const double time, const DiagnosticStatus & status)
108+
{
109+
for (const auto & [key, value] : status.values) {
110+
const double data = std::stod(value);
111+
labels.at(key)->setText(QString::fromStdString(toString(data)));
112+
plots.at(key)->append(time, data);
113+
updateMinMax(data);
114+
}
115+
116+
{
117+
const auto area = chart->chart()->plotArea();
118+
const auto rect = chart->chart()->legend()->rect();
119+
chart->chart()->legend()->setGeometry(
120+
QRectF(area.x(), area.y(), area.width(), rect.height()));
121+
chart->chart()->axes(Qt::Horizontal).front()->setRange(time - 100.0, time);
122+
}
123+
124+
{
125+
table->setCellWidget(0, 0, labels.at("metric_name"));
126+
}
127+
128+
for (size_t i = 0; i < status.values.size(); ++i) {
129+
table->setCellWidget(0, i + 1, labels.at(status.values.at(i).key));
130+
}
131+
}
132+
133+
void updateMinMax(double data)
134+
{
135+
if (data < y_range_min) {
136+
y_range_min = data > 0.0 ? 0.9 * data : 1.1 * data;
137+
chart->chart()->axes(Qt::Vertical).front()->setMin(y_range_min);
138+
}
139+
140+
if (data > y_range_max) {
141+
y_range_max = data > 0.0 ? 1.1 * data : 0.9 * data;
142+
chart->chart()->axes(Qt::Vertical).front()->setMax(y_range_max);
143+
}
144+
}
145+
146+
void updateTable() { table->update(); }
147+
148+
void updateGraph() { chart->update(); }
149+
150+
QChartView * getChartView() const { return chart; }
151+
152+
QTableWidget * getTable() const { return table; }
153+
154+
std::unordered_map<std::string, QLabel *> getLabels() const { return labels; }
155+
156+
private:
157+
static std::optional<std::string> getValue(const DiagnosticStatus & status, std::string && key)
158+
{
159+
const auto itr = std::find_if(
160+
status.values.begin(), status.values.end(),
161+
[&](const auto & value) { return value.key == key; });
162+
163+
if (itr == status.values.end()) {
164+
return std::nullopt;
165+
}
166+
167+
return itr->value;
168+
}
169+
170+
static std::string toString(const double & value)
171+
{
172+
std::stringstream ss;
173+
ss << std::scientific << std::setprecision(2) << value;
174+
return ss.str();
175+
}
176+
177+
QChartView * chart;
178+
QTableWidget * table;
179+
180+
std::unordered_map<std::string, QLabel *> labels;
181+
std::unordered_map<std::string, QLineSeries *> plots;
182+
183+
double y_range_min{std::numeric_limits<double>::max()};
184+
double y_range_max{std::numeric_limits<double>::lowest()};
185+
};
186+
187+
class MetricsVisualizePanel : public rviz_common::Panel
188+
{
189+
Q_OBJECT
190+
191+
public:
192+
explicit MetricsVisualizePanel(QWidget * parent = nullptr);
193+
void onInitialize() override;
194+
195+
private Q_SLOTS:
196+
// Slot functions triggered by UI events
197+
void onTopicChanged();
198+
void onSpecificMetricChanged();
199+
void onClearButtonClicked();
200+
void onTabChanged();
201+
202+
private:
203+
// ROS 2 node and subscriptions for handling metrics data
204+
rclcpp::Node::SharedPtr raw_node_;
205+
rclcpp::TimerBase::SharedPtr timer_;
206+
std::unordered_map<std::string, rclcpp::Subscription<DiagnosticArray>::SharedPtr> subscriptions_;
207+
208+
// Topics from which metrics are collected
209+
std::vector<std::string> topics_ = {
210+
"/diagnostic/planning_evaluator/metrics", "/diagnostic/perception_online_evaluator/metrics"};
211+
212+
// Timer and metrics message callback
213+
void onTimer();
214+
void onMetrics(const DiagnosticArray::ConstSharedPtr & msg, const std::string & topic_name);
215+
216+
// Functions to update UI based on selected metrics
217+
void updateViews();
218+
void updateSelectedMetric(const std::string & metric_name);
219+
220+
// UI components
221+
QGridLayout * grid_;
222+
QComboBox * topic_selector_;
223+
QTabWidget * tab_widget_;
224+
225+
// "Specific Metrics" tab components
226+
QComboBox * specific_metric_selector_;
227+
QChartView * specific_metric_chart_view_;
228+
QTableWidget * specific_metric_table_;
229+
230+
// Selected metrics data
231+
std::optional<std::pair<std::string, Metric>> selected_metrics_;
232+
233+
// Cache for received messages by topics
234+
std::unordered_map<std::string, DiagnosticArray::ConstSharedPtr> current_msg_map_;
235+
236+
// Mapping from topics to metrics widgets (tables and charts)
237+
std::unordered_map<
238+
std::string, std::unordered_map<std::string, std::pair<QTableWidget *, QChartView *>>>
239+
topic_widgets_map_;
240+
241+
// Synchronization
242+
std::mutex mutex_;
243+
244+
// Stored metrics data
245+
std::unordered_map<std::string, Metric> metrics_;
246+
247+
// Utility functions for managing widget visibility based on topics
248+
void updateWidgetVisibility(const std::string & target_topic, const bool show);
249+
void showCurrentTopicWidgets();
250+
void hideInactiveTopicWidgets();
251+
};
252+
} // namespace rviz_plugins
253+
254+
#endif // METRICS_VISUALIZE_PANEL_HPP_

‎evaluation/tier4_metrics_rviz_plugin/package.xml

+33
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,33 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>tier4_metrics_rviz_plugin</name>
5+
<version>0.0.0</version>
6+
<description>The tier4_metrics_rviz_plugin package</description>
7+
<maintainer email="satoshi.ota@tier4.jp">Satoshi OTA</maintainer>
8+
<maintainer email="kyoichi.sugahara@tier4.jp">Kyoichi Sugahara</maintainer>
9+
<maintainer email="maxime.clement@tier4.jp">Maxime CLEMENT</maintainer>
10+
<maintainer email="kosuke.takeuchi@tier4.jp">Kosuke Takeuchi</maintainer>
11+
<maintainer email="fumiya.watanabe@tier4.jp">Fumiya Watanabe</maintainer>
12+
<license>Apache License 2.0</license>
13+
14+
<buildtool_depend>ament_cmake_auto</buildtool_depend>
15+
<buildtool_depend>autoware_cmake</buildtool_depend>
16+
17+
<depend>diagnostic_msgs</depend>
18+
<depend>libqt5-charts-dev</depend>
19+
<depend>libqt5-core</depend>
20+
<depend>libqt5-gui</depend>
21+
<depend>libqt5-widgets</depend>
22+
<depend>qtbase5-dev</depend>
23+
<depend>rclcpp</depend>
24+
<depend>rviz_common</depend>
25+
26+
<test_depend>ament_lint_auto</test_depend>
27+
<test_depend>autoware_lint_common</test_depend>
28+
29+
<export>
30+
<build_type>ament_cmake</build_type>
31+
<rviz plugin="${prefix}/plugins/plugin_description.xml"/>
32+
</export>
33+
</package>

‎evaluation/tier4_metrics_rviz_plugin/plugins/plugin_description.xml

+5
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
<library path="tier4_metrics_rviz_plugin">
2+
<class type="rviz_plugins::MetricsVisualizePanel" base_class_type="rviz_common::Panel">
3+
<description>MetricsVisualizePanel</description>
4+
</class>
5+
</library>

0 commit comments

Comments
 (0)