feat(autoware_planning_evaluator): add resampled_relative_angle metrics #10020

Kazunori-Nakajima · 2025-01-27T00:18:16Z

Description

Added metrics to look at angular differences in trajectory points.
This metric enables detection of severe route misalignments, such as bends of more than 90° in local trajectory.

How was this PR tested?

simplescreenrecorder-2025-01-27_09.08.46.mp4

Notes for reviewers

None.

Interface changes

None.

Effects on system behavior

None.

github-actions · 2025-01-27T00:18:32Z

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.

kosuke55 · 2025-01-27T01:25:35Z

evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp

+  // Resample trajctory points
+  std::vector<TrajectoryPoint> resample_traj_points{};
+  for (size_t idx = 0; idx < traj.points.size(); idx = idx + 3) {
+    resample_traj_points.push_back(traj.points.at(idx));
+  }


Is this necessary for calculation costs?

Thank you very much for comment!
It seems to not necessary. This processing time is at most 0.5 ms.
I deleted this code -> 1b29489

thanks!
0.5ms seems not so problem

( for reduce proccesing time more: https://star4.slack.com/archives/C03QW0GU6P7/p1737942266253849?thread_ts=1737937464.395459&cid=C03QW0GU6P7 )

Thank you very much!
I fixed it.

default.mp4

kosuke55 · 2025-01-27T02:26:19Z

evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp

+  // We need at least three points to compute relative angle
+  if (traj.points.size() < 2) {
+    return stat;
+  }


now we can delete this lines

kosuke55 · 2025-01-27T08:15:12Z

rename metrics name to resampled_relative_angle
fix ci check

Signed-off-by: Kasunori-Nakajima <[email protected]>

kosuke55 · 2025-01-28T01:56:05Z

@Kazunori-Nakajima
ci unit test fails, it seems that you need to update test code for using vehicle_info in it.

…dd_large_relative_angle

Signed-off-by: Kasunori-Nakajima <[email protected]>

…ngle

Kazunori-Nakajima · 2025-01-28T11:21:23Z

@kosuke55

ci unit test fails, it seems that you need to update test code for using vehicle_info in it.

I fix unit test, and add resampled_relative_angle unit test -> 4c90661
Could you please review unit test? Test is all pass in local.

colcon test --packages-select autoware_planning_evaluator --event-handlers console_cohesion+

codecov · 2025-01-28T11:26:24Z

Codecov Report

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

Project coverage is 29.27%. Comparing base (461dbc1) to head (7a4a59e).
Report is 4 commits behind head on main.

Files with missing lines	Patch %	Lines
...e_planning_evaluator/src/motion_evaluator_node.cpp	0.00%	3 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main   #10020      +/-   ##
==========================================
+ Coverage   29.21%   29.27%   +0.06%     
==========================================
  Files        1439     1440       +1     
  Lines      108115   108009     -106     
  Branches    42638    42545      -93     
==========================================
+ Hits        31588    31624      +36     
+ Misses      73485    73342     -143     
- Partials     3042     3043       +1

Flag	Coverage Δ		*Carryforward flag
differential	`9.24% <83.33%> (?)`
total	`29.27% <ø> (+0.05%)`	⬆️	Carriedforward from e062639

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

evaluator/autoware_planning_evaluator/test/test_planning_evaluator_node.cpp

evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp

Signed-off-by: Kasunori-Nakajima <[email protected]>

kosuke55 · 2025-01-29T08:06:11Z

evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp

+        // Calc diff yaw between base pose yaw and target pose yaw
+        const double front_diff_yaw =
+          autoware::universe_utils::normalizeRadian(front_target_yaw - base_yaw);
+        const double back_diff_yaw =
+          autoware::universe_utils::normalizeRadian(back_target_yaw - base_yaw);
+
+        stat.add(std::abs(std::max(front_diff_yaw, back_diff_yaw)));


Suggested change

// Calc diff yaw between base pose yaw and target pose yaw

const double front_diff_yaw =

autoware::universe_utils::normalizeRadian(front_target_yaw - base_yaw);

const double back_diff_yaw =

autoware::universe_utils::normalizeRadian(back_target_yaw - base_yaw);

stat.add(std::abs(std::max(front_diff_yaw, back_diff_yaw)));

// Calc diff yaw between base pose yaw and target pose yaw

const double front_diff_yaw =

std::abs(autoware::universe_utils::normalizeRadian(front_target_yaw - base_yaw));

const double back_diff_yaw =

std::abs(autoware::universe_utils::normalizeRadian(back_target_yaw - base_yaw));

stat.add(std::max(front_diff_yaw, back_diff_yaw));

should compare abs value

Thank you very much!
That true, I fixed.7a4a59e

Signed-off-by: Kasunori-Nakajima <[email protected]>

kosuke55 · 2025-01-30T07:59:36Z

@kyoichi-sugahara @maxime-clem
could you approve as a code owner?

maxime-clem · 2025-01-30T09:12:00Z

...toware_planning_evaluator/include/autoware/planning_evaluator/metrics/trajectory_metrics.hpp

+ * @return calculated statistics
+ */
+Accumulator<double> calcTrajectoryResampledRelativeAngle(
+  const Trajectory & traj, const double & vehicle_length_m);


(minor) primitive types do not need to be passed by reference.

Suggested change

const Trajectory & traj, const double & vehicle_length_m);

