feat(autoware_planning_evaluator): new obstacle metrics

[xtk8532704]

Description

This PR refactors obstacle metrics implementations and adds 2 new metrics for safety checks.

Refactored metrics:

• Obstacle Distance: The distance from the object's center point to the ego's future trajectory.
• Time To Collision (TTC): Time to collision with the target object
  New metrics:
• Post Encroaching Time (PET): Time difference between the target object leaving the overlap region and the ego entering the overlap region for potential collision targets
  • Targets that do not actually collide but whose future trajectories overlap with the ego's future trajectory.
  • An object cannot have both TTC and PET metrics simultaneously, because if the former exists, the latter is clearly zero.
• Deceleration Rate to Avoid Collision (DRAC): The minimum deceleration required to avoid a collision with the target object.
  • This metric is published alongside TTC.

What has changed

• All 4 obstacle metrics can now be published for each object, instead of being the worst value for all objects. You can use the worst_only parameter to control whether to publish them for each object.

before: {_metric_name_}/{min, max, mean}

- name: obstacle_ttc/min
  unit: ''
  value: '1.0'
...

after: {_metric_name_}/{_object_uuid_, worst}

--
- name: obstacle_ttc/ce8956bc2dc2ba8ab06ccae6d27d88fd
  unit: ''
  value: '3.1054147628049962'
- name: obstacle_ttc/worst
  unit: ''
  value: '3.1054147628049962'
...

• parameter changes:
  • rename obstacle.dist_thr_m -> obstacle.collision_thr_m.

Related links

Parent Issue:

• Link

Private Links (Japanese):

• Safety check designs
• Demonstration slides for TTC, PET, and DRAC

How was this PR tested?

Psim.
Unit Tests for 2 new metrics are added.

Notes for reviewers

You can check this doc first to understand the algorithm for the obstacle metrics.

If the code is hard to read, you can just play with Psim to check if the new metrics' behaviors work well.

Interface changes

None.

ROS Parameter Changes

Additions and removals

Change type	Parameter Name	Type	Default Value	Description
Added	obstacle.worst_only	bool	false	if true, only the worst obstacle metric value is published
Added	obstacle.use_ego_traj_vel	bool	fals	if true, the planned trajectory velocity is used
Added	obstacle.stop_velocity_mps	double	0.2777	[m/s] velocity threshold to consider the object or ego is static, used to speed up the calculation
Added	obstacle.min_time_interval_s	double	0.05	[s] minimum time interval between two successive points to use for ego trajectory resampling to speed up the calculation.
Added	obstacle.min_spatial_interval_m	double	0.1	[m] minimum spatial interval between two successive points to use for ego trajectory resampling to speed up the calculation.

Modifications

Version	Parameter Name	Type	Default Value	Description
Old	obstacle.dist_thr_m	double	0.0	distance threshold to consider a collision/overlapping occurs between object footprints and ego trajectory footprints
New	obstacle.collision_thr_m	double	0.0	distance threshold to consider a collision/overlapping occurs between object footprints and ego trajectory footprints

Effects on system behavior

The processing time for new planning_evaluator is < 30 ms under peak conditions, and typically < 3 ms if there are no complex trajectory overlaps between the ego and objects present. (verified using Psim; situation under real-world conditions cannot be guaranteed.)

[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]

I'm fixing unit tests.

[codecov]

Codecov Report

❌ Patch coverage is 86.18421% with 42 lines in your changes missing coverage. Please review.
✅ Project coverage is 18.37%. Comparing base (f50b5c3) to head (175051c).
⚠️ Report is 6 commits behind head on main.

Files with missing lines	Patch %	Lines
...ning_evaluator/src/obstacle_metrics_calculator.cpp	80.00%	18 Missing and 19 partials ⚠️
...planning_evaluator/src/planning_evaluator_node.cpp	92.85%	0 Missing and 3 partials ⚠️
...planning_evaluator/obstacle_metrics_calculator.hpp	93.93%	0 Missing and 2 partials ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main   #11761      +/-   ##
==========================================
+ Coverage   17.98%   18.37%   +0.38%     
==========================================
  Files        1812     1806       -6     
  Lines      125542   124965     -577     
  Branches    43666    43469     -197     
==========================================
+ Hits        22581    22959     +378     
+ Misses      84121    83394     -727     
+ Partials    18840    18612     -228     

Flag	Coverage Δ		*Carryforward flag
daily	20.39% <ø> (-0.08%)	⬇️	Carriedforward from 77ba0fd
daily-cuda	17.99% <ø> (+0.02%)	⬆️	Carriedforward from 77ba0fd
differential	43.87% <86.18%> (?)
total-cuda	18.07% <ø> (+0.09%)	⬆️	Carriedforward from 77ba0fd

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

[kosuke55]

(minor)

Suggested change

	auto trajectory_start = now();
	const auto trajectory_start = now();

other parts too!

[xtk8532704]

fixed at c9daa26

[kosuke55]

( minor: I prefer returning structs over passing them by reference)

[xtk8532704]

fixed at c9daa26

[kosuke55]

(minor)

Suggested change

	double vel_curr = ego_odometry_->twist.twist.linear.x;
	const double vel_curr = ego_odometry_->twist.twist.linear.x;

other parts too

[xtk8532704]

vel_curr is changing during the algorithm loop

[kosuke55]

Suggested change

	// // Debug prints, uncomment to check trajectory points if needed
	// std::cerr << "Obstacle traj" << std::endl;
	// for (size_t i = 0; i < obstacle_trajectory_points_.size(); ++i) {
	// const auto & p = obstacle_trajectory_points_[i];
	// std::cerr << "[" << i << "] t: " << p.time_from_start_s << ", pos: (" << p.pose.position.x
	// << ", "
	// << p.pose.position.y << "), is_overlapping: " <<
	// p.is_overlapping_with_ego_trajectory << ", is_collision: "
	// << p.is_collision_with_ego_trajectory << ", first_overlapping_idx: " <<
	// p.first_overlapping_ego_trajectory_index << ", last_overlapping_idx: " <<
	// p.last_overlapping_ego_trajectory_index << std::endl;
	// }
	// std::cerr << "Ego traj" << std::endl;
	// for (size_t i = 0; i < ego_trajectory_points_.size(); ++i) {
	// const auto & p = ego_trajectory_points_[i];
	// std::cerr << "[" << i << "] t: " << p.time_from_start_s << ", pos: (" << p.pose.position.x
	// << ", "
	// << p.pose.position.y << ")" << std::endl;
	// }
	// ------------------------------------------------------------------------------------------------

This part can be deleted?

[xtk8532704]

fixed at c9daa26

[kosuke55]

I haven't checked the detail of logic but the diff seems good overall. Leaving some minor comments, but I'll approve it now as code owner