feat: autoware trajectory kinematics rviz (autowarefoundation#392) (#39)

* feat: new panel trajectory kinematic rviz

* fix

* chore: fix cspell

* chore: font name Segoe should be allowed

---------

Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com>

Author: Owen-Liuyuxuan

.cspell-partial.json

@@ -29,6 +29,7 @@
     "usecols",
     "velb",
     "viridis",
-    "zdata"
+    "zdata",
+    "Segoe"
   ]
 }

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
+)

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.

common/autoware_trajectory_kinematics_rviz_plugin/icons/classes/TrajectoryKinematicsPanel.png

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autoware_trajectory_kinematics_rviz_plugin</name>
+  <version>0.46.0</version>
+  <description>RViz2 panel to plot trajectory kinematics (velocity, acceleration, curvature, lateral acceleration) from Trajectory or ScoredCandidateTrajectories topics.</description>
+  <maintainer email="taiki.tanaka@tier4.jp">Taiki Tanaka</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_internal_planning_msgs</depend>
+  <depend>autoware_planning_msgs</depend>
+  <depend>libqt5-charts-dev</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rviz_common</depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

common/autoware_trajectory_kinematics_rviz_plugin/package.xml

@@ -0,0 +1,8 @@
+<library path="autoware_trajectory_kinematics_rviz_plugin">
+  <class
+    name="autoware_trajectory_kinematics_rviz_plugin/TrajectoryKinematicsPanel"
+    type="autoware::visualization::trajectory_kinematics_rviz_plugin::TrajectoryKinematicsPanel"
+    base_class_type="rviz_common::Panel">
+    <description>Plot kinematics along autoware_planning_msgs/Trajectory or autoware_internal_planning_msgs/ScoredCandidateTrajectories (multi-series comparison).</description>
+  </class>
+</library>

common/autoware_trajectory_kinematics_rviz_plugin/plugins/plugin_description.xml

@@ -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 <cstdint>
+#include <string>
+#include <vector>
+
+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<SeriesPoint> 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<AxisDefinition> & xAxisDefinitions()
+{
+  static const std::vector<AxisDefinition> 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<AxisDefinition> & yAxisDefinitions()
+{
+  static const std::vector<AxisDefinition> 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_

common/autoware_trajectory_kinematics_rviz_plugin/src/kinematics_types.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 <QColor>
+
+#include <string>
+
+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_