Skip to content

Commit 449edae

Browse files
miurshmiursh
authored
Merge branch 'main' into refactor/trt-build-log
2 parents 336bc78 + 1c4c3e3 commit 449edae

File tree

31 files changed

+551
-137
lines changed

31 files changed

+551
-137
lines changed

‎evaluator/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+
set(CMAKE_INCLUDE_CURRENT_DIR ON)
12+
add_definitions(-DQT_NO_KEYWORDS)
13+
14+
ament_auto_add_library(${PROJECT_NAME} SHARED
15+
src/metrics_visualize_panel.cpp
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+
)

‎evaluator/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.

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

18.4 KB
Loading

‎evaluator/tier4_metrics_rviz_plugin/package.xml

+30
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
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+
<license>Apache License 2.0</license>
11+
12+
<buildtool_depend>ament_cmake_auto</buildtool_depend>
13+
<buildtool_depend>autoware_cmake</buildtool_depend>
14+
15+
<depend>diagnostic_msgs</depend>
16+
<depend>libqt5-core</depend>
17+
<depend>libqt5-gui</depend>
18+
<depend>libqt5-widgets</depend>
19+
<depend>qtbase5-dev</depend>
20+
<depend>rclcpp</depend>
21+
<depend>rviz_common</depend>
22+
23+
<test_depend>ament_lint_auto</test_depend>
24+
<test_depend>autoware_lint_common</test_depend>
25+
26+
<export>
27+
<build_type>ament_cmake</build_type>
28+
<rviz plugin="${prefix}/plugins/plugin_description.xml"/>
29+
</export>
30+
</package>

‎evaluator/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>

‎evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp

+86
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,86 @@
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+
#include "metrics_visualize_panel.hpp"
17+
18+
#include <rviz_common/display_context.hpp>
19+
20+
#include <X11/Xlib.h>
21+
22+
#include <algorithm>
23+
#include <optional>
24+
#include <string>
25+
#include <vector>
26+
27+
namespace rviz_plugins
28+
{
29+
MetricsVisualizePanel::MetricsVisualizePanel(QWidget * parent)
30+
: rviz_common::Panel(parent), grid_(new QGridLayout())
31+
{
32+
setLayout(grid_);
33+
}
34+
35+
void MetricsVisualizePanel::onInitialize()
36+
{
37+
using std::placeholders::_1;
38+
39+
raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
40+
41+
sub_ = raw_node_->create_subscription<DiagnosticArray>(
42+
"/diagnostic/planning_evaluator/metrics", rclcpp::QoS{1},
43+
std::bind(&MetricsVisualizePanel::onMetrics, this, _1));
44+
45+
const auto period = std::chrono::milliseconds(static_cast<int64_t>(1e3 / 10));
46+
timer_ = raw_node_->create_wall_timer(period, [&]() { onTimer(); });
47+
}
48+
49+
void MetricsVisualizePanel::onTimer()
50+
{
51+
std::lock_guard<std::mutex> message_lock(mutex_);
52+
53+
for (auto & [name, metric] : metrics_) {
54+
metric.updateGraph();
55+
metric.updateTable();
56+
}
57+
}
58+
59+
void MetricsVisualizePanel::onMetrics(const DiagnosticArray::ConstSharedPtr msg)
60+
{
61+
std::lock_guard<std::mutex> message_lock(mutex_);
62+
63+
const auto time = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9;
64+
65+
constexpr size_t GRAPH_COL_SIZE = 5;
66+
for (size_t i = 0; i < msg->status.size(); ++i) {
67+
const auto & status = msg->status.at(i);
68+
69+
if (metrics_.count(status.name) == 0) {
70+
auto metric = Metric(status);
71+
metrics_.emplace(status.name, metric);
72+
grid_->addWidget(metric.getTable(), i / GRAPH_COL_SIZE * 2, i % GRAPH_COL_SIZE);
73+
grid_->setRowStretch(i / GRAPH_COL_SIZE * 2, false);
74+
grid_->addWidget(metric.getChartView(), i / GRAPH_COL_SIZE * 2 + 1, i % GRAPH_COL_SIZE);
75+
grid_->setRowStretch(i / GRAPH_COL_SIZE * 2 + 1, true);
76+
grid_->setColumnStretch(i % GRAPH_COL_SIZE, true);
77+
}
78+
79+
metrics_.at(status.name).updateData(time, status);
80+
}
81+
}
82+
83+
} // namespace rviz_plugins
84+
85+
#include <pluginlib/class_list_macros.hpp>
86+
PLUGINLIB_EXPORT_CLASS(rviz_plugins::MetricsVisualizePanel, rviz_common::Panel)

‎evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.hpp

+205
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,205 @@
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 <QGridLayout>
23+
#include <QHeaderView>
24+
#include <QLabel>
25+
#include <QLineSeries>
26+
#include <QPainter>
27+
#include <QTableWidget>
28+
#include <QVBoxLayout>
29+
#endif
30+
31+
#include <rclcpp/rclcpp.hpp>
32+
#include <rviz_common/panel.hpp>
33+
34+
#include <diagnostic_msgs/msg/diagnostic_array.hpp>
35+
36+
#include <limits>
37+
#include <string>
38+
#include <unordered_map>
39+
40+
namespace rviz_plugins
41+
{
42+
43+
using diagnostic_msgs::msg::DiagnosticArray;
44+
using diagnostic_msgs::msg::DiagnosticStatus;
45+
using diagnostic_msgs::msg::KeyValue;
46+
using QtCharts::QChart;
47+
using QtCharts::QChartView;
48+
using QtCharts::QLineSeries;
49+
50+
struct Metric
51+
{
52+
public:
53+
explicit Metric(const DiagnosticStatus & status) : chart(new QChartView), table(new QTableWidget)
54+
{
55+
init(status);
56+
}
57+
58+
void init(const DiagnosticStatus & status)
59+
{
60+
QStringList header{};
61+
62+
{
63+
auto label = new QLabel;
64+
label->setAlignment(Qt::AlignCenter);
65+
label->setText("metric_name");
66+
labels.emplace("metric_name", label);
67+
68+
header.push_back(QString::fromStdString(status.name));
69+
}
70+
71+
for (const auto & [key, value] : status.values) {
72+
auto label = new QLabel;
73+
label->setAlignment(Qt::AlignCenter);
74+
labels.emplace(key, label);
75+
76+
auto plot = new QLineSeries;
77+
plot->setName(QString::fromStdString(key));
78+
plots.emplace(key, plot);
79+
chart->chart()->addSeries(plot);
80+
chart->chart()->createDefaultAxes();
81+
82+
header.push_back(QString::fromStdString(key));
83+
}
84+
85+
{
86+
chart->chart()->setTitle(QString::fromStdString(status.name));
87+
chart->chart()->legend()->setVisible(true);
88+
chart->chart()->legend()->detachFromChart();
89+
chart->setRenderHint(QPainter::Antialiasing);
90+
}
91+
92+
{
93+
table->setColumnCount(status.values.size() + 1);
94+
table->setHorizontalHeaderLabels(header);
95+
table->verticalHeader()->hide();
96+
table->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch);
97+
table->setRowCount(1);
98+
table->setSizeAdjustPolicy(QAbstractScrollArea::AdjustToContents);
99+
}
100+
}
101+
102+
void updateData(const double time, const DiagnosticStatus & status)
103+
{
104+
for (const auto & [key, value] : status.values) {
105+
const double data = std::stod(value);
106+
labels.at(key)->setText(QString::fromStdString(toString(data)));
107+
plots.at(key)->append(time, data);
108+
updateMinMax(data);
109+
}
110+
111+
{
112+
const auto area = chart->chart()->plotArea();
113+
const auto rect = chart->chart()->legend()->rect();
114+
chart->chart()->legend()->setGeometry(
115+
QRectF(area.x(), area.y(), area.width(), rect.height()));
116+
chart->chart()->axes(Qt::Horizontal).front()->setRange(time - 100.0, time);
117+
}
118+
119+
{
120+
table->setCellWidget(0, 0, labels.at("metric_name"));
121+
}
122+
123+
for (size_t i = 0; i < status.values.size(); ++i) {
124+
table->setCellWidget(0, i + 1, labels.at(status.values.at(i).key));
125+
}
126+
}
127+
128+
void updateMinMax(double data)
129+
{
130+
if (data < y_range_min) {
131+
y_range_min = data > 0.0 ? 0.9 * data : 1.1 * data;
132+
chart->chart()->axes(Qt::Vertical).front()->setMin(y_range_min);
133+
}
134+
135+
if (data > y_range_max) {
136+
y_range_max = data > 0.0 ? 1.1 * data : 0.9 * data;
137+
chart->chart()->axes(Qt::Vertical).front()->setMax(y_range_max);
138+
}
139+
}
140+
141+
void updateTable() { table->update(); }
142+
143+
void updateGraph() { chart->update(); }
144+
145+
QChartView * getChartView() const { return chart; }
146+
147+
QTableWidget * getTable() const { return table; }
148+
149+
private:
150+
static std::optional<std::string> getValue(const DiagnosticStatus & status, std::string && key)
151+
{
152+
const auto itr = std::find_if(
153+
status.values.begin(), status.values.end(),
154+
[&](const auto & value) { return value.key == key; });
155+
156+
if (itr == status.values.end()) {
157+
return std::nullopt;
158+
}
159+
160+
return itr->value;
161+
}
162+
163+
static std::string toString(const double & value)
164+
{
165+
std::stringstream ss;
166+
ss << std::scientific << std::setprecision(2) << value;
167+
return ss.str();
168+
}
169+
170+
QChartView * chart;
171+
QTableWidget * table;
172+
173+
std::unordered_map<std::string, QLabel *> labels;
174+
std::unordered_map<std::string, QLineSeries *> plots;
175+
176+
double y_range_min{std::numeric_limits<double>::max()};
177+
double y_range_max{std::numeric_limits<double>::lowest()};
178+
};
179+
180+
class MetricsVisualizePanel : public rviz_common::Panel
181+
{
182+
Q_OBJECT
183+
184+
public:
185+
explicit MetricsVisualizePanel(QWidget * parent = nullptr);
186+
void onInitialize() override;
187+
188+
private Q_SLOTS:
189+
190+
private:
191+
rclcpp::Node::SharedPtr raw_node_;
192+
rclcpp::TimerBase::SharedPtr timer_;
193+
rclcpp::Subscription<DiagnosticArray>::SharedPtr sub_;
194+
195+
void onTimer();
196+
void onMetrics(const DiagnosticArray::ConstSharedPtr msg);
197+
198+
QGridLayout * grid_;
199+
200+
std::mutex mutex_;
201+
std::unordered_map<std::string, Metric> metrics_;
202+
};
203+
} // namespace rviz_plugins
204+
205+
#endif // METRICS_VISUALIZE_PANEL_HPP_

‎localization/localization_error_monitor/src/diagnostics.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracy(
2323
diagnostic_msgs::msg::DiagnosticStatus stat;
2424

2525
diagnostic_msgs::msg::KeyValue key_value;
26-
key_value.key = "localization_accuracy";
26+
key_value.key = "localization_error_ellipse";
2727
key_value.value = std::to_string(ellipse_size);
2828
stat.values.push_back(key_value);
2929

@@ -47,7 +47,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracyLateralDirection
4747
diagnostic_msgs::msg::DiagnosticStatus stat;
4848

4949
diagnostic_msgs::msg::KeyValue key_value;
50-
key_value.key = "localization_accuracy_lateral_direction";
50+
key_value.key = "localization_error_ellipse_lateral_direction";
5151
key_value.value = std::to_string(ellipse_size);
5252
stat.values.push_back(key_value);
5353

0 commit comments

Comments
 (0)