rename msg and speed optimization

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

evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml

@@ -6,7 +6,7 @@
       - curvature
       - point_interval
       - relative_angle
-      - large_relative_angle
+      - resampled_relative_angle
       - length
       - duration
       - velocity

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

@@ -29,7 +29,7 @@ enum class Metric {
   curvature,
   point_interval,
   relative_angle,
-  large_relative_angle,
+  resampled_relative_angle,
   length,
   duration,
   velocity,
@@ -57,7 +57,7 @@ static const std::unordered_map<std::string, Metric> str_to_metric = {
   {"curvature", Metric::curvature},
   {"point_interval", Metric::point_interval},
   {"relative_angle", Metric::relative_angle},
-  {"large_relative_angle", Metric::large_relative_angle},
+  {"resampled_relative_angle", Metric::resampled_relative_angle},
   {"length", Metric::length},
   {"duration", Metric::duration},
   {"velocity", Metric::velocity},
@@ -80,7 +80,7 @@ static const std::unordered_map<Metric, std::string> metric_to_str = {
   {Metric::curvature, "curvature"},
   {Metric::point_interval, "point_interval"},
   {Metric::relative_angle, "relative_angle"},
-  {Metric::large_relative_angle, "large_relative_angle"},
+  {Metric::resampled_relative_angle, "resampled_relative_angle"},
   {Metric::length, "length"},
   {Metric::duration, "duration"},
   {Metric::velocity, "velocity"},
@@ -104,7 +104,7 @@ static const std::unordered_map<Metric, std::string> metric_descriptions = {
   {Metric::curvature, "Curvature[1/rad]"},
   {Metric::point_interval, "Interval_between_points[m]"},
   {Metric::relative_angle, "Relative_angle[rad]"},
-  {Metric::large_relative_angle, "Large_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]"},

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

@@ -46,7 +46,7 @@ Accumulator<double> calcTrajectoryRelativeAngle(
  * @param [in] vehicle_length_m input vehicle length
  * @return calculated statistics
  */
-Accumulator<double> calcTrajectoryLargeRelativeAngle(
+Accumulator<double> calcTrajectoryResampledRelativeAngle(
   const Trajectory & traj, const double & vehicle_length_m);
 
 /**

evaluator/autoware_planning_evaluator/schema/autoware_planning_evaluator.schema.json

@@ -26,7 +26,7 @@
             "curvature",
             "point_interval",
             "relative_angle",
-            "large_relative_angle",
+            "resampled_relative_angle",
             "length",
             "duration",
             "velocity",

evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp

@@ -80,29 +80,33 @@ Accumulator<double> calcTrajectoryRelativeAngle(
   return stat;
 }
 
-Accumulator<double> calcTrajectoryLargeRelativeAngle(
-  const Trajectory & traj, const double & vehicle_length)
+Accumulator<double> calcTrajectoryResampledRelativeAngle(
+  const Trajectory & traj, const double & vehicle_length_m)
 {
   Accumulator<double> stat;
 
-  for (size_t base_id = 0; base_id < resample_traj.points.size() - 1; ++base_id) {
+  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) {
     // Get base pose yaw
-    const double base_yaw = tf2::getYaw(resample_traj.points.at(base_id).pose.orientation);
+    const double base_yaw = tf2::getYaw(traj.points.at(base_id).pose.orientation);
 
-    // Get target pose yaw
-    const auto points_distance = vehicle_length / 2;
-    const auto target_pose = autoware::motion_utils::calcLongitudinalOffsetPose(
-      resample_traj.points, base_id, points_distance);
-    const double target_yaw = tf2::getYaw(target_pose->orientation);
+    for (size_t i = base_id; i < arc_length.size() -1; ++i) {
+      if (arc_length[i] >= arc_length[base_id] + resample_offset) {
+        // Get target pose yaw
+        const double target_yaw = tf2::getYaw(traj.points.at(i).pose.orientation);
 
-    // Calc diff yaw between base pose yaw and target pose yaw
-    const double diff_yaw = autoware::universe_utils::normalizeRadian(target_yaw - base_yaw);
-
-    stat.add(std::abs(diff_yaw));
+        // Calc diff yaw between base pose yaw and target pose yaw
+        const double diff_yaw = autoware::universe_utils::normalizeRadian(target_yaw - base_yaw);
+        stat.add(std::abs(diff_yaw));
+        break;
+      }
+    }
   }
   return stat;
 }
 
+
 Accumulator<double> calcTrajectoryCurvature(const Trajectory & traj)
 {
   Accumulator<double> stat;

evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp

@@ -34,8 +34,8 @@ std::optional<Accumulator<double>> MetricsCalculator::calculate(
       return metrics::calcTrajectoryInterval(traj);
     case Metric::relative_angle:
       return metrics::calcTrajectoryRelativeAngle(traj, parameters.trajectory.min_point_dist_m);
-    case Metric::large_relative_angle:
-      return metrics::calcTrajectoryLargeRelativeAngle(traj, vehicle_length_m);
+    case Metric::resampled_relative_angle:
+      return metrics::calcTrajectoryResampledRelativeAngle(traj, vehicle_length_m);
     case Metric::length:
       return metrics::calcTrajectoryLength(traj);
     case Metric::duration: