From bd8483437fe25769191ac986403039df136c05c7 Mon Sep 17 00:00:00 2001 From: Kazunori-Nakajima <169115284+Kazunori-Nakajima@users.noreply.github.com> Date: Mon, 3 Feb 2025 11:20:26 +0900 Subject: [PATCH] feat(autoware_planning_evaluator): add resampled_relative_angle metrics (#10020) * feat(autoware_planning_evaluator): add new large_relative_angle metrics Signed-off-by: Kasunori-Nakajima * fix copyright and vehicle_length_m Signed-off-by: Kasunori-Nakajima * style(pre-commit): autofix * del: resample trajectory Signed-off-by: Kasunori-Nakajima * del: traj points check Signed-off-by: Kasunori-Nakajima * rename msg and speed optimization Signed-off-by: Kasunori-Nakajima * style(pre-commit): autofix * add unit_test and fix resample_relative_angle Signed-off-by: Kasunori-Nakajima * style(pre-commit): autofix * include tuple to test Signed-off-by: Kasunori-Nakajima * target two point, update unit test value Signed-off-by: Kasunori-Nakajima * fix abs Signed-off-by: Kasunori-Nakajima * fix for loop bag and primitive type Signed-off-by: Kasunori-Nakajima --------- Signed-off-by: Kasunori-Nakajima Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../config/planning_evaluator.param.yaml | 1 + .../planning_evaluator/metrics/metric.hpp | 6 ++- .../metrics/trajectory_metrics.hpp | 14 +++++- .../planning_evaluator/metrics_calculator.hpp | 7 ++- .../motion_evaluator_node.hpp | 6 ++- .../planning_evaluator_node.hpp | 5 ++- .../autoware_planning_evaluator/package.xml | 1 + .../autoware_planning_evaluator.schema.json | 1 + .../src/metrics/trajectory_metrics.cpp | 35 +++++++++++++++ .../src/metrics_calculator.cpp | 6 ++- .../src/motion_evaluator_node.cpp | 8 ++-- .../src/planning_evaluator_node.cpp | 8 ++-- .../test/test_planning_evaluator_node.cpp | 43 +++++++++++++++++-- 13 files changed, 124 insertions(+), 17 deletions(-) diff --git a/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml b/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml index 7605ed2a5e859..d065f66cf0b2c 100644 --- a/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml +++ b/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml @@ -6,6 +6,7 @@ - curvature - point_interval - relative_angle + - resampled_relative_angle - length - duration - velocity diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metric.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metric.hpp index 7c207bf6c8f57..bcd70c3418469 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metric.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metric.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2025 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -29,6 +29,7 @@ enum class Metric { curvature, point_interval, relative_angle, + resampled_relative_angle, length, duration, velocity, @@ -56,6 +57,7 @@ static const std::unordered_map str_to_metric = { {"curvature", Metric::curvature}, {"point_interval", Metric::point_interval}, {"relative_angle", Metric::relative_angle}, + {"resampled_relative_angle", Metric::resampled_relative_angle}, {"length", Metric::length}, {"duration", Metric::duration}, {"velocity", Metric::velocity}, @@ -78,6 +80,7 @@ static const std::unordered_map metric_to_str = { {Metric::curvature, "curvature"}, {Metric::point_interval, "point_interval"}, {Metric::relative_angle, "relative_angle"}, + {Metric::resampled_relative_angle, "resampled_relative_angle"}, {Metric::length, "length"}, {Metric::duration, "duration"}, {Metric::velocity, "velocity"}, @@ -101,6 +104,7 @@ static const std::unordered_map metric_descriptions = { {Metric::curvature, "Curvature[1/rad]"}, {Metric::point_interval, "Interval_between_points[m]"}, {Metric::relative_angle, "Relative_angle[rad]"}, + {Metric::resampled_relative_angle, "Resampled_relative_angle[rad]"}, {Metric::length, "Trajectory_length[m]"}, {Metric::duration, "Trajectory_duration[s]"}, {Metric::velocity, "Trajectory_velocity[m/s]"}, diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/trajectory_metrics.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/trajectory_metrics.hpp index c530a8d6d8b05..d9784452a343e 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/trajectory_metrics.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/trajectory_metrics.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2025 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -15,6 +15,9 @@ #ifndef AUTOWARE__PLANNING_EVALUATOR__METRICS__TRAJECTORY_METRICS_HPP_ #define AUTOWARE__PLANNING_EVALUATOR__METRICS__TRAJECTORY_METRICS_HPP_ +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/math/accumulator.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -37,6 +40,15 @@ using autoware_planning_msgs::msg::TrajectoryPoint; Accumulator calcTrajectoryRelativeAngle( const Trajectory & traj, const double min_dist_threshold); +/** + * @brief calculate large relative angle metric (angle between successive points) + * @param [in] traj input trajectory + * @param [in] vehicle_length_m input vehicle length + * @return calculated statistics + */ +Accumulator calcTrajectoryResampledRelativeAngle( + const Trajectory & traj, const double vehicle_length_m); + /** * @brief calculate metric for the distance between trajectory points * @param [in] traj input trajectory diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics_calculator.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics_calculator.hpp index fe9d9eaf4278b..55626bd65b19c 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics_calculator.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics_calculator.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2025 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -47,9 +47,12 @@ class MetricsCalculator /** * @brief calculate * @param [in] metric Metric enum value + * @param [in] traj input trajectory + * @param [in] vehicle_length_m input vehicle length * @return string describing the requested metric */ - std::optional> calculate(const Metric metric, const Trajectory & traj) const; + std::optional> calculate( + const Metric metric, const Trajectory & traj, const double vehicle_length_m) const; std::optional> calculate( const Metric metric, const Pose & base_pose, const Pose & target_pose) const; diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/motion_evaluator_node.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/motion_evaluator_node.hpp index 7eed6e5645abc..4c5d6c8ced09c 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/motion_evaluator_node.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/motion_evaluator_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2025 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -21,6 +21,8 @@ #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" +#include + #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" @@ -34,6 +36,7 @@ namespace planning_diagnostics { using autoware::universe_utils::Accumulator; +using autoware::vehicle_info_utils::VehicleInfo; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -67,6 +70,7 @@ class MotionEvaluatorNode : public rclcpp::Node MetricsCalculator metrics_calculator_; // Metrics std::vector metrics_; + VehicleInfo vehicle_info_; std::deque stamps_; Trajectory accumulated_trajectory_; }; diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp index 8c0b49ce2dd26..2568ea19fdb32 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2025 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -24,6 +24,7 @@ #include #include #include +#include #include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp" @@ -44,6 +45,7 @@ namespace planning_diagnostics { using autoware::universe_utils::Accumulator; +using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::PoseWithUuidStamped; using autoware_planning_msgs::msg::Trajectory; @@ -167,6 +169,7 @@ class PlanningEvaluatorNode : public rclcpp::Node metric_accumulators_; // 3(min, max, mean) * metric_size rclcpp::TimerBase::SharedPtr timer_; + VehicleInfo vehicle_info_; std::optional prev_acc_stamped_{std::nullopt}; }; } // namespace planning_diagnostics diff --git a/evaluator/autoware_planning_evaluator/package.xml b/evaluator/autoware_planning_evaluator/package.xml index 22c3b93035e9d..f8d960479085d 100644 --- a/evaluator/autoware_planning_evaluator/package.xml +++ b/evaluator/autoware_planning_evaluator/package.xml @@ -19,6 +19,7 @@ autoware_planning_msgs autoware_route_handler autoware_universe_utils + autoware_vehicle_info_utils eigen geometry_msgs nav_msgs diff --git a/evaluator/autoware_planning_evaluator/schema/autoware_planning_evaluator.schema.json b/evaluator/autoware_planning_evaluator/schema/autoware_planning_evaluator.schema.json index 263bd374a7e45..506cbe4c57471 100644 --- a/evaluator/autoware_planning_evaluator/schema/autoware_planning_evaluator.schema.json +++ b/evaluator/autoware_planning_evaluator/schema/autoware_planning_evaluator.schema.json @@ -26,6 +26,7 @@ "curvature", "point_interval", "relative_angle", + "resampled_relative_angle", "length", "duration", "velocity", diff --git a/evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp index bd5eed06f96f8..9943318490c12 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp @@ -16,6 +16,9 @@ #include "autoware/planning_evaluator/metrics/metrics_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" + +#include + namespace planning_diagnostics { namespace metrics @@ -80,6 +83,38 @@ Accumulator calcTrajectoryRelativeAngle( return stat; } +Accumulator calcTrajectoryResampledRelativeAngle( + const Trajectory & traj, const double vehicle_length_m) +{ + Accumulator stat; + + 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 + 1 < arc_length.size(); ++base_id) { + // Get base pose yaw + const double base_yaw = tf2::getYaw(traj.points.at(base_id).pose.orientation); + + for (size_t target_id = base_id + 1; 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)); + break; + } + } + } + return stat; +} + Accumulator calcTrajectoryCurvature(const Trajectory & traj) { Accumulator stat; diff --git a/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp index c30420a5632fa..0a7033877c30e 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2025 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -24,7 +24,7 @@ namespace planning_diagnostics { std::optional> MetricsCalculator::calculate( - const Metric metric, const Trajectory & traj) const + const Metric metric, const Trajectory & traj, const double vehicle_length_m) const { // Functions to calculate trajectory metrics switch (metric) { @@ -34,6 +34,8 @@ std::optional> MetricsCalculator::calculate( return metrics::calcTrajectoryInterval(traj); case Metric::relative_angle: return metrics::calcTrajectoryRelativeAngle(traj, parameters.trajectory.min_point_dist_m); + case Metric::resampled_relative_angle: + return metrics::calcTrajectoryResampledRelativeAngle(traj, vehicle_length_m); case Metric::length: return metrics::calcTrajectoryLength(traj); case Metric::duration: diff --git a/evaluator/autoware_planning_evaluator/src/motion_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/motion_evaluator_node.cpp index d43aed1ec5687..3c7cc95742245 100644 --- a/evaluator/autoware_planning_evaluator/src/motion_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/motion_evaluator_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2025 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -27,7 +27,8 @@ namespace planning_diagnostics { MotionEvaluatorNode::MotionEvaluatorNode(const rclcpp::NodeOptions & node_options) -: Node("motion_evaluator", node_options) +: Node("motion_evaluator", node_options), + vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()) { tf_buffer_ptr_ = std::make_unique(this->get_clock()); tf_listener_ptr_ = std::make_unique(*tf_buffer_ptr_); @@ -57,7 +58,8 @@ MotionEvaluatorNode::~MotionEvaluatorNode() json j; for (Metric metric : metrics_) { const std::string base_name = metric_to_str.at(metric) + "/"; - const auto & stat = metrics_calculator_.calculate(metric, accumulated_trajectory_); + const auto & stat = metrics_calculator_.calculate( + metric, accumulated_trajectory_, vehicle_info_.vehicle_length_m); if (stat) { j[base_name + "min"] = stat->min(); j[base_name + "max"] = stat->max(); diff --git a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp index 53319ffa4c1fd..848b0cb8091b4 100644 --- a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2025 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -34,7 +34,8 @@ namespace planning_diagnostics { PlanningEvaluatorNode::PlanningEvaluatorNode(const rclcpp::NodeOptions & node_options) -: Node("planning_evaluator", node_options) +: Node("planning_evaluator", node_options), + vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()) { using std::placeholders::_1; tf_buffer_ = std::make_unique(this->get_clock()); @@ -298,7 +299,8 @@ void PlanningEvaluatorNode::onTrajectory( auto start = now(); for (Metric metric : metrics_) { - const auto metric_stat = metrics_calculator_.calculate(Metric(metric), *traj_msg); + const auto metric_stat = + metrics_calculator_.calculate(Metric(metric), *traj_msg, vehicle_info_.vehicle_length_m); if (!metric_stat) { continue; } diff --git a/evaluator/autoware_planning_evaluator/test/test_planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/test/test_planning_evaluator_node.cpp index f202bb4ab0e6f..df2eba0147d8c 100644 --- a/evaluator/autoware_planning_evaluator/test/test_planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/test/test_planning_evaluator_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2025 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -30,6 +30,7 @@ #include #include #include +#include #include #include @@ -53,9 +54,12 @@ class EvalTest : public ::testing::Test rclcpp::NodeOptions options; const auto share_dir = ament_index_cpp::get_package_share_directory("autoware_planning_evaluator"); + const auto autoware_test_utils_dir = + ament_index_cpp::get_package_share_directory("autoware_test_utils"); options.arguments( - {"--ros-args", "--params-file", share_dir + "/config/planning_evaluator.param.yaml", "-p", - "output_metrics:=false"}); + {"--ros-args", "-p", "output_metrics:=false", "--params-file", + share_dir + "/config/planning_evaluator.param.yaml", "--params-file", + autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml"}); dummy_node = std::make_shared("planning_evaluator_test_node"); eval_node = std::make_shared(options); @@ -83,6 +87,7 @@ class EvalTest : public ::testing::Test dummy_node, "/planning_evaluator/input/modified_goal", 1); tf_broadcaster_ = std::make_unique(dummy_node); + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*eval_node).getVehicleInfo(); publishEgoPose(0.0, 0.0, 0.0); } @@ -118,6 +123,25 @@ class EvalTest : public ::testing::Test return t; } + Trajectory makeTrajectory(const std::vector> & traj) + { + Trajectory t; + t.header.frame_id = "map"; + TrajectoryPoint p; + for (const std::tuple & point : traj) { + p.pose.position.x = std::get<0>(point); + p.pose.position.y = std::get<1>(point); + tf2::Quaternion q; + q.setRPY(0.0, 0.0, std::get<2>(point)); + p.pose.orientation.x = q.x(); + p.pose.orientation.y = q.y(); + p.pose.orientation.z = q.z(); + p.pose.orientation.w = q.w(); + t.points.push_back(p); + } + return t; + } + void publishTrajectory(const Trajectory & traj) { traj_pub_->publish(traj); @@ -213,6 +237,9 @@ class EvalTest : public ::testing::Test rclcpp::Subscription::SharedPtr metric_sub_; // TF broadcaster std::unique_ptr tf_broadcaster_; + +public: + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; }; TEST_F(EvalTest, TestCurvature) @@ -250,6 +277,16 @@ TEST_F(EvalTest, TestRelativeAngle) EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), 0.0); } +TEST_F(EvalTest, TestResampledRelativeAngle) +{ + setTargetMetric(planning_diagnostics::Metric::resampled_relative_angle); + Trajectory t = makeTrajectory({{0.0, 0.0, 0.0}, {vehicle_info_.vehicle_length_m, 0.0, 0.0}}); + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), 0.0); + t = makeTrajectory( + {{0.0, 0.0, 0.0}, {vehicle_info_.vehicle_length_m, vehicle_info_.vehicle_length_m, M_PI_4}}); + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), M_PI_4); +} + TEST_F(EvalTest, TestLength) { setTargetMetric(planning_diagnostics::Metric::length);