From b2e215a2afa5383a14a2bc9ac80c3ff2b32be391 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara <32741405+kyoichi-sugahara@users.noreply.github.com> Date: Mon, 23 Dec 2024 18:24:08 +0900 Subject: [PATCH] feat(planning_evaluator): add evaluation feature of trajectory lateral displacement (#9718) * feat(planning_evaluator): add evaluation feature of trajectory lateral displacement Signed-off-by: Kyoichi Sugahara * feat(metrics_calculator): implement lookahead trajectory calculation and remove deprecated method Signed-off-by: Kyoichi Sugahara * fix(planning_evaluator): rename lateral_trajectory_displacement to trajectory_lateral_displacement for consistency Signed-off-by: Kyoichi Sugahara --------- Signed-off-by: Kyoichi Sugahara --- .../autoware_planning_evaluator/README.md | 1 + .../config/planning_evaluator.param.yaml | 3 +- .../planning_evaluator/metrics/metric.hpp | 4 ++ .../metrics/metrics_utils.hpp | 24 +++++++++ .../metrics/stability_metrics.hpp | 18 +++++++ .../planning_evaluator/metrics_calculator.hpp | 15 ++---- .../planning_evaluator/parameters.hpp | 1 + .../src/metrics/metrics_utils.cpp | 49 +++++++++++++++++ .../src/metrics/stability_metrics.cpp | 44 +++++++++++++++ .../src/metrics_calculator.cpp | 54 ++++++------------- .../src/planning_evaluator_node.cpp | 4 +- 11 files changed, 164 insertions(+), 53 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..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 @@ -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..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 @@ -25,6 +25,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 +36,29 @@ using autoware_planning_msgs::msg::Trajectory; */ 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 + * 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..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 @@ -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 @@ -83,23 +84,13 @@ 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_; 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..7451264f037a6 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,52 @@ 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 = + 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..b99a4ebd20050 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,47 @@ 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( + 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); + stat.add(std::abs(dist)); + } + 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..201fbcba0e9f7 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" @@ -53,20 +54,23 @@ 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( + 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() @@ -117,33 +122,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 diff --git a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp index 9318ec1db4234..5889f15e48390 100644 --- a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp @@ -51,6 +51,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"); @@ -350,7 +352,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) {