feat: cherry-pick PRs for control_evaluator up to the latest version

evaluator/autoware_control_evaluator/config/control_evaluator.param.yaml

@@ -1,5 +1,9 @@
 /**:
   ros__parameters:
+    output_metrics_only_moving:
+      enabled: true # if true, aggregate metrics for output_metrics statistics only when the ego is moving
+      metric_list:
+        - closest_object_distance
     planning_factor_metrics:
       topic_prefix: /planning/planning_factors/
       stop_deviation:

evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp

@@ -57,6 +57,7 @@
 using autoware_vehicle_msgs::msg::SteeringReport;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
+using geometry_msgs::msg::Twist;
 using nav_msgs::msg::Odometry;
 using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin;
 using autoware_planning_msgs::msg::LaneletRoute;
@@ -89,7 +90,9 @@
   void AddKinematicStateMetricMsg(
     const Odometry & odom, const AccelWithCovarianceStamped & accel_stamped);
   void AddSteeringMetricMsg(const SteeringReport & steering_report);
-  void AddStopDeviationMetricMsg(const Odometry & odom);
+  void AddStopDeviationMetricMsg();
+  void AddVelocityDeviationMetricMsg(
+    const Trajectory & traj, const Pose & ego_pose, const Twist & twist);
   void onTimer();
 
 private:
@@ -128,28 +131,34 @@
   double distance_filter_thr_m_;
 
   // Metric
-  const std::vector<Metric> metrics_ = {// collect all metrics
-                                        Metric::velocity,
-                                        Metric::acceleration,
-                                        Metric::jerk,
-                                        Metric::lateral_deviation,
-                                        Metric::lateral_deviation_abs,
-                                        Metric::yaw_deviation,
-                                        Metric::yaw_deviation_abs,
-                                        Metric::goal_longitudinal_deviation,
-                                        Metric::goal_longitudinal_deviation_abs,
-                                        Metric::goal_lateral_deviation,
-                                        Metric::goal_lateral_deviation_abs,
-                                        Metric::goal_yaw_deviation,
-                                        Metric::goal_yaw_deviation_abs,
-                                        Metric::left_boundary_distance,
-                                        Metric::right_boundary_distance,
-                                        Metric::steering_angle,
-                                        Metric::steering_rate,
-                                        Metric::steering_acceleration,
-                                        Metric::stop_deviation,
-                                        Metric::stop_deviation_abs,
-                                        Metric::closest_object_distance};
+  const std::vector<Metric> metrics_ = {
+    // collect all metrics
+    Metric::velocity,
+    Metric::acceleration,
+    Metric::lateral_acceleration_abs,
+    Metric::jerk,
+    Metric::lateral_deviation,
+    Metric::lateral_deviation_abs,
+    Metric::yaw_deviation,
+    Metric::yaw_deviation_abs,
+    Metric::goal_longitudinal_deviation,
+    Metric::goal_longitudinal_deviation_abs,
+    Metric::goal_lateral_deviation,
+    Metric::goal_lateral_deviation_abs,
+    Metric::goal_yaw_deviation,
+    Metric::goal_yaw_deviation_abs,
+    Metric::left_boundary_distance,
+    Metric::right_boundary_distance,
+    Metric::left_uncrossable_boundary_distance,
+    Metric::right_uncrossable_boundary_distance,
+    Metric::steering_angle,
+    Metric::steering_angle_abs,
+    Metric::steering_rate,
+    Metric::steering_acceleration,
+    Metric::stop_deviation,
+    Metric::stop_deviation_abs,
+    Metric::closest_object_distance,
+    Metric::longitudinal_velocity_deviation};
 
   std::array<Accumulator<double>, static_cast<size_t>(Metric::SIZE)>
     metric_accumulators_;  // 3(min, max, mean) * metric_size
@@ -162,6 +171,10 @@
   std::optional<double> prev_steering_angle_{std::nullopt};
   std::optional<double> prev_steering_rate_{std::nullopt};
   std::optional<double> prev_steering_angle_timestamp_{std::nullopt};
+
+  // for output_metrics_only_moving
+  float ego_speed_{0.0};
+  std::array<bool, static_cast<size_t>(Metric::SIZE)> is_output_metrics_only_moving{};
 };
 }  // namespace control_diagnostics
 

evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/deviation_metrics.hpp

@@ -66,6 +66,15 @@ double calcLateralDeviation(const Pose & base_pose, const Point & target_point);
  */
 double calcYawDeviation(const Pose & base_pose, const Pose & target_pose);
 
+/**
+ * @brief calculate longitudinal velocity deviation from base_pose to target_pose
+ * @param [in] pose input base_pose
+ * @param [in] pose input target_pose
+ * @return longitudinal velocity deviation from base_pose to target_pose
+ */
+double calcLongitudinalVelocityDeviation(
+  const Trajectory & traj, const Pose & pose, const double longitudinal_velocity);
+
 }  // namespace metrics
 }  // namespace control_diagnostics
 

evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp

