feat(planning_evaluator): add evaluation feature of trajectory lateral displacement

[kyoichi-sugahara]

Description

This PR introduces a new trajectory lateral displacement metric and enhances the stability evaluation capabilities of the planning evaluator by incorporating ego vehicle velocity and evaluation time parameters.

The main purpose of this metric is to detect trajectory chattering (rapid oscillations in planned paths), which can lead to unstable vehicle behavior.

• Added new metric trajectory_lateral_displacement to evaluate cumulative lateral displacement
• Added ego odometry support to the metrics calculator for velocity-aware evaluations
• Introduced evaluation_time_s parameter (default: 5.0s) for trajectory evaluation duration
• Added utility function calc_lookahead_trajectory_distance to calculate distance from ego position to trajectory end

Implementation Details

• The new trajectory lateral displacement metric:
  • Evaluates the cumulative absolute lateral displacement between two trajectories
  • Uses ego vehicle velocity and evaluation time to determine the evaluation section length
  • Samples points along trajectories to measure lateral offsets
  • Considers trajectory length constraints based on ego position

Related links

Parent Issue:

• Link

How was this PR tested?

run simulator with foxglove visualization

2024-12-23_11.46.10.mp4

Notes for reviewers

None.

Interface changes

ROS Parameter Changes

Additions and removals

Change type	Parameter Name	Type	Default Value	Description
Added	trajectory.evaluation_time_s	double	5.0	time duration for trajectory evaluation in seconds

Effects on system behavior

None.

[github-actions]

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.

[xtk8532704]

LGTM~

[codecov]

Codecov Report

Attention: Patch coverage is 93.87755% with 3 lines in your changes missing coverage. Please review.

Project coverage is 29.76%. Comparing base (07db183) to head (2a221df).
Report is 2 commits behind head on main.

Files with missing lines	Patch %	Lines
...anning_evaluator/src/metrics/stability_metrics.cpp	86.66%	0 Missing and 2 partials ⚠️
...e_planning_evaluator/src/metrics/metrics_utils.cpp	94.11%	0 Missing and 1 partial ⚠️

Additional details and impacted files

@@           Coverage Diff           @@
##             main    #9718   +/-   ##
=======================================
  Coverage   29.75%   29.76%           
=======================================
  Files        1444     1445    +1     
  Lines      108689   108737   +48     
  Branches    42663    42677   +14     
=======================================
+ Hits        32336    32361   +25     
- Misses      73171    73192   +21     
- Partials     3182     3184    +2     

Flag	Coverage Δ		*Carryforward flag
differential	8.37% <93.87%> (?)
total	29.73% <ø> (-0.02%)	⬇️	Carriedforward from 9bc173c

*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.