[codex] Add vehicle-constrained GICP prior

[FieldDiTian]

Summary

This PR updates gicp_localization to make the LiDAR deskew path, aux-Luminar matching, and GICP prior more consistent with the AV-24 race-car platform.

Final diff scope:

• gicp_localization/src/localization.cc
• gicp_localization/include/gicp_localization/localization.h
• gicp_localization/cfg/localization.yaml
• gicp_localization/README.md

Main behavior changes:

• Preserve the configured Luminar sensor type through startup so deskew uses the Luminar timestamp path.
• Match auxiliary Luminar sweeps using per-point absolute timestamp ranges, not header stamp alone.
• Add vehicle motion consistency diagnostics/gating for accepted GICP candidates.
• Add a vehicle-constrained prior generated from the last trusted pose, observer velocity, scan dt, and IMU yaw rate.
• Use the vehicle prior as the single GICP initial guess when it is available, and fall back to it on rejected frames.
• Publish vehicle-prior debug topics for RViz/Foxglove/replay auditing.

Commit-by-commit

8cd08b8 - deskew timestep fix

• Initializes the default sensor type before getParams() so localization/sensor_type: "luminar" remains active after parameter loading.
• Adds a trusted motion reference updated from initial pose, accepted GICP pose, and GT snap recovery.
• Adds evaluateMotionConsistency() to compare a candidate against the last trusted trajectory point.
• Adds motion debug topics for forward displacement, heading error, curvature, yaw rate, and rejection state.
• Rejects physically implausible accepted candidates before they update current_pose, observer state, or the next trusted reference.

99a9699 - Fix Luminar aux timestamp matching

• Adds localization/lidar_concat/luminar_time_threshold.
• Adds shared Luminar absolute timestamp decoding for point-time range checks.
• Matches aux Luminar scans by their per-point timestamp range relative to the primary scan.
• Drops timestamp-misaligned aux scans instead of appending an early/late sweep into the merged cloud.
• Sets default aux time offsets to explicit zero entries for the two configured aux Luminar topics.
• Commit-level validation recorded in the commit message: Run5 data on Run3 local-ENU map smoke at start-offset 160 for 60s, 666 audited frames, IMU integration failed count 0, scan time span p50/max about 0.049s.

20afcae - Add vehicle-constrained GICP prior

• Adds VehiclePriorResult and computeVehiclePrior().
• Builds T_vehicle_prior from the last trusted pose, observer/world velocity, scan dt, and IMU yaw rate.
• Constrains the prior with ground-vehicle limits: forward speed, lateral speed, yaw rate, no reverse-speed contribution, and flat z hold from the trusted reference.
• Uses the vehicle prior as the single GICP initial guess when a trusted motion reference is available.
• On rejected frames, falls back to the vehicle prior when it was used; otherwise keeps the existing raw IMU T_prior fallback.
• Adds debug outputs:
  • gicp/localization/debug/vehicle_prior_pose
  • gicp/localization/debug/prior_delta_trans_m
  • gicp/localization/debug/prior_delta_yaw_deg
  • gicp/localization/debug/vehicle_prior_used
• Tightens default motion-consistency and vehicle-prior parameters toward race-car motion constraints.
• Updates the README to document T_prior, T_vehicle_prior, candidate gating, and GT snap recovery responsibilities.

Validation

• git diff --check origin/art-jazzy...HEAD passed.
• source /opt/ros/jazzy/setup.bash && colcon build --packages-select gicp_localization --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release passed.
  • The build emitted only the existing PCL/FLANN CMake dev warning.

[Copilot]

Pull request overview

This PR updates gicp_localization to better match AV-24 race-car motion assumptions by (1) making Luminar deskew/aux concatenation rely on absolute per-point time alignment, and (2) adding a vehicle-constrained prior plus motion-consistency gating/diagnostics to stabilize GICP initialization and rejection handling.

Changes:

• Preserve the configured Luminar sensor type through startup and improve aux Luminar sweep matching using per-point absolute timestamp ranges.
• Add motion-consistency diagnostics/gating and a vehicle-constrained prior used as the single GICP initial guess when control is ready.
• Add debug topics and documentation updates covering the active prior path and new gates.

Reviewed changes

Copilot reviewed 4 out of 4 changed files in this pull request and generated 1 comment.

File	Description
gicp_localization/src/localization.cc	Implements Luminar timestamp-range matching, motion-consistency gate, and vehicle prior; wires debug publishers and integrates the prior into GICP initialization/fallback logic.
gicp_localization/include/gicp_localization/localization.h	Adds data structures and parameters for motion consistency and vehicle-prior computation, plus new debug publishers/state.
gicp_localization/cfg/localization.yaml	Adds/tightens defaults for Luminar time-range thresholding, motion-consistency gating, and vehicle-prior limits for race-car dynamics.
gicp_localization/README.md	Updates pipeline documentation to describe the vehicle-prior correction path, additional gates, and new debug metrics.

---

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