@@ -28,6 +28,7 @@ namespace control_diagnostics
 enum class Metric {
   velocity,
   acceleration,
+  lateral_acceleration_abs,
   jerk,
   lateral_deviation,
   lateral_deviation_abs,
@@ -44,17 +45,20 @@ enum class Metric {
   left_uncrossable_boundary_distance,
   right_uncrossable_boundary_distance,
   steering_angle,
+  steering_angle_abs,
   steering_rate,
   steering_acceleration,
   stop_deviation,
   stop_deviation_abs,
   closest_object_distance,
+  longitudinal_velocity_deviation,
   SIZE,
 };
 
 static const std::unordered_map<std::string, Metric> str_to_metric = {
   {"velocity", Metric::velocity},
   {"acceleration", Metric::acceleration},
+  {"lateral_acceleration_abs", Metric::lateral_acceleration_abs},
   {"jerk", Metric::jerk},
   {"lateral_deviation", Metric::lateral_deviation},
   {"lateral_deviation_abs", Metric::lateral_deviation_abs},
@@ -71,16 +75,19 @@ static const std::unordered_map<std::string, Metric> str_to_metric = {
   {"left_uncrossable_boundary_distance", Metric::left_uncrossable_boundary_distance},
   {"right_uncrossable_boundary_distance", Metric::right_uncrossable_boundary_distance},
   {"steering_angle", Metric::steering_angle},
+  {"steering_angle_abs", Metric::steering_angle_abs},
   {"steering_rate", Metric::steering_rate},
   {"steering_acceleration", Metric::steering_acceleration},
   {"stop_deviation", Metric::stop_deviation},
   {"stop_deviation_abs", Metric::stop_deviation_abs},
   {"closest_object_distance", Metric::closest_object_distance},
+  {"longitudinal_velocity_deviation", Metric::longitudinal_velocity_deviation},
 };
 
 static const std::unordered_map<Metric, std::string> metric_to_str = {
   {Metric::velocity, "velocity"},
   {Metric::acceleration, "acceleration"},
+  {Metric::lateral_acceleration_abs, "lateral_acceleration_abs"},
   {Metric::jerk, "jerk"},
   {Metric::lateral_deviation, "lateral_deviation"},
   {Metric::lateral_deviation_abs, "lateral_deviation_abs"},
@@ -97,17 +104,20 @@ static const std::unordered_map<Metric, std::string> metric_to_str = {
   {Metric::left_uncrossable_boundary_distance, "left_uncrossable_boundary_distance"},
   {Metric::right_uncrossable_boundary_distance, "right_uncrossable_boundary_distance"},
   {Metric::steering_angle, "steering_angle"},
+  {Metric::steering_angle_abs, "steering_angle_abs"},
   {Metric::steering_rate, "steering_rate"},
   {Metric::steering_acceleration, "steering_acceleration"},
   {Metric::stop_deviation, "stop_deviation"},
   {Metric::stop_deviation_abs, "stop_deviation_abs"},
   {Metric::closest_object_distance, "closest_object_distance"},
+  {Metric::longitudinal_velocity_deviation, "longitudinal_velocity_deviation"},
 };
 
 // Metrics descriptions
 static const std::unordered_map<Metric, std::string> metric_descriptions = {
   {Metric::velocity, "Velocity[m/s]"},
   {Metric::acceleration, "Acceleration[m/s^2]"},
+  {Metric::lateral_acceleration_abs, "Absolute lateral acceleration[m/s^2]"},
   {Metric::jerk, "Jerk[m/s^3]"},
   {Metric::lateral_deviation,
    "Lateral deviation from the reference trajectory[m], positive value means the ego is on the "
@@ -135,6 +145,7 @@ static const std::unordered_map<Metric, std::string> metric_descriptions = {
   {Metric::left_uncrossable_boundary_distance, "Distance to the left uncrossable boundary[m]"},
   {Metric::right_uncrossable_boundary_distance, "Distance to the right uncrossable boundary[m]"},
   {Metric::steering_angle, "Steering angle[rad]"},
+  {Metric::steering_angle_abs, "Absolute steering angle[rad]"},
   {Metric::steering_rate, "Steering angle rate[rad/s]"},
   {Metric::steering_acceleration, "Steering angle acceleration[rad/s^2]"},
   {Metric::stop_deviation,
@@ -145,6 +156,9 @@ static const std::unordered_map<Metric, std::string> metric_descriptions = {
   {Metric::closest_object_distance,
    "Distance to the closest object[m], the objects outside of the distance_filter_thr_m (default: "
    "30m) are ignored"},
+  {Metric::longitudinal_velocity_deviation,
+   "Longitudinal velocity deviation from the reference trajectory[m], positive value means the "
+   "actual velocity is larger than the planned velocity."},
 };
 
 namespace details

evaluator/autoware_control_evaluator/schema/autoware_control_evaluator.schema.json

@@ -6,6 +6,25 @@
     "autoware_control_evaluator": {
       "type": "object",
       "properties": {
+        "output_metrics_only_moving": {
+          "description": "aggregate some metrics for output_metrics statistics only when the ego is moving",
+          "type": "object",
+          "properties": {
+            "enabled": {
+              "description": "enable output_metrics_only_moving",
+              "type": "boolean",
+              "default": true
+            },
+            "metric_list": {
+              "description": "list of metrics to enable for output_metrics_only_moving",
+              "type": "array",
+              "items": {
+                "type": "string"
+              },
+              "default": ["closest_object_distance"]
+            }
+          }
+        },
         "planning_factor_metrics": {
           "description": "metrics' parameters for planning_factor evaluation",
           "type": "object",