pre-commit

Signed-off-by: xtk8532704 <[email protected]>

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

@@ -78,6 +78,7 @@ class ControlEvaluatorNode : public rclcpp::Node
   void AddLaneletInfoMsg(const Pose & ego_pose);
   void AddKinematicStateMetricMsg(
     const Odometry & odom, const AccelWithCovarianceStamped & accel_stamped);
+  void AddSteeringMetricMsg(const SteeringReport & steering_report);
 
   void onTimer();
 
@@ -97,7 +98,7 @@ class ControlEvaluatorNode : public rclcpp::Node
   autoware::universe_utils::InterProcessPollingSubscriber<PathWithLaneId> behavior_path_subscriber_{
     this, "~/input/behavior_path"};
   autoware::universe_utils::InterProcessPollingSubscriber<SteeringReport> steering_sub_{
-    this, "~/input/steering_report"};
+    this, "~/input/steering_status"};
 
   rclcpp::Publisher<autoware_internal_debug_msgs::msg::Float64Stamped>::SharedPtr
     processing_time_pub_;
@@ -112,8 +113,16 @@ class ControlEvaluatorNode : public rclcpp::Node
   // Metric
   const std::vector<Metric> metrics_ = {
     // collect all metrics
-    Metric::lateral_deviation,      Metric::yaw_deviation,      Metric::goal_longitudinal_deviation,
-    Metric::goal_lateral_deviation, Metric::goal_yaw_deviation,
+    Metric::lateral_deviation,
+    Metric::yaw_deviation,
+    Metric::goal_longitudinal_deviation,
+    Metric::goal_lateral_deviation,
+    Metric::goal_yaw_deviation,
+    Metric::left_boundary_distance,
+    Metric::right_boundary_distance,
+    Metric::steering_angle,
+    Metric::steering_rate,
+    Metric::steering_acceleration,
   };
 
   std::array<Accumulator<double>, static_cast<size_t>(Metric::SIZE)>
@@ -124,6 +133,9 @@ class ControlEvaluatorNode : public rclcpp::Node
   autoware::route_handler::RouteHandler route_handler_;
   rclcpp::TimerBase::SharedPtr timer_;
   std::optional<AccelWithCovarianceStamped> prev_acc_stamped_{std::nullopt};
+  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};
 };
 }  // namespace control_diagnostics
 

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

@@ -33,6 +33,9 @@ enum class Metric {
   goal_yaw_deviation,
   left_boundary_distance,
   right_boundary_distance,
+  steering_angle,
+  steering_rate,
+  steering_acceleration,
   SIZE,
 };
 
@@ -44,6 +47,9 @@ static const std::unordered_map<std::string, Metric> str_to_metric = {
   {"goal_yaw_deviation", Metric::goal_yaw_deviation},
   {"left_boundary_distance", Metric::left_boundary_distance},
   {"right_boundary_distance", Metric::right_boundary_distance},
+  {"steering_angle", Metric::steering_angle},
+  {"steering_rate", Metric::steering_rate},
+  {"steering_acceleration", Metric::steering_acceleration},
 };
 
 static const std::unordered_map<Metric, std::string> metric_to_str = {
@@ -54,6 +60,9 @@ static const std::unordered_map<Metric, std::string> metric_to_str = {
   {Metric::goal_yaw_deviation, "goal_yaw_deviation"},
   {Metric::left_boundary_distance, "left_boundary_distance"},
   {Metric::right_boundary_distance, "right_boundary_distance"},
+  {Metric::steering_angle, "steering_angle"},
+  {Metric::steering_rate, "steering_rate"},
+  {Metric::steering_acceleration, "steering_acceleration"},
 };
 
 // Metrics descriptions
@@ -65,6 +74,9 @@ static const std::unordered_map<Metric, std::string> metric_descriptions = {
   {Metric::goal_yaw_deviation, "Yaw deviation from the goal point[rad]"},
   {Metric::left_boundary_distance, "Signed distance to the left boundary[m]"},
   {Metric::right_boundary_distance, "Signed distance to the right boundary[m]"},
+  {Metric::steering_angle, "Steering angle[rad]"},
+  {Metric::steering_rate, "Steering angle rate[rad/s]"},
+  {Metric::steering_acceleration, "Steering angle acceleration[rad/s^2]"},
 };
 
 namespace details

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

@@ -14,10 +14,11 @@
 
 #ifndef AUTOWARE__CONTROL_EVALUATOR__METRICS__METRICS_UTILS_HPP_
 #define AUTOWARE__CONTROL_EVALUATOR__METRICS__METRICS_UTILS_HPP_
-
 #include <autoware/route_handler/route_handler.hpp>
 #include <autoware/universe_utils/geometry/boost_geometry.hpp>
 
+#include <vector>
+
 namespace control_diagnostics
 {
 namespace metrics

evaluator/autoware_control_evaluator/package.xml

@@ -22,14 +22,14 @@
   <depend>autoware_test_utils</depend>
   <depend>autoware_universe_utils</depend>
   <depend>autoware_vehicle_info_utils</depend>
+  <depend>autoware_vehicle_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>nlohmann-json-dev</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
-  <depend>autoware_vehicle_msgs</depend>
   <depend>tier4_metric_msgs</depend>
   <depend>tier4_planning_msgs</depend>
 

evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp

@@ -244,6 +244,47 @@ void ControlEvaluatorNode::AddKinematicStateMetricMsg(
   return;
 }
 
+void ControlEvaluatorNode::AddSteeringMetricMsg(const SteeringReport & steering_status)
+{
+  // steering angle
+  double cur_steering_angle = steering_status.steering_tire_angle;
+  const double cur_t = static_cast<double>(steering_status.stamp.sec) +
+                       static_cast<double>(steering_status.stamp.nanosec) * 1e-9;
+  AddMetricMsg(Metric::steering_angle, cur_steering_angle);
+
+  if (!prev_steering_angle_timestamp_.has_value()) {
+    prev_steering_angle_timestamp_ = cur_t;
+    prev_steering_angle_ = cur_steering_angle;
+    return;
+  }
+
+  // d_t
+  const double dt = cur_t - prev_steering_angle_timestamp_.value();
+  if (dt < std::numeric_limits<double>::epsilon()) {
+    prev_steering_angle_timestamp_ = cur_t;
+    prev_steering_angle_ = cur_steering_angle;
+    return;
+  }
+
+  // steering rate
+  const double steering_rate = (cur_steering_angle - prev_steering_angle_.value()) / dt;
+  AddMetricMsg(Metric::steering_rate, steering_rate);
+
+  // steering acceleration
+  if (!prev_steering_rate_.has_value()) {
+    prev_steering_angle_timestamp_ = cur_t;
+    prev_steering_angle_ = cur_steering_angle;
+    prev_steering_rate_ = steering_rate;
+    return;
+  }
+  const double steering_acceleration = (steering_rate - prev_steering_rate_.value()) / dt;
+  AddMetricMsg(Metric::steering_acceleration, steering_acceleration);
+
+  prev_steering_angle_timestamp_ = cur_t;
+  prev_steering_angle_ = cur_steering_angle;
+  prev_steering_rate_ = steering_rate;
+}
+
 void ControlEvaluatorNode::AddLateralDeviationMetricMsg(
   const Trajectory & traj, const Point & ego_point)
 {
@@ -294,7 +335,7 @@ void ControlEvaluatorNode::onTimer()
   const auto odom = odometry_sub_.takeData();
   const auto acc = accel_sub_.takeData();
   const auto behavior_path = behavior_path_subscriber_.takeData();
-  const auto steering_ = steering_sub_.takeData();
+  const auto steering_status = steering_sub_.takeData();
 
   // calculate deviation metrics
   if (odom && traj && !traj->points.empty()) {
@@ -320,8 +361,8 @@ void ControlEvaluatorNode::onTimer()
     const Pose ego_pose = odom->pose.pose;
     AddBoundaryDistanceMetricMsg(*behavior_path, ego_pose);
   }
-  if (steering_) {
-    //TODO think and add metric(steering_angle, the difference between the steering angle and the previous steering angle,etc)
+  if (steering_status) {
+    AddSteeringMetricMsg(*steering_status);
   }
   // Publish metrics
   metrics_msg_.stamp = now();

evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp

@@ -27,6 +27,7 @@
 #include <lanelet2_core/Forward.h>
 
 #include <cstddef>
+#include <vector>
 namespace control_diagnostics
 {
 namespace metrics