Skip to content

Commit bd84834

Browse files
feat(autoware_planning_evaluator): add resampled_relative_angle metrics (autowarefoundation#10020)
* feat(autoware_planning_evaluator): add new large_relative_angle metrics Signed-off-by: Kasunori-Nakajima <kazunori.nakajima@tier4.jp> * fix copyright and vehicle_length_m Signed-off-by: Kasunori-Nakajima <kazunori.nakajima@tier4.jp> * style(pre-commit): autofix * del: resample trajectory Signed-off-by: Kasunori-Nakajima <kazunori.nakajima@tier4.jp> * del: traj points check Signed-off-by: Kasunori-Nakajima <kazunori.nakajima@tier4.jp> * rename msg and speed optimization Signed-off-by: Kasunori-Nakajima <kazunori.nakajima@tier4.jp> * style(pre-commit): autofix * add unit_test and fix resample_relative_angle Signed-off-by: Kasunori-Nakajima <kazunori.nakajima@tier4.jp> * style(pre-commit): autofix * include tuple to test Signed-off-by: Kasunori-Nakajima <kazunori.nakajima@tier4.jp> * target two point, update unit test value Signed-off-by: Kasunori-Nakajima <kazunori.nakajima@tier4.jp> * fix abs Signed-off-by: Kasunori-Nakajima <kazunori.nakajima@tier4.jp> * fix for loop bag and primitive type Signed-off-by: Kasunori-Nakajima <kazunori.nakajima@tier4.jp> --------- Signed-off-by: Kasunori-Nakajima <kazunori.nakajima@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent 145f251 commit bd84834

13 files changed

+124
-17
lines changed

evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml

+1
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66
- curvature
77
- point_interval
88
- relative_angle
9+
- resampled_relative_angle
910
- length
1011
- duration
1112
- velocity

evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metric.hpp

+5-1
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
// Copyright 2021 Tier IV, Inc.
1+
// Copyright 2025 Tier IV, Inc.
22
//
33
// Licensed under the Apache License, Version 2.0 (the "License");
44
// you may not use this file except in compliance with the License.
@@ -29,6 +29,7 @@ enum class Metric {
2929
curvature,
3030
point_interval,
3131
relative_angle,
32+
resampled_relative_angle,
3233
length,
3334
duration,
3435
velocity,
@@ -56,6 +57,7 @@ static const std::unordered_map<std::string, Metric> str_to_metric = {
5657
{"curvature", Metric::curvature},
5758
{"point_interval", Metric::point_interval},
5859
{"relative_angle", Metric::relative_angle},
60+
{"resampled_relative_angle", Metric::resampled_relative_angle},
5961
{"length", Metric::length},
6062
{"duration", Metric::duration},
6163
{"velocity", Metric::velocity},
@@ -78,6 +80,7 @@ static const std::unordered_map<Metric, std::string> metric_to_str = {
7880
{Metric::curvature, "curvature"},
7981
{Metric::point_interval, "point_interval"},
8082
{Metric::relative_angle, "relative_angle"},
83+
{Metric::resampled_relative_angle, "resampled_relative_angle"},
8184
{Metric::length, "length"},
8285
{Metric::duration, "duration"},
8386
{Metric::velocity, "velocity"},
@@ -101,6 +104,7 @@ static const std::unordered_map<Metric, std::string> metric_descriptions = {
101104
{Metric::curvature, "Curvature[1/rad]"},
102105
{Metric::point_interval, "Interval_between_points[m]"},
103106
{Metric::relative_angle, "Relative_angle[rad]"},
107+
{Metric::resampled_relative_angle, "Resampled_relative_angle[rad]"},
104108
{Metric::length, "Trajectory_length[m]"},
105109
{Metric::duration, "Trajectory_duration[s]"},
106110
{Metric::velocity, "Trajectory_velocity[m/s]"},

evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/trajectory_metrics.hpp

+13-1
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
// Copyright 2021 Tier IV, Inc.
1+
// Copyright 2025 Tier IV, Inc.
22
//
33
// Licensed under the Apache License, Version 2.0 (the "License");
44
// you may not use this file except in compliance with the License.
@@ -15,6 +15,9 @@
1515
#ifndef AUTOWARE__PLANNING_EVALUATOR__METRICS__TRAJECTORY_METRICS_HPP_
1616
#define AUTOWARE__PLANNING_EVALUATOR__METRICS__TRAJECTORY_METRICS_HPP_
1717

18+
#include "autoware/motion_utils/resample/resample.hpp"
19+
#include "autoware/motion_utils/trajectory/conversion.hpp"
20+
#include "autoware/motion_utils/trajectory/trajectory.hpp"
1821
#include "autoware/universe_utils/math/accumulator.hpp"
1922

2023
#include "autoware_planning_msgs/msg/trajectory.hpp"
@@ -37,6 +40,15 @@ using autoware_planning_msgs::msg::TrajectoryPoint;
3740
Accumulator<double> calcTrajectoryRelativeAngle(
3841
const Trajectory & traj, const double min_dist_threshold);
3942

43+
/**
44+
* @brief calculate large relative angle metric (angle between successive points)
45+
* @param [in] traj input trajectory
46+
* @param [in] vehicle_length_m input vehicle length
47+
* @return calculated statistics
48+
*/
49+
Accumulator<double> calcTrajectoryResampledRelativeAngle(
50+
const Trajectory & traj, const double vehicle_length_m);
51+
4052
/**
4153
* @brief calculate metric for the distance between trajectory points
4254
* @param [in] traj input trajectory

evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics_calculator.hpp

+5-2
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
// Copyright 2021 Tier IV, Inc.
1+
// Copyright 2025 Tier IV, Inc.
22
//
33
// Licensed under the Apache License, Version 2.0 (the "License");
44
// you may not use this file except in compliance with the License.
@@ -47,9 +47,12 @@ class MetricsCalculator
4747
/**
4848
* @brief calculate
4949
* @param [in] metric Metric enum value
50+
* @param [in] traj input trajectory
51+
* @param [in] vehicle_length_m input vehicle length
5052
* @return string describing the requested metric
5153
*/
52-
std::optional<Accumulator<double>> calculate(const Metric metric, const Trajectory & traj) const;
54+
std::optional<Accumulator<double>> calculate(
55+
const Metric metric, const Trajectory & traj, const double vehicle_length_m) const;
5356
std::optional<Accumulator<double>> calculate(
5457
const Metric metric, const Pose & base_pose, const Pose & target_pose) const;
5558

evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/motion_evaluator_node.hpp

+5-1
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
// Copyright 2021 Tier IV, Inc.
1+
// Copyright 2025 Tier IV, Inc.
22
//
33
// Licensed under the Apache License, Version 2.0 (the "License");
44
// you may not use this file except in compliance with the License.
@@ -21,6 +21,8 @@
2121
#include "tf2_ros/buffer.h"
2222
#include "tf2_ros/transform_listener.h"
2323

24+
#include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
25+
2426
#include "autoware_planning_msgs/msg/trajectory.hpp"
2527
#include "autoware_planning_msgs/msg/trajectory_point.hpp"
2628
#include "geometry_msgs/msg/transform_stamped.hpp"
@@ -34,6 +36,7 @@
3436
namespace planning_diagnostics
3537
{
3638
using autoware::universe_utils::Accumulator;
39+
using autoware::vehicle_info_utils::VehicleInfo;
3740
using autoware_planning_msgs::msg::Trajectory;
3841
using autoware_planning_msgs::msg::TrajectoryPoint;
3942

@@ -67,6 +70,7 @@ class MotionEvaluatorNode : public rclcpp::Node
6770
MetricsCalculator metrics_calculator_;
6871
// Metrics
6972
std::vector<Metric> metrics_;
73+
VehicleInfo vehicle_info_;
7074
std::deque<rclcpp::Time> stamps_;
7175
Trajectory accumulated_trajectory_;
7276
};

evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp

+4-1
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
// Copyright 2021 Tier IV, Inc.
1+
// Copyright 2025 Tier IV, Inc.
22
//
33
// Licensed under the Apache License, Version 2.0 (the "License");
44
// you may not use this file except in compliance with the License.
@@ -24,6 +24,7 @@
2424
#include <autoware/route_handler/route_handler.hpp>
2525
#include <autoware/universe_utils/ros/polling_subscriber.hpp>
2626
#include <autoware/universe_utils/system/stop_watch.hpp>
27+
#include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
2728

2829
#include "autoware_perception_msgs/msg/predicted_objects.hpp"
2930
#include "autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp"
@@ -44,6 +45,7 @@
4445
namespace planning_diagnostics
4546
{
4647
using autoware::universe_utils::Accumulator;
48+
using autoware::vehicle_info_utils::VehicleInfo;
4749
using autoware_perception_msgs::msg::PredictedObjects;
4850
using autoware_planning_msgs::msg::PoseWithUuidStamped;
4951
using autoware_planning_msgs::msg::Trajectory;
@@ -167,6 +169,7 @@ class PlanningEvaluatorNode : public rclcpp::Node
167169
metric_accumulators_; // 3(min, max, mean) * metric_size
168170

169171
rclcpp::TimerBase::SharedPtr timer_;
172+
VehicleInfo vehicle_info_;
170173
std::optional<AccelWithCovarianceStamped> prev_acc_stamped_{std::nullopt};
171174
};
172175
} // namespace planning_diagnostics

evaluator/autoware_planning_evaluator/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
<depend>autoware_planning_msgs</depend>
2020
<depend>autoware_route_handler</depend>
2121
<depend>autoware_universe_utils</depend>
22+
<depend>autoware_vehicle_info_utils</depend>
2223
<depend>eigen</depend>
2324
<depend>geometry_msgs</depend>
2425
<depend>nav_msgs</depend>

evaluator/autoware_planning_evaluator/schema/autoware_planning_evaluator.schema.json

+1
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@
2626
"curvature",
2727
"point_interval",
2828
"relative_angle",
29+
"resampled_relative_angle",
2930
"length",
3031
"duration",
3132
"velocity",

evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp

+35
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,9 @@
1616

1717
#include "autoware/planning_evaluator/metrics/metrics_utils.hpp"
1818
#include "autoware/universe_utils/geometry/geometry.hpp"
19+
20+
#include <algorithm>
21+
1922
namespace planning_diagnostics
2023
{
2124
namespace metrics
@@ -80,6 +83,38 @@ Accumulator<double> calcTrajectoryRelativeAngle(
8083
return stat;
8184
}
8285

86+
Accumulator<double> calcTrajectoryResampledRelativeAngle(
87+
const Trajectory & traj, const double vehicle_length_m)
88+
{
89+
Accumulator<double> stat;
90+
91+
const auto resample_offset = vehicle_length_m / 2;
92+
const auto arc_length =
93+
autoware::motion_utils::calcSignedArcLengthPartialSum(traj.points, 0, traj.points.size());
94+
for (size_t base_id = 0; base_id + 1 < arc_length.size(); ++base_id) {
95+
// Get base pose yaw
96+
const double base_yaw = tf2::getYaw(traj.points.at(base_id).pose.orientation);
97+
98+
for (size_t target_id = base_id + 1; target_id < arc_length.size(); ++target_id) {
99+
if (arc_length[target_id] >= arc_length[base_id] + resample_offset) {
100+
// Get target pose yaw
101+
const double front_target_yaw = tf2::getYaw(traj.points.at(target_id).pose.orientation);
102+
const double back_target_yaw = tf2::getYaw(traj.points.at(target_id - 1).pose.orientation);
103+
104+
// Calc diff yaw between base pose yaw and target pose yaw
105+
const double front_diff_yaw =
106+
std::abs(autoware::universe_utils::normalizeRadian(front_target_yaw - base_yaw));
107+
const double back_diff_yaw =
108+
std::abs(autoware::universe_utils::normalizeRadian(back_target_yaw - base_yaw));
109+
110+
stat.add(std::max(front_diff_yaw, back_diff_yaw));
111+
break;
112+
}
113+
}
114+
}
115+
return stat;
116+
}
117+
83118
Accumulator<double> calcTrajectoryCurvature(const Trajectory & traj)
84119
{
85120
Accumulator<double> stat;

evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp

+4-2
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
// Copyright 2021 Tier IV, Inc.
1+
// Copyright 2025 Tier IV, Inc.
22
//
33
// Licensed under the Apache License, Version 2.0 (the "License");
44
// you may not use this file except in compliance with the License.
@@ -24,7 +24,7 @@
2424
namespace planning_diagnostics
2525
{
2626
std::optional<Accumulator<double>> MetricsCalculator::calculate(
27-
const Metric metric, const Trajectory & traj) const
27+
const Metric metric, const Trajectory & traj, const double vehicle_length_m) const
2828
{
2929
// Functions to calculate trajectory metrics
3030
switch (metric) {
@@ -34,6 +34,8 @@ std::optional<Accumulator<double>> MetricsCalculator::calculate(
3434
return metrics::calcTrajectoryInterval(traj);
3535
case Metric::relative_angle:
3636
return metrics::calcTrajectoryRelativeAngle(traj, parameters.trajectory.min_point_dist_m);
37+
case Metric::resampled_relative_angle:
38+
return metrics::calcTrajectoryResampledRelativeAngle(traj, vehicle_length_m);
3739
case Metric::length:
3840
return metrics::calcTrajectoryLength(traj);
3941
case Metric::duration:

evaluator/autoware_planning_evaluator/src/motion_evaluator_node.cpp

+5-3
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
// Copyright 2021 Tier IV, Inc.
1+
// Copyright 2025 Tier IV, Inc.
22
//
33
// Licensed under the Apache License, Version 2.0 (the "License");
44
// you may not use this file except in compliance with the License.
@@ -27,7 +27,8 @@
2727
namespace planning_diagnostics
2828
{
2929
MotionEvaluatorNode::MotionEvaluatorNode(const rclcpp::NodeOptions & node_options)
30-
: Node("motion_evaluator", node_options)
30+
: Node("motion_evaluator", node_options),
31+
vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo())
3132
{
3233
tf_buffer_ptr_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
3334
tf_listener_ptr_ = std::make_unique<tf2_ros::TransformListener>(*tf_buffer_ptr_);
@@ -57,7 +58,8 @@ MotionEvaluatorNode::~MotionEvaluatorNode()
5758
json j;
5859
for (Metric metric : metrics_) {
5960
const std::string base_name = metric_to_str.at(metric) + "/";
60-
const auto & stat = metrics_calculator_.calculate(metric, accumulated_trajectory_);
61+
const auto & stat = metrics_calculator_.calculate(
62+
metric, accumulated_trajectory_, vehicle_info_.vehicle_length_m);
6163
if (stat) {
6264
j[base_name + "min"] = stat->min();
6365
j[base_name + "max"] = stat->max();

evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp

+5-3
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
// Copyright 2021 Tier IV, Inc.
1+
// Copyright 2025 Tier IV, Inc.
22
//
33
// Licensed under the Apache License, Version 2.0 (the "License");
44
// you may not use this file except in compliance with the License.
@@ -34,7 +34,8 @@
3434
namespace planning_diagnostics
3535
{
3636
PlanningEvaluatorNode::PlanningEvaluatorNode(const rclcpp::NodeOptions & node_options)
37-
: Node("planning_evaluator", node_options)
37+
: Node("planning_evaluator", node_options),
38+
vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo())
3839
{
3940
using std::placeholders::_1;
4041
tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
@@ -298,7 +299,8 @@ void PlanningEvaluatorNode::onTrajectory(
298299
auto start = now();
299300

300301
for (Metric metric : metrics_) {
301-
const auto metric_stat = metrics_calculator_.calculate(Metric(metric), *traj_msg);
302+
const auto metric_stat =
303+
metrics_calculator_.calculate(Metric(metric), *traj_msg, vehicle_info_.vehicle_length_m);
302304
if (!metric_stat) {
303305
continue;
304306
}

evaluator/autoware_planning_evaluator/test/test_planning_evaluator_node.cpp

+40-3
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
// Copyright 2024 Tier IV, Inc.
1+
// Copyright 2025 Tier IV, Inc.
22
//
33
// Licensed under the Apache License, Version 2.0 (the "License");
44
// you may not use this file except in compliance with the License.
@@ -30,6 +30,7 @@
3030
#include <iostream>
3131
#include <memory>
3232
#include <string>
33+
#include <tuple>
3334
#include <utility>
3435
#include <vector>
3536

@@ -53,9 +54,12 @@ class EvalTest : public ::testing::Test
5354
rclcpp::NodeOptions options;
5455
const auto share_dir =
5556
ament_index_cpp::get_package_share_directory("autoware_planning_evaluator");
57+
const auto autoware_test_utils_dir =
58+
ament_index_cpp::get_package_share_directory("autoware_test_utils");
5659
options.arguments(
57-
{"--ros-args", "--params-file", share_dir + "/config/planning_evaluator.param.yaml", "-p",
58-
"output_metrics:=false"});
60+
{"--ros-args", "-p", "output_metrics:=false", "--params-file",
61+
share_dir + "/config/planning_evaluator.param.yaml", "--params-file",
62+
autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml"});
5963

6064
dummy_node = std::make_shared<rclcpp::Node>("planning_evaluator_test_node");
6165
eval_node = std::make_shared<EvalNode>(options);
@@ -83,6 +87,7 @@ class EvalTest : public ::testing::Test
8387
dummy_node, "/planning_evaluator/input/modified_goal", 1);
8488

8589
tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(dummy_node);
90+
vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*eval_node).getVehicleInfo();
8691
publishEgoPose(0.0, 0.0, 0.0);
8792
}
8893

@@ -118,6 +123,25 @@ class EvalTest : public ::testing::Test
118123
return t;
119124
}
120125

126+
Trajectory makeTrajectory(const std::vector<std::tuple<double, double, double>> & traj)
127+
{
128+
Trajectory t;
129+
t.header.frame_id = "map";
130+
TrajectoryPoint p;
131+
for (const std::tuple<double, double, double> & point : traj) {
132+
p.pose.position.x = std::get<0>(point);
133+
p.pose.position.y = std::get<1>(point);
134+
tf2::Quaternion q;
135+
q.setRPY(0.0, 0.0, std::get<2>(point));
136+
p.pose.orientation.x = q.x();
137+
p.pose.orientation.y = q.y();
138+
p.pose.orientation.z = q.z();
139+
p.pose.orientation.w = q.w();
140+
t.points.push_back(p);
141+
}
142+
return t;
143+
}
144+
121145
void publishTrajectory(const Trajectory & traj)
122146
{
123147
traj_pub_->publish(traj);
@@ -213,6 +237,9 @@ class EvalTest : public ::testing::Test
213237
rclcpp::Subscription<MetricArrayMsg>::SharedPtr metric_sub_;
214238
// TF broadcaster
215239
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
240+
241+
public:
242+
autoware::vehicle_info_utils::VehicleInfo vehicle_info_;
216243
};
217244

218245
TEST_F(EvalTest, TestCurvature)
@@ -250,6 +277,16 @@ TEST_F(EvalTest, TestRelativeAngle)
250277
EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), 0.0);
251278
}
252279

280+
TEST_F(EvalTest, TestResampledRelativeAngle)
281+
{
282+
setTargetMetric(planning_diagnostics::Metric::resampled_relative_angle);
283+
Trajectory t = makeTrajectory({{0.0, 0.0, 0.0}, {vehicle_info_.vehicle_length_m, 0.0, 0.0}});
284+
EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), 0.0);
285+
t = makeTrajectory(
286+
{{0.0, 0.0, 0.0}, {vehicle_info_.vehicle_length_m, vehicle_info_.vehicle_length_m, M_PI_4}});
287+
EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), M_PI_4);
288+
}
289+
253290
TEST_F(EvalTest, TestLength)
254291
{
255292
setTargetMetric(planning_diagnostics::Metric::length);

0 commit comments

Comments
 (0)