const Trajectory & traj, const double vehicle_length_m);

maxime-clem · 2025-01-30T09:14:10Z

evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp

+  const auto resample_offset = vehicle_length_m / 2;
+  const auto arc_length =
+    autoware::motion_utils::calcSignedArcLengthPartialSum(traj.points, 0, traj.points.size());
+  for (size_t base_id = 0; base_id < arc_length.size() - 1; ++base_id) {


(minor) since size_t is an unsigned type, if arc_length.size() is equal to 0 then a bug will occur when doing arc_length.size() - 1. Therefore I recommend adding one on the left side of the comparison instead.

Suggested change

for (size_t base_id = 0; base_id < arc_length.size() - 1; ++base_id) {

for (size_t base_id = 0; base_id + 1 < arc_length.size(); ++base_id) {

maxime-clem · 2025-01-30T09:17:35Z

evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp

+    for (size_t target_id = base_id; target_id < arc_length.size(); ++target_id) {
+      if (arc_length[target_id] >= arc_length[base_id] + resample_offset) {
+        // Get target pose yaw
+        const double front_target_yaw = tf2::getYaw(traj.points.at(target_id).pose.orientation);
+        const double back_target_yaw = tf2::getYaw(traj.points.at(target_id - 1).pose.orientation);


When base_id = 0, then the first value of target_id will be 0. I suspect there could be a bug here when doing target_id - 1.

oh, thanks for your review

kyoichi-sugahara · 2025-01-30T11:16:08Z

evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp

+    for (size_t target_id = base_id; target_id < arc_length.size(); ++target_id) {
+      if (arc_length[target_id] >= arc_length[base_id] + resample_offset) {
+        // Get target pose yaw
+        const double front_target_yaw = tf2::getYaw(traj.points.at(target_id).pose.orientation);
+        const double back_target_yaw = tf2::getYaw(traj.points.at(target_id - 1).pose.orientation);
+
+        // Calc diff yaw between base pose yaw and target pose yaw
+        const double front_diff_yaw =
+          std::abs(autoware::universe_utils::normalizeRadian(front_target_yaw - base_yaw));
+        const double back_diff_yaw =
+          std::abs(autoware::universe_utils::normalizeRadian(back_target_yaw - base_yaw));
+
+        stat.add(std::max(front_diff_yaw, back_diff_yaw));


Suggested change

for (size_t target_id = base_id; target_id < arc_length.size(); ++target_id) {

if (arc_length[target_id] >= arc_length[base_id] + resample_offset) {

// Get target pose yaw

const double front_target_yaw = tf2::getYaw(traj.points.at(target_id).pose.orientation);

const double back_target_yaw = tf2::getYaw(traj.points.at(target_id - 1).pose.orientation);

// Calc diff yaw between base pose yaw and target pose yaw

const double front_diff_yaw =

std::abs(autoware::universe_utils::normalizeRadian(front_target_yaw - base_yaw));

const double back_diff_yaw =

std::abs(autoware::universe_utils::normalizeRadian(back_target_yaw - base_yaw));

stat.add(std::max(front_diff_yaw, back_diff_yaw));

// Get arc length offset from base point

const double needed_arc = arc_length[base_id] + resample_offset;

for (size_t target_id = base_id; target_id < arc_length.size(); ++target_id) {

// Linear interpolate between points when needed_arc is found

if (arc_length[target_id] >= needed_arc) {

// Get arc lengths for linear interpolation

const double arc_1 = arc_length[target_id - 1];

const double arc_2 = arc_length[target_id];

// Get linear interpolation ratio from 0 to 1

double ratio = (needed_arc - arc_1) / (arc_2 - arc_1);

ratio = std::max(0.0, std::min(1.0, ratio));

const double yaw_1 = tf2::getYaw(traj.points[target_id - 1].pose.orientation);

const double yaw_2 = tf2::getYaw(traj.points[target_id].pose.orientation);

// Linear interpolate yaw

double diff = autoware::universe_utils::normalizeRadian(yaw_2 - yaw_1);

double interp_yaw = yaw_1 + diff * ratio;

interp_yaw = autoware::universe_utils::normalizeRadian(interp_yaw);

const double diff_yaw =

std::abs(autoware::universe_utils::normalizeRadian(interp_yaw - base_yaw));

stat.add(diff_yaw);

Minor comment: Please ignore this depending on the anomalies you want to detect.
if there is motivation to keep distance intervals constant, I suggest that simple linear interpolation might be option. Here's an example as a suggestion.

Kazunori-Nakajima requested a review from kosuke55 January 27, 2025 00:18

Kazunori-Nakajima requested review from kyoichi-sugahara, maxime-clem and xtk8532704 as code owners January 27, 2025 00:18

github-actions bot added the component:evaluator Evaluation tools for planning, localization etc. (auto-assigned) label Jan 27, 2025

kosuke55 reviewed Jan 27, 2025

View reviewed changes

kosuke55 added the run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) label Jan 27, 2025

kosuke55 approved these changes Jan 27, 2025

View reviewed changes

kosuke55 assigned kyoichi-sugahara and maxime-clem Jan 27, 2025

kosuke55 changed the title ~~feat(autoware_planning_evaluator): add new large_relative_angle metrics~~ feat(autoware_planning_evaluator): add resampled_relative_angle metrics Jan 27, 2025

Kazunori-Nakajima and others added 7 commits January 27, 2025 23:14

feat(autoware_planning_evaluator): add new large_relative_angle metrics

8b282fd

Signed-off-by: Kasunori-Nakajima <[email protected]>

fix copyright and vehicle_length_m

6882359

Signed-off-by: Kasunori-Nakajima <[email protected]>

style(pre-commit): autofix

83fa7d6

del: resample trajectory

aebfab5

Signed-off-by: Kasunori-Nakajima <[email protected]>

del: traj points check

9bf1c02

Signed-off-by: Kasunori-Nakajima <[email protected]>

rename msg and speed optimization

eb677c3

Signed-off-by: Kasunori-Nakajima <[email protected]>

style(pre-commit): autofix

9246146

kosuke55 force-pushed the feat/planning_evaluator/add_large_relative_angle branch from 98463cc to 9246146 Compare January 27, 2025 14:14

Kazunori-Nakajima and others added 5 commits January 28, 2025 15:36

Merge branch 'autowarefoundation:main' into feat/planning_evaluator/a…

87dcc78

…dd_large_relative_angle

add unit_test and fix resample_relative_angle

4c90661

Signed-off-by: Kasunori-Nakajima <[email protected]>

style(pre-commit): autofix

4a5e221

include tuple to test

4517826

Signed-off-by: Kasunori-Nakajima <[email protected]>

Merge branch 'main' into feat/planning_evaluator/add_large_relative_a…

688ee64

…ngle

kosuke55 reviewed Jan 29, 2025

View reviewed changes

evaluator/autoware_planning_evaluator/test/test_planning_evaluator_node.cpp Outdated Show resolved Hide resolved

kosuke55 reviewed Jan 29, 2025

View reviewed changes

evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp Outdated Show resolved Hide resolved

target two point, update unit test value

e062639

Signed-off-by: Kasunori-Nakajima <[email protected]>

kosuke55 reviewed Jan 29, 2025

View reviewed changes

fix abs

7a4a59e

Signed-off-by: Kasunori-Nakajima <[email protected]>

maxime-clem approved these changes Jan 30, 2025

View reviewed changes

kyoichi-sugahara reviewed Jan 30, 2025

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

feat(autoware_planning_evaluator): add resampled_relative_angle metrics #10020

feat(autoware_planning_evaluator): add resampled_relative_angle metrics #10020

Kazunori-Nakajima commented Jan 27, 2025

github-actions bot commented Jan 27, 2025 •

edited

Loading

kosuke55 Jan 27, 2025

Kazunori-Nakajima Jan 27, 2025

kosuke55 Jan 27, 2025

Kazunori-Nakajima Jan 27, 2025

kosuke55 Jan 27, 2025

Kazunori-Nakajima Jan 27, 2025

kosuke55 commented Jan 27, 2025

kosuke55 commented Jan 28, 2025

Kazunori-Nakajima commented Jan 28, 2025

codecov bot commented Jan 28, 2025 •

edited

Loading

kosuke55 Jan 29, 2025

kosuke55 Jan 29, 2025

Kazunori-Nakajima Jan 30, 2025

kosuke55 commented Jan 30, 2025

maxime-clem Jan 30, 2025

maxime-clem Jan 30, 2025

maxime-clem Jan 30, 2025

kosuke55 Jan 30, 2025

kyoichi-sugahara Jan 30, 2025

	const Trajectory & traj, const double & vehicle_length_m);
	const Trajectory & traj, const double vehicle_length_m);

	for (size_t base_id = 0; base_id < arc_length.size() - 1; ++base_id) {
	for (size_t base_id = 0; base_id + 1 < arc_length.size(); ++base_id) {

-    for (size_t target_id = base_id; target_id < arc_length.size(); ++target_id) {
-      if (arc_length[target_id] >= arc_length[base_id] + resample_offset) {
-        // Get target pose yaw
-        const double front_target_yaw = tf2::getYaw(traj.points.at(target_id).pose.orientation);
-        const double back_target_yaw = tf2::getYaw(traj.points.at(target_id - 1).pose.orientation);
-        // Calc diff yaw between base pose yaw and target pose yaw
-        const double front_diff_yaw =
-          std::abs(autoware::universe_utils::normalizeRadian(front_target_yaw - base_yaw));
-        const double back_diff_yaw =
-          std::abs(autoware::universe_utils::normalizeRadian(back_target_yaw - base_yaw));
-        stat.add(std::max(front_diff_yaw, back_diff_yaw));
+    // Get arc length offset from base point
+    const double needed_arc = arc_length[base_id] + resample_offset;
+    for (size_t target_id = base_id; target_id < arc_length.size(); ++target_id) {
+      // Linear interpolate between points when needed_arc is found
+      if (arc_length[target_id] >= needed_arc) {
+        // Get arc lengths for linear interpolation
+        const double arc_1 = arc_length[target_id - 1];
+        const double arc_2 = arc_length[target_id];
+        // Get linear interpolation ratio from 0 to 1
+        double ratio = (needed_arc - arc_1) / (arc_2 - arc_1);
+        ratio = std::max(0.0, std::min(1.0, ratio));
+        const double yaw_1 = tf2::getYaw(traj.points[target_id - 1].pose.orientation);
+        const double yaw_2 = tf2::getYaw(traj.points[target_id].pose.orientation);
+        // Linear interpolate yaw
+        double diff = autoware::universe_utils::normalizeRadian(yaw_2 - yaw_1);
+        double interp_yaw = yaw_1 + diff * ratio;
+        interp_yaw = autoware::universe_utils::normalizeRadian(interp_yaw);
+        const double diff_yaw =
+          std::abs(autoware::universe_utils::normalizeRadian(interp_yaw - base_yaw));
+        stat.add(diff_yaw);

feat(autoware_planning_evaluator): add resampled_relative_angle metrics #10020

Are you sure you want to change the base?

feat(autoware_planning_evaluator): add resampled_relative_angle metrics #10020

Conversation

Kazunori-Nakajima commented Jan 27, 2025

Description

Related links

How was this PR tested?

Notes for reviewers

Interface changes

Effects on system behavior

github-actions bot commented Jan 27, 2025 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

kosuke55 commented Jan 27, 2025

kosuke55 commented Jan 28, 2025

Kazunori-Nakajima commented Jan 28, 2025

codecov bot commented Jan 28, 2025 • edited Loading

Codecov Report

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

kosuke55 commented Jan 30, 2025

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

github-actions bot commented Jan 27, 2025 •

edited

Loading

codecov bot commented Jan 28, 2025 •

edited

Loading