From 2aa69b41fddfd647b77213d783925cd165573d93 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Mon, 23 Dec 2024 11:59:28 +0900 Subject: [PATCH 1/7] feat(planning_evaluator): add evaluation feature of trajectory lateral displacement Signed-off-by: Kyoichi Sugahara --- .../autoware_planning_evaluator/README.md | 1 + .../config/planning_evaluator.param.yaml | 1 + .../planning_evaluator/metrics/metric.hpp | 4 ++ .../metrics/metrics_utils.hpp | 14 ++++++- .../metrics/stability_metrics.hpp | 18 ++++++++ .../planning_evaluator/metrics_calculator.hpp | 4 +- .../planning_evaluator/parameters.hpp | 1 + .../src/metrics/metrics_utils.cpp | 20 +++++++++ .../src/metrics/stability_metrics.cpp | 42 +++++++++++++++++++ .../src/metrics_calculator.cpp | 9 +++- .../src/planning_evaluator_node.cpp | 4 +- 11 files changed, 113 insertions(+), 5 deletions(-) diff --git a/evaluator/autoware_planning_evaluator/README.md b/evaluator/autoware_planning_evaluator/README.md index 4fcdf4d7e55fd..b9dd3201a2541 100644 --- a/evaluator/autoware_planning_evaluator/README.md +++ b/evaluator/autoware_planning_evaluator/README.md @@ -13,6 +13,7 @@ Metrics are calculated using the following information: - the previous trajectory `T(-1)`. - the _reference_ trajectory assumed to be used as the reference to plan `T(0)`. - the current ego pose. +- the current ego odometry. - the set of objects in the environment. These information are maintained by an instance of class `MetricsCalculator` diff --git a/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml b/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml index e00851de63b9c..a7143a7d9897a 100644 --- a/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml +++ b/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml @@ -25,6 +25,7 @@ trajectory: min_point_dist_m: 0.1 # [m] minimum distance between two successive points to use for angle calculation + evaluation_time_s: 5.0 # [s] time duration for trajectory evaluation in seconds lookahead: max_dist_m: 5.0 # [m] maximum distance from ego along the trajectory to use for calculation max_time_s: 3.0 # [s] maximum time ahead of ego along the trajectory to use for calculation 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 13dad65b239b1..22b365881ce28 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 @@ -40,6 +40,7 @@ enum class Metric { lateral_trajectory_displacement, stability, stability_frechet, + trajectory_lateral_displacement, obstacle_distance, obstacle_ttc, modified_goal_longitudinal_deviation, @@ -66,6 +67,7 @@ static const std::unordered_map str_to_metric = { {"lateral_trajectory_displacement", Metric::lateral_trajectory_displacement}, {"stability", Metric::stability}, {"stability_frechet", Metric::stability_frechet}, + {"trajectory_lateral_displacement", Metric::trajectory_lateral_displacement}, {"obstacle_distance", Metric::obstacle_distance}, {"obstacle_ttc", Metric::obstacle_ttc}, {"modified_goal_longitudinal_deviation", Metric::modified_goal_longitudinal_deviation}, @@ -87,6 +89,7 @@ static const std::unordered_map metric_to_str = { {Metric::lateral_trajectory_displacement, "lateral_trajectory_displacement"}, {Metric::stability, "stability"}, {Metric::stability_frechet, "stability_frechet"}, + {Metric::trajectory_lateral_displacement, "trajectory_lateral_displacement"}, {Metric::obstacle_distance, "obstacle_distance"}, {Metric::obstacle_ttc, "obstacle_ttc"}, {Metric::modified_goal_longitudinal_deviation, "modified_goal_longitudinal_deviation"}, @@ -109,6 +112,7 @@ static const std::unordered_map metric_descriptions = { {Metric::lateral_trajectory_displacement, "Nearest Pose Lateral Deviation[m]"}, {Metric::stability, "Stability[m]"}, {Metric::stability_frechet, "StabilityFrechet[m]"}, + {Metric::trajectory_lateral_displacement, "Trajectory_lateral_displacement[m]"}, {Metric::obstacle_distance, "Obstacle_distance[m]"}, {Metric::obstacle_ttc, "Obstacle_time_to_collision[s]"}, {Metric::modified_goal_longitudinal_deviation, "Modified_goal_longitudinal_deviation[m]"}, diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp index 0006e49c3338a..98c7351d0f141 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp @@ -14,7 +14,6 @@ #ifndef AUTOWARE__PLANNING_EVALUATOR__METRICS__METRICS_UTILS_HPP_ #define AUTOWARE__PLANNING_EVALUATOR__METRICS__METRICS_UTILS_HPP_ - #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -25,6 +24,7 @@ namespace metrics namespace utils { using autoware_planning_msgs::msg::Trajectory; +using geometry_msgs::msg::Pose; /** * @brief find the index in the trajectory at the given distance of the given index @@ -35,6 +35,18 @@ using autoware_planning_msgs::msg::Trajectory; */ size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, const double distance); +/** + * @brief calculate the total distance from ego position to the end of trajectory + * @details finds the nearest point to ego position on the trajectory and calculates + * the cumulative distance by summing up the distances between consecutive points + * from that position to the end of the trajectory. + * + * @param [in] traj input trajectory to calculate distance along + * @param [in] ego_pose current ego vehicle pose + * @return total distance from ego position to trajectory end in meters + */ +double calc_lookahead_trajectory_distance(const Trajectory & traj, const Pose & ego_pose); + } // namespace utils } // namespace metrics } // namespace planning_diagnostics diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp index 38f388feeadda..69df00b26551b 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp @@ -18,6 +18,7 @@ #include "autoware/universe_utils/math/accumulator.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" +#include namespace planning_diagnostics { @@ -42,6 +43,23 @@ Accumulator calcFrechetDistance(const Trajectory & traj1, const Trajecto */ Accumulator calcLateralDistance(const Trajectory & traj1, const Trajectory & traj2); +/** + * @brief calculate the total lateral displacement between two trajectories + * @details Evaluates the cumulative absolute lateral displacement by sampling points + * along the first trajectory and measuring their offset from the second trajectory. + * The evaluation section length is determined by the ego vehicle's velocity and + * the specified evaluation time. + * + * @param traj1 first trajectory to compare + * @param traj2 second trajectory to compare against + * @param [in] ego_odom current ego vehicle odometry containing pose and velocity + * @param [in] trajectory_eval_time_s time duration for trajectory evaluation in seconds + * @return statistical accumulator containing the total lateral displacement + */ +Accumulator calcTrajectoryLateralDisplacement( + const Trajectory traj1, const Trajectory traj2, const nav_msgs::msg::Odometry & ego_odom, + const double trajectory_eval_time_s); + } // namespace metrics } // namespace planning_diagnostics 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 97d23cad10af2..90fe334309130 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 @@ -23,6 +23,7 @@ #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/pose.hpp" +#include #include @@ -74,7 +75,7 @@ class MetricsCalculator * @brief set the ego pose * @param [in] traj input previous trajectory */ - void setEgoPose(const geometry_msgs::msg::Pose & pose); + void setEgoPose(const nav_msgs::msg::Odometry & ego_odometry); /** * @brief get the ego pose @@ -100,6 +101,7 @@ class MetricsCalculator Trajectory previous_trajectory_lookahead_; PredictedObjects dynamic_objects_; geometry_msgs::msg::Pose ego_pose_; + nav_msgs::msg::Odometry ego_odometry_; PoseWithUuidStamped modified_goal_; }; // class MetricsCalculator diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/parameters.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/parameters.hpp index cd894fecc2331..94920195ee89c 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/parameters.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/parameters.hpp @@ -31,6 +31,7 @@ struct Parameters struct { double min_point_dist_m = 0.1; + double evaluation_time_s = 5.0; struct { double max_dist_m = 5.0; diff --git a/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp b/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp index 669afdd7229b0..2ec2e7a29ce9b 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp @@ -12,8 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/planning_evaluator/metrics/trajectory_metrics.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" + namespace planning_diagnostics { namespace metrics @@ -23,6 +25,7 @@ namespace utils using autoware::universe_utils::calcDistance2d; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::Pose; size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, const double distance) { @@ -41,6 +44,23 @@ size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, cons return target_id; } +double calc_lookahead_trajectory_distance(const Trajectory & traj, const Pose & ego_pose) +{ + const auto ego_index = + autoware::motion_utils::findNearestSegmentIndex(traj.points, ego_pose.position); + double dist = 0.0; + auto curr_point_it = std::next(traj.points.begin(), ego_index); + auto prev_point_it = curr_point_it; + for (size_t i = 0; i < traj.points.size(); ++i) { + const auto d = autoware::universe_utils::calcDistance2d( + prev_point_it->pose.position, curr_point_it->pose.position); + dist += d; + prev_point_it = curr_point_it; + ++curr_point_it; + } + + return dist; +} } // namespace utils } // namespace metrics } // namespace planning_diagnostics diff --git a/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp index e6ede0d07b9b3..2a352e6566b32 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp @@ -14,7 +14,9 @@ #include "autoware/planning_evaluator/metrics/stability_metrics.hpp" +#include "autoware/motion_utils/resample/resample.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/planning_evaluator/metrics/metrics_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include @@ -96,5 +98,45 @@ Accumulator calcLateralDistance(const Trajectory & traj1, const Trajecto return stat; } +Accumulator calcTrajectoryLateralDisplacement( + const Trajectory traj1, const Trajectory traj2, const nav_msgs::msg::Odometry & ego_odom, + const double trajectory_eval_time_s) +{ + Accumulator stat; + + if (traj1.points.empty() || traj2.points.empty()) { + return stat; + } + + const double ego_velocity = + std::hypot(ego_odom.twist.twist.linear.x, ego_odom.twist.twist.linear.y); + + const double evaluation_section_length = trajectory_eval_time_s * std::abs(ego_velocity); + + const double traj1_lookahead_distance = + utils::calc_lookahead_trajectory_distance(traj1, ego_odom.pose.pose); + const double traj2_lookahead_distance = + utils::calc_lookahead_trajectory_distance(traj2, ego_odom.pose.pose); + + if ( + traj1_lookahead_distance < evaluation_section_length || + traj2_lookahead_distance < evaluation_section_length) { + return stat; + } + + constexpr double num_evaluation_points = 10.0; + const double interval = evaluation_section_length / num_evaluation_points; + + const auto resampled_traj1 = autoware::motion_utils::resampleTrajectory(traj1, interval); + double total_lateral_displacement = 0.0; + for (const auto & point : resampled_traj1.points) { + const auto p0 = autoware::universe_utils::getPoint(point); + const double dist = autoware::motion_utils::calcLateralOffset(traj2.points, p0); + total_lateral_displacement += std::abs(dist); + } + stat.add(total_lateral_displacement); + return stat; +} + } // namespace metrics } // namespace planning_diagnostics diff --git a/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp index b7676c2fdab6c..79a7f537fff97 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp @@ -20,6 +20,7 @@ #include "autoware/planning_evaluator/metrics/stability_metrics.hpp" #include "autoware/planning_evaluator/metrics/trajectory_metrics.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" + namespace planning_diagnostics { std::optional> MetricsCalculator::calculate( @@ -67,6 +68,9 @@ std::optional> MetricsCalculator::calculate( getLookaheadTrajectory( traj, parameters.trajectory.lookahead.max_dist_m, parameters.trajectory.lookahead.max_time_s)); + case Metric::trajectory_lateral_displacement: + return metrics::calcTrajectoryLateralDisplacement( + previous_trajectory_, traj, ego_odometry_, parameters.trajectory.evaluation_time_s); case Metric::obstacle_distance: return metrics::calcDistanceToObstacle(dynamic_objects_, traj); case Metric::obstacle_ttc: @@ -107,9 +111,10 @@ void MetricsCalculator::setPredictedObjects(const PredictedObjects & objects) dynamic_objects_ = objects; } -void MetricsCalculator::setEgoPose(const geometry_msgs::msg::Pose & pose) +void MetricsCalculator::setEgoPose(const nav_msgs::msg::Odometry & ego_odometry) { - ego_pose_ = pose; + ego_pose_ = ego_odometry.pose.pose; + ego_odometry_ = ego_odometry; } Pose MetricsCalculator::getEgoPose() diff --git a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp index a61e56cd532ad..2a613c8f99520 100644 --- a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp @@ -50,6 +50,8 @@ PlanningEvaluatorNode::PlanningEvaluatorNode(const rclcpp::NodeOptions & node_op declare_parameter("trajectory.lookahead.max_dist_m"); metrics_calculator_.parameters.trajectory.lookahead.max_time_s = declare_parameter("trajectory.lookahead.max_time_s"); + metrics_calculator_.parameters.trajectory.evaluation_time_s = + declare_parameter("trajectory.evaluation_time_s"); metrics_calculator_.parameters.obstacle.dist_thr_m = declare_parameter("obstacle.dist_thr_m"); @@ -343,7 +345,7 @@ void PlanningEvaluatorNode::onModifiedGoal( void PlanningEvaluatorNode::onOdometry(const Odometry::ConstSharedPtr odometry_msg) { if (!odometry_msg) return; - metrics_calculator_.setEgoPose(odometry_msg->pose.pose); + metrics_calculator_.setEgoPose(*odometry_msg); { getRouteData(); if (route_handler_.isHandlerReady() && odometry_msg) { From 104fb0601b56d4666befa72ae147a171ec36e7f6 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Mon, 23 Dec 2024 17:10:40 +0900 Subject: [PATCH 2/7] chore(metrics_utils): add missing newline for code style consistency Signed-off-by: Kyoichi Sugahara --- .../autoware/planning_evaluator/metrics/metrics_utils.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp index 98c7351d0f141..123046bcb0416 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp @@ -14,6 +14,7 @@ #ifndef AUTOWARE__PLANNING_EVALUATOR__METRICS__METRICS_UTILS_HPP_ #define AUTOWARE__PLANNING_EVALUATOR__METRICS__METRICS_UTILS_HPP_ + #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" From 4bb65ba4369a15dcddbf772d0a13cd466b387456 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Mon, 23 Dec 2024 17:12:27 +0900 Subject: [PATCH 3/7] feat(metrics_calculator): implement lookahead trajectory calculation and remove deprecated method Signed-off-by: Kyoichi Sugahara --- .../planning_evaluator/metrics_calculator.hpp | 11 ----- .../src/metrics/metrics_utils.cpp | 29 ++++++++++++ .../src/metrics_calculator.cpp | 46 ++++--------------- 3 files changed, 38 insertions(+), 48 deletions(-) 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 90fe334309130..fe9d9eaf4278b 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 @@ -84,17 +84,6 @@ class MetricsCalculator Pose getEgoPose(); private: - /** - * @brief trim a trajectory from the current ego pose to some fixed time or distance - * @param [in] traj input trajectory to trim - * @param [in] max_dist_m [m] maximum distance ahead of the ego pose - * @param [in] max_time_s [s] maximum time ahead of the ego pose - * @return sub-trajectory starting from the ego pose and of maximum length max_dist_m, maximum - * duration max_time_s - */ - Trajectory getLookaheadTrajectory( - const Trajectory & traj, const double max_dist_m, const double max_time_s) const; - Trajectory reference_trajectory_; Trajectory reference_trajectory_lookahead_; Trajectory previous_trajectory_; diff --git a/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp b/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp index 2ec2e7a29ce9b..7451264f037a6 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp @@ -44,6 +44,35 @@ size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, cons return target_id; } +Trajectory get_lookahead_trajectory( + const Trajectory & traj, const Pose & ego_pose, const double max_dist_m, const double max_time_s) +{ + if (traj.points.empty()) { + return traj; + } + + const auto ego_index = + autoware::motion_utils::findNearestSegmentIndex(traj.points, ego_pose.position); + Trajectory lookahead_traj; + lookahead_traj.header = traj.header; + double dist = 0.0; + double time = 0.0; + auto curr_point_it = std::next(traj.points.begin(), ego_index); + auto prev_point_it = curr_point_it; + while (curr_point_it != traj.points.end() && dist <= max_dist_m && time <= max_time_s) { + lookahead_traj.points.push_back(*curr_point_it); + const auto d = autoware::universe_utils::calcDistance2d( + prev_point_it->pose.position, curr_point_it->pose.position); + dist += d; + if (prev_point_it->longitudinal_velocity_mps != 0.0) { + time += d / std::abs(prev_point_it->longitudinal_velocity_mps); + } + prev_point_it = curr_point_it; + ++curr_point_it; + } + return lookahead_traj; +} + double calc_lookahead_trajectory_distance(const Trajectory & traj, const Pose & ego_pose) { const auto ego_index = diff --git a/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp index 79a7f537fff97..e212175e488ad 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp @@ -16,6 +16,7 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/planning_evaluator/metrics/deviation_metrics.hpp" +#include "autoware/planning_evaluator/metrics/metrics_utils.hpp" #include "autoware/planning_evaluator/metrics/obstacle_metrics.hpp" #include "autoware/planning_evaluator/metrics/stability_metrics.hpp" #include "autoware/planning_evaluator/metrics/trajectory_metrics.hpp" @@ -54,19 +55,19 @@ std::optional> MetricsCalculator::calculate( return metrics::calcLateralTrajectoryDisplacement(previous_trajectory_, traj, ego_pose_); case Metric::stability_frechet: return metrics::calcFrechetDistance( - getLookaheadTrajectory( - previous_trajectory_, parameters.trajectory.lookahead.max_dist_m, + metrics::utils::get_lookahead_trajectory( + previous_trajectory_, ego_pose_, parameters.trajectory.lookahead.max_dist_m, parameters.trajectory.lookahead.max_time_s), - getLookaheadTrajectory( - traj, parameters.trajectory.lookahead.max_dist_m, + metrics::utils::get_lookahead_trajectory( + traj, ego_pose_, parameters.trajectory.lookahead.max_dist_m, parameters.trajectory.lookahead.max_time_s)); case Metric::stability: return metrics::calcLateralDistance( - getLookaheadTrajectory( - previous_trajectory_, parameters.trajectory.lookahead.max_dist_m, + metrics::utils::get_lookahead_trajectory( + previous_trajectory_, ego_pose_, parameters.trajectory.lookahead.max_dist_m, parameters.trajectory.lookahead.max_time_s), - getLookaheadTrajectory( - traj, parameters.trajectory.lookahead.max_dist_m, + metrics::utils::get_lookahead_trajectory( + traj, ego_pose_, parameters.trajectory.lookahead.max_dist_m, parameters.trajectory.lookahead.max_time_s)); case Metric::trajectory_lateral_displacement: return metrics::calcTrajectoryLateralDisplacement( @@ -122,33 +123,4 @@ Pose MetricsCalculator::getEgoPose() return ego_pose_; } -Trajectory MetricsCalculator::getLookaheadTrajectory( - const Trajectory & traj, const double max_dist_m, const double max_time_s) const -{ - if (traj.points.empty()) { - return traj; - } - - const auto ego_index = - autoware::motion_utils::findNearestSegmentIndex(traj.points, ego_pose_.position); - Trajectory lookahead_traj; - lookahead_traj.header = traj.header; - double dist = 0.0; - double time = 0.0; - auto curr_point_it = std::next(traj.points.begin(), ego_index); - auto prev_point_it = curr_point_it; - while (curr_point_it != traj.points.end() && dist <= max_dist_m && time <= max_time_s) { - lookahead_traj.points.push_back(*curr_point_it); - const auto d = autoware::universe_utils::calcDistance2d( - prev_point_it->pose.position, curr_point_it->pose.position); - dist += d; - if (prev_point_it->longitudinal_velocity_mps != 0.0) { - time += d / std::abs(prev_point_it->longitudinal_velocity_mps); - } - prev_point_it = curr_point_it; - ++curr_point_it; - } - return lookahead_traj; -} - } // namespace planning_diagnostics From 051d7e8d4368f233f7a4e7b13271be2549bdca04 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Mon, 23 Dec 2024 17:13:09 +0900 Subject: [PATCH 4/7] feat(stability_metrics): enhance lateral displacement calculation using lookahead trajectory Signed-off-by: Kyoichi Sugahara --- .../src/metrics/stability_metrics.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp index 2a352e6566b32..b99a4ebd20050 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp @@ -127,14 +127,16 @@ Accumulator calcTrajectoryLateralDisplacement( constexpr double num_evaluation_points = 10.0; const double interval = evaluation_section_length / num_evaluation_points; - const auto resampled_traj1 = autoware::motion_utils::resampleTrajectory(traj1, interval); - double total_lateral_displacement = 0.0; + const auto resampled_traj1 = autoware::motion_utils::resampleTrajectory( + utils::get_lookahead_trajectory( + traj1, ego_odom.pose.pose, evaluation_section_length, trajectory_eval_time_s), + interval); + for (const auto & point : resampled_traj1.points) { const auto p0 = autoware::universe_utils::getPoint(point); const double dist = autoware::motion_utils::calcLateralOffset(traj2.points, p0); - total_lateral_displacement += std::abs(dist); + stat.add(std::abs(dist)); } - stat.add(total_lateral_displacement); return stat; } From 5d7f8d58ca6c25077a6274b9b20efad1e38b0fd5 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Mon, 23 Dec 2024 17:15:23 +0900 Subject: [PATCH 5/7] fix(planning_evaluator): rename lateral_trajectory_displacement to trajectory_lateral_displacement for consistency Signed-off-by: Kyoichi Sugahara --- .../config/planning_evaluator.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml b/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml index a7143a7d9897a..73c1f3dfded09 100644 --- a/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml +++ b/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml @@ -14,7 +14,7 @@ - lateral_deviation - yaw_deviation - velocity_deviation - - lateral_trajectory_displacement + - trajectory_lateral_displacement - stability - stability_frechet - obstacle_distance From 2f06b233d799a6918f6c036ace480f7f87b07c76 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Mon, 23 Dec 2024 17:17:31 +0900 Subject: [PATCH 6/7] chore(metrics_calculator): remove unnecessary blank line for code style consistency Signed-off-by: Kyoichi Sugahara --- evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp index e212175e488ad..201fbcba0e9f7 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp @@ -21,7 +21,6 @@ #include "autoware/planning_evaluator/metrics/stability_metrics.hpp" #include "autoware/planning_evaluator/metrics/trajectory_metrics.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" - namespace planning_diagnostics { std::optional> MetricsCalculator::calculate( From 2a221dfc1f609a191939a869504e1d9a709450ee Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Mon, 23 Dec 2024 17:52:50 +0900 Subject: [PATCH 7/7] feat(metrics_utils): add function to trim trajectory based on distance and time from ego pose Signed-off-by: Kyoichi Sugahara --- .../planning_evaluator/metrics/metrics_utils.hpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp index 123046bcb0416..9f2b99007af7d 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp @@ -36,6 +36,17 @@ using geometry_msgs::msg::Pose; */ size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, const double distance); +/** + * @brief trim a trajectory from the current ego pose to some fixed time or distance + * @param [in] traj input trajectory to trim + * @param [in] max_dist_m [m] maximum distance ahead of the ego pose + * @param [in] max_time_s [s] maximum time ahead of the ego pose + * @return sub-trajectory starting from the ego pose and of maximum length max_dist_m, maximum + * duration max_time_s + */ +Trajectory get_lookahead_trajectory( + const Trajectory & traj, const Pose & ego_pose, const double max_dist_m, const double max_time_s); + /** * @brief calculate the total distance from ego position to the end of trajectory * @details finds the nearest point to ego position on the trajectory and calculates