feat(autoware_trajectory_optimizer): optimizer semantic speed tracker by danielsanchezaran · Pull Request #12289 · autowarefoundation/autoware_universe

danielsanchezaran · 2026-03-12T10:37:48Z

Description

This PR introduces a SemanticSpeedTracker to the autoware_trajectory_optimizer that detects
deceleration-to-stop zones in the input trajectory and uses that information to preserve the
planner's intended velocity profile through the QP path smoother.

Problem

The QP smoother optimizes trajectory geometry (x, y positions) and then recalculates velocities
from the smoothed geometry using finite differences (v[i] = dist(p[i], p[i-1]) / dt). In
deceleration zones approaching a stop, the planner produces closely-spaced points with near-zero
velocities. The geometric recalculation and subsequent moving-average smoothing replace this
intended profile with artificially elevated velocities (typically ~0.6 m/s at the stop point),
preventing the vehicle from stopping correctly when downstream velocity optimization is enabled.

Solution

Stop approach detection (TrajectoryPointFixer): A two-stage detection pipeline identifies
stop approach zones:

Geometric cluster detection: consecutive points closer than min_dist_to_resample_m with
decreasing velocity toward the endpoint are marked as stop candidates.
Velocity-based fallback (detect_velocity_based_stop): for constant-spacing trajectories
(e.g. after Akima resampling) where no geometric clusters exist, scans the velocity profile
for the first decelerating point below stop_detection_velocity_threshold_mps.
build_stop_approach_ranges: processes all candidates, confirms velocity direction (stop
approach vs. take-off), traces back the deceleration onset, and populates slow_down_ranges
with arc length coordinates for downstream remap support.

Velocity preservation (TrajectoryQPSmoother): After geometric velocity recalculation and
moving-average smoothing, the planner's original velocity profile is restored for all points
within each detected stop approach range. The stop point is forced to zero unconditionally. This
gives the downstream jerk smoother a correct v_ref profile to plan against.

Stop position pinning (TrajectoryQPSmoother): The position of each detected stop point is
added as a hard equality constraint in the OSQP problem, ensuring the path smoother cannot
displace the stop position regardless of smoothness weight settings.

Index remapping (TrajectorySplineSmoother): After Akima resampling changes the point count,
remap_to_trajectory() uses stored arc length coordinates to update slow_down_ranges indices
to the new trajectory, keeping the semantic information valid for future plugins.

How was this PR tested?

Tested in simulation with trajectories containing deceleration-to-stop sequences. With both the
QP smoother and velocity optimizer (jerk smoother) enabled, the output trajectory now correctly
reaches zero velocity at the detected stop point, matching the behavior observed with the QP
smoother disabled.

The video shows how the optimizer output (blue) correctly tracks the stop velocity.

Screencast.from.2026.03.12.19.27.14.webm

Notes for reviewers

The SemanticSpeedTracker is passed by reference through all plugin optimize_trajectory()
calls. Plugins that do not use it mark the parameter [[maybe_unused]].
The detection threshold stop_detection_velocity_threshold_mps (default: 0.3 m/s) controls
the velocity-based fallback sensitivity. It should be set above the maximum velocity expected
in the stop zone but below normal driving velocities.
The slow_down_ranges arc length coordinates (start_s_m, end_s_m) are populated at
detection time and survive Akima resampling via remap_to_trajectory(), making the semantic
information available to future downstream plugins.

Interface changes

ROS Parameter Changes

Additions and removals

Change type	Parameter Name	Type	Default Value	Description
Added	`trajectory_point_fixer.stop_detection_velocity_threshold_mps`	`double`	`0.3`	Velocity threshold for stop detection in deceleration zone identification [m/s]

Effects on system behavior

When use_trajectory_point_fixer: true and use_qp_smoother: true, the output trajectory
velocity profile in deceleration-to-stop zones now matches the planner's intended profile.
The stop point velocity is explicitly forced to zero, which allows the downstream jerk-filtered
velocity smoother (smooth_velocities: true) to correctly plan a kinematically feasible
deceleration to a full stop.

There is no behavior change when the QP smoother is disabled or when no stop approach is
detected in the trajectory.

github-actions · 2026-03-12T10:38: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.

Copilot

Pull request overview

This PR adds a cross-plugin “semantic speed” mechanism to preserve deceleration-to-stop intent through geometry-only smoothing, by detecting stop-approach zones early and reusing that information in later plugins (notably the QP smoother and Akima resampling).

Changes:

Introduce SemanticSpeedTracker and thread it through the trajectory optimizer plugin interface to share stop-approach semantics across plugins.
Detect stop-approach ranges in TrajectoryPointFixer utilities (cluster-based + velocity-profile fallback) and remap those ranges after Akima resampling.
In TrajectoryQPSmoother, (a) hard-pin detected stop-point positions in OSQP constraints and (b) restore the original velocity profile within detected stop-approach ranges (forcing the stop point to 0 m/s).

Reviewed changes

Copilot reviewed 25 out of 25 changed files in this pull request and generated 9 comments.

Show a summary per file

File	Description
planning/autoware_trajectory_optimizer/tests/test_trajectory_point_fixer_utils.cpp	Updates tests to pass `SemanticSpeedTracker` into updated utility APIs.
planning/autoware_trajectory_optimizer/src/trajectory_optimizer_plugins/trajectory_velocity_optimizer.cpp	Updates plugin API to accept `SemanticSpeedTracker&` (unused).
planning/autoware_trajectory_optimizer/src/trajectory_optimizer_plugins/trajectory_spline_smoother.cpp	Computes arc length and remaps semantic ranges after Akima resampling.
planning/autoware_trajectory_optimizer/src/trajectory_optimizer_plugins/trajectory_qp_smoother.cpp	Uses semantic ranges to pin stop positions and to restore stop-approach velocities post-smoothing.
planning/autoware_trajectory_optimizer/src/trajectory_optimizer_plugins/trajectory_point_fixer.cpp	Runs stop-approach detection pipeline and populates semantic ranges.
planning/autoware_trajectory_optimizer/src/trajectory_optimizer_plugins/trajectory_mpt_optimizer.cpp	Updates plugin API to accept `SemanticSpeedTracker&` (unused).
planning/autoware_trajectory_optimizer/src/trajectory_optimizer_plugins/trajectory_kinematic_feasibility_enforcer.cpp	Updates plugin API to accept `SemanticSpeedTracker&` (unused).
planning/autoware_trajectory_optimizer/src/trajectory_optimizer_plugins/trajectory_extender.cpp	Updates plugin API to accept `SemanticSpeedTracker&` (unused).
planning/autoware_trajectory_optimizer/src/trajectory_optimizer_plugins/trajectory_eb_smoother_optimizer.cpp	Updates plugin API to accept `SemanticSpeedTracker&` (unused).
planning/autoware_trajectory_optimizer/src/trajectory_optimizer_plugins/plugin_utils/trajectory_point_fixer_utils.cpp	Implements stop candidate detection + stop-approach range building; updates proximity utilities to write semantic candidates.
planning/autoware_trajectory_optimizer/src/trajectory_optimizer.cpp	Instantiates one `SemanticSpeedTracker` per candidate trajectory and passes it through plugins.
planning/autoware_trajectory_optimizer/schema/trajectory_optimizer.schema.json	Adds schema parameter for velocity-based stop detection threshold.
planning/autoware_trajectory_optimizer/include/autoware/trajectory_optimizer/trajectory_optimizer_structs.hpp	Adds `SemanticSpeedTracker` and slow-down range representation + remap logic.
planning/autoware_trajectory_optimizer/include/autoware/trajectory_optimizer/trajectory_optimizer_plugins/trajectory_velocity_optimizer.hpp	Updates plugin interface signature for semantic tracker.
planning/autoware_trajectory_optimizer/include/autoware/trajectory_optimizer/trajectory_optimizer_plugins/trajectory_spline_smoother.hpp	Updates plugin interface signature for semantic tracker.
planning/autoware_trajectory_optimizer/include/autoware/trajectory_optimizer/trajectory_optimizer_plugins/trajectory_qp_smoother.hpp	Updates APIs/docs to pass semantic tracker into QP solve/matrix prep/post-process.
planning/autoware_trajectory_optimizer/include/autoware/trajectory_optimizer/trajectory_optimizer_plugins/trajectory_point_fixer.hpp	Adds new parameter and updates plugin interface signature for semantic tracker.
planning/autoware_trajectory_optimizer/include/autoware/trajectory_optimizer/trajectory_optimizer_plugins/trajectory_optimizer_plugin_base.hpp	Changes base plugin API to include `SemanticSpeedTracker&`.
planning/autoware_trajectory_optimizer/include/autoware/trajectory_optimizer/trajectory_optimizer_plugins/trajectory_mpt_optimizer.hpp	Updates plugin interface signature for semantic tracker.
planning/autoware_trajectory_optimizer/include/autoware/trajectory_optimizer/trajectory_optimizer_plugins/trajectory_kinematic_feasibility_enforcer.hpp	Updates plugin interface signature for semantic tracker.
planning/autoware_trajectory_optimizer/include/autoware/trajectory_optimizer/trajectory_optimizer_plugins/trajectory_extender.hpp	Updates plugin interface signature for semantic tracker.
planning/autoware_trajectory_optimizer/include/autoware/trajectory_optimizer/trajectory_optimizer_plugins/trajectory_eb_smoother_optimizer.hpp	Updates plugin interface signature for semantic tracker.
planning/autoware_trajectory_optimizer/include/autoware/trajectory_optimizer/trajectory_optimizer_plugins/plugin_utils/trajectory_point_fixer_utils.hpp	Updates utility signatures and adds new stop detection/range builder declarations.
planning/autoware_trajectory_optimizer/config/plugins/trajectory_qp_smoother.param.yaml	Minor formatting cleanup in plugin params file.
planning/autoware_trajectory_optimizer/config/plugins/trajectory_point_fixer.param.yaml	Adds new stop detection threshold parameter to point fixer config.

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

Copilot · 2026-03-12T10:43:56Z

+  trajectory_point_fixer_utils::detect_velocity_based_stop(
+    traj_points, semantic_speed_tracker, fixer_params_.stop_detection_velocity_threshold_mps);
+
+  trajectory_point_fixer_utils::build_stop_approach_ranges(traj_points, semantic_speed_tracker);


detect_velocity_based_stop() is called before build_stop_approach_ranges(), and it short-circuits whenever stop_point_candidates is non-empty. If earlier steps add only false-positive candidates (which build_stop_approach_ranges() then filters out), the velocity-based fallback never runs and no stop approach ranges are produced. Consider running the fallback based on the resulting slow_down_ranges (e.g., build ranges from geometric candidates first, then if slow_down_ranges is empty run velocity-based detection and rebuild).

Suggested change

trajectory_point_fixer_utils::detect_velocity_based_stop(

traj_points, semantic_speed_tracker, fixer_params_.stop_detection_velocity_threshold_mps);

trajectory_point_fixer_utils::build_stop_approach_ranges(traj_points, semantic_speed_tracker);

// First, build stop approach ranges from existing stop candidates (e.g., geometric/semantic).

trajectory_point_fixer_utils::build_stop_approach_ranges(traj_points, semantic_speed_tracker);

// If no slow down ranges were produced, fall back to velocity-based stop detection and rebuild.

if (semantic_speed_tracker.slow_down_ranges.empty()) {

trajectory_point_fixer_utils::detect_velocity_based_stop(

traj_points, semantic_speed_tracker, fixer_params_.stop_detection_velocity_threshold_mps);

trajectory_point_fixer_utils::build_stop_approach_ranges(traj_points, semantic_speed_tracker);

}

Copilot · 2026-03-12T10:43:56Z

+  // Collect stop point indices from slowdown ranges, skipping any already covered by
+  // the start/end hard-constraint windows to avoid duplicate constraints.
+  std::vector<int> stop_constraint_indices;
+  for (const auto & range : semantic_speed_tracker.get_slow_down_ranges()) {
+    const int idx = static_cast<int>(range.end_index);
+    const bool in_start_window = idx < num_points_start;
+    const bool in_end_window = idx >= N - num_points_end;
+    if (!in_start_window && !in_end_window) {
+      stop_constraint_indices.push_back(idx);
+    }
+  }


stop_constraint_indices can contain duplicates (e.g., overlapping slowdown ranges or repeated candidates), which will add duplicate equality rows to A. This unnecessarily increases the OSQP problem size and can degrade solve time. Consider validating indices are within [0, N) and deduplicating them (e.g., sort + unique) before computing num_constraints and filling constraints.

Copilot · 2026-03-12T10:43:56Z

+void detect_velocity_based_stop(
+  const TrajectoryPoints & traj_points, SemanticSpeedTracker & semantic_speed_tracker,
+  const double stop_velocity_threshold_mps)
+{
+  if (!semantic_speed_tracker.stop_point_candidates.empty()) {
+    return;
+  }
+
+  const float threshold = static_cast<float>(stop_velocity_threshold_mps);
+
+  for (size_t i = 1; i < traj_points.size(); ++i) {
+    const float v = traj_points[i].longitudinal_velocity_mps;
+    const float v_prev = traj_points[i - 1].longitudinal_velocity_mps;
+    if (std::abs(v) < threshold && v < v_prev) {
+      semantic_speed_tracker.stop_point_candidates.push_back(i);
+      break;
+    }
+  }
+}
+
+void build_stop_approach_ranges(
+  const TrajectoryPoints & traj_points, SemanticSpeedTracker & semantic_speed_tracker)
+{
+  const std::vector<size_t> stop_pts = semantic_speed_tracker.stop_point_candidates;
+  semantic_speed_tracker.clear_stop_approaches();
+
+  std::vector<double> cumulative_s(traj_points.size(), 0.0);
+  for (size_t i = 1; i < traj_points.size(); ++i) {
+    cumulative_s[i] = cumulative_s[i - 1] +
+                      autoware_utils_geometry::calc_distance2d(traj_points[i - 1], traj_points[i]);
+  }
+
+  for (const size_t stop_idx : stop_pts) {
+    if (stop_idx == 0 || stop_idx >= traj_points.size()) {
+      continue;
+    }
+    // Velocity is decreasing toward this point → stop approach.
+    // Velocity is increasing toward this point → take-off, skip.
+    if (
+      traj_points[stop_idx].longitudinal_velocity_mps >=
+      traj_points[stop_idx - 1].longitudinal_velocity_mps) {
+      continue;
+    }
+
+    size_t decel_start = stop_idx;
+    while (decel_start > 0 && std::abs(traj_points[decel_start - 1].longitudinal_velocity_mps) >
+                                std::abs(traj_points[decel_start].longitudinal_velocity_mps)) {
+      --decel_start;
+    }
+
+    semantic_speed_tracker.add_stop_approach(
+      {decel_start, stop_idx, cumulative_s[decel_start], cumulative_s[stop_idx]});
  }
 }


New stop-approach detection logic (detect_velocity_based_stop / build_stop_approach_ranges) is introduced here, but the existing unit tests for trajectory_point_fixer_utils don’t cover these behaviors. Adding targeted tests (forward + reverse, stop approach vs take-off, and constant-spacing trajectories) would help prevent regressions since these ranges now affect both velocity preservation and hard constraints downstream.

Copilot · 2026-03-12T10:43:56Z

+  for (size_t i = 1; i < traj_points.size(); ++i) {
+    const float v = traj_points[i].longitudinal_velocity_mps;
+    const float v_prev = traj_points[i - 1].longitudinal_velocity_mps;
+    if (std::abs(v) < threshold && v < v_prev) {


detect_velocity_based_stop() compares signed velocities (v < v_prev). This fails to detect a deceleration-to-stop in reverse-driving trajectories (e.g., -1.0 → -0.2 increases numerically but slows in magnitude). Use absolute speed comparisons consistently (and/or explicitly handle sign) when deciding whether the vehicle is slowing toward a stop.

Suggested change

if (std::abs(v) < threshold && v < v_prev) {

const float speed = std::abs(v);

const float prev_speed = std::abs(v_prev);

if (speed < threshold && speed < prev_speed) {

Copilot · 2026-03-12T10:43:56Z

+    if (
+      traj_points[stop_idx].longitudinal_velocity_mps >=
+      traj_points[stop_idx - 1].longitudinal_velocity_mps) {


build_stop_approach_ranges() filters take-off vs stop approach using a signed comparison (v[stop_idx] >= v[stop_idx-1]). For reverse driving, this misclassifies a valid stop approach as take-off (e.g., -1.0 → -0.2). Consider basing the stop/take-off decision on absolute speed (and ensuring consistent sign) so reverse trajectories are handled correctly.

Suggested change

if (

traj_points[stop_idx].longitudinal_velocity_mps >=

traj_points[stop_idx - 1].longitudinal_velocity_mps) {

const double v_prev = traj_points[stop_idx - 1].longitudinal_velocity_mps;

const double v_curr = traj_points[stop_idx].longitudinal_velocity_mps;

const bool same_direction = (v_prev * v_curr) > 0.0;

if (same_direction && std::abs(v_curr) >= std::abs(v_prev)) {

Copilot · 2026-03-12T10:43:57Z

  /**
   * @brief Construct matrices for OSQP solver
   * @param input_trajectory Original trajectory for fidelity term
+   * @param semantic_speed_tracker Tracker for velocity-based fidelity weighting
   * @param H Hessian matrix (objective quadratic term)
   * @param A Constraint matrix
   * @param f_vec Gradient vector (objective linear term)
   * @param l_vec Lower bounds for constraints
   * @param u_vec Upper bounds for constraints
   */
  void prepare_osqp_matrices(
-    const TrajectoryPoints & input_trajectory, Eigen::MatrixXd & H, Eigen::MatrixXd & A,
-    std::vector<double> & f_vec, std::vector<double> & l_vec, std::vector<double> & u_vec) const;
+    const TrajectoryPoints & input_trajectory, const SemanticSpeedTracker & semantic_speed_tracker,
+    Eigen::MatrixXd & H, Eigen::MatrixXd & A, std::vector<double> & f_vec,
+    std::vector<double> & l_vec, std::vector<double> & u_vec) const;


The Doxygen comment for prepare_osqp_matrices() says semantic_speed_tracker is used for velocity-based fidelity weighting, but the current implementation uses it for hard stop-point constraints (and velocity preservation happens in post-processing). Update the comment to reflect the actual use to avoid misleading future maintainers.

Copilot · 2026-03-12T10:43:57Z

+              "type": "number",
+              "default": 0.3,
+              "minimum": 0,
+              "description": "Velocity threshold for fallback stop detection: if remove_close_proximity_points did not fire, any slow cluster whose minimum velocity is below this threshold is treated as a stop approach [m/s]"


The schema description for stop_detection_velocity_threshold_mps mentions "if remove_close_proximity_points did not fire" and "slow cluster" minimum velocity, but the code path uses a velocity-profile scan (detect_velocity_based_stop) and also uses the threshold in the resampling-based detector. Please align the description with the implemented behavior so parameter intent/configuration is clear.

Suggested change

"description": "Velocity threshold for fallback stop detection: if remove_close_proximity_points did not fire, any slow cluster whose minimum velocity is below this threshold is treated as a stop approach [m/s]"

"description": "Velocity threshold used by the stop-detection logic: trajectory points or segments whose velocity falls below this value are treated as part of a stop or stop approach in both the velocity-profile-based and resampling-based detectors [m/s]"

codecov · 2026-03-12T10:46:56Z

Codecov Report

❌ Patch coverage is 16.66667% with 75 lines in your changes missing coverage. Please review.
✅ Project coverage is 18.54%. Comparing base (d4106f8) to head (6de5e56).
⚠️ Report is 2 commits behind head on main.

Files with missing lines	Patch %	Lines
...ctory_optimizer_plugins/trajectory_qp_smoother.cpp	0.00%	36 Missing ⚠️
...gins/plugin_utils/trajectory_point_fixer_utils.cpp	16.12%	24 Missing and 2 partials ⚠️
...ctory_optimizer_plugins/trajectory_point_fixer.cpp	0.00%	9 Missing ⚠️
...jectory_optimizer/trajectory_optimizer_structs.hpp	0.00%	2 Missing ⚠️
..._trajectory_optimizer/src/trajectory_optimizer.cpp	0.00%	2 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main   #12289      +/-   ##
==========================================
+ Coverage   18.36%   18.54%   +0.17%     
==========================================
  Files        1849     1850       +1     
  Lines      128005   128285     +280     
  Branches    45554    45618      +64     
==========================================
+ Hits        23507    23788     +281     
- Misses      85042    85280     +238     
+ Partials    19456    19217     -239

Flag	Coverage Δ		*Carryforward flag
daily	`20.76% <ø> (+<0.01%)`	⬆️	Carriedforward from 3f3bc57
daily-cuda	`18.36% <ø> (+<0.01%)`	⬆️	Carriedforward from 3f3bc57
differential	`17.47% <16.66%> (?)`
total-cuda	`18.36% <ø> (+<0.01%)`	⬆️	Carriedforward from 3f3bc57

*This pull request uses carry forward flags. Click here to find out more.

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

🚀 New features to boost your workflow:

❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

Owen-Liuyuxuan

LGTM

I noticed that you have solved some of the problems I noticed in the old branch, including reverse speed stopping.

In general, for now this setting is serving the QP smoother and not other smoothers. Do we need to point this out?

danielsanchezaran · 2026-03-13T01:39:57Z

LGTM

I noticed that you have solved some of the problems I noticed in the old branch, including reverse speed stopping.

In general, for now this setting is serving the QP smoother and not other smoothers. Do we need to point this out?

Thank you for the review. I believe that it is well explained in the updated docs that most of the work is done by the pointfixer and the qp smoother. In the future I would like to integrate it with the velocityOptimizer's smoothing too though.

Owen-Liuyuxuan · 2026-03-13T02:22:27Z

@danielsanchezaran I am not code owner so you may need checks from @go-sakayori

Owen-Liuyuxuan · 2026-03-13T02:44:26Z

@danielsanchezaran Remind me to update autoware_launch config after merge.

…er slow downs Signed-off-by: Daniel <danielsanchezaran@gmail.com>

Fix 1 — Hard constraint on stop positions (prepare_osqp_matrices): Stop point indices from slow_down_ranges are collected, filtered to exclude any already covered by the existing start/end constraint windows (no duplicate rows in A), and added as equality constraints. The stop position is now anchored regardless of weight ratios. Fix 2 — Velocity restore at stop points (post_process_trajectory): After the moving average smoothing pass, the input velocity is restored at each stop point. Since the stop position is now geometrically correct (fix 1), the geometric velocity is already near-zero — this step just trims the residual smoothing blur. The velocity at stop-1 is already small (arc length preservation + anchored stop), so there's no jump. Signed-off-by: Daniel <danielsanchezaran@gmail.com>

…hrough QP smoother - Add SemanticSpeedTracker stop detection via velocity profile (build_stop_approach_ranges) replaces the slow_speed_ranges cross-validation approach with a direct velocity-direction check: stop points are detected first, then classified as stop approaches vs take-offs - QP smoother preserves planner velocity profile in detected deceleration zones by overwriting geometric recalculation with input velocities, forcing stop to zero; moving average handles the transition boundary - Velocity optimizer forces max_velocity=0 at detected stop points when jerk smoothing enabled - Spline smoother remaps semantic tracker indices after resampling via arc length lookup - Add stop_detection_velocity_threshold_mps param (default 0.3 m/s) to trajectory_point_fixer - Remove slow_speed_ranges intermediate representation and its cross-validation machinery Signed-off-by: Daniel <danielsanchezaran@gmail.com>

The arc length preservation soft constraint in the QP is no longer needed now that the velocity profile in deceleration zones is preserved by overwriting geometric recalculation with input velocities. The stop position is still pinned via hard constraints. Signed-off-by: Daniel <danielsanchezaran@gmail.com>

The stop velocity is now correctly set to zero by the QP smoother via the deceleration zone velocity overwrite. The jerk smoother's searchZeroVelocityIndex handles the stop naturally without needing explicit max_velocity_per_point forcing from the semantic tracker. Signed-off-by: Daniel <danielsanchezaran@gmail.com>

- Remove is_stop_point() method (no callers) - Remove duration_s field from SlowSpeedInfo (write-only, never read) - Remove is_stop_approach flag from SlowSpeedInfo (always true, never read) - Rename stop_points to stop_point_candidates to clarify its staging role before build_stop_approach_ranges() processes them into slow_down_ranges Signed-off-by: Daniel <danielsanchezaran@gmail.com>

Signed-off-by: Daniel <danielsanchezaran@gmail.com>

- Reorder detection: build_stop_approach_ranges runs first; velocity-based fallback only fires when the result is empty, preventing false-positive candidates from silencing it - Deduplicate stop_constraint_indices before adding OSQP equality constraints - Use absolute speed comparison in detect_velocity_based_stop and build_stop_approach_ranges (correct for signed velocity conventions) - Fix stale Doxygen on prepare_osqp_matrices: describes actual use (hard stop position constraints) not the old velocity-fidelity weighting - Update schema description for stop_detection_velocity_threshold_mps - Update qp_smoother.md and trajectory_optimizer_specification.md to document stop handling, SemanticSpeedTracker, and revised Known Limitations Signed-off-by: Daniel <danielsanchezaran@gmail.com>

Remove blank lines between list item headers and their descriptions to match the prettier version used in GitHub Actions. Signed-off-by: Daniel <danielsanchezaran@gmail.com>

…toryOptimizerData - Add SemanticSpeedTracker as a member of TrajectoryOptimizerData, removing it from the optimize_trajectory virtual interface. A fresh data instance is created per trajectory in the optimizer loop, so the tracker resets automatically between trajectories. - Fix stale stop-point candidates bug: build_stop_approach_ranges now uses take_stop_point_candidates() which atomically returns and clears the staging area, preventing leftover candidates from the first call polluting the velocity-based fallback second call. - Encapsulate stop_point_candidates_ and slow_down_ranges_ as private members with named accessors (add_stop_candidate, take_stop_point_candidates, get_slow_down_ranges) to enforce the two-stage detection contract. - Remove dead remap_to_trajectory call from TrajectorySplineSmoother: the QP smoother already consumed slow_down_ranges before the spline smoother runs, so the remap had no downstream effect. Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

Copilot

Pull request overview

Copilot reviewed 27 out of 27 changed files in this pull request and generated 3 comments.

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

+            "min_dist_to_resample_m",
+            "stop_detection_velocity_threshold_mps"


+  // Returns the staged stop candidates and clears the staging area atomically.
+  // Used by build_stop_approach_ranges() to consume and reset pending candidates.
+  std::vector<size_t> take_stop_point_candidates()
+  {
+    std::vector<size_t> result;
+    result.swap(stop_point_candidates_);
+    return result;
+  }


+void detect_velocity_based_stop(
+  const TrajectoryPoints & traj_points, SemanticSpeedTracker & semantic_speed_tracker,
+  const double stop_velocity_threshold_mps)
+{
+  const float threshold = static_cast<float>(stop_velocity_threshold_mps);
+
+  for (size_t i = 1; i < traj_points.size(); ++i) {
+    const float speed = std::abs(traj_points[i].longitudinal_velocity_mps);
+    const float prev_speed = std::abs(traj_points[i - 1].longitudinal_velocity_mps);
+    if (speed < threshold && speed < prev_speed) {
+      semantic_speed_tracker.add_stop_candidate(i);
+      break;
+    }
+  }
+}


- Clarify remove_close_proximity_points docstring: close proximity implies low speed at constant dt, so stop candidate registration is intentional. - Remove misleading 'atomically' wording from take_stop_point_candidates comment; the swap is O(1) but not thread-safe. Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

go-sakayori · 2026-03-13T03:03:15Z

      resample_close_points: true     # Enable resampling of close proximity points
      min_dist_to_remove_m: 0.01  # [m] Minimum distance to remove close proximity points
      min_dist_to_resample_m: 0.05   # [m] Minimum distance to merge close proximity points
+      stop_detection_velocity_threshold_mps: 0.3  # [m/s] Velocity threshold for fallback stop detection in slow clusters


memo: There are so many different parameters to detect stopping state in autoware. Ideally they should be aligned. The same value is used for diffusion planner, so it is alright
for now.

…autowarefoundation#12289) * use a semantic speed tracker and update longitudinal weights fro better slow downs Signed-off-by: Daniel <danielsanchezaran@gmail.com> * ● All changes are in. Here's what was implemented: Fix 1 — Hard constraint on stop positions (prepare_osqp_matrices): Stop point indices from slow_down_ranges are collected, filtered to exclude any already covered by the existing start/end constraint windows (no duplicate rows in A), and added as equality constraints. The stop position is now anchored regardless of weight ratios. Fix 2 — Velocity restore at stop points (post_process_trajectory): After the moving average smoothing pass, the input velocity is restored at each stop point. Since the stop position is now geometrically correct (fix 1), the geometric velocity is already near-zero — this step just trims the residual smoothing blur. The velocity at stop-1 is already small (arc length preservation + anchored stop), so there's no jump. Signed-off-by: Daniel <danielsanchezaran@gmail.com> * feat: add semantic stop detection and preserve deceleration profile through QP smoother - Add SemanticSpeedTracker stop detection via velocity profile (build_stop_approach_ranges) replaces the slow_speed_ranges cross-validation approach with a direct velocity-direction check: stop points are detected first, then classified as stop approaches vs take-offs - QP smoother preserves planner velocity profile in detected deceleration zones by overwriting geometric recalculation with input velocities, forcing stop to zero; moving average handles the transition boundary - Velocity optimizer forces max_velocity=0 at detected stop points when jerk smoothing enabled - Spline smoother remaps semantic tracker indices after resampling via arc length lookup - Add stop_detection_velocity_threshold_mps param (default 0.3 m/s) to trajectory_point_fixer - Remove slow_speed_ranges intermediate representation and its cross-validation machinery Signed-off-by: Daniel <danielsanchezaran@gmail.com> * refactor: remove arc length preservation from QP smoother The arc length preservation soft constraint in the QP is no longer needed now that the velocity profile in deceleration zones is preserved by overwriting geometric recalculation with input velocities. The stop position is still pinned via hard constraints. Signed-off-by: Daniel <danielsanchezaran@gmail.com> * refactor: remove use_semantic_stop_points from velocity optimizer The stop velocity is now correctly set to zero by the QP smoother via the deceleration zone velocity overwrite. The jerk smoother's searchZeroVelocityIndex handles the stop naturally without needing explicit max_velocity_per_point forcing from the semantic tracker. Signed-off-by: Daniel <danielsanchezaran@gmail.com> * refactor: clean up SemanticSpeedTracker and SlowSpeedInfo - Remove is_stop_point() method (no callers) - Remove duration_s field from SlowSpeedInfo (write-only, never read) - Remove is_stop_approach flag from SlowSpeedInfo (always true, never read) - Rename stop_points to stop_point_candidates to clarify its staging role before build_stop_approach_ranges() processes them into slow_down_ranges Signed-off-by: Daniel <danielsanchezaran@gmail.com> * undo param change Signed-off-by: Daniel <danielsanchezaran@gmail.com> * fix: address Copilot review feedback on PR 12289 - Reorder detection: build_stop_approach_ranges runs first; velocity-based fallback only fires when the result is empty, preventing false-positive candidates from silencing it - Deduplicate stop_constraint_indices before adding OSQP equality constraints - Use absolute speed comparison in detect_velocity_based_stop and build_stop_approach_ranges (correct for signed velocity conventions) - Fix stale Doxygen on prepare_osqp_matrices: describes actual use (hard stop position constraints) not the old velocity-fidelity weighting - Update schema description for stop_detection_velocity_threshold_mps - Update qp_smoother.md and trajectory_optimizer_specification.md to document stop handling, SemanticSpeedTracker, and revised Known Limitations Signed-off-by: Daniel <danielsanchezaran@gmail.com> * fix: align qp_smoother.md markdown style with CI/CD prettier Remove blank lines between list item headers and their descriptions to match the prettier version used in GitHub Actions. Signed-off-by: Daniel <danielsanchezaran@gmail.com> * refactor(trajectory_optimizer): move SemanticSpeedTracker into TrajectoryOptimizerData - Add SemanticSpeedTracker as a member of TrajectoryOptimizerData, removing it from the optimize_trajectory virtual interface. A fresh data instance is created per trajectory in the optimizer loop, so the tracker resets automatically between trajectories. - Fix stale stop-point candidates bug: build_stop_approach_ranges now uses take_stop_point_candidates() which atomically returns and clears the staging area, preventing leftover candidates from the first call polluting the velocity-based fallback second call. - Encapsulate stop_point_candidates_ and slow_down_ranges_ as private members with named accessors (add_stop_candidate, take_stop_point_candidates, get_slow_down_ranges) to enforce the two-stage detection contract. - Remove dead remap_to_trajectory call from TrajectorySplineSmoother: the QP smoother already consumed slow_down_ranges before the spline smoother runs, so the remap had no downstream effect. Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * fix(trajectory_optimizer): address Copilot review comments - Clarify remove_close_proximity_points docstring: close proximity implies low speed at constant dt, so stop candidate registration is intentional. - Remove misleading 'atomically' wording from take_stop_point_candidates comment; the swap is O(1) but not thread-safe. Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> --------- Signed-off-by: Daniel <danielsanchezaran@gmail.com> Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

…autowarefoundation#12289) (#2778) * use a semantic speed tracker and update longitudinal weights fro better slow downs * ● All changes are in. Here's what was implemented: Fix 1 — Hard constraint on stop positions (prepare_osqp_matrices): Stop point indices from slow_down_ranges are collected, filtered to exclude any already covered by the existing start/end constraint windows (no duplicate rows in A), and added as equality constraints. The stop position is now anchored regardless of weight ratios. Fix 2 — Velocity restore at stop points (post_process_trajectory): After the moving average smoothing pass, the input velocity is restored at each stop point. Since the stop position is now geometrically correct (fix 1), the geometric velocity is already near-zero — this step just trims the residual smoothing blur. The velocity at stop-1 is already small (arc length preservation + anchored stop), so there's no jump. * feat: add semantic stop detection and preserve deceleration profile through QP smoother - Add SemanticSpeedTracker stop detection via velocity profile (build_stop_approach_ranges) replaces the slow_speed_ranges cross-validation approach with a direct velocity-direction check: stop points are detected first, then classified as stop approaches vs take-offs - QP smoother preserves planner velocity profile in detected deceleration zones by overwriting geometric recalculation with input velocities, forcing stop to zero; moving average handles the transition boundary - Velocity optimizer forces max_velocity=0 at detected stop points when jerk smoothing enabled - Spline smoother remaps semantic tracker indices after resampling via arc length lookup - Add stop_detection_velocity_threshold_mps param (default 0.3 m/s) to trajectory_point_fixer - Remove slow_speed_ranges intermediate representation and its cross-validation machinery * refactor: remove arc length preservation from QP smoother The arc length preservation soft constraint in the QP is no longer needed now that the velocity profile in deceleration zones is preserved by overwriting geometric recalculation with input velocities. The stop position is still pinned via hard constraints. * refactor: remove use_semantic_stop_points from velocity optimizer The stop velocity is now correctly set to zero by the QP smoother via the deceleration zone velocity overwrite. The jerk smoother's searchZeroVelocityIndex handles the stop naturally without needing explicit max_velocity_per_point forcing from the semantic tracker. * refactor: clean up SemanticSpeedTracker and SlowSpeedInfo - Remove is_stop_point() method (no callers) - Remove duration_s field from SlowSpeedInfo (write-only, never read) - Remove is_stop_approach flag from SlowSpeedInfo (always true, never read) - Rename stop_points to stop_point_candidates to clarify its staging role before build_stop_approach_ranges() processes them into slow_down_ranges * undo param change * fix: address Copilot review feedback on PR 12289 - Reorder detection: build_stop_approach_ranges runs first; velocity-based fallback only fires when the result is empty, preventing false-positive candidates from silencing it - Deduplicate stop_constraint_indices before adding OSQP equality constraints - Use absolute speed comparison in detect_velocity_based_stop and build_stop_approach_ranges (correct for signed velocity conventions) - Fix stale Doxygen on prepare_osqp_matrices: describes actual use (hard stop position constraints) not the old velocity-fidelity weighting - Update schema description for stop_detection_velocity_threshold_mps - Update qp_smoother.md and trajectory_optimizer_specification.md to document stop handling, SemanticSpeedTracker, and revised Known Limitations * fix: align qp_smoother.md markdown style with CI/CD prettier Remove blank lines between list item headers and their descriptions to match the prettier version used in GitHub Actions. * refactor(trajectory_optimizer): move SemanticSpeedTracker into TrajectoryOptimizerData - Add SemanticSpeedTracker as a member of TrajectoryOptimizerData, removing it from the optimize_trajectory virtual interface. A fresh data instance is created per trajectory in the optimizer loop, so the tracker resets automatically between trajectories. - Fix stale stop-point candidates bug: build_stop_approach_ranges now uses take_stop_point_candidates() which atomically returns and clears the staging area, preventing leftover candidates from the first call polluting the velocity-based fallback second call. - Encapsulate stop_point_candidates_ and slow_down_ranges_ as private members with named accessors (add_stop_candidate, take_stop_point_candidates, get_slow_down_ranges) to enforce the two-stage detection contract. - Remove dead remap_to_trajectory call from TrajectorySplineSmoother: the QP smoother already consumed slow_down_ranges before the spline smoother runs, so the remap had no downstream effect. * fix(trajectory_optimizer): address Copilot review comments - Clarify remove_close_proximity_points docstring: close proximity implies low speed at constant dt, so stop candidate registration is intentional. - Remove misleading 'atomically' wording from take_stop_point_candidates comment; the swap is O(1) but not thread-safe. --------- Signed-off-by: Daniel <danielsanchezaran@gmail.com> Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> Co-authored-by: danielsanchezaran <daniel.sanchez@tier4.jp>

…autowarefoundation#12289) * use a semantic speed tracker and update longitudinal weights fro better slow downs Signed-off-by: Daniel <danielsanchezaran@gmail.com> * ● All changes are in. Here's what was implemented: Fix 1 — Hard constraint on stop positions (prepare_osqp_matrices): Stop point indices from slow_down_ranges are collected, filtered to exclude any already covered by the existing start/end constraint windows (no duplicate rows in A), and added as equality constraints. The stop position is now anchored regardless of weight ratios. Fix 2 — Velocity restore at stop points (post_process_trajectory): After the moving average smoothing pass, the input velocity is restored at each stop point. Since the stop position is now geometrically correct (fix 1), the geometric velocity is already near-zero — this step just trims the residual smoothing blur. The velocity at stop-1 is already small (arc length preservation + anchored stop), so there's no jump. Signed-off-by: Daniel <danielsanchezaran@gmail.com> * feat: add semantic stop detection and preserve deceleration profile through QP smoother - Add SemanticSpeedTracker stop detection via velocity profile (build_stop_approach_ranges) replaces the slow_speed_ranges cross-validation approach with a direct velocity-direction check: stop points are detected first, then classified as stop approaches vs take-offs - QP smoother preserves planner velocity profile in detected deceleration zones by overwriting geometric recalculation with input velocities, forcing stop to zero; moving average handles the transition boundary - Velocity optimizer forces max_velocity=0 at detected stop points when jerk smoothing enabled - Spline smoother remaps semantic tracker indices after resampling via arc length lookup - Add stop_detection_velocity_threshold_mps param (default 0.3 m/s) to trajectory_point_fixer - Remove slow_speed_ranges intermediate representation and its cross-validation machinery Signed-off-by: Daniel <danielsanchezaran@gmail.com> * refactor: remove arc length preservation from QP smoother The arc length preservation soft constraint in the QP is no longer needed now that the velocity profile in deceleration zones is preserved by overwriting geometric recalculation with input velocities. The stop position is still pinned via hard constraints. Signed-off-by: Daniel <danielsanchezaran@gmail.com> * refactor: remove use_semantic_stop_points from velocity optimizer The stop velocity is now correctly set to zero by the QP smoother via the deceleration zone velocity overwrite. The jerk smoother's searchZeroVelocityIndex handles the stop naturally without needing explicit max_velocity_per_point forcing from the semantic tracker. Signed-off-by: Daniel <danielsanchezaran@gmail.com> * refactor: clean up SemanticSpeedTracker and SlowSpeedInfo - Remove is_stop_point() method (no callers) - Remove duration_s field from SlowSpeedInfo (write-only, never read) - Remove is_stop_approach flag from SlowSpeedInfo (always true, never read) - Rename stop_points to stop_point_candidates to clarify its staging role before build_stop_approach_ranges() processes them into slow_down_ranges Signed-off-by: Daniel <danielsanchezaran@gmail.com> * undo param change Signed-off-by: Daniel <danielsanchezaran@gmail.com> * fix: address Copilot review feedback on PR 12289 - Reorder detection: build_stop_approach_ranges runs first; velocity-based fallback only fires when the result is empty, preventing false-positive candidates from silencing it - Deduplicate stop_constraint_indices before adding OSQP equality constraints - Use absolute speed comparison in detect_velocity_based_stop and build_stop_approach_ranges (correct for signed velocity conventions) - Fix stale Doxygen on prepare_osqp_matrices: describes actual use (hard stop position constraints) not the old velocity-fidelity weighting - Update schema description for stop_detection_velocity_threshold_mps - Update qp_smoother.md and trajectory_optimizer_specification.md to document stop handling, SemanticSpeedTracker, and revised Known Limitations Signed-off-by: Daniel <danielsanchezaran@gmail.com> * fix: align qp_smoother.md markdown style with CI/CD prettier Remove blank lines between list item headers and their descriptions to match the prettier version used in GitHub Actions. Signed-off-by: Daniel <danielsanchezaran@gmail.com> * refactor(trajectory_optimizer): move SemanticSpeedTracker into TrajectoryOptimizerData - Add SemanticSpeedTracker as a member of TrajectoryOptimizerData, removing it from the optimize_trajectory virtual interface. A fresh data instance is created per trajectory in the optimizer loop, so the tracker resets automatically between trajectories. - Fix stale stop-point candidates bug: build_stop_approach_ranges now uses take_stop_point_candidates() which atomically returns and clears the staging area, preventing leftover candidates from the first call polluting the velocity-based fallback second call. - Encapsulate stop_point_candidates_ and slow_down_ranges_ as private members with named accessors (add_stop_candidate, take_stop_point_candidates, get_slow_down_ranges) to enforce the two-stage detection contract. - Remove dead remap_to_trajectory call from TrajectorySplineSmoother: the QP smoother already consumed slow_down_ranges before the spline smoother runs, so the remap had no downstream effect. Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * fix(trajectory_optimizer): address Copilot review comments - Clarify remove_close_proximity_points docstring: close proximity implies low speed at constant dt, so stop candidate registration is intentional. - Remove misleading 'atomically' wording from take_stop_point_candidates comment; the swap is O(1) but not thread-safe. Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> --------- Signed-off-by: Daniel <danielsanchezaran@gmail.com> Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* feat: improve parameters and the constraints (autowarefoundation#12271) * feat: improve parameters and the initial constraints Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> * feat: clean up constraints Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> * fix docs and test Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> * enforce end points and test Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> * feat: fix docs and ci Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> * fix test Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> * fix builg Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> * fix spell ci Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> * fix cppcheck Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> * feat: fix cppcheck Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> * feat: fix cppcheck Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --------- Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> * feat(autoware_trajectory_optimizer): force 3 point trajectory (autowarefoundation#12287) * force the optimizer to output a 3 point trajectory with 0 velocity if the output trajectory has less than 3 points Signed-off-by: Daniel <danielsanchezaran@gmail.com> * review comments Signed-off-by: Daniel <danielsanchezaran@gmail.com> * add debug logs Signed-off-by: Daniel <danielsanchezaran@gmail.com> --------- Signed-off-by: Daniel <danielsanchezaran@gmail.com> * feat(autoware_trajectory_optimizer): optimizer semantic speed tracker (autowarefoundation#12289) * use a semantic speed tracker and update longitudinal weights fro better slow downs Signed-off-by: Daniel <danielsanchezaran@gmail.com> * ● All changes are in. Here's what was implemented: Fix 1 — Hard constraint on stop positions (prepare_osqp_matrices): Stop point indices from slow_down_ranges are collected, filtered to exclude any already covered by the existing start/end constraint windows (no duplicate rows in A), and added as equality constraints. The stop position is now anchored regardless of weight ratios. Fix 2 — Velocity restore at stop points (post_process_trajectory): After the moving average smoothing pass, the input velocity is restored at each stop point. Since the stop position is now geometrically correct (fix 1), the geometric velocity is already near-zero — this step just trims the residual smoothing blur. The velocity at stop-1 is already small (arc length preservation + anchored stop), so there's no jump. Signed-off-by: Daniel <danielsanchezaran@gmail.com> * feat: add semantic stop detection and preserve deceleration profile through QP smoother - Add SemanticSpeedTracker stop detection via velocity profile (build_stop_approach_ranges) replaces the slow_speed_ranges cross-validation approach with a direct velocity-direction check: stop points are detected first, then classified as stop approaches vs take-offs - QP smoother preserves planner velocity profile in detected deceleration zones by overwriting geometric recalculation with input velocities, forcing stop to zero; moving average handles the transition boundary - Velocity optimizer forces max_velocity=0 at detected stop points when jerk smoothing enabled - Spline smoother remaps semantic tracker indices after resampling via arc length lookup - Add stop_detection_velocity_threshold_mps param (default 0.3 m/s) to trajectory_point_fixer - Remove slow_speed_ranges intermediate representation and its cross-validation machinery Signed-off-by: Daniel <danielsanchezaran@gmail.com> * refactor: remove arc length preservation from QP smoother The arc length preservation soft constraint in the QP is no longer needed now that the velocity profile in deceleration zones is preserved by overwriting geometric recalculation with input velocities. The stop position is still pinned via hard constraints. Signed-off-by: Daniel <danielsanchezaran@gmail.com> * refactor: remove use_semantic_stop_points from velocity optimizer The stop velocity is now correctly set to zero by the QP smoother via the deceleration zone velocity overwrite. The jerk smoother's searchZeroVelocityIndex handles the stop naturally without needing explicit max_velocity_per_point forcing from the semantic tracker. Signed-off-by: Daniel <danielsanchezaran@gmail.com> * refactor: clean up SemanticSpeedTracker and SlowSpeedInfo - Remove is_stop_point() method (no callers) - Remove duration_s field from SlowSpeedInfo (write-only, never read) - Remove is_stop_approach flag from SlowSpeedInfo (always true, never read) - Rename stop_points to stop_point_candidates to clarify its staging role before build_stop_approach_ranges() processes them into slow_down_ranges Signed-off-by: Daniel <danielsanchezaran@gmail.com> * undo param change Signed-off-by: Daniel <danielsanchezaran@gmail.com> * fix: address Copilot review feedback on PR 12289 - Reorder detection: build_stop_approach_ranges runs first; velocity-based fallback only fires when the result is empty, preventing false-positive candidates from silencing it - Deduplicate stop_constraint_indices before adding OSQP equality constraints - Use absolute speed comparison in detect_velocity_based_stop and build_stop_approach_ranges (correct for signed velocity conventions) - Fix stale Doxygen on prepare_osqp_matrices: describes actual use (hard stop position constraints) not the old velocity-fidelity weighting - Update schema description for stop_detection_velocity_threshold_mps - Update qp_smoother.md and trajectory_optimizer_specification.md to document stop handling, SemanticSpeedTracker, and revised Known Limitations Signed-off-by: Daniel <danielsanchezaran@gmail.com> * fix: align qp_smoother.md markdown style with CI/CD prettier Remove blank lines between list item headers and their descriptions to match the prettier version used in GitHub Actions. Signed-off-by: Daniel <danielsanchezaran@gmail.com> * refactor(trajectory_optimizer): move SemanticSpeedTracker into TrajectoryOptimizerData - Add SemanticSpeedTracker as a member of TrajectoryOptimizerData, removing it from the optimize_trajectory virtual interface. A fresh data instance is created per trajectory in the optimizer loop, so the tracker resets automatically between trajectories. - Fix stale stop-point candidates bug: build_stop_approach_ranges now uses take_stop_point_candidates() which atomically returns and clears the staging area, preventing leftover candidates from the first call polluting the velocity-based fallback second call. - Encapsulate stop_point_candidates_ and slow_down_ranges_ as private members with named accessors (add_stop_candidate, take_stop_point_candidates, get_slow_down_ranges) to enforce the two-stage detection contract. - Remove dead remap_to_trajectory call from TrajectorySplineSmoother: the QP smoother already consumed slow_down_ranges before the spline smoother runs, so the remap had no downstream effect. Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * fix(trajectory_optimizer): address Copilot review comments - Clarify remove_close_proximity_points docstring: close proximity implies low speed at constant dt, so stop candidate registration is intentional. - Remove misleading 'atomically' wording from take_stop_point_candidates comment; the swap is O(1) but not thread-safe. Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> --------- Signed-off-by: Daniel <danielsanchezaran@gmail.com> Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * add modifier to pipeline, disable long stop traejctories by default Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> --------- Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> Signed-off-by: Daniel <danielsanchezaran@gmail.com> Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> Co-authored-by: Yuxuan Liu <619684051@qq.com>

…autowarefoundation#12289) * use a semantic speed tracker and update longitudinal weights fro better slow downs Signed-off-by: Daniel <danielsanchezaran@gmail.com> * ● All changes are in. Here's what was implemented: Fix 1 — Hard constraint on stop positions (prepare_osqp_matrices): Stop point indices from slow_down_ranges are collected, filtered to exclude any already covered by the existing start/end constraint windows (no duplicate rows in A), and added as equality constraints. The stop position is now anchored regardless of weight ratios. Fix 2 — Velocity restore at stop points (post_process_trajectory): After the moving average smoothing pass, the input velocity is restored at each stop point. Since the stop position is now geometrically correct (fix 1), the geometric velocity is already near-zero — this step just trims the residual smoothing blur. The velocity at stop-1 is already small (arc length preservation + anchored stop), so there's no jump. Signed-off-by: Daniel <danielsanchezaran@gmail.com> * feat: add semantic stop detection and preserve deceleration profile through QP smoother - Add SemanticSpeedTracker stop detection via velocity profile (build_stop_approach_ranges) replaces the slow_speed_ranges cross-validation approach with a direct velocity-direction check: stop points are detected first, then classified as stop approaches vs take-offs - QP smoother preserves planner velocity profile in detected deceleration zones by overwriting geometric recalculation with input velocities, forcing stop to zero; moving average handles the transition boundary - Velocity optimizer forces max_velocity=0 at detected stop points when jerk smoothing enabled - Spline smoother remaps semantic tracker indices after resampling via arc length lookup - Add stop_detection_velocity_threshold_mps param (default 0.3 m/s) to trajectory_point_fixer - Remove slow_speed_ranges intermediate representation and its cross-validation machinery Signed-off-by: Daniel <danielsanchezaran@gmail.com> * refactor: remove arc length preservation from QP smoother The arc length preservation soft constraint in the QP is no longer needed now that the velocity profile in deceleration zones is preserved by overwriting geometric recalculation with input velocities. The stop position is still pinned via hard constraints. Signed-off-by: Daniel <danielsanchezaran@gmail.com> * refactor: remove use_semantic_stop_points from velocity optimizer The stop velocity is now correctly set to zero by the QP smoother via the deceleration zone velocity overwrite. The jerk smoother's searchZeroVelocityIndex handles the stop naturally without needing explicit max_velocity_per_point forcing from the semantic tracker. Signed-off-by: Daniel <danielsanchezaran@gmail.com> * refactor: clean up SemanticSpeedTracker and SlowSpeedInfo - Remove is_stop_point() method (no callers) - Remove duration_s field from SlowSpeedInfo (write-only, never read) - Remove is_stop_approach flag from SlowSpeedInfo (always true, never read) - Rename stop_points to stop_point_candidates to clarify its staging role before build_stop_approach_ranges() processes them into slow_down_ranges Signed-off-by: Daniel <danielsanchezaran@gmail.com> * undo param change Signed-off-by: Daniel <danielsanchezaran@gmail.com> * fix: address Copilot review feedback on PR 12289 - Reorder detection: build_stop_approach_ranges runs first; velocity-based fallback only fires when the result is empty, preventing false-positive candidates from silencing it - Deduplicate stop_constraint_indices before adding OSQP equality constraints - Use absolute speed comparison in detect_velocity_based_stop and build_stop_approach_ranges (correct for signed velocity conventions) - Fix stale Doxygen on prepare_osqp_matrices: describes actual use (hard stop position constraints) not the old velocity-fidelity weighting - Update schema description for stop_detection_velocity_threshold_mps - Update qp_smoother.md and trajectory_optimizer_specification.md to document stop handling, SemanticSpeedTracker, and revised Known Limitations Signed-off-by: Daniel <danielsanchezaran@gmail.com> * fix: align qp_smoother.md markdown style with CI/CD prettier Remove blank lines between list item headers and their descriptions to match the prettier version used in GitHub Actions. Signed-off-by: Daniel <danielsanchezaran@gmail.com> * refactor(trajectory_optimizer): move SemanticSpeedTracker into TrajectoryOptimizerData - Add SemanticSpeedTracker as a member of TrajectoryOptimizerData, removing it from the optimize_trajectory virtual interface. A fresh data instance is created per trajectory in the optimizer loop, so the tracker resets automatically between trajectories. - Fix stale stop-point candidates bug: build_stop_approach_ranges now uses take_stop_point_candidates() which atomically returns and clears the staging area, preventing leftover candidates from the first call polluting the velocity-based fallback second call. - Encapsulate stop_point_candidates_ and slow_down_ranges_ as private members with named accessors (add_stop_candidate, take_stop_point_candidates, get_slow_down_ranges) to enforce the two-stage detection contract. - Remove dead remap_to_trajectory call from TrajectorySplineSmoother: the QP smoother already consumed slow_down_ranges before the spline smoother runs, so the remap had no downstream effect. Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * fix(trajectory_optimizer): address Copilot review comments - Clarify remove_close_proximity_points docstring: close proximity implies low speed at constant dt, so stop candidate registration is intentional. - Remove misleading 'atomically' wording from take_stop_point_candidates comment; the swap is O(1) but not thread-safe. Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> --------- Signed-off-by: Daniel <danielsanchezaran@gmail.com> Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

…autowarefoundation/autoware_universe#12289) * use a semantic speed tracker and update longitudinal weights fro better slow downs Signed-off-by: Daniel <danielsanchezaran@gmail.com> * ● All changes are in. Here's what was implemented: Fix 1 — Hard constraint on stop positions (prepare_osqp_matrices): Stop point indices from slow_down_ranges are collected, filtered to exclude any already covered by the existing start/end constraint windows (no duplicate rows in A), and added as equality constraints. The stop position is now anchored regardless of weight ratios. Fix 2 — Velocity restore at stop points (post_process_trajectory): After the moving average smoothing pass, the input velocity is restored at each stop point. Since the stop position is now geometrically correct (fix 1), the geometric velocity is already near-zero — this step just trims the residual smoothing blur. The velocity at stop-1 is already small (arc length preservation + anchored stop), so there's no jump. Signed-off-by: Daniel <danielsanchezaran@gmail.com> * feat: add semantic stop detection and preserve deceleration profile through QP smoother - Add SemanticSpeedTracker stop detection via velocity profile (build_stop_approach_ranges) replaces the slow_speed_ranges cross-validation approach with a direct velocity-direction check: stop points are detected first, then classified as stop approaches vs take-offs - QP smoother preserves planner velocity profile in detected deceleration zones by overwriting geometric recalculation with input velocities, forcing stop to zero; moving average handles the transition boundary - Velocity optimizer forces max_velocity=0 at detected stop points when jerk smoothing enabled - Spline smoother remaps semantic tracker indices after resampling via arc length lookup - Add stop_detection_velocity_threshold_mps param (default 0.3 m/s) to trajectory_point_fixer - Remove slow_speed_ranges intermediate representation and its cross-validation machinery Signed-off-by: Daniel <danielsanchezaran@gmail.com> * refactor: remove arc length preservation from QP smoother The arc length preservation soft constraint in the QP is no longer needed now that the velocity profile in deceleration zones is preserved by overwriting geometric recalculation with input velocities. The stop position is still pinned via hard constraints. Signed-off-by: Daniel <danielsanchezaran@gmail.com> * refactor: remove use_semantic_stop_points from velocity optimizer The stop velocity is now correctly set to zero by the QP smoother via the deceleration zone velocity overwrite. The jerk smoother's searchZeroVelocityIndex handles the stop naturally without needing explicit max_velocity_per_point forcing from the semantic tracker. Signed-off-by: Daniel <danielsanchezaran@gmail.com> * refactor: clean up SemanticSpeedTracker and SlowSpeedInfo - Remove is_stop_point() method (no callers) - Remove duration_s field from SlowSpeedInfo (write-only, never read) - Remove is_stop_approach flag from SlowSpeedInfo (always true, never read) - Rename stop_points to stop_point_candidates to clarify its staging role before build_stop_approach_ranges() processes them into slow_down_ranges Signed-off-by: Daniel <danielsanchezaran@gmail.com> * undo param change Signed-off-by: Daniel <danielsanchezaran@gmail.com> * fix: address Copilot review feedback on PR 12289 - Reorder detection: build_stop_approach_ranges runs first; velocity-based fallback only fires when the result is empty, preventing false-positive candidates from silencing it - Deduplicate stop_constraint_indices before adding OSQP equality constraints - Use absolute speed comparison in detect_velocity_based_stop and build_stop_approach_ranges (correct for signed velocity conventions) - Fix stale Doxygen on prepare_osqp_matrices: describes actual use (hard stop position constraints) not the old velocity-fidelity weighting - Update schema description for stop_detection_velocity_threshold_mps - Update qp_smoother.md and trajectory_optimizer_specification.md to document stop handling, SemanticSpeedTracker, and revised Known Limitations Signed-off-by: Daniel <danielsanchezaran@gmail.com> * fix: align qp_smoother.md markdown style with CI/CD prettier Remove blank lines between list item headers and their descriptions to match the prettier version used in GitHub Actions. Signed-off-by: Daniel <danielsanchezaran@gmail.com> * refactor(trajectory_optimizer): move SemanticSpeedTracker into TrajectoryOptimizerData - Add SemanticSpeedTracker as a member of TrajectoryOptimizerData, removing it from the optimize_trajectory virtual interface. A fresh data instance is created per trajectory in the optimizer loop, so the tracker resets automatically between trajectories. - Fix stale stop-point candidates bug: build_stop_approach_ranges now uses take_stop_point_candidates() which atomically returns and clears the staging area, preventing leftover candidates from the first call polluting the velocity-based fallback second call. - Encapsulate stop_point_candidates_ and slow_down_ranges_ as private members with named accessors (add_stop_candidate, take_stop_point_candidates, get_slow_down_ranges) to enforce the two-stage detection contract. - Remove dead remap_to_trajectory call from TrajectorySplineSmoother: the QP smoother already consumed slow_down_ranges before the spline smoother runs, so the remap had no downstream effect. Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * fix(trajectory_optimizer): address Copilot review comments - Clarify remove_close_proximity_points docstring: close proximity implies low speed at constant dt, so stop candidate registration is intentional. - Remove misleading 'atomically' wording from take_stop_point_candidates comment; the swap is O(1) but not thread-safe. Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> --------- Signed-off-by: Daniel <danielsanchezaran@gmail.com> Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

danielsanchezaran requested a review from Copilot March 12, 2026 10:37

danielsanchezaran requested review from SakodaShintaro, go-sakayori and yukkysaito as code owners March 12, 2026 10:37

github-project-automation Bot added this to Software Working Group Mar 12, 2026

github-project-automation Bot moved this to To Triage in Software Working Group Mar 12, 2026

github-actions Bot added the component:planning Route planning, decision-making, and navigation. (auto-assigned) label Mar 12, 2026

Copilot started reviewing on behalf of danielsanchezaran March 12, 2026 10:38

View session

danielsanchezaran added the run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) label Mar 12, 2026

Copilot AI reviewed Mar 12, 2026

View reviewed changes

github-actions Bot added the type:documentation Creating or refining documentation. (auto-assigned) label Mar 12, 2026

Owen-Liuyuxuan approved these changes Mar 13, 2026

View reviewed changes

danielsanchezaran requested a review from Copilot March 13, 2026 05:31

danielsanchezaran added 10 commits March 13, 2026 14:32

use a semantic speed tracker and update longitudinal weights fro bett…

a135731

…er slow downs Signed-off-by: Daniel <danielsanchezaran@gmail.com>

undo param change

7e54582

Signed-off-by: Daniel <danielsanchezaran@gmail.com>

fix: align qp_smoother.md markdown style with CI/CD prettier

6f2136a

Remove blank lines between list item headers and their descriptions to match the prettier version used in GitHub Actions. Signed-off-by: Daniel <danielsanchezaran@gmail.com>

danielsanchezaran force-pushed the feat/optimizer-semantci-speed-tracker branch from d2873a5 to 3f3bc57 Compare March 13, 2026 05:34

Copilot AI reviewed Mar 13, 2026

View reviewed changes

Copilot started reviewing on behalf of danielsanchezaran March 13, 2026 05:41

View session

go-sakayori approved these changes Mar 13, 2026

View reviewed changes

danielsanchezaran merged commit 0a309ac into autowarefoundation:main Mar 13, 2026
43 of 44 checks passed

danielsanchezaran deleted the feat/optimizer-semantci-speed-tracker branch March 13, 2026 07:56

github-project-automation Bot moved this from To Triage to Done in Software Working Group Mar 13, 2026

Owen-Liuyuxuan mentioned this pull request Mar 13, 2026

feat(autoware_trajectory_optimizer): optimizer semantic speed tracker (#12289) tier4/autoware_universe#2778

Merged

This was referenced May 2, 2026

chore: bump version to 0.51.0 #12543

Closed

chore: bump version to 1.8.0 for humble #12544

Merged

-    if (
-      traj_points[stop_idx].longitudinal_velocity_mps >=
-      traj_points[stop_idx - 1].longitudinal_velocity_mps) {
+    const double v_prev = traj_points[stop_idx - 1].longitudinal_velocity_mps;
+    const double v_curr = traj_points[stop_idx].longitudinal_velocity_mps;
+    const bool same_direction = (v_prev * v_curr) > 0.0;
+    if (same_direction && std::abs(v_curr) >= std::abs(v_prev)) {

	"description": "Velocity threshold for fallback stop detection: if remove_close_proximity_points did not fire, any slow cluster whose minimum velocity is below this threshold is treated as a stop approach [m/s]"
	"description": "Velocity threshold used by the stop-detection logic: trajectory points or segments whose velocity falls below this value are treated as part of a stop or stop approach in both the velocity-profile-based and resampling-based detectors [m/s]"

		"min_dist_to_resample_m",
		"stop_detection_velocity_threshold_mps"

Conversation

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

Uh oh!

Description

Problem

Solution

Related links

How was this PR tested?

Notes for reviewers

Interface changes

ROS Parameter Changes

Additions and removals

Effects on system behavior

Uh oh!

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

Uh oh!

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Uh oh!

Copilot AI Mar 12, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Mar 12, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Mar 12, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Mar 12, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Mar 12, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Mar 12, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Mar 12, 2026

Choose a reason for hiding this comment

Uh oh!

danielsanchezaran Mar 13, 2026

Choose a reason for hiding this comment

Uh oh!

Uh oh!

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

Uh oh!

Codecov Report

Uh oh!

Owen-Liuyuxuan left a comment

Choose a reason for hiding this comment

Uh oh!

danielsanchezaran commented Mar 13, 2026

Uh oh!

Owen-Liuyuxuan commented Mar 13, 2026

Uh oh!

Owen-Liuyuxuan commented Mar 13, 2026

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Uh oh!

go-sakayori Mar 13, 2026

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Reviewers

Uh oh!

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants

danielsanchezaran commented Mar 12, 2026 •

edited

Loading

github-actions Bot commented Mar 12, 2026 •

edited

Loading

codecov Bot commented Mar 12, 2026 •

edited

Loading