feat(autoware_control_evaluator): add new steering metrics (#10012)

Author: xtk8532704

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

@@ -25,6 +25,7 @@
 #include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include "autoware_vehicle_msgs/msg/steering_report.hpp"
 #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
 #include <autoware_internal_debug_msgs/msg/float64_stamped.hpp>
 #include <autoware_planning_msgs/msg/lanelet_route.hpp>
@@ -45,6 +46,7 @@ using autoware::universe_utils::LineString2d;
 using autoware::universe_utils::Point2d;
 using autoware::vehicle_info_utils::VehicleInfo;
 using autoware_planning_msgs::msg::Trajectory;
+using autoware_vehicle_msgs::msg::SteeringReport;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
 using nav_msgs::msg::Odometry;
@@ -75,6 +77,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();
 
@@ -93,6 +96,8 @@ class ControlEvaluatorNode : public rclcpp::Node
     vector_map_subscriber_{this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()};
   autoware::universe_utils::InterProcessPollingSubscriber<PathWithLaneId> behavior_path_subscriber_{
     this, "~/input/behavior_path"};
+  autoware::universe_utils::InterProcessPollingSubscriber<SteeringReport> steering_sub_{
+    this, "~/input/steering_status"};
 
   rclcpp::Publisher<autoware_internal_debug_msgs::msg::Float64Stamped>::SharedPtr
     processing_time_pub_;
@@ -107,8 +112,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)>
@@ -119,6 +132,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/launch/control_evaluator.launch.xml

@@ -6,6 +6,7 @@
   <arg name="input/vector_map" default="/map/vector_map"/>
   <arg name="input/route" default="/planning/mission_planning/route"/>
   <arg name="input/behavior_path" default="/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id"/>
+  <arg name="input/steering_status" default="/vehicle/status/steering_status"/>
 
   <!-- control evaluator -->
   <group>
@@ -17,6 +18,7 @@
       <remap from="~/input/vector_map" to="$(var input/vector_map)"/>
       <remap from="~/input/route" to="$(var input/route)"/>
       <remap from="~/input/behavior_path" to="$(var input/behavior_path)"/>
+      <remap from="~/input/steering_status" to="$(var input/steering_status)"/>
     </node>
   </group>
 </launch>

evaluator/autoware_control_evaluator/package.xml

@@ -22,6 +22,7 @@
   <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>

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,6 +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_status = steering_sub_.takeData();
 
   // calculate deviation metrics
   if (odom && traj && !traj->points.empty()) {
@@ -320,6 +362,10 @@ void ControlEvaluatorNode::onTimer()
     AddBoundaryDistanceMetricMsg(*behavior_path, ego_pose);
   }
 
+  if (steering_status) {
+    AddSteeringMetricMsg(*steering_status);
+  }
+
   // Publish metrics
   metrics_msg_.stamp = now();
   metrics_pub_->publish(metrics_msg_);