diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp index 4803f78f1f255..0d58f6e5919bb 100644 --- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp +++ b/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 behavior_path_subscriber_{ this, "~/input/behavior_path"}; autoware::universe_utils::InterProcessPollingSubscriber steering_sub_{ - this, "~/input/steering_report"}; + this, "~/input/steering_status"}; rclcpp::Publisher::SharedPtr processing_time_pub_; @@ -112,8 +113,16 @@ class ControlEvaluatorNode : public rclcpp::Node // Metric const std::vector 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, static_cast(Metric::SIZE)> @@ -124,6 +133,9 @@ class ControlEvaluatorNode : public rclcpp::Node autoware::route_handler::RouteHandler route_handler_; rclcpp::TimerBase::SharedPtr timer_; std::optional prev_acc_stamped_{std::nullopt}; + std::optional prev_steering_angle_{std::nullopt}; + std::optional prev_steering_rate_{std::nullopt}; + std::optional prev_steering_angle_timestamp_{std::nullopt}; }; } // namespace control_diagnostics diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp index ab2533d0f6ed3..39ef6fa26dfa8 100644 --- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp +++ b/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 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_to_str = { @@ -54,6 +60,9 @@ static const std::unordered_map 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_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 diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metrics_utils.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metrics_utils.hpp index 0e9e89f764db7..4d9b6263f27d3 100644 --- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metrics_utils.hpp +++ b/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 #include +#include + namespace control_diagnostics { namespace metrics diff --git a/evaluator/autoware_control_evaluator/package.xml b/evaluator/autoware_control_evaluator/package.xml index 9daaaf32cf17b..0bc319e485387 100644 --- a/evaluator/autoware_control_evaluator/package.xml +++ b/evaluator/autoware_control_evaluator/package.xml @@ -22,6 +22,7 @@ autoware_test_utils autoware_universe_utils autoware_vehicle_info_utils + autoware_vehicle_msgs nav_msgs nlohmann-json-dev pluginlib @@ -29,7 +30,6 @@ rclcpp_components tf2 tf2_ros - autoware_vehicle_msgs tier4_metric_msgs tier4_planning_msgs diff --git a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp index 781cf4ebbd786..d6be17dfc7c14 100644 --- a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp +++ b/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(steering_status.stamp.sec) + + static_cast(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::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(); diff --git a/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp b/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp index 02f69b6563108..3fe95d4decb8d 100644 --- a/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp +++ b/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp @@ -27,6 +27,7 @@ #include #include +#include namespace control_diagnostics { namespace metrics