[yhisaki]

No description provided.

[mergify]

This pull request does not currently match the merge queue conditions, so it cannot be queued from here. The box comes back if it matches again.

[sonarqubecloud]

Quality Gate passed

Issues
2 New issues
0 Accepted issues

Measures
0 Security Hotspots
0.0% Coverage on New Code
0.0% Duplication on New Code

See analysis details on SonarQube Cloud

[Owen-Liuyuxuan]

Review summary

Road-border avoidance already laterally shifts the raw model trajectory so the ego footprint does not sit on type=road_border linestrings, then feeds that as the acados tracking reference.

The optimizer still trades that reference against smoothness and vehicle limits, so the solved path can cut back into a border the reference was pushed off. This PR closes that loop:

1. Shift the raw output → reference (existing).
2. Optimize.
3. Run the same footprint check on the optimized trajectory.
4. If it still overlaps, add that missing clearance onto the reference (total offset from raw capped at max_lateral_shift_m) and solve again.
5. Repeat up to road_border_avoidance.max_reoptimizations (package yaml 5; schema / declare_parameter default 0 = re-check only, no extra solves).

If a retry fails to converge, the last converged solve is kept. The candidate is still dropped if the first solve fails (same as before: no raw fallback).

To make several solves per cycle safe, TrajectoryOptimizer::optimize() is now side-effect free. Warm start and goal latch are explicit: set_goal() once per cycle, latch_goal_if_reached() per candidate, accept() only for the solution that is actually used. All retries in one cycle warm-start from the previous cycle, not from the previous retry. Debug ~/debug/road_border_avoidance/adjusted_trajectory is the final reference of the used solve.

That is the right problem for DP/ML planner: geometric pre-shift is not a hard constraint in the OCP. The optimizer API split is a good structural change.

Findings

[P3] Overlap warning counts propagated points, not colliding poses (refine_candidate)

remaining_overlapping_points is num_shifted_points + num_unresolved_points. With propagate_shift: true (default), adjust() increments num_shifted_points for later points that never overlapped, once an earlier pose set a carried offset. The re-opt loop condition is still correct (modified() implies at least one real overlap), but the throttled WARN “still overlaps a road border at %d points” over-counts. A dedicated overlap counter (or unresolved plus poses that actually had an offending border) would match the log.

[P3] Shipped default and schema default disagree

Config sets max_reoptimizations: 5; schema and node default are 0. Anyone launching this yaml pays up to 6 SQP solves per candidate per cycle (10 Hz). Anyone who only gets declare defaults does not re-optimize. These should be one number, and 5 should be justified against max_sqp_iterations / timer budget.

Residual risks / gaps

• No unit test for shift_reference or the re-opt loop (overlap → extra reference shift → second solve; cap hit; retry fail → keep last converged).
• Re-check still uses full adjust() including propagate_shift, so a local mid-horizon overlap can push the whole tail of the reference. Documented as “same-stage shift,” but it can ratchet the tail toward max_lateral_shift_m over retries.
• Same-cycle retries do not warm-start from the previous retry; a later retry may fail and you keep an overlapping earlier solve. Intentional for temporal consistency; still the main residual quality risk.

Would not block on the P3s. Would like a short PR body, a true overlap metric in the WARN, and at least one test that an optimized path that recuts a border actually triggers a second solve.