feat(autoware_trajectory): add temporal trajectory class by yhisaki · Pull Request #998 · autowarefoundation/autoware_core

yhisaki · 2026-03-26T09:42:53Z

Description

Before this PR, autoware_trajectory only provided spatial trajectory utilities, and it did not support consistent time-parameterized trajectory evaluation, cropping, or stopline editing on a TrajectoryPoint sequence.

This PR adds and refactors temporal trajectory support in autoware_trajectory.

Add TemporalTrajectory and the internal TimeDistanceMapping utility to evaluate a trajectory from both time and distance.
Add temporal operations such as crop_time, crop_distance, set_stopline, set_time_offset, time_to_distance, and distance_to_time, and make zero-length crop edge cases valid.
Extend existing utilities to work with temporal trajectories, including add_offset, crossed, find_intervals, and pretty_build_temporal.
Improve robustness around duplicate timestamps, duplicate spatial/time bases during interpolation, stop-interval lookup, and ambiguous stop-distance to time conversion behavior.
Clarify and harden duplicate-point handling by keeping trajectory bases strictly increasing with epsilon-sized increments during build, while still allowing interpolators such as Linear, CubicSpline, AkimaSpline, and SphericalLinear to remove near-duplicate bases using k_points_minimum_dist_threshold before interpolation.
Add examples and tests covering temporal trajectory construction, cropping, offsetting, stopline handling, time rebasing, crossing queries, and utility integration.
Make plot generation in plotting-enabled tests opt-in via ENABLE_TEST_PLOT=1.

Execution Results of examples

example_temporal_trajectory.cpp

example_temporal_crop.cpp

example_temporal_curvature.cpp

Notes for reviewers

Please review this PR in the following order:

include/autoware/trajectory/temporal_trajectory.hpp and src/temporal_trajectory.cpp
include/autoware/trajectory/detail/time_distance_mapping.hpp and src/detail/time_distance_mapping.cpp
Utility integrations in utils/add_offset.hpp, utils/crop.hpp, utils/crossed.hpp, utils/find_intervals.hpp, utils/pretty_build.hpp, utils/set_stopline.hpp, and utils/set_time_offset.hpp
Coverage in test/test_temporal_trajectory.cpp, test/test_utils_temporal_trajectory.cpp, test/test_add_offset.cpp, and test/test_crossed.cpp
README / example updates and the opt-in plot test change in test/test_main.cpp

Effects on system behavior

autoware::experimental::trajectory::TemporalTrajectory becomes a new public API for time-parameterized TrajectoryPoint handling.
add_offset now supports TemporalTrajectory in addition to spatial Trajectory<PointType>.
crossed now supports TemporalTrajectory and returns TimeDistancePair values.
find_intervals now supports temporal queries and returns TemporalInterval with both distance and time ranges.
pretty_build_temporal becomes available as a convenience wrapper for temporal trajectory construction.
set_time_offset becomes available to shift the public time axis without rebuilding the spatial trajectory.
distance_to_time now supports disambiguating stop plateaus with return_end_time; by default it returns the first stop time, and with true it returns the stop end time.
Duplicate spatial points and duplicate timestamps are now handled in two stages: trajectory/time bases are first sanitized to stay strictly increasing, and interpolators then remove near-duplicate bases when required for stable interpolation.
Out-of-range time_to_distance, distance_to_time, compute_from_time, and compute_from_distance queries throw std::out_of_range.
crossed, crossed_with_constraint, and crossed_with_polygon no longer take optional search-range arguments; callers should crop the trajectory first when they want a limited search range.
add_offset no longer adjusts velocity fields when applying a geometric offset.
Plot-generating test code now stays inactive unless ENABLE_TEST_PLOT=1 is set.

github-actions · 2026-03-26T09:43:06Z

Thank you for contributing to the Autoware project!

🚧 If your pull request is in progress, switch it to draft mode.

Please ensure:

You've checked our contribution guidelines.
Your PR follows our pull request guidelines.
All required CI checks pass before marking the PR ready for review.

…calculation Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

Apply trajectory offsets with full quaternion rotation instead of yaw only so roll, pitch, and vertical offsets are respected. Update the utility documentation and extend tests to cover pitched and fully oriented trajectory points. Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

Simplify the offset test helpers by using a single trajectory point factory that covers position and full orientation inputs. This keeps the test setup consistent across yaw-only and 3D pose cases while preserving coverage for combined offset behavior. Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

Trim the example to focus on generating and visualizing offset trajectories from position-only points. Use trajectory direction alignment to derive orientations instead of manually assigning yaw and remove unused velocity diagnostics to keep the sample easier to follow. Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

…istance management features Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

…tionality Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

Extract time-distance mapping into a dedicated helper and use it for time range cropping, stopline edits, and distance-to-time queries. This allows temporal trajectories to build from single-point inputs and from points with duplicate timestamps by extending time bases instead of failing. It also simplifies restore behavior and adds an example mode to plot restored underlying points. Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

Add add_offset() utility function to compute trajectory offset from base_link by a fixed vehicle-frame offset. - Add position offset using pose orientation (or trajectory azimuth for Point) - Adjust velocity for rotational motion using heading_rate_rps - Add unit tests for position and velocity offset - Add example with circular arc trajectory and plotting Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

…tions and related tests Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

Add crop_distance() method to crop the trajectory by distance interval instead of time interval. - Add crop_distance(start_distance, length) declaration to header - Add implementation that converts distance to time for time-distance mapping - Crop spatial trajectory and update time range accordingly Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

…of returning nullopt Change TimeDistanceMapping::time_at_distance() to clamp the input distance to the valid range instead of returning std::nullopt when the distance is outside the visible trajectory range. - Clamp distance to [start_distance(), end_distance()] - Update documentation to reflect new behavior Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

…le instead of optional Change TimeDistanceMapping::time_at_distance() and TemporalTrajectory::distance_to_time() to return double instead of std::optional<double>. The distance is now clamped to the valid range instead of returning nullopt when out of bounds. - Update return type from std::optional<double> to double - Add distance clamping in time_at_distance() - Update all callers to use non-optional return value Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

Update test_temporal_trajectory.cpp to use the new double return type for distance_to_time() instead of std::optional<double>. Add new test cases: - distance_to_time_clamps_below_range - distance_to_time_clamps_above_range - compute_from_distance_at_boundaries - compute_from_time_at_boundaries - restore_after_crop_time Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

…ory::add_offset Use compute_from_distance() instead of spatial_traj.compute() to preserve the time_from_start field when building the offset trajectory. This ensures the duration is correctly maintained after offset. Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

…ting in tests Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

…rajectory processing Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

…additional scenarios and refactor existing tests Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

…enhance tests for duplicate points handling Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

…s to support end time retrieval and update related tests Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

…d accuracy in restored trajectory validation Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

…tion and improve error handling for duration and length parameters Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

…nhance curvature calculation for improved accuracy Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

- Updated various functions to replace the use of `k_points_minimum_dist_threshold` with `k_epsilon_distance` for better precision in distance comparisons. - Enhanced the `is_almost_same` function to include quaternion comparisons using `tf2` for improved accuracy in orientation checks. - Modified the `pretty_build_temporal` function to utilize `k_epsilon_distance` for trajectory length validation. - Adjusted test cases across multiple files to reflect the new threshold constants and ensure consistency in distance checks. - Introduced `is_almost_same` as a parameter in the `crossed_with_constraint_impl` function to allow for customizable comparison logic. Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

Kotakku · 2026-04-21T09:54:23Z

+  friend TemporalTrajectory set_stopline(TemporalTrajectory trajectory, double arc_length);
+  friend TemporalTrajectory set_stopline(
+    TemporalTrajectory trajectory, double arc_length, double duration);


The two set_stopline overloads share a name but behave quite differently, which is confusing. I'd recommend splitting them into distinct function names to avoid confusion.

Kotakku · 2026-04-21T10:01:08Z

+  /** @brief Return the user-configured time offset in seconds. */
+  [[nodiscard]] double time_offset() const;
+
+  /** @brief Return the stored time bases. */


The current docstring "Return the stored time bases." is slightly misleading.
the function actually returns the cropped/offset-adjusted bases, not the raw stored ones. Suggest aligning with the spatial Trajectory<T>::get_underlying_bases() wording, e.g. "Get the underlying time bases of the trajectory".

…t_stop_duration for clarity Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

…precated for clarity Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

…_minimum_dist_threshold Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

yhisaki force-pushed the feat/temporal-trajectory branch 10 times, most recently from 33aba16 to 4bb9904 Compare April 2, 2026 09:19

yhisaki force-pushed the feat/temporal-trajectory branch 2 times, most recently from b912720 to 87b17db Compare April 9, 2026 04:07

yhisaki added 17 commits April 10, 2026 17:45

test(autoware_trajectory): add test for single point combined offset …

aded1b9

…calculation Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

docs(autoware_trajectory): add add_offset image to README

a72e010

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

feat(autoware_trajectory): add temporal trajectory class

d4fd172

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

feat(temporal_trajectory): enhance TemporalTrajectory with time and d…

13d0478

…istance management features Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

refactor(trajectory): replace array indexing with at() for safer access

c0b5735

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

feat(autoware_trajectory): add time-distance mapping and related func…

c19e4e3

…tionality Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

refactor(autoware_trajectory): remove unused velocity adjustment func…

231e29a

…tions and related tests Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

yhisaki and others added 7 commits April 20, 2026 16:16

feat(autoware_trajectory): add test_plot utility for conditional plot…

9d81f71

…ting in tests Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

feat(autoware_trajectory): document handling of duplicate points in t…

d27be83

…rajectory processing Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

feat(autoware_trajectory): enhance tests for TemporalTrajectory with …

1d876ef

…additional scenarios and refactor existing tests Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

feat(autoware_trajectory): improve distance-to-time calculations and …

5e54710

…enhance tests for duplicate points handling Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

feat(autoware_trajectory): enhance distance-to-time conversion method…

8c369be

…s to support end time retrieval and update related tests Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

feat(autoware_trajectory): refactor CropTemporal tests for clarity an…

67f9874

…d accuracy in restored trajectory validation Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

Merge branch 'main' into feat/temporal-trajectory

c40eb2d

yhisaki marked this pull request as ready for review April 20, 2026 11:38

yhisaki requested review from a team, Kotakku, PanConChicharron, kosuke55, mkquda, sasakisasaki, soblin and takayuki5168 as code owners April 20, 2026 11:38

yhisaki and others added 5 commits April 21, 2026 11:55

feat(autoware_trajectory): remove unused includes to streamline code

ab962a8

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

feat(autoware_trajectory): enhance distance-to-time mapping documenta…

c6754eb

…tion and improve error handling for duration and length parameters Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

feat(autoware_trajectory): simplify temporal trajectory builder and e…

c3f5f7c

…nhance curvature calculation for improved accuracy Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

Merge branch 'main' into feat/temporal-trajectory

47fde05

yhisaki added the run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) label Apr 21, 2026

Kotakku reviewed Apr 21, 2026

View reviewed changes

yhisaki added 3 commits April 21, 2026 19:13

feat(autoware_trajectory): rename set_stopline with duration to inser…

d6efeed

…t_stop_duration for clarity Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

feat(autoware_trajectory): mark k_points_minimum_dist_threshold as de…

c9258da

…precated for clarity Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

feat(autoware_trajectory): update deprecation annotation for k_points…

0b20975

…_minimum_dist_threshold Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

yhisaki marked this pull request as draft April 21, 2026 11:18

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

feat(autoware_trajectory): add temporal trajectory class#998

feat(autoware_trajectory): add temporal trajectory class#998
yhisaki wants to merge 60 commits intoautowarefoundation:mainfrom
yhisaki:feat/temporal-trajectory

yhisaki commented Mar 26, 2026 •

edited

Loading

Uh oh!

github-actions Bot commented Mar 26, 2026 •

edited

Loading

Uh oh!

Kotakku Apr 21, 2026

Uh oh!

Kotakku Apr 21, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

Conversation

yhisaki commented Mar 26, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Description

Execution Results of examples

Related links

Notes for reviewers

Effects on system behavior

Uh oh!

github-actions Bot commented Mar 26, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Kotakku Apr 21, 2026

Choose a reason for hiding this comment

Uh oh!

Kotakku Apr 21, 2026

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

yhisaki commented Mar 26, 2026 •

edited

Loading

github-actions Bot commented Mar 26, 2026 •

edited

Loading