diff --git a/.cspell-partial.json b/.cspell-partial.json index 9949d2678..3c93c036b 100644 --- a/.cspell-partial.json +++ b/.cspell-partial.json @@ -29,6 +29,7 @@ "usecols", "velb", "viridis", - "zdata" + "zdata", + "Segoe" ] } diff --git a/common/autoware_trajectory_kinematics_rviz_plugin/CMakeLists.txt b/common/autoware_trajectory_kinematics_rviz_plugin/CMakeLists.txt new file mode 100644 index 000000000..bb10ffa42 --- /dev/null +++ b/common/autoware_trajectory_kinematics_rviz_plugin/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_trajectory_kinematics_rviz_plugin) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Qt5 REQUIRED Core Widgets Charts) +set(QT_LIBRARIES Qt5::Widgets Qt5::Charts) +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) +add_definitions(-DQT_NO_KEYWORDS) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/trajectory_message_adapter.cpp + src/trajectory_series_manager.cpp + src/trajectory_kinematics_chart_view.cpp + src/trajectory_kinematics_plot_widget.cpp + src/trajectory_kinematics_panel.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} +) + +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + icons + plugins +) diff --git a/common/autoware_trajectory_kinematics_rviz_plugin/README.md b/common/autoware_trajectory_kinematics_rviz_plugin/README.md new file mode 100644 index 000000000..09056c122 --- /dev/null +++ b/common/autoware_trajectory_kinematics_rviz_plugin/README.md @@ -0,0 +1,23 @@ +# autoware_trajectory_kinematics_rviz_plugin + +RViz2 **panel plugin** that plots kinematic quantities along planned paths and compares **multiple trajectories** on one Qt Charts graph. + +## Features + +- Subscribe to one or more topics publishing: + - `autoware_planning_msgs/msg/Trajectory`, or + - `autoware_internal_planning_msgs/msg/ScoredCandidateTrajectories` +- Choose **X** (time from start or arc length) and **Y** (velocity, acceleration, curvature, lateral acceleration). +- Checkbox which series to plot; optional **fixed X/Y axis ranges**; hover tooltip with crosshair. +- Panel layout and topic list are stored in the RViz config (`save` / `load`). + +## Usage + +1. Build the workspace with this package in the overlay (standard `colcon build`). +2. Start RViz2, **Add Panel** → select **`autoware_trajectory_kinematics_rviz_plugin/TrajectoryKinematicsPanel`**. +3. Choose message kind, add topics (dropdown or “Other topic”), tick trajectories to plot, set axes. + +## ROS interfaces + +- **Subscribes** (user-configured): topics you add, as `Trajectory` or `ScoredCandidateTrajectories` depending on the kind selector. +- **Does not publish** topics. diff --git a/common/autoware_trajectory_kinematics_rviz_plugin/icons/classes/TrajectoryKinematicsPanel.png b/common/autoware_trajectory_kinematics_rviz_plugin/icons/classes/TrajectoryKinematicsPanel.png new file mode 100644 index 000000000..6a6757371 Binary files /dev/null and b/common/autoware_trajectory_kinematics_rviz_plugin/icons/classes/TrajectoryKinematicsPanel.png differ diff --git a/common/autoware_trajectory_kinematics_rviz_plugin/package.xml b/common/autoware_trajectory_kinematics_rviz_plugin/package.xml new file mode 100644 index 000000000..f3eaa5924 --- /dev/null +++ b/common/autoware_trajectory_kinematics_rviz_plugin/package.xml @@ -0,0 +1,27 @@ + + + + autoware_trajectory_kinematics_rviz_plugin + 0.46.0 + RViz2 panel to plot trajectory kinematics (velocity, acceleration, curvature, lateral acceleration) from Trajectory or ScoredCandidateTrajectories topics. + Taiki Tanaka + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_internal_planning_msgs + autoware_planning_msgs + libqt5-charts-dev + pluginlib + rclcpp + rviz_common + + ament_cmake_gtest + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/autoware_trajectory_kinematics_rviz_plugin/plugins/plugin_description.xml b/common/autoware_trajectory_kinematics_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 000000000..7601ba863 --- /dev/null +++ b/common/autoware_trajectory_kinematics_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,8 @@ + + + Plot kinematics along autoware_planning_msgs/Trajectory or autoware_internal_planning_msgs/ScoredCandidateTrajectories (multi-series comparison). + + diff --git a/common/autoware_trajectory_kinematics_rviz_plugin/src/kinematics_types.hpp b/common/autoware_trajectory_kinematics_rviz_plugin/src/kinematics_types.hpp new file mode 100644 index 000000000..60c766086 --- /dev/null +++ b/common/autoware_trajectory_kinematics_rviz_plugin/src/kinematics_types.hpp @@ -0,0 +1,156 @@ +// Copyright 2026 TIER IV, Inc. +// +// 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 KINEMATICS_TYPES_HPP_ +#define KINEMATICS_TYPES_HPP_ + +#include +#include +#include + +namespace autoware::visualization::trajectory_kinematics_rviz_plugin +{ + +/// @brief One sample along a trajectory polyline with derived kinematics for plotting. +struct SeriesPoint +{ + double time_from_start_sec{0.0}; + double arc_length_m{0.0}; + double longitudinal_velocity_mps{0.0}; + double acceleration_mps2{0.0}; + /// Signed planar curvature [1/m] (Menger estimate on XY). + double curvature_1pm{0.0}; + /// Approximate lateral acceleration [m/s²] using a_lat ≈ v_x² κ (planar bicycle-style estimate). + double lateral_acceleration_mps2{0.0}; +}; + +/// @brief A plottable series: ROS topic, logical key, human-readable label, and sampled points. +struct TrajectorySeriesData +{ + std::string topic; + /// Stable id within the topic (e.g. "trajectory", "candidate_3"). + std::string key; + std::string label; + std::vector points; +}; + +/// @brief Selects which scalar is read from SeriesPoint for an axis or tooltip. +enum class AxisId : std::uint8_t { + TIME_FROM_START = 0, + ARC_LENGTH, + LONGITUDINAL_VELOCITY, + ACCELERATION, + CURVATURE, + LATERAL_ACCELERATION, +}; + +/// @brief Human-readable labels and units for axis titles and tooltips. +struct AxisDefinition +{ + AxisId id; + const char * label; + const char * unit; +}; + +/// @brief X-axis choices (time or arc length). +inline const std::vector & xAxisDefinitions() +{ + static const std::vector defs = { + {AxisId::TIME_FROM_START, "Time from start", "s"}, + {AxisId::ARC_LENGTH, "Arc length", "m"}, + }; + return defs; +} + +/// @brief Y-axis choices (longitudinal/lateral motion and curvature). +inline const std::vector & yAxisDefinitions() +{ + static const std::vector defs = { + {AxisId::LONGITUDINAL_VELOCITY, "Longitudinal velocity", "m/s"}, + {AxisId::ACCELERATION, "Acceleration", "m/s²"}, + {AxisId::CURVATURE, "Curvature (signed)", "1/m"}, + {AxisId::LATERAL_ACCELERATION, "Lateral acceleration (v²κ)", "m/s²"}, + }; + return defs; +} + +/// @brief Maps a kinematics axis id to the corresponding field in `p`. +inline double accessAxisValue(const SeriesPoint & p, AxisId id) +{ + switch (id) { + case AxisId::TIME_FROM_START: + return p.time_from_start_sec; + case AxisId::ARC_LENGTH: + return p.arc_length_m; + case AxisId::LONGITUDINAL_VELOCITY: + return p.longitudinal_velocity_mps; + case AxisId::ACCELERATION: + return p.acceleration_mps2; + case AxisId::CURVATURE: + return p.curvature_1pm; + case AxisId::LATERAL_ACCELERATION: + return p.lateral_acceleration_mps2; + } +#if defined(__GNUC__) || defined(__clang__) + __builtin_unreachable(); +#else + return 0.0; +#endif +} + +/// @brief Looks up axis metadata from xAxisDefinitions() then yAxisDefinitions(). +inline const AxisDefinition * findAxisDefinition(AxisId id) +{ + for (const auto & d : xAxisDefinitions()) { + if (d.id == id) { + return &d; + } + } + for (const auto & d : yAxisDefinitions()) { + if (d.id == id) { + return &d; + } + } + return nullptr; +} + +/// @brief Returns the display label for `id`, or `"?"` if unknown. +inline const char * axisLabel(AxisId id) +{ + const AxisDefinition * d = findAxisDefinition(id); + return d ? d->label : "?"; +} + +/// @brief Returns the unit string for `id`, or empty if unknown. +inline const char * axisUnit(AxisId id) +{ + const AxisDefinition * d = findAxisDefinition(id); + return d ? d->unit : ""; +} + +/// @brief Optional fixed axis ranges for the plot (honored only when the corresponding lock flag is +/// true). +struct PlotAxisRangeOptions +{ + bool lock_x{false}; + double x_min{0.0}; + double x_max{1.0}; + bool lock_y{false}; + double y_min{-1.0}; + double y_max{1.0}; +}; + +} // namespace autoware::visualization::trajectory_kinematics_rviz_plugin + +#endif // KINEMATICS_TYPES_HPP_ diff --git a/common/autoware_trajectory_kinematics_rviz_plugin/src/material_colors.hpp b/common/autoware_trajectory_kinematics_rviz_plugin/src/material_colors.hpp new file mode 100644 index 000000000..97b68da6c --- /dev/null +++ b/common/autoware_trajectory_kinematics_rviz_plugin/src/material_colors.hpp @@ -0,0 +1,113 @@ +// Copyright 2026 TIER IV, Inc. +// +// 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. +// +// Duplicated from tier4_state_rviz_plugin/src/include/material_colors.hpp for a self-contained +// dependency graph. Palette matches AutowareStatePanel styling. + +#ifndef MATERIAL_COLORS_HPP_ +#define MATERIAL_COLORS_HPP_ + +#include + +#include + +namespace autoware::visualization::trajectory_kinematics_rviz_plugin::style +{ + +/// @brief Material Design 3–style palette as hex strings for RViz panel QSS and QColor +/// construction. +struct MaterialColors +{ + std::string primary = "#8BD0F0"; + std::string surface_tint = "#8BD0F0"; + std::string on_primary = "#003546"; + std::string primary_container = "#004D64"; + std::string on_primary_container = "#BEE9FF"; + std::string secondary = "#B4CAD6"; + std::string on_secondary = "#1F333C"; + std::string secondary_container = "#354A54"; + std::string on_secondary_container = "#D0E6F2"; + std::string tertiary = "#C6C2EA"; + std::string on_tertiary = "#2F2D4D"; + std::string tertiary_container = "#454364"; + std::string on_tertiary_container = "#E3DFFF"; + std::string error = "#FFB4AB"; + std::string on_error = "#690005"; + std::string error_container = "#820008"; + std::string error_press = "#982127"; + std::string on_error_container = "#8c0f16"; + std::string background = "#0F1417"; + std::string on_background = "#DFE3E7"; + std::string surface = "#0F1417"; + std::string on_surface = "#DFE3E7"; + std::string surface_variant = "#40484C"; + std::string on_surface_variant = "#C0C8CD"; + std::string outline = "#8A9297"; + std::string outline_variant = "#40484C"; + std::string shadow = "#000000"; + std::string scrim = "#000000"; + std::string inverse_surface = "#DFE3E7"; + std::string inverse_on_surface = "#2C3134"; + std::string inverse_primary = "#126682"; + std::string primary_fixed = "#BEE9FF"; + std::string on_primary_fixed = "#001F2A"; + std::string primary_fixed_dim = "#8BD0F0"; + std::string on_primary_fixed_variant = "#004D64"; + std::string secondary_fixed = "#D0E6F2"; + std::string on_secondary_fixed = "#081E27"; + std::string secondary_fixed_dim = "#B4CAD6"; + std::string on_secondary_fixed_variant = "#354A54"; + std::string tertiary_fixed = "#E3DFFF"; + std::string on_tertiary_fixed = "#1A1836"; + std::string tertiary_fixed_dim = "#C6C2EA"; + std::string on_tertiary_fixed_variant = "#454364"; + std::string surface_dim = "#0F1417"; + std::string surface_bright = "#353A3D"; + std::string surface_container_lowest = "#0A0F11"; + std::string surface_container_low = "#171C1F"; + std::string surface_container = "#1B2023"; + std::string surface_container_high = "#262B2E"; + std::string surface_container_highest = "#303538"; + std::string disabled_elevated_button_bg = "#292D30"; + std::string success = "#8DF08B"; + std::string warning = "#EEF08B"; + std::string info = "#8BD0F0"; + std::string danger = "#F08B8B"; + + std::string enabled_button_bg = "#8BD0F0"; + std::string hover_button_bg = "#84c2e6"; + std::string pressed_button_bg = "#699BB8"; + std::string checked_button_bg = "#699BB8"; + std::string disabled_button_bg = "#292d30"; + std::string disabled_button_text = "#6e7276"; + + std::string on_surface_hover_bg = "#212429"; + std::string on_surface_pressed_bg = "#292d32"; + std::string on_surface_disabled = "#5e6266"; + + std::string surface_container_low_hover = "#262931"; + std::string surface_container_low_pressed = "#2d303a"; +}; + +inline const MaterialColors default_colors{}; + +/// Convert a CSS-style `#RGB` string from the palette to `QColor`. +inline QColor hexToQColor(const std::string & hex) +{ + return QColor(QString::fromStdString(hex)); +} + +} // namespace autoware::visualization::trajectory_kinematics_rviz_plugin::style + +#endif // MATERIAL_COLORS_HPP_ diff --git a/common/autoware_trajectory_kinematics_rviz_plugin/src/trajectory_kinematics_chart_view.cpp b/common/autoware_trajectory_kinematics_rviz_plugin/src/trajectory_kinematics_chart_view.cpp new file mode 100644 index 000000000..21ea5b7c4 --- /dev/null +++ b/common/autoware_trajectory_kinematics_rviz_plugin/src/trajectory_kinematics_chart_view.cpp @@ -0,0 +1,275 @@ +// Copyright 2026 TIER IV, Inc. +// +// 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 "trajectory_kinematics_chart_view.hpp" + +#include "material_colors.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace autoware::visualization::trajectory_kinematics_rviz_plugin +{ +namespace +{ + +/// Plot X axis is always time or arc length; both are non-decreasing along sampled points. +bool xAxisIsMonotonicInSeries(AxisId x_axis) +{ + return x_axis == AxisId::TIME_FROM_START || x_axis == AxisId::ARC_LENGTH; +} + +size_t nearestPointIndexLinear(const TrajectorySeriesData & s, AxisId x_axis, double x_target) +{ + if (s.points.empty()) { + return 0; + } + size_t best = 0; + double best_d = std::numeric_limits::infinity(); + for (size_t i = 0; i < s.points.size(); ++i) { + const double xv = accessAxisValue(s.points[i], x_axis); + if (!std::isfinite(xv)) { + continue; + } + const double d = std::abs(xv - x_target); + if (d < best_d) { + best_d = d; + best = i; + } + } + return best; +} + +size_t nearestPointIndexMonotonic(const TrajectorySeriesData & s, AxisId x_axis, double x_target) +{ + if (s.points.empty()) { + return 0; + } + const auto & pts = s.points; + auto less_x = [x_axis](const SeriesPoint & p, double xv) { + return accessAxisValue(p, x_axis) < xv; + }; + auto it = std::lower_bound(pts.begin(), pts.end(), x_target, less_x); + if (it == pts.begin()) { + return 0; + } + if (it == pts.end()) { + return pts.size() - 1; + } + auto prev = it - 1; + const double d0 = std::abs(accessAxisValue(*prev, x_axis) - x_target); + const double d1 = std::abs(accessAxisValue(*it, x_axis) - x_target); + return d0 <= d1 ? static_cast(std::distance(pts.begin(), prev)) + : static_cast(std::distance(pts.begin(), it)); +} + +size_t nearestPointIndexForSeries(const TrajectorySeriesData & s, AxisId x_axis, double x_target) +{ + if (xAxisIsMonotonicInSeries(x_axis)) { + return nearestPointIndexMonotonic(s, x_axis, x_target); + } + return nearestPointIndexLinear(s, x_axis, x_target); +} + +QtCharts::QLineSeries * firstDataLineSeries( + QtCharts::QChart * ch, const QtCharts::QLineSeries * crosshair) +{ + if (ch == nullptr) { + return nullptr; + } + for (QtCharts::QAbstractSeries * ser : ch->series()) { + auto * ls = qobject_cast(ser); + if (ls == nullptr || ls == crosshair) { + continue; + } + return ls; + } + return nullptr; +} + +constexpr int kTooltipMinIntervalMs = 50; +constexpr double kTooltipSameXEps = 1e-7; + +} // namespace + +TrajectoryKinematicsChartView::TrajectoryKinematicsChartView(QWidget * parent) +: QtCharts::QChartView(parent) +{ + setMouseTracking(true); + setRubberBand(QtCharts::QChartView::NoRubberBand); + + const auto & pal = style::default_colors; + setStyleSheet(QStringLiteral( + "QToolTip {" + " background-color: %1;" + " color: %2;" + " border: 1px solid %3;" + " border-radius: 6px;" + " padding: 6px 8px;" + " font-size: 11px;" + "}") + .arg(QString::fromStdString(pal.surface_container_high)) + .arg(QString::fromStdString(pal.on_surface)) + .arg(QString::fromStdString(pal.outline_variant))); +} + +void TrajectoryKinematicsChartView::setCrosshairSeries(QtCharts::QLineSeries * series) +{ + crosshair_series_ = series; +} + +void TrajectoryKinematicsChartView::setHoverSeriesData( + const std::vector & series, const std::vector & colors, + AxisId x_axis, AxisId y_axis) +{ + hover_series_ = series; + hover_colors_ = colors; + x_axis_ = x_axis; + y_axis_ = y_axis; +} + +void TrajectoryKinematicsChartView::mouseMoveEvent(QMouseEvent * event) +{ + QtCharts::QChartView::mouseMoveEvent(event); + + auto * ch = chart(); + if (hover_series_.empty() || ch == nullptr) { + QToolTip::hideText(); + if (crosshair_series_) { + crosshair_series_->clear(); + } + return; + } + + auto * ref_series = firstDataLineSeries(ch, crosshair_series_); + if (ref_series == nullptr) { + QToolTip::hideText(); + if (crosshair_series_) { + crosshair_series_->clear(); + } + return; + } + + // Widget → scene → chart item space: required before QChart::mapToValue for correct axis + // mapping. + const QPointF scene_pos = mapToScene(event->pos()); + const QPointF chart_pos = ch->mapFromScene(scene_pos); + if (!ch->plotArea().contains(chart_pos)) { + QToolTip::hideText(); + if (crosshair_series_) { + crosshair_series_->clear(); + } + return; + } + + // mapToValue uses the attached axes; ref_series must be a data line (not the crosshair) so X + // is read in plot units. + const QPointF value = ch->mapToValue(chart_pos, ref_series); + const double x_cursor = value.x(); + if (!std::isfinite(x_cursor)) { + QToolTip::hideText(); + if (crosshair_series_) { + crosshair_series_->clear(); + } + return; + } + + const QList vaxes = ch->axes(Qt::Vertical); + auto * ay = qobject_cast(vaxes.isEmpty() ? nullptr : vaxes.first()); + if (crosshair_series_ && ay != nullptr) { + const double y_lo = ay->min(); + const double y_hi = ay->max(); + crosshair_series_->clear(); + crosshair_series_->append(x_cursor, y_lo); + crosshair_series_->append(x_cursor, y_hi); + } + + // Throttle rich HTML tooltip updates: crosshair still moves, but QString rebuilds are skipped at + // high pointer rate. + const bool same_x = + last_tooltip_x_valid_ && std::abs(x_cursor - last_tooltip_x_) <= kTooltipSameXEps; + const bool within_rate = + tooltip_rate_timer_.isValid() && tooltip_rate_timer_.elapsed() < kTooltipMinIntervalMs; + if (same_x && within_rate) { + return; + } + tooltip_rate_timer_.restart(); + last_tooltip_x_ = x_cursor; + last_tooltip_x_valid_ = true; + + const auto & pal = style::default_colors; + const QString bg = QString::fromStdString(pal.surface_container_high); + const QString fg = QString::fromStdString(pal.on_surface); + const QString muted = QString::fromStdString(pal.on_surface_variant); + + QString html; + html += QStringLiteral( + "") + .arg(bg) + .arg(fg); + html += QStringLiteral( + "
%1 = %2 " + "%3
") + .arg(QString::fromUtf8(axisLabel(x_axis_))) + .arg(x_cursor, 0, 'f', 4) + .arg(QString::fromUtf8(axisUnit(x_axis_))) + .arg(muted); + + for (size_t i = 0; i < hover_series_.size(); ++i) { + const auto & s = hover_series_[i]; + if (s.points.empty()) { + continue; + } + const size_t idx = nearestPointIndexForSeries(s, x_axis_, x_cursor); + const double yv = accessAxisValue(s.points[idx], y_axis_); + const QColor c = (i < hover_colors_.size()) ? hover_colors_[i] : QColor(200, 200, 200); + html += QStringLiteral( + "
" + "■" + " %2 %3" + "
") + .arg(c.name(QColor::HexRgb)) + .arg(yv, 0, 'f', 4) + .arg(QString::fromUtf8(axisUnit(y_axis_))) + .arg(muted) + .arg(fg); + } + html += QStringLiteral(""); + + QToolTip::showText(event->globalPos(), html, this, QRect(), 8000); +} + +void TrajectoryKinematicsChartView::leaveEvent(QEvent * event) +{ + QToolTip::hideText(); + last_tooltip_x_valid_ = false; + if (crosshair_series_) { + crosshair_series_->clear(); + } + QtCharts::QChartView::leaveEvent(event); +} + +} // namespace autoware::visualization::trajectory_kinematics_rviz_plugin diff --git a/common/autoware_trajectory_kinematics_rviz_plugin/src/trajectory_kinematics_chart_view.hpp b/common/autoware_trajectory_kinematics_rviz_plugin/src/trajectory_kinematics_chart_view.hpp new file mode 100644 index 000000000..6e64c5ea1 --- /dev/null +++ b/common/autoware_trajectory_kinematics_rviz_plugin/src/trajectory_kinematics_chart_view.hpp @@ -0,0 +1,80 @@ +// Copyright 2026 TIER IV, Inc. +// +// 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 TRAJECTORY_KINEMATICS_CHART_VIEW_HPP_ +#define TRAJECTORY_KINEMATICS_CHART_VIEW_HPP_ + +#include "kinematics_types.hpp" + +#include +#include +#include +#include +#include + +#include + +namespace autoware::visualization::trajectory_kinematics_rviz_plugin +{ + +/// @brief Qt Charts view with a vertical crosshair and a multi-series HTML tooltip at the pointer +/// X. +/// +/// Maps mouse position to plot coordinates via QChart::mapToValue(), finds the nearest sample per +/// series along the current X axis (binary search when that axis is monotonic in point order), and +/// throttles tooltip updates to reduce overhead during motion. +class TrajectoryKinematicsChartView : public QtCharts::QChartView +{ + Q_OBJECT + +public: + /// @brief Enables mouse tracking and styles tooltips to match the panel palette. + explicit TrajectoryKinematicsChartView(QWidget * parent = nullptr); + + /// @brief Registers the dedicated line series drawn as the vertical crosshair (updated on hover). + void setCrosshairSeries(QtCharts::QLineSeries * series); + + /// @brief Supplies the trajectories and colors used for tooltip content and nearest-point lookup. + /// @param series Same trajectories currently plotted (order matches `colors`). + /// @param colors Swatch colors for HTML rows (may be shorter than `series`; a neutral gray is + /// used if missing). + /// @param x_axis Axis used for cursor X and for choosing the nearest point index in each series. + /// @param y_axis Axis whose value is shown per series at that index. + void setHoverSeriesData( + const std::vector & series, const std::vector & colors, + AxisId x_axis, AxisId y_axis); + +protected: + /// @brief Updates crosshair endpoints, optional HTML tooltip, and rate limiting state. + void mouseMoveEvent(QMouseEvent * event) override; + + /// @brief Hides the tooltip and clears the crosshair when the pointer leaves the view. + void leaveEvent(QEvent * event) override; + +private: + std::vector hover_series_; + std::vector hover_colors_; + AxisId x_axis_{AxisId::TIME_FROM_START}; + AxisId y_axis_{AxisId::LONGITUDINAL_VELOCITY}; + + QPointer crosshair_series_; + + QElapsedTimer tooltip_rate_timer_; + double last_tooltip_x_{0.0}; + bool last_tooltip_x_valid_{false}; +}; + +} // namespace autoware::visualization::trajectory_kinematics_rviz_plugin + +#endif // TRAJECTORY_KINEMATICS_CHART_VIEW_HPP_ diff --git a/common/autoware_trajectory_kinematics_rviz_plugin/src/trajectory_kinematics_panel.cpp b/common/autoware_trajectory_kinematics_rviz_plugin/src/trajectory_kinematics_panel.cpp new file mode 100644 index 000000000..1c3d0303a --- /dev/null +++ b/common/autoware_trajectory_kinematics_rviz_plugin/src/trajectory_kinematics_panel.cpp @@ -0,0 +1,811 @@ +// Copyright 2026 TIER IV, Inc. +// +// 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 "trajectory_kinematics_panel.hpp" + +#include "material_colors.hpp" +#include "ui_font.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::visualization::trajectory_kinematics_rviz_plugin +{ +namespace +{ + +constexpr char kTrajectoryType[] = "autoware_planning_msgs/msg/Trajectory"; +constexpr char kScoredType[] = "autoware_internal_planning_msgs/msg/ScoredCandidateTrajectories"; + +/// Shown when the graph has no matching topics (not a valid subscription target). +const QString kEmptyTopicPlaceholder = QStringLiteral("(No matching topics — press Refresh)"); + +/// Interval for refreshing the ROS graph topic list (ms). +constexpr int kTopicPollIntervalMs = 4000; + +/// Debounce window for merging high-rate `dataUpdated` → panel refresh (ms). +constexpr int kPlotRefreshCoalesceMs = 50; + +/// Accent colors for series (aligned with material info / success / warning / tertiary). +const QColor kColorCycle[] = { + style::hexToQColor(style::default_colors.info), + style::hexToQColor(style::default_colors.success), + style::hexToQColor(style::default_colors.warning), + style::hexToQColor(style::default_colors.tertiary_fixed), + style::hexToQColor(style::default_colors.secondary_fixed_dim), + style::hexToQColor(style::default_colors.primary_fixed_dim), +}; + +constexpr int kColorCycleCount = static_cast(sizeof(kColorCycle) / sizeof(kColorCycle[0])); + +QIcon makeTrajectoryColorIcon(const QColor & c) +{ + constexpr int d = 20; + QPixmap pm(d, d); + pm.fill(Qt::transparent); + QPainter painter(&pm); + painter.setRenderHint(QPainter::Antialiasing, true); + painter.setBrush(c); + painter.setPen(QColor(255, 255, 255, 100)); + painter.drawRoundedRect(2, 2, d - 4, d - 4, 4, 4); + painter.end(); + return QIcon(pm); +} + +QColor colorForSeriesListIndex(int index_in_all_series) +{ + return kColorCycle[index_in_all_series % kColorCycleCount]; +} + +QString panelStyleSheet(const style::MaterialColors & c) +{ + return QStringLiteral( + "QWidget#TkmPanelRoot {" + " background-color: %1; color: %2;" + " font-family: \"Segoe UI\", \"Ubuntu\", \"Noto Sans\", \"Liberation Sans\", " + "sans-serif;" + " font-size: 13px; font-weight: 600;" + "}" + "QWidget#TkmPanelRoot QLabel { color: %2; font-weight: 600; }" + "QWidget#TkmPanelRoot QComboBox {" + " background-color: %3; color: %2; border: 1px solid %4; border-radius: 6px;" + " padding: 4px 8px; min-height: 24px; font-weight: 600;" + "}" + "QWidget#TkmPanelRoot QComboBox:hover { border-color: %5; }" + "QWidget#TkmPanelRoot QComboBox::drop-down { border: none; width: 20px; }" + "QWidget#TkmPanelRoot QComboBox QAbstractItemView {" + " background-color: %3; color: %2; selection-background-color: %6; font-weight: 600;" + "}" + "QWidget#TkmPanelRoot QPushButton {" + " background-color: %7; color: %8; border: none; border-radius: 6px;" + " padding: 6px 14px; font-weight: 700;" + "}" + "QWidget#TkmPanelRoot QPushButton:hover { background-color: %9; }" + "QWidget#TkmPanelRoot QPushButton:pressed { background-color: %10; }" + "QWidget#TkmPanelRoot QListWidget {" + " background-color: %3; color: %2; border: 1px solid %4; border-radius: 6px;" + " font-weight: 600;" + "}" + "QWidget#TkmPanelRoot QLineEdit#TkmManualTopic {" + " background-color: %3; color: %2; border: 1px solid %4; border-radius: 6px;" + " padding: 5px 10px; font-weight: 600;" + "}" + "QWidget#TkmPanelRoot QDoubleSpinBox {" + " background-color: %3; color: %2; border: 1px solid %4; border-radius: 6px;" + " padding: 3px 6px; min-height: 22px; font-weight: 600;" + "}" + "QWidget#TkmPanelRoot QCheckBox { color: %2; font-weight: 600; spacing: 6px; }") + .arg(QString::fromStdString(c.background)) + .arg(QString::fromStdString(c.on_surface)) + .arg(QString::fromStdString(c.surface_container)) + .arg(QString::fromStdString(c.outline_variant)) + .arg(QString::fromStdString(c.outline)) + .arg(QString::fromStdString(c.primary_container)) + .arg(QString::fromStdString(c.enabled_button_bg)) + .arg(QString::fromStdString(c.on_primary)) + .arg(QString::fromStdString(c.hover_button_bg)) + .arg(QString::fromStdString(c.pressed_button_bg)); +} + +} // namespace + +TrajectoryKinematicsPanel::TrajectoryKinematicsPanel(QWidget * parent) : rviz_common::Panel(parent) +{ + setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + + panel_root_ = new QWidget(this); + panel_root_->setObjectName(QStringLiteral("TkmPanelRoot")); + panel_root_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + + auto * root = new QVBoxLayout(panel_root_); + root->setSpacing(6); + root->setContentsMargins(6, 6, 6, 6); + + { + auto * row = new QHBoxLayout(); + row->addWidget(new QLabel(QStringLiteral("Topic:"))); + topic_combo_ = new QComboBox(); + topic_combo_->setEditable(false); + topic_combo_->setMinimumWidth(260); + topic_combo_->setMaxVisibleItems(18); + topic_combo_->setSizeAdjustPolicy(QComboBox::AdjustToMinimumContentsLengthWithIcon); + topic_combo_->setMinimumContentsLength(28); + topic_combo_->setToolTip(QStringLiteral( + "Open the list and click a topic. Use “Other topic” below to type a name not " + "listed.")); + row->addWidget(topic_combo_, 1); + kind_combo_ = new QComboBox(); + kind_combo_->addItem( + QStringLiteral("Trajectory"), static_cast(TopicMessageKind::Trajectory)); + kind_combo_->addItem( + QStringLiteral("ScoredCandidateTrajectories"), + static_cast(TopicMessageKind::ScoredCandidateTrajectories)); + row->addWidget(kind_combo_); + refresh_topics_button_ = new QPushButton(QStringLiteral("Refresh topics")); + refresh_topics_button_->setToolTip( + QStringLiteral("Reload topic list from the ROS graph (filtered by message type)")); + row->addWidget(refresh_topics_button_); + add_topic_button_ = new QPushButton(QStringLiteral("Add")); + remove_topic_button_ = new QPushButton(QStringLiteral("Remove")); + remove_topic_button_->setToolTip(QStringLiteral( + "Select one or more trajectories below, then remove their ROS topic " + "subscription(s).")); + row->addWidget(add_topic_button_); + row->addWidget(remove_topic_button_); + root->addLayout(row); + } + + { + auto * row = new QHBoxLayout(); + row->addWidget(new QLabel(QStringLiteral("Other topic:"))); + manual_topic_edit_ = new QLineEdit(); + manual_topic_edit_->setObjectName(QStringLiteral("TkmManualTopic")); + manual_topic_edit_->setPlaceholderText( + QStringLiteral("Optional: type a full topic name if it is not in the list above")); + manual_topic_edit_->setToolTip(QStringLiteral( + "If set, Add uses this text instead of the dropdown (for topics not yet on " + "the graph).")); + row->addWidget(manual_topic_edit_, 1); + root->addLayout(row); + } + + { + auto * series_label = new QLabel(QStringLiteral("Trajectories (chart color)")); + series_label->setStyleSheet(QStringLiteral("font-weight: 700;")); + root->addWidget(series_label); + series_list_ = new QListWidget(); + series_list_->setSelectionMode(QAbstractItemView::ExtendedSelection); + series_list_->setToolTip(QStringLiteral( + "Swatch = plot line color. Check rows to plot. Select row(s) and use Remove " + "to drop that topic subscription.")); + series_list_->setIconSize(QSize(20, 20)); + series_list_->setMinimumHeight(160); + root->addWidget(series_list_); + } + + { + auto * row = new QHBoxLayout(); + row->addWidget(new QLabel(QStringLiteral("X axis:"))); + x_axis_combo_ = new QComboBox(); + row->addWidget(x_axis_combo_); + row->addWidget(new QLabel(QStringLiteral("Y axis:"))); + y_axis_combo_ = new QComboBox(); + row->addWidget(y_axis_combo_); + root->addLayout(row); + } + + { + auto * row = new QHBoxLayout(); + fix_x_range_ = new QCheckBox(QStringLiteral("Fix X range")); + plot_x_min_spin_ = new QDoubleSpinBox(); + plot_x_max_spin_ = new QDoubleSpinBox(); + for (auto * sp : {plot_x_min_spin_, plot_x_max_spin_}) { + sp->setDecimals(4); + sp->setRange(-1e9, 1e9); + sp->setMaximumWidth(130); + } + plot_x_min_spin_->setValue(0.0); + plot_x_max_spin_->setValue(20.0); + plot_x_min_spin_->setToolTip(QStringLiteral("Used when “Fix X range” is checked")); + plot_x_max_spin_->setToolTip(QStringLiteral("Must be greater than min")); + row->addWidget(fix_x_range_); + row->addWidget(new QLabel(QStringLiteral("min"))); + row->addWidget(plot_x_min_spin_); + row->addWidget(new QLabel(QStringLiteral("max"))); + row->addWidget(plot_x_max_spin_); + row->addStretch(); + root->addLayout(row); + } + + { + auto * row = new QHBoxLayout(); + fix_y_range_ = new QCheckBox(QStringLiteral("Fix Y range")); + plot_y_min_spin_ = new QDoubleSpinBox(); + plot_y_max_spin_ = new QDoubleSpinBox(); + for (auto * sp : {plot_y_min_spin_, plot_y_max_spin_}) { + sp->setDecimals(4); + sp->setRange(-1e9, 1e9); + sp->setMaximumWidth(130); + } + plot_y_min_spin_->setValue(-5.0); + plot_y_max_spin_->setValue(15.0); + plot_y_min_spin_->setToolTip(QStringLiteral("Used when “Fix Y range” is checked")); + plot_y_max_spin_->setToolTip(QStringLiteral("Must be greater than min")); + row->addWidget(fix_y_range_); + row->addWidget(new QLabel(QStringLiteral("min"))); + row->addWidget(plot_y_min_spin_); + row->addWidget(new QLabel(QStringLiteral("max"))); + row->addWidget(plot_y_max_spin_); + row->addStretch(); + root->addLayout(row); + } + + { + auto * row = new QHBoxLayout(); + row->addWidget(new QLabel(QStringLiteral("End v:"))); + metric_velocity_ = new QLabel(QStringLiteral("-")); + row->addWidget(metric_velocity_); + row->addWidget(new QLabel(QStringLiteral("End a:"))); + metric_accel_ = new QLabel(QStringLiteral("-")); + row->addWidget(metric_accel_); + row->addWidget(new QLabel(QStringLiteral("End t:"))); + metric_duration_ = new QLabel(QStringLiteral("-")); + row->addWidget(metric_duration_); + root->addLayout(row); + } + + plot_widget_ = new TrajectoryKinematicsPlotWidget(); + root->addWidget(plot_widget_, 1); + + auto * outer = new QVBoxLayout(this); + outer->setContentsMargins(2, 2, 2, 2); + outer->addWidget(panel_root_); + setLayout(outer); + + topic_poll_timer_ = new QTimer(this); + topic_poll_timer_->setInterval(kTopicPollIntervalMs); + + plot_refresh_coalesce_timer_ = new QTimer(this); + plot_refresh_coalesce_timer_->setSingleShot(true); + plot_refresh_coalesce_timer_->setInterval(kPlotRefreshCoalesceMs); + + populateAxisCombos(); + applyPanelStyle(); + + connect(add_topic_button_, &QPushButton::clicked, this, &TrajectoryKinematicsPanel::onAddTopic); + connect( + remove_topic_button_, &QPushButton::clicked, this, &TrajectoryKinematicsPanel::onRemoveTopic); + connect( + refresh_topics_button_, &QPushButton::clicked, this, + &TrajectoryKinematicsPanel::refreshTopicList); + connect( + kind_combo_, qOverload(&QComboBox::currentIndexChanged), this, + &TrajectoryKinematicsPanel::refreshTopicList); + connect(topic_poll_timer_, &QTimer::timeout, this, &TrajectoryKinematicsPanel::refreshTopicList); + connect( + plot_refresh_coalesce_timer_, &QTimer::timeout, this, + &TrajectoryKinematicsPanel::processDataUpdated); + connect( + series_list_, &QListWidget::itemChanged, this, &TrajectoryKinematicsPanel::onSeriesItemChanged); + connect( + x_axis_combo_, qOverload(&QComboBox::currentIndexChanged), this, + &TrajectoryKinematicsPanel::onAxisChanged); + connect( + y_axis_combo_, qOverload(&QComboBox::currentIndexChanged), this, + &TrajectoryKinematicsPanel::onAxisChanged); + connect(fix_x_range_, &QCheckBox::toggled, this, &TrajectoryKinematicsPanel::refreshPlot); + connect(fix_y_range_, &QCheckBox::toggled, this, &TrajectoryKinematicsPanel::refreshPlot); + connect( + plot_x_min_spin_, qOverload(&QDoubleSpinBox::valueChanged), this, + &TrajectoryKinematicsPanel::refreshPlot); + connect( + plot_x_max_spin_, qOverload(&QDoubleSpinBox::valueChanged), this, + &TrajectoryKinematicsPanel::refreshPlot); + connect( + plot_y_min_spin_, qOverload(&QDoubleSpinBox::valueChanged), this, + &TrajectoryKinematicsPanel::refreshPlot); + connect( + plot_y_max_spin_, qOverload(&QDoubleSpinBox::valueChanged), this, + &TrajectoryKinematicsPanel::refreshPlot); +} + +void TrajectoryKinematicsPanel::applyPanelStyle() +{ + const QFont ui_font = makePanelUiFont(); + panel_root_->setFont(ui_font); + plot_widget_->setFont(ui_font); + panel_root_->setStyleSheet(panelStyleSheet(style::default_colors)); +} + +PlotAxisRangeOptions TrajectoryKinematicsPanel::readPlotRangeOptions() const +{ + PlotAxisRangeOptions o; + o.lock_x = fix_x_range_->isChecked(); + o.x_min = plot_x_min_spin_->value(); + o.x_max = plot_x_max_spin_->value(); + o.lock_y = fix_y_range_->isChecked(); + o.y_min = plot_y_min_spin_->value(); + o.y_max = plot_y_max_spin_->value(); + return o; +} + +void TrajectoryKinematicsPanel::populateAxisCombos() +{ + x_axis_combo_->clear(); + for (const auto & d : xAxisDefinitions()) { + x_axis_combo_->addItem(QString::fromUtf8(d.label), static_cast(d.id)); + } + y_axis_combo_->clear(); + for (const auto & d : yAxisDefinitions()) { + y_axis_combo_->addItem(QString::fromUtf8(d.label), static_cast(d.id)); + } + x_axis_combo_->setCurrentIndex(0); + y_axis_combo_->setCurrentIndex(0); +} + +void TrajectoryKinematicsPanel::onInitialize() +{ + auto node_abstraction = getDisplayContext()->getRosNodeAbstraction().lock(); + if (!node_abstraction) { + RCLCPP_ERROR( + rclcpp::get_logger("TrajectoryKinematicsPanel"), + "Failed to acquire ROS node abstraction; panel cannot subscribe."); + return; + } + node_ = node_abstraction->get_raw_node(); + series_manager_ = std::make_unique(node_, this); + // QueuedConnection: subscription callbacks may not run on the GUI thread; slot runs in the + // panel's thread. + connect( + series_manager_.get(), &TrajectorySeriesManager::dataUpdated, this, + &TrajectoryKinematicsPanel::onDataUpdated, Qt::QueuedConnection); + refreshTopicList(); + // Topic polling starts in showEvent when the panel is visible. `load()` runs after onInitialize + // when a layout is restored and calls applyTopicConfigsToManager() again; this call covers the + // fresh-panel case (no saved config yet). + applyTopicConfigsToManager(); +} + +void TrajectoryKinematicsPanel::refreshTopicList() +{ + if (!node_) { + return; + } + + const auto kind = static_cast(kind_combo_->currentData().toInt()); + const char * want_type = (kind == TopicMessageKind::Trajectory) ? kTrajectoryType : kScoredType; + + std::map> topic_map; + try { + topic_map = node_->get_topic_names_and_types(); + } catch (const std::exception & e) { + RCLCPP_WARN( + rclcpp::get_logger("TrajectoryKinematicsPanel"), "get_topic_names_and_types failed: %s", + e.what()); + return; + } + + std::vector names; + for (const auto & kv : topic_map) { + for (const auto & typ : kv.second) { + if (typ == want_type) { + names.push_back(kv.first); + break; + } + } + } + std::sort(names.begin(), names.end()); + + const QString previous = + (topic_combo_->currentIndex() >= 0) ? topic_combo_->currentText() : QString(); + + topic_combo_->blockSignals(true); + topic_combo_->clear(); + if (names.empty()) { + topic_combo_->addItem(kEmptyTopicPlaceholder); + topic_combo_->setCurrentIndex(0); + } else { + for (const auto & n : names) { + topic_combo_->addItem(QString::fromStdString(n)); + } + const int idx = topic_combo_->findText(previous, Qt::MatchExactly); + topic_combo_->setCurrentIndex(idx >= 0 ? idx : 0); + } + topic_combo_->blockSignals(false); +} + +std::string TrajectoryKinematicsPanel::normalizeTopicString(std::string s) +{ + const auto first = s.find_first_not_of(" \t"); + if (first == std::string::npos) { + return {}; + } + const auto last = s.find_last_not_of(" \t"); + s = s.substr(first, last - first + 1); + if (!s.empty() && s.front() != '/') { + s.insert(s.begin(), '/'); + } + return s; +} + +void TrajectoryKinematicsPanel::onAddTopic() +{ + std::string topic; + const QString manual = manual_topic_edit_->text().trimmed(); + if (!manual.isEmpty()) { + topic = normalizeTopicString(manual.toStdString()); + } else { + if (topic_combo_->count() == 0 || topic_combo_->currentIndex() < 0) { + return; + } + const QString picked = topic_combo_->currentText(); + if (picked == kEmptyTopicPlaceholder) { + return; + } + topic = normalizeTopicString(picked.toStdString()); + } + if (topic.empty()) { + return; + } + const auto kind = static_cast(kind_combo_->currentData().toInt()); + for (const auto & p : configured_topics_) { + if (p.first == topic && p.second == kind) { + return; + } + } + configured_topics_.emplace_back(topic, kind); + manual_topic_edit_->clear(); + applyTopicConfigsToManager(); +} + +void TrajectoryKinematicsPanel::onRemoveTopic() +{ + const QList selected = series_list_->selectedItems(); + if (selected.isEmpty()) { + return; + } + std::set topics_to_remove; + for (const auto * item : selected) { + const QString t = item->data(static_cast(SeriesListItemRole::kTopicName)).toString(); + if (!t.isEmpty()) { + topics_to_remove.insert(t.toStdString()); + } + } + if (topics_to_remove.empty()) { + return; + } + configured_topics_.erase( + std::remove_if( + configured_topics_.begin(), configured_topics_.end(), + [&topics_to_remove](const std::pair & p) { + return topics_to_remove.count(p.first) > 0; + }), + configured_topics_.end()); + manual_topic_edit_->clear(); + applyTopicConfigsToManager(); +} + +void TrajectoryKinematicsPanel::applyTopicConfigsToManager() +{ + if (!series_manager_) { + return; + } + series_manager_->setTopicConfigs(configured_topics_); + processDataUpdated(); +} + +void TrajectoryKinematicsPanel::onDataUpdated() +{ + // Single-shot timer merges bursts of identical-topic replays into one list rebuild + plot update. + plot_refresh_coalesce_timer_->start(); +} + +void TrajectoryKinematicsPanel::processDataUpdated() +{ + if (!series_manager_) { + return; + } + const auto all = series_manager_->allSeries(); + std::unordered_map present; + for (const auto & s : all) { + const std::string id = TrajectorySeriesManager::seriesId(s); + present[id] = true; + // New series default to checked so first receipt shows a line without extra clicks. + if (series_checked_.find(id) == series_checked_.end()) { + series_checked_[id] = true; + } + } + // Drop checkbox state for ids that disappeared (topic removed or candidate list shrunk). + for (auto it = series_checked_.begin(); it != series_checked_.end();) { + if (present.find(it->first) == present.end()) { + it = series_checked_.erase(it); + } else { + ++it; + } + } + rebuildSeriesListWidget(); + refreshPlot(); +} + +void TrajectoryKinematicsPanel::rebuildSeriesListWidget() +{ + if (!series_manager_) { + return; + } + const auto all = series_manager_->allSeries(); + std::vector> current_sig; + current_sig.reserve(all.size()); + for (const auto & s : all) { + current_sig.emplace_back(TrajectorySeriesManager::seriesId(s), s.label); + } + // Skip QListWidget rebuild when only point values changed but ids/labels are unchanged (keeps + // selection/focus stable). + if (current_sig == last_series_list_signature_) { + return; + } + last_series_list_signature_ = std::move(current_sig); + + series_list_->blockSignals(true); + series_list_->clear(); + for (size_t ai = 0; ai < all.size(); ++ai) { + const auto & s = all[ai]; + const std::string id = TrajectorySeriesManager::seriesId(s); + auto * item = new QListWidgetItem(QString::fromStdString(s.label)); + item->setData(static_cast(SeriesListItemRole::kSeriesId), QString::fromStdString(id)); + item->setData(static_cast(SeriesListItemRole::kAllSeriesIndex), static_cast(ai)); + item->setData( + static_cast(SeriesListItemRole::kTopicName), QString::fromStdString(s.topic)); + item->setIcon(makeTrajectoryColorIcon(colorForSeriesListIndex(static_cast(ai)))); + item->setFlags(item->flags() | Qt::ItemIsUserCheckable); + const bool checked = series_checked_[id]; + item->setCheckState(checked ? Qt::Checked : Qt::Unchecked); + series_list_->addItem(item); + } + series_list_->blockSignals(false); +} + +void TrajectoryKinematicsPanel::onSeriesItemChanged() +{ + for (int i = 0; i < series_list_->count(); ++i) { + auto * item = series_list_->item(i); + const std::string id = + item->data(static_cast(SeriesListItemRole::kSeriesId)).toString().toStdString(); + series_checked_[id] = (item->checkState() == Qt::Checked); + } + refreshPlot(); +} + +void TrajectoryKinematicsPanel::onAxisChanged() +{ + refreshPlot(); +} + +void TrajectoryKinematicsPanel::refreshPlot() +{ + if (!series_manager_ || !plot_widget_) { + return; + } + const AxisId x_axis = static_cast(x_axis_combo_->currentData().toInt()); + const AxisId y_axis = static_cast(y_axis_combo_->currentData().toInt()); + const auto all = series_manager_->allSeries(); + std::vector selected; + std::vector colors; + for (int i = 0; i < series_list_->count(); ++i) { + auto * item = series_list_->item(i); + if (item->checkState() != Qt::Checked) { + continue; + } + const int ai = item->data(static_cast(SeriesListItemRole::kAllSeriesIndex)).toInt(); + if (ai < 0 || static_cast(ai) >= all.size()) { + continue; + } + const std::string want_id = + item->data(static_cast(SeriesListItemRole::kSeriesId)).toString().toStdString(); + // Stale list row: `all` order can change while the widget still holds an old index — skip + // mismatched ids. + if (TrajectorySeriesManager::seriesId(all[static_cast(ai)]) != want_id) { + continue; + } + selected.push_back(all[static_cast(ai)]); + colors.push_back(colorForSeriesListIndex(ai)); + } + plot_widget_->updatePlot(selected, colors, x_axis, y_axis, readPlotRangeOptions()); + updateSummary(selected); +} + +void TrajectoryKinematicsPanel::updateSummary(const std::vector & plotted) +{ + if (plotted.empty() || plotted.front().points.empty()) { + metric_velocity_->setText(QStringLiteral("-")); + metric_accel_->setText(QStringLiteral("-")); + metric_duration_->setText(QStringLiteral("-")); + return; + } + const auto & back = plotted.front().points.back(); + metric_velocity_->setText(QString::number(back.longitudinal_velocity_mps, 'f', 3) + " m/s"); + metric_accel_->setText(QString::number(back.acceleration_mps2, 'f', 3) + " m/s²"); + metric_duration_->setText(QString::number(back.time_from_start_sec, 'f', 3) + " s"); +} + +std::string TrajectoryKinematicsPanel::serializeTopics( + const std::vector> & v) +{ + std::ostringstream oss; + for (size_t i = 0; i < v.size(); ++i) { + if (i > 0) { + oss << ";"; + } + oss << v[i].first << "|" << static_cast(v[i].second); + } + return oss.str(); +} + +bool TrajectoryKinematicsPanel::deserializeTopics( + const std::string & s, std::vector> * out) +{ + out->clear(); + if (s.empty()) { + return true; + } + std::istringstream iss(s); + std::string segment; + while (std::getline(iss, segment, ';')) { + const auto pos = segment.find('|'); + if (pos == std::string::npos) { + continue; + } + std::string topic = segment.substr(0, pos); + int kind_i = 0; + try { + kind_i = std::stoi(segment.substr(pos + 1)); + } catch (const std::exception &) { + continue; + } + topic = normalizeTopicString(std::move(topic)); + if (topic.empty()) { + continue; + } + const auto kind = static_cast(kind_i); + out->emplace_back(topic, kind); + } + return true; +} + +void TrajectoryKinematicsPanel::save(rviz_common::Config config) const +{ + rviz_common::Panel::save(config); + config.mapSetValue("topics_config", QString::fromStdString(serializeTopics(configured_topics_))); + config.mapSetValue("x_axis", static_cast(x_axis_combo_->currentData().toInt())); + config.mapSetValue("y_axis", static_cast(y_axis_combo_->currentData().toInt())); + QStringList checked_ids; + for (const auto & kv : series_checked_) { + if (kv.second) { + checked_ids.push_back(QString::fromStdString(kv.first)); + } + } + checked_ids.sort(); + config.mapSetValue("checked_series", checked_ids.join(QStringLiteral(","))); + config.mapSetValue("plot_lock_x", fix_x_range_->isChecked() ? 1 : 0); + config.mapSetValue("plot_lock_y", fix_y_range_->isChecked() ? 1 : 0); + config.mapSetValue("plot_x_min", QString::number(plot_x_min_spin_->value(), 'g', 12)); + config.mapSetValue("plot_x_max", QString::number(plot_x_max_spin_->value(), 'g', 12)); + config.mapSetValue("plot_y_min", QString::number(plot_y_min_spin_->value(), 'g', 12)); + config.mapSetValue("plot_y_max", QString::number(plot_y_max_spin_->value(), 'g', 12)); +} + +void TrajectoryKinematicsPanel::load(const rviz_common::Config & config) +{ + rviz_common::Panel::load(config); + QString topics_q; + if (config.mapGetString("topics_config", &topics_q)) { + deserializeTopics(topics_q.toStdString(), &configured_topics_); + } + int x_axis = static_cast(AxisId::TIME_FROM_START); + int y_axis = static_cast(AxisId::LONGITUDINAL_VELOCITY); + config.mapGetInt("x_axis", &x_axis); + config.mapGetInt("y_axis", &y_axis); + for (int i = 0; i < x_axis_combo_->count(); ++i) { + if (x_axis_combo_->itemData(i).toInt() == x_axis) { + x_axis_combo_->setCurrentIndex(i); + break; + } + } + for (int i = 0; i < y_axis_combo_->count(); ++i) { + if (y_axis_combo_->itemData(i).toInt() == y_axis) { + y_axis_combo_->setCurrentIndex(i); + break; + } + } + QString checked_q; + series_checked_.clear(); + if (config.mapGetString("checked_series", &checked_q)) { + for (const auto & p : checked_q.split(',')) { + if (p.isEmpty()) { + continue; + } + series_checked_[p.toStdString()] = true; + } + } + + int plot_lock_x = 0; + int plot_lock_y = 0; + if (config.mapGetInt("plot_lock_x", &plot_lock_x)) { + fix_x_range_->setChecked(plot_lock_x != 0); + } + if (config.mapGetInt("plot_lock_y", &plot_lock_y)) { + fix_y_range_->setChecked(plot_lock_y != 0); + } + QString q_string; + if (config.mapGetString("plot_x_min", &q_string)) { + plot_x_min_spin_->setValue(q_string.toDouble()); + } + if (config.mapGetString("plot_x_max", &q_string)) { + plot_x_max_spin_->setValue(q_string.toDouble()); + } + if (config.mapGetString("plot_y_min", &q_string)) { + plot_y_min_spin_->setValue(q_string.toDouble()); + } + if (config.mapGetString("plot_y_max", &q_string)) { + plot_y_max_spin_->setValue(q_string.toDouble()); + } + + applyTopicConfigsToManager(); +} + +void TrajectoryKinematicsPanel::showEvent(QShowEvent * event) +{ + rviz_common::Panel::showEvent(event); + if (topic_poll_timer_) { + topic_poll_timer_->start(); + } +} + +void TrajectoryKinematicsPanel::hideEvent(QHideEvent * event) +{ + if (topic_poll_timer_) { + topic_poll_timer_->stop(); + } + rviz_common::Panel::hideEvent(event); +} + +} // namespace autoware::visualization::trajectory_kinematics_rviz_plugin + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::visualization::trajectory_kinematics_rviz_plugin::TrajectoryKinematicsPanel, + rviz_common::Panel) diff --git a/common/autoware_trajectory_kinematics_rviz_plugin/src/trajectory_kinematics_panel.hpp b/common/autoware_trajectory_kinematics_rviz_plugin/src/trajectory_kinematics_panel.hpp new file mode 100644 index 000000000..a91b7308b --- /dev/null +++ b/common/autoware_trajectory_kinematics_rviz_plugin/src/trajectory_kinematics_panel.hpp @@ -0,0 +1,189 @@ +// Copyright 2026 TIER IV, Inc. +// +// 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 TRAJECTORY_KINEMATICS_PANEL_HPP_ +#define TRAJECTORY_KINEMATICS_PANEL_HPP_ + +#include "kinematics_types.hpp" +#include "trajectory_kinematics_plot_widget.hpp" +#include "trajectory_series_manager.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class QHideEvent; +class QShowEvent; + +#include +#include +#include +#include +#include + +namespace autoware::visualization::trajectory_kinematics_rviz_plugin +{ + +/// @brief Qt item roles for metadata stored on each row of `series_list_` (series identity and +/// source topic). +enum class SeriesListItemRole : int { + kSeriesId = Qt::UserRole, + kAllSeriesIndex = Qt::UserRole + 1, + kTopicName = Qt::UserRole + 2, +}; + +/// @brief RViz panel that subscribes to trajectory topics, lists selectable series, and plots +/// kinematics. +/// +/// Subscriptions and cached samples are owned by TrajectorySeriesManager. The panel wires UI state +/// (axes, fixed ranges, which series are checked) to TrajectoryKinematicsPlotWidget and persists +/// configuration via rviz_common::Config. +class TrajectoryKinematicsPanel : public rviz_common::Panel +{ + Q_OBJECT + +public: + /// @brief Builds the widget hierarchy, timers, and signal connections (ROS node is set in + /// onInitialize()). + explicit TrajectoryKinematicsPanel(QWidget * parent = nullptr); + + /// @brief Acquires the display ROS node, constructs TrajectorySeriesManager, and applies saved or + /// default topics. + void onInitialize() override; + + /// @brief Restores topics, axes, checked series, and plot range locks from RViz layout storage. + void load(const rviz_common::Config & config) override; + + /// @brief Persists topic configuration, axis selection, checked series, and plot range settings. + void save(rviz_common::Config config) const override; + +protected: + /// @brief Starts periodic polling of the ROS graph for topic names (only while the panel is + /// visible). + void showEvent(QShowEvent * event) override; + + /// @brief Stops topic polling when the panel is hidden to avoid idle work. + void hideEvent(QHideEvent * event) override; + +private Q_SLOTS: + /// @brief Appends the chosen (or manually entered) topic and message kind to the subscription + /// list. + void onAddTopic(); + + /// @brief Removes subscriptions for ROS topic names taken from the current list selection. + void onRemoveTopic(); + + /// @brief Restarts the coalesce timer so high-rate `dataUpdated` bursts collapse to one refresh. + void onDataUpdated(); + + /// @brief Merges new series into `series_checked_`, rebuilds the list if its signature changed, + /// and redraws the plot. + void processDataUpdated(); + + /// @brief Persists checkbox state per series id and refreshes the plot. + void onSeriesItemChanged(); + + /// @brief Replots when X/Y axis combo selection changes. + void onAxisChanged(); + + /// @brief Refills the topic dropdown from `get_topic_names_and_types()` filtered by the selected + /// message type. + void refreshTopicList(); + + /// @brief Pushes checked series and current axes/ranges to the plot widget and updates end-point + /// summary labels. + void refreshPlot(); + +private: + /// @brief Calls TrajectorySeriesManager::setTopicConfigs() with `configured_topics_` and triggers + /// a full data pass. + void applyTopicConfigsToManager(); + + /// @brief Trims whitespace and ensures a leading `/` for ROS topic names. + static std::string normalizeTopicString(std::string s); + + /// @brief Rebuilds `series_list_` from TrajectorySeriesManager::allSeries() when the (id, label) + /// signature changes. + void rebuildSeriesListWidget(); + + /// @brief Shows end-of-path velocity, acceleration, and time from the first plotted series (if + /// any). + void updateSummary(const std::vector & plotted); + + /// @brief Fills X/Y axis combos from xAxisDefinitions() / yAxisDefinitions(). + void populateAxisCombos(); + + /// @brief Applies Material Design–style colors and the panel UI font to child widgets. + void applyPanelStyle(); + + /// @brief Reads fixed-range checkboxes and spin values into PlotAxisRangeOptions. + PlotAxisRangeOptions readPlotRangeOptions() const; + + /// @brief Encodes topic configuration as `topic|kind` segments joined by `;` for RViz + /// persistence. + static std::string serializeTopics( + const std::vector> & v); + + /// @brief Parses serializeTopics() output into `out` (best-effort; invalid segments are skipped). + static bool deserializeTopics( + const std::string & s, std::vector> * out); + + rclcpp::Node::SharedPtr node_; + std::unique_ptr series_manager_; + + QWidget * panel_root_{nullptr}; + QComboBox * topic_combo_{nullptr}; + QLineEdit * manual_topic_edit_{nullptr}; + QComboBox * kind_combo_{nullptr}; + QPushButton * refresh_topics_button_{nullptr}; + QPushButton * add_topic_button_{nullptr}; + QPushButton * remove_topic_button_{nullptr}; + + QListWidget * series_list_{nullptr}; + QComboBox * x_axis_combo_{nullptr}; + QComboBox * y_axis_combo_{nullptr}; + + QCheckBox * fix_x_range_{nullptr}; + QDoubleSpinBox * plot_x_min_spin_{nullptr}; + QDoubleSpinBox * plot_x_max_spin_{nullptr}; + QCheckBox * fix_y_range_{nullptr}; + QDoubleSpinBox * plot_y_min_spin_{nullptr}; + QDoubleSpinBox * plot_y_max_spin_{nullptr}; + + QLabel * metric_velocity_{nullptr}; + QLabel * metric_accel_{nullptr}; + QLabel * metric_duration_{nullptr}; + + TrajectoryKinematicsPlotWidget * plot_widget_{nullptr}; + + QTimer * topic_poll_timer_{nullptr}; + /// Coalesces rapid `dataUpdated` signals before rebuilding list + plot (GUI thread). + QTimer * plot_refresh_coalesce_timer_{nullptr}; + + std::vector> configured_topics_; + std::unordered_map series_checked_; + /// When unchanged, skip rebuilding the list so selection is stable during message updates. + /// Pairs are (series id, display label) so label-only updates (e.g. scores) refresh the list. + std::vector> last_series_list_signature_; +}; + +} // namespace autoware::visualization::trajectory_kinematics_rviz_plugin + +#endif // TRAJECTORY_KINEMATICS_PANEL_HPP_ diff --git a/common/autoware_trajectory_kinematics_rviz_plugin/src/trajectory_kinematics_plot_widget.cpp b/common/autoware_trajectory_kinematics_rviz_plugin/src/trajectory_kinematics_plot_widget.cpp new file mode 100644 index 000000000..2dd6f7f70 --- /dev/null +++ b/common/autoware_trajectory_kinematics_rviz_plugin/src/trajectory_kinematics_plot_widget.cpp @@ -0,0 +1,346 @@ +// Copyright 2026 TIER IV, Inc. +// +// 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 "trajectory_kinematics_plot_widget.hpp" + +#include "material_colors.hpp" +#include "ui_font.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::visualization::trajectory_kinematics_rviz_plugin +{ +namespace +{ + +void applyAxisTheme(QtCharts::QValueAxis * axis, const style::MaterialColors & pal) +{ + const QColor fg = style::hexToQColor(pal.on_surface_variant); + const QColor title = style::hexToQColor(pal.on_surface); + axis->setLabelsColor(fg); + axis->setTitleBrush(QBrush(title)); + axis->setLinePenColor(style::hexToQColor(pal.outline_variant)); + axis->setGridLineColor(style::hexToQColor(pal.surface_container_high)); +} + +void applyChartTheme(QtCharts::QChart * chart, const style::MaterialColors & pal) +{ + chart->setBackgroundRoundness(6); + chart->setMargins(QMargins(8, 8, 8, 8)); + chart->setBackgroundBrush(QBrush(style::hexToQColor(pal.surface_container))); + chart->setTitleBrush(QBrush(style::hexToQColor(pal.on_surface))); + chart->legend()->setLabelColor(style::hexToQColor(pal.on_surface_variant)); + chart->legend()->setBackgroundVisible(true); + chart->legend()->setBrush(QBrush(style::hexToQColor(pal.surface_container_low))); +} + +void applyChartFonts(QtCharts::QChart * chart, QtCharts::QValueAxis * ax, QtCharts::QValueAxis * ay) +{ + const QFont base = makePanelUiFont(); + + QFont title_font = base; + title_font.setPixelSize(14); + title_font.setWeight(QFont::Bold); + chart->setTitleFont(title_font); + + QFont legend_font = base; + legend_font.setPixelSize(12); + legend_font.setWeight(QFont::DemiBold); + chart->legend()->setFont(legend_font); + + QFont axis_title = base; + axis_title.setPixelSize(12); + axis_title.setWeight(QFont::DemiBold); + QFont axis_labels = base; + axis_labels.setPixelSize(11); + axis_labels.setWeight(QFont::Medium); + + ax->setTitleFont(axis_title); + ax->setLabelsFont(axis_labels); + ay->setTitleFont(axis_title); + ay->setLabelsFont(axis_labels); +} + +std::string buildLegendStructureKey( + const std::vector & series, const std::vector & colors, + AxisId x_axis, AxisId y_axis) +{ + std::ostringstream oss; + oss << static_cast(x_axis) << '|' << static_cast(y_axis) << '|' << series.size() << '|'; + // Field separator \x01: ROS topic/key follow naming rules; labels are built from ASCII topic, + // generator metadata, and numeric scores — none use 0x01 in practice. + for (size_t i = 0; i < series.size(); ++i) { + oss << series[i].topic << '\x01' << series[i].key << '\x01' << series[i].label << '\x01'; + if (i < colors.size()) { + oss << colors[i].rgba(); + } + oss << '\x02'; + } + return oss.str(); +} + +void computeDataBounds( + const std::vector & series, AxisId x_axis, AxisId y_axis, double * xmin, + double * xmax, double * ymin, double * ymax) +{ + *xmin = std::numeric_limits::infinity(); + *xmax = -std::numeric_limits::infinity(); + *ymin = std::numeric_limits::infinity(); + *ymax = -std::numeric_limits::infinity(); + for (const auto & s : series) { + for (const auto & p : s.points) { + const double xv = accessAxisValue(p, x_axis); + const double yv = accessAxisValue(p, y_axis); + if (std::isfinite(xv) && std::isfinite(yv)) { + *xmin = std::min(*xmin, xv); + *xmax = std::max(*xmax, xv); + *ymin = std::min(*ymin, yv); + *ymax = std::max(*ymax, yv); + } + } + } +} + +QVector pointsForSeries(const TrajectorySeriesData & s, AxisId x_axis, AxisId y_axis) +{ + QVector pts; + for (const auto & p : s.points) { + const double xv = accessAxisValue(p, x_axis); + const double yv = accessAxisValue(p, y_axis); + if (std::isfinite(xv) && std::isfinite(yv)) { + pts.append(QPointF(xv, yv)); + } + } + return pts; +} + +} // namespace + +TrajectoryKinematicsPlotWidget::TrajectoryKinematicsPlotWidget(QWidget * parent) : QWidget(parent) +{ + setObjectName("TkmPlotHost"); + auto * layout = new QVBoxLayout(this); + layout->setContentsMargins(0, 0, 0, 0); + chart_view_ = new TrajectoryKinematicsChartView(this); + chart_view_->setRenderHint(QPainter::Antialiasing); + chart_view_->setMinimumHeight(220); + chart_view_->setMouseTracking(true); + layout->addWidget(chart_view_); + setLayout(layout); + + const auto & pal = style::default_colors; + setFont(makePanelUiFont()); + setStyleSheet(QStringLiteral( + "QWidget#TkmPlotHost { background-color: %1; border-radius: 8px;" + " font-family: \"Segoe UI\", \"Ubuntu\", \"Noto Sans\", sans-serif;" + " font-size: 13px; font-weight: 600; }") + .arg(QString::fromStdString(pal.surface_container))); +} + +void TrajectoryKinematicsPlotWidget::applyAxisRanges( + QtCharts::QValueAxis * ax, QtCharts::QValueAxis * ay, double xmin, double xmax, double ymin, + double ymax, const PlotAxisRangeOptions & range_opts) const +{ + if (range_opts.lock_x && range_opts.x_max > range_opts.x_min) { + ax->setRange(range_opts.x_min, range_opts.x_max); + } else if (std::isfinite(xmin) && std::isfinite(xmax) && xmin < xmax) { + const double pad = 0.02 * (xmax - xmin); + ax->setRange(xmin - pad, xmax + pad); + } else { + ax->setRange(0.0, 1.0); + } + + if (range_opts.lock_y && range_opts.y_max > range_opts.y_min) { + ay->setRange(range_opts.y_min, range_opts.y_max); + } else if (std::isfinite(ymin) && std::isfinite(ymax)) { + if (ymin < ymax) { + const double pad = 0.02 * (ymax - ymin); + ay->setRange(ymin - pad, ymax + pad); + } else { + ay->setRange(ymin - 1.0, ymax + 1.0); + } + } else { + ay->setRange(-1.0, 1.0); + } +} + +bool TrajectoryKinematicsPlotWidget::tryUpdateSeriesInPlace( + const std::vector & series, const std::vector & colors, + AxisId x_axis, AxisId y_axis, const PlotAxisRangeOptions & range_opts) +{ + // Structure key encodes axes, series count, topic/key/label, and color — any change forces a + // full QChart rebuild. + const std::string key = buildLegendStructureKey(series, colors, x_axis, y_axis); + if (key != last_legend_structure_key_) { + return false; + } + if ( + x_axis_ptr_ == nullptr || y_axis_ptr_ == nullptr || crosshair_series_ == nullptr || + data_line_series_.size() != series.size()) { + return false; + } + + double xmin = 0.0; + double xmax = 0.0; + double ymin = 0.0; + double ymax = 0.0; + computeDataBounds(series, x_axis, y_axis, &xmin, &xmax, &ymin, &ymax); + + for (size_t si = 0; si < series.size(); ++si) { + auto * ls = data_line_series_[si]; + const QVector pts = pointsForSeries(series[si], x_axis, y_axis); + if (pts.isEmpty()) { + ls->clear(); + } else { + ls->replace(pts); + } + const QColor col = si < colors.size() ? colors[si] : QColor(139, 208, 240); + if (ls->color() != col) { + ls->setColor(col); + } + } + + applyAxisRanges(x_axis_ptr_, y_axis_ptr_, xmin, xmax, ymin, ymax, range_opts); + chart_view_->setHoverSeriesData(series, colors, x_axis, y_axis); + return true; +} + +void TrajectoryKinematicsPlotWidget::rebuildChart( + const std::vector & series, const std::vector & colors, + AxisId x_axis, AxisId y_axis, const PlotAxisRangeOptions & range_opts) +{ + const auto & pal = style::default_colors; + + data_line_series_.clear(); + crosshair_series_ = nullptr; + x_axis_ptr_ = nullptr; + y_axis_ptr_ = nullptr; + + QtCharts::QChart * old_chart = chart_view_->chart(); + + auto * chart = new QtCharts::QChart(); + chart->legend()->setVisible(true); + chart->setTitle("Trajectory kinematics"); + applyChartTheme(chart, pal); + + double xmin = std::numeric_limits::infinity(); + double xmax = -std::numeric_limits::infinity(); + double ymin = std::numeric_limits::infinity(); + double ymax = -std::numeric_limits::infinity(); + + for (size_t si = 0; si < series.size(); ++si) { + const auto & s = series[si]; + auto * ls = new QtCharts::QLineSeries(); + ls->setName(QString::fromStdString(s.label)); + const QColor col = si < colors.size() ? colors[si] : QColor(139, 208, 240); + ls->setColor(col); + + for (const auto & p : s.points) { + const double xv = accessAxisValue(p, x_axis); + const double yv = accessAxisValue(p, y_axis); + if (std::isfinite(xv) && std::isfinite(yv)) { + ls->append(xv, yv); + xmin = std::min(xmin, xv); + xmax = std::max(xmax, xv); + ymin = std::min(ymin, yv); + ymax = std::max(ymax, yv); + } + } + + chart->addSeries(ls); + data_line_series_.push_back(ls); + } + + auto * crosshair = new QtCharts::QLineSeries(); + crosshair->setName(QString()); + crosshair->setPen(QPen(QColor(255, 255, 255, 210), 1.5, Qt::SolidLine)); + chart->addSeries(crosshair); + crosshair_series_ = crosshair; + + auto * ax = new QtCharts::QValueAxis(); + ax->setTitleText(QStringLiteral("%1 [%2]") + .arg(QString::fromUtf8(axisLabel(x_axis))) + .arg(QString::fromUtf8(axisUnit(x_axis)))); + ax->setLabelFormat("%.3f"); + applyAxisTheme(ax, pal); + x_axis_ptr_ = ax; + + auto * ay = new QtCharts::QValueAxis(); + ay->setTitleText(QStringLiteral("%1 [%2]") + .arg(QString::fromUtf8(axisLabel(y_axis))) + .arg(QString::fromUtf8(axisUnit(y_axis)))); + ay->setLabelFormat("%.3f"); + applyAxisTheme(ay, pal); + y_axis_ptr_ = ay; + + applyChartFonts(chart, ax, ay); + + chart->addAxis(ax, Qt::AlignBottom); + chart->addAxis(ay, Qt::AlignLeft); + + for (auto * ser : chart->series()) { + if (ser == crosshair) { + continue; + } + ser->attachAxis(ax); + ser->attachAxis(ay); + } + // Crosshair shares the same value axes so mapToValue() in the chart view stays consistent with + // data lines. + crosshair->attachAxis(ax); + crosshair->attachAxis(ay); + + applyAxisRanges(ax, ay, xmin, xmax, ymin, ymax, range_opts); + + chart_view_->setChart(chart); + chart_view_->setCrosshairSeries(crosshair); + chart_view_->setHoverSeriesData(series, colors, x_axis, y_axis); + for (QtCharts::QLegendMarker * m : chart->legend()->markers(crosshair)) { + m->setVisible(false); + } + + std::unique_ptr dispose_previous(old_chart); + + last_legend_structure_key_ = buildLegendStructureKey(series, colors, x_axis, y_axis); +} + +void TrajectoryKinematicsPlotWidget::updatePlot( + const std::vector & series, const std::vector & colors, + AxisId x_axis, AxisId y_axis, const PlotAxisRangeOptions & range_opts) +{ + if (tryUpdateSeriesInPlace(series, colors, x_axis, y_axis, range_opts)) { + return; + } + rebuildChart(series, colors, x_axis, y_axis, range_opts); +} + +} // namespace autoware::visualization::trajectory_kinematics_rviz_plugin diff --git a/common/autoware_trajectory_kinematics_rviz_plugin/src/trajectory_kinematics_plot_widget.hpp b/common/autoware_trajectory_kinematics_rviz_plugin/src/trajectory_kinematics_plot_widget.hpp new file mode 100644 index 000000000..ab8a40ea1 --- /dev/null +++ b/common/autoware_trajectory_kinematics_rviz_plugin/src/trajectory_kinematics_plot_widget.hpp @@ -0,0 +1,91 @@ +// Copyright 2026 TIER IV, Inc. +// +// 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 TRAJECTORY_KINEMATICS_PLOT_WIDGET_HPP_ +#define TRAJECTORY_KINEMATICS_PLOT_WIDGET_HPP_ + +#include "kinematics_types.hpp" +#include "trajectory_kinematics_chart_view.hpp" + +#include +#include + +#include +#include + +namespace QtCharts +{ +class QLineSeries; +class QValueAxis; +} // namespace QtCharts + +namespace autoware::visualization::trajectory_kinematics_rviz_plugin +{ + +/// @brief Hosts a TrajectoryKinematicsChartView and owns Qt Charts line series for trajectories +/// plus a crosshair. +/// +/// Prefer updating existing QLineSeries data in place when the set of series +/// (topic/key/label/color) and axes are unchanged, to avoid rebuilding QChart and resetting legend +/// state on every message. +class TrajectoryKinematicsPlotWidget : public QWidget +{ +public: + /// @brief Creates the chart view with antialiasing and a minimum plot height. + explicit TrajectoryKinematicsPlotWidget(QWidget * parent = nullptr); + + /// @brief Refreshes the plot: in-place point replacement when structure matches, otherwise full + /// chart rebuild. + /// @param series Trajectories to draw (each becomes one QLineSeries named from `label`). + /// @param colors Line colors aligned with `series`. + /// @param x_axis Horizontal quantity (time or arc length). + /// @param y_axis Vertical quantity (velocity, acceleration, curvature, or lateral acceleration). + /// @param range_opts Optional locked min/max for X and/or Y; otherwise ranges are padded from + /// data bounds. + void updatePlot( + const std::vector & series, const std::vector & colors, + AxisId x_axis, AxisId y_axis, const PlotAxisRangeOptions & range_opts); + +private: + /// @brief Constructs a new QChart, axes, data series, invisible crosshair series, and legend; + /// disposes the previous chart. + void rebuildChart( + const std::vector & series, const std::vector & colors, + AxisId x_axis, AxisId y_axis, const PlotAxisRangeOptions & range_opts); + + /// @brief If `last_legend_structure_key_` still matches, replaces points and updates axes without + /// recreating the chart. + /// @return True if the chart was updated in place; false if rebuildChart() is required. + bool tryUpdateSeriesInPlace( + const std::vector & series, const std::vector & colors, + AxisId x_axis, AxisId y_axis, const PlotAxisRangeOptions & range_opts); + + /// @brief Sets axis ranges from `range_opts` when locked; otherwise uses data min/max with + /// padding (or defaults if empty/invalid). + void applyAxisRanges( + QtCharts::QValueAxis * ax, QtCharts::QValueAxis * ay, double xmin, double xmax, double ymin, + double ymax, const PlotAxisRangeOptions & range_opts) const; + + TrajectoryKinematicsChartView * chart_view_{nullptr}; + + std::string last_legend_structure_key_; + std::vector data_line_series_; + QtCharts::QLineSeries * crosshair_series_{nullptr}; + QtCharts::QValueAxis * x_axis_ptr_{nullptr}; + QtCharts::QValueAxis * y_axis_ptr_{nullptr}; +}; + +} // namespace autoware::visualization::trajectory_kinematics_rviz_plugin + +#endif // TRAJECTORY_KINEMATICS_PLOT_WIDGET_HPP_ diff --git a/common/autoware_trajectory_kinematics_rviz_plugin/src/trajectory_message_adapter.cpp b/common/autoware_trajectory_kinematics_rviz_plugin/src/trajectory_message_adapter.cpp new file mode 100644 index 000000000..1f61ace3a --- /dev/null +++ b/common/autoware_trajectory_kinematics_rviz_plugin/src/trajectory_message_adapter.cpp @@ -0,0 +1,161 @@ +// Copyright 2026 TIER IV, Inc. +// +// 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 "trajectory_message_adapter.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +namespace autoware::visualization::trajectory_kinematics_rviz_plugin +{ +namespace +{ +double durationToSec(const builtin_interfaces::msg::Duration & d) +{ + return static_cast(d.sec) + static_cast(d.nanosec) * 1e-9; +} + +/// Menger curvature on three consecutive XY points: magnitude from radius, sign from +/// triangle orientation. +double mengerCurvatureSigned2D( + const geometry_msgs::msg::Point & p0, const geometry_msgs::msg::Point & p1, + const geometry_msgs::msg::Point & p2) +{ + const double abx = p1.x - p0.x; + const double aby = p1.y - p0.y; + const double bcx = p2.x - p1.x; + const double bcy = p2.y - p1.y; + const double acx = p2.x - p0.x; + const double acy = p2.y - p0.y; + const double a = std::hypot(abx, aby); + const double b = std::hypot(bcx, bcy); + const double c = std::hypot(acx, acy); + if (a < 1e-9 || b < 1e-9 || c < 1e-9) { + return 0.0; + } + const double cross = abx * bcy - aby * bcx; + const double area = 0.5 * std::abs(cross); + const double kappa_mag = 4.0 * area / (a * b * c); + const double sign = cross >= 0.0 ? 1.0 : -1.0; + return sign * kappa_mag; +} + +std::string lookupGeneratorName( + const std::vector & infos, + const unique_identifier_msgs::msg::UUID & id) +{ + for (const auto & g : infos) { + if (g.generator_id.uuid.size() != 16U || id.uuid.size() != 16U) { + continue; + } + if (std::equal(g.generator_id.uuid.begin(), g.generator_id.uuid.end(), id.uuid.begin())) { + return g.generator_name.data; + } + } + return {}; +} + +} // namespace + +std::vector buildSeriesFromTrajectoryPoints( + const std::vector & points) +{ + const size_t n = points.size(); + std::vector out(n); + if (n == 0U) { + return out; + } + + double arc = 0.0; + for (size_t i = 0; i < n; ++i) { + out[i].time_from_start_sec = durationToSec(points[i].time_from_start); + out[i].longitudinal_velocity_mps = static_cast(points[i].longitudinal_velocity_mps); + out[i].acceleration_mps2 = static_cast(points[i].acceleration_mps2); + if (i > 0U) { + const double dx = points[i].pose.position.x - points[i - 1U].pose.position.x; + const double dy = points[i].pose.position.y - points[i - 1U].pose.position.y; + arc += std::hypot(dx, dy); + } + out[i].arc_length_m = arc; + } + + std::vector kappa(n, 0.0); + if (n >= 3U) { + for (size_t i = 1U; i + 1U < n; ++i) { + kappa[i] = mengerCurvatureSigned2D( + points[i - 1U].pose.position, points[i].pose.position, points[i + 1U].pose.position); + } + // Endpoints lack a full triple: reuse the neighbor interior estimate for a stable open + // polyline. + kappa[0] = kappa[1]; + kappa[n - 1U] = kappa[n - 2U]; + } + + for (size_t i = 0; i < n; ++i) { + out[i].curvature_1pm = kappa[i]; + const double v = out[i].longitudinal_velocity_mps; + out[i].lateral_acceleration_mps2 = v * v * kappa[i]; + } + + return out; +} + +std::vector trajectoryMsgToSeries( + const autoware_planning_msgs::msg::Trajectory & msg, const std::string & topic_name) +{ + TrajectorySeriesData series; + series.topic = topic_name; + series.key = "trajectory"; + series.label = topic_name + " (Trajectory)"; + series.points = buildSeriesFromTrajectoryPoints(msg.points); + return {std::move(series)}; +} + +std::vector scoredCandidateTrajectoriesToSeries( + const autoware_internal_planning_msgs::msg::ScoredCandidateTrajectories & msg, + const std::string & topic_name) +{ + std::vector out; + out.reserve(msg.scored_candidate_trajectories.size()); + for (size_t i = 0; i < msg.scored_candidate_trajectories.size(); ++i) { + const auto & sct = msg.scored_candidate_trajectories[i]; + TrajectorySeriesData series; + series.topic = topic_name; + std::ostringstream key; + key << "candidate_" << i; + series.key = key.str(); + + std::string gen = + lookupGeneratorName(msg.generator_info, sct.candidate_trajectory.generator_id); + if (gen.empty()) { + gen = "candidate_" + std::to_string(i); + } + + std::ostringstream label; + label << topic_name << " [" << gen << "] score=" << static_cast(sct.score); + series.label = label.str(); + + series.points = buildSeriesFromTrajectoryPoints(sct.candidate_trajectory.points); + out.push_back(std::move(series)); + } + return out; +} + +} // namespace autoware::visualization::trajectory_kinematics_rviz_plugin diff --git a/common/autoware_trajectory_kinematics_rviz_plugin/src/trajectory_message_adapter.hpp b/common/autoware_trajectory_kinematics_rviz_plugin/src/trajectory_message_adapter.hpp new file mode 100644 index 000000000..a6f30a324 --- /dev/null +++ b/common/autoware_trajectory_kinematics_rviz_plugin/src/trajectory_message_adapter.hpp @@ -0,0 +1,48 @@ +// Copyright 2026 TIER IV, Inc. +// +// 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 TRAJECTORY_MESSAGE_ADAPTER_HPP_ +#define TRAJECTORY_MESSAGE_ADAPTER_HPP_ + +#include "kinematics_types.hpp" + +#include +#include + +#include +#include + +namespace autoware::visualization::trajectory_kinematics_rviz_plugin +{ + +/// @brief Builds per-point kinematics samples from planning trajectory points (arc length, Menger +/// curvature, lateral accel). +/// @param points Message trajectory points in path order. +/// @return One `SeriesPoint` per input point; empty if `points` is empty. +std::vector buildSeriesFromTrajectoryPoints( + const std::vector & points); + +/// @brief Wraps a single `Trajectory` message as one labeled series under `topic_name`. +std::vector trajectoryMsgToSeries( + const autoware_planning_msgs::msg::Trajectory & msg, const std::string & topic_name); + +/// @brief Expands scored candidates into one series per entry (labels include generator name and +/// score when available). +std::vector scoredCandidateTrajectoriesToSeries( + const autoware_internal_planning_msgs::msg::ScoredCandidateTrajectories & msg, + const std::string & topic_name); + +} // namespace autoware::visualization::trajectory_kinematics_rviz_plugin + +#endif // TRAJECTORY_MESSAGE_ADAPTER_HPP_ diff --git a/common/autoware_trajectory_kinematics_rviz_plugin/src/trajectory_series_manager.cpp b/common/autoware_trajectory_kinematics_rviz_plugin/src/trajectory_series_manager.cpp new file mode 100644 index 000000000..418d63564 --- /dev/null +++ b/common/autoware_trajectory_kinematics_rviz_plugin/src/trajectory_series_manager.cpp @@ -0,0 +1,207 @@ +// Copyright 2026 TIER IV, Inc. +// +// 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 "trajectory_series_manager.hpp" + +#include "trajectory_message_adapter.hpp" + +#include + +#include +#include +#include +#include + +namespace +{ +constexpr std::uint64_t kFnvOffset = 14695981039346656037ULL; +constexpr std::uint64_t kFnvPrime = 1099511628211ULL; + +void fnv1aByte(std::uint64_t * h, unsigned char b) +{ + *h ^= static_cast(b); + *h *= kFnvPrime; +} + +void fnv1aBytes(std::uint64_t * h, const void * data, std::size_t len) +{ + const auto * p = static_cast(data); + for (std::size_t i = 0; i < len; ++i) { + fnv1aByte(h, p[i]); + } +} + +void fnv1aDouble(std::uint64_t * h, double v) +{ + static_assert( + sizeof(double) == sizeof(std::uint64_t), + "FNV fingerprint: std::memcpy double to uint64_t for hashing"); + std::uint64_t bits = 0; + std::memcpy(&bits, &v, sizeof(bits)); + fnv1aBytes(h, &bits, sizeof(bits)); +} + +using autoware::visualization::trajectory_kinematics_rviz_plugin::TrajectorySeriesData; + +/// Content fingerprint for deduplicating identical replays on the same topic. +std::uint64_t fingerprintSeriesVector(const std::vector & vec) +{ + std::uint64_t h = kFnvOffset; + for (const auto & s : vec) { + for (unsigned char c : s.topic) { + fnv1aByte(&h, c); + } + fnv1aByte(&h, 0); + for (unsigned char c : s.key) { + fnv1aByte(&h, c); + } + fnv1aByte(&h, 0); + for (unsigned char c : s.label) { + fnv1aByte(&h, c); + } + fnv1aByte(&h, 0); + const std::size_t number_points = s.points.size(); + fnv1aBytes(&h, &number_points, sizeof(number_points)); + for (const auto & pt : s.points) { + fnv1aDouble(&h, pt.time_from_start_sec); + fnv1aDouble(&h, pt.arc_length_m); + fnv1aDouble(&h, pt.longitudinal_velocity_mps); + fnv1aDouble(&h, pt.acceleration_mps2); + fnv1aDouble(&h, pt.curvature_1pm); + fnv1aDouble(&h, pt.lateral_acceleration_mps2); + } + } + return h; +} + +} // namespace + +namespace autoware::visualization::trajectory_kinematics_rviz_plugin +{ + +TrajectorySeriesManager::TrajectorySeriesManager(rclcpp::Node::SharedPtr node, QObject * parent) +: QObject(parent), node_(std::move(node)) +{ +} + +void TrajectorySeriesManager::setTopicConfigs( + const std::vector> & configs) +{ + subscriptions_.clear(); + + { + std::lock_guard lock(mutex_); + last_series_by_topic_.clear(); + last_series_fingerprint_by_topic_.clear(); + } + + if (!node_) { + return; + } + + const auto qos = rclcpp::QoS(10); + + for (const auto & entry : configs) { + const std::string & topic = entry.first; + if (topic.empty()) { + continue; + } + if (entry.second == TopicMessageKind::Trajectory) { + // QPointer: if this QObject is destroyed while the executor still holds the subscription, + // skip the callback. + QPointer weak_this(this); + auto sub = node_->create_subscription( + topic, qos, + [weak_this, topic](const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg) { + if (!weak_this) { + return; + } + weak_this->onTrajectory(msg, topic); + }); + subscriptions_[topic] = sub; + } else { + // Same QObject lifetime guard as Trajectory subscriptions. + QPointer weak_this(this); + auto sub = node_->create_subscription< + autoware_internal_planning_msgs::msg::ScoredCandidateTrajectories>( + topic, qos, + [weak_this, topic]( + const autoware_internal_planning_msgs::msg::ScoredCandidateTrajectories::ConstSharedPtr + msg) { + if (!weak_this) { + return; + } + weak_this->onScoredCandidates(msg, topic); + }); + subscriptions_[topic] = sub; + } + } +} + +std::vector TrajectorySeriesManager::allSeries() const +{ + std::lock_guard lock(mutex_); + std::vector out; + for (const auto & kv : last_series_by_topic_) { + for (const auto & s : kv.second) { + out.push_back(s); + } + } + return out; +} + +std::string TrajectorySeriesManager::seriesId(const TrajectorySeriesData & s) +{ + return s.topic + "|" + s.key; +} + +void TrajectorySeriesManager::onTrajectory( + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg, const std::string & topic) +{ + auto series_vec = trajectoryMsgToSeries(*msg, topic); + const std::uint64_t fp = fingerprintSeriesVector(series_vec); + { + std::lock_guard lock(mutex_); + // Suppress duplicate GUI refreshes when the decoded trajectory is bitwise unchanged (e.g. + // static bag loop). + const auto it = last_series_fingerprint_by_topic_.find(topic); + if (it != last_series_fingerprint_by_topic_.end() && it->second == fp) { + return; + } + last_series_fingerprint_by_topic_[topic] = fp; + last_series_by_topic_[topic] = std::move(series_vec); + } + Q_EMIT dataUpdated(); +} + +void TrajectorySeriesManager::onScoredCandidates( + const autoware_internal_planning_msgs::msg::ScoredCandidateTrajectories::ConstSharedPtr msg, + const std::string & topic) +{ + auto series_vec = scoredCandidateTrajectoriesToSeries(*msg, topic); + const std::uint64_t fp = fingerprintSeriesVector(series_vec); + { + std::lock_guard lock(mutex_); + // Same deduplication as onTrajectory: skip emit when candidate set and scores are unchanged. + const auto it = last_series_fingerprint_by_topic_.find(topic); + if (it != last_series_fingerprint_by_topic_.end() && it->second == fp) { + return; + } + last_series_fingerprint_by_topic_[topic] = fp; + last_series_by_topic_[topic] = std::move(series_vec); + } + Q_EMIT dataUpdated(); +} + +} // namespace autoware::visualization::trajectory_kinematics_rviz_plugin diff --git a/common/autoware_trajectory_kinematics_rviz_plugin/src/trajectory_series_manager.hpp b/common/autoware_trajectory_kinematics_rviz_plugin/src/trajectory_series_manager.hpp new file mode 100644 index 000000000..285d69167 --- /dev/null +++ b/common/autoware_trajectory_kinematics_rviz_plugin/src/trajectory_series_manager.hpp @@ -0,0 +1,96 @@ +// Copyright 2026 TIER IV, Inc. +// +// 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 TRAJECTORY_SERIES_MANAGER_HPP_ +#define TRAJECTORY_SERIES_MANAGER_HPP_ + +#include "kinematics_types.hpp" + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::visualization::trajectory_kinematics_rviz_plugin +{ + +/// @brief ROS message type used when subscribing to a topic name. +enum class TopicMessageKind { + Trajectory = 0, + ScoredCandidateTrajectories = 1, +}; + +/// @brief Subscribes to trajectory-related topics, converts messages to TrajectorySeriesData, and +/// emits dataUpdated. +/// +/// Callbacks use QPointer for safe delivery if the manager is destroyed. Per-topic FNV-1a +/// fingerprints suppress duplicate `dataUpdated` emissions when the decoded series content is +/// identical to the last message (e.g. repeated static transforms on the same bag frame). +class TrajectorySeriesManager : public QObject +{ + Q_OBJECT + +public: + /// @brief Stores the ROS node used to create subscriptions (typically the RViz display node). + explicit TrajectorySeriesManager(rclcpp::Node::SharedPtr node, QObject * parent = nullptr); + + /// @brief Replaces all subscriptions to match `configs` (empty topic strings are skipped); clears + /// cached series for removed topics. + void setTopicConfigs(const std::vector> & configs); + + /// @brief Returns a concatenation of the latest `TrajectorySeriesData` vectors for every + /// subscribed topic (order not guaranteed across topics). + std::vector allSeries() const; + + /// @brief Stable key for UI and persistence: `topic + "|" + key` (key distinguishes candidates on + /// one topic). + static std::string seriesId(const TrajectorySeriesData & s); + +Q_SIGNALS: + /// @brief Emitted after a topic's cached series change to a new fingerprint (may arrive at high + /// rate). + void dataUpdated(); + +private: + /// @brief Converts Trajectory to one series and updates cache; emits only if the content + /// fingerprint changed. + void onTrajectory( + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg, const std::string & topic); + + /// @brief Converts ScoredCandidateTrajectories to multiple series and updates cache; emits only + /// if the fingerprint changed. + void onScoredCandidates( + const autoware_internal_planning_msgs::msg::ScoredCandidateTrajectories::ConstSharedPtr msg, + const std::string & topic); + + rclcpp::Node::SharedPtr node_; + mutable std::mutex mutex_; + std::map> subscriptions_; + std::map> last_series_by_topic_; + /// @brief FNV-1a hash of decoded series per topic; identical replays skip `dataUpdated`. + std::map last_series_fingerprint_by_topic_; +}; + +} // namespace autoware::visualization::trajectory_kinematics_rviz_plugin + +#endif // TRAJECTORY_SERIES_MANAGER_HPP_ diff --git a/common/autoware_trajectory_kinematics_rviz_plugin/src/ui_font.hpp b/common/autoware_trajectory_kinematics_rviz_plugin/src/ui_font.hpp new file mode 100644 index 000000000..e24aa4065 --- /dev/null +++ b/common/autoware_trajectory_kinematics_rviz_plugin/src/ui_font.hpp @@ -0,0 +1,59 @@ +// Copyright 2026 TIER IV, Inc. +// +// 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 UI_FONT_HPP_ +#define UI_FONT_HPP_ + +#include +#include +#include + +namespace autoware::visualization::trajectory_kinematics_rviz_plugin +{ + +/// @brief Returns a cached UI font: first match among preferred sans families, 13px, demibold, full +/// hinting. +inline QFont makePanelUiFont() +{ + static const QFont kFont = []() { + static const QStringList kPreferredFamilies = { + QStringLiteral("Segoe UI"), QStringLiteral("Ubuntu"), QStringLiteral("Noto Sans"), + QStringLiteral("Liberation Sans"), QStringLiteral("DejaVu Sans"), + }; + + const QStringList families = QFontDatabase().families(); + QString chosen; + for (const QString & fam : kPreferredFamilies) { + if (families.contains(fam)) { + chosen = fam; + break; + } + } + + QFont font; + if (!chosen.isEmpty()) { + font.setFamily(chosen); + } + font.setPixelSize(13); + font.setWeight(QFont::DemiBold); + font.setStyleHint(QFont::SansSerif, QFont::PreferAntialias); + font.setHintingPreference(QFont::PreferFullHinting); + return font; + }(); + return kFont; +} + +} // namespace autoware::visualization::trajectory_kinematics_rviz_plugin + +#endif // UI_FONT_HPP_