From 949f7edb394c19a52f6899c902696dc999a09295 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Thu, 6 Mar 2025 13:46:21 +0900 Subject: [PATCH] feat(control_validator): add diag to check control component latency Signed-off-by: satoshi-ota --- .../config/control_validator.param.yaml | 1 + .../control_validator/control_validator.hpp | 10 ++++++++++ .../msg/ControlValidatorStatus.msg | 2 ++ .../autoware_control_validator/package.xml | 1 + .../src/control_validator.cpp | 19 ++++++++++++++++++- .../launch/control.launch.xml | 1 + 6 files changed, 33 insertions(+), 1 deletion(-) diff --git a/control/autoware_control_validator/config/control_validator.param.yaml b/control/autoware_control_validator/config/control_validator.param.yaml index 577bb7dbf7706..884509e1bddf8 100644 --- a/control/autoware_control_validator/config/control_validator.param.yaml +++ b/control/autoware_control_validator/config/control_validator.param.yaml @@ -12,6 +12,7 @@ rolling_back_velocity: 0.5 over_velocity_offset: 2.0 over_velocity_ratio: 0.2 + nominal_latency_threshold: 0.01 vel_lpf_gain: 0.9 # Time constant 0.33 hold_velocity_error_until_stop: true diff --git a/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp b/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp index 88d116490065d..61cadf3f36fe8 100644 --- a/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp +++ b/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp @@ -25,6 +25,7 @@ #include #include +#include #include #include #include @@ -38,6 +39,7 @@ namespace autoware::control_validator { +using autoware_control_msgs::msg::Control; using autoware_control_validator::msg::ControlValidatorStatus; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -51,6 +53,7 @@ struct ValidationParams double rolling_back_velocity; double over_velocity_ratio; double over_velocity_offset; + double nominal_latency_threshold; }; /** @@ -67,6 +70,12 @@ class ControlValidator : public rclcpp::Node */ explicit ControlValidator(const rclcpp::NodeOptions & options); + /** + * @brief Callback function for the control component output. + * @param msg Control message + */ + void on_control_cmd(const Control::ConstSharedPtr msg); + /** * @brief Callback function for the predicted trajectory. * @param msg Predicted trajectory message @@ -134,6 +143,7 @@ class ControlValidator : public rclcpp::Node DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg) const; // Subscribers and publishers + rclcpp::Subscription::SharedPtr sub_control_cmd_; rclcpp::Subscription::SharedPtr sub_predicted_traj_; autoware_utils::InterProcessPollingSubscriber::SharedPtr sub_kinematics_; autoware_utils::InterProcessPollingSubscriber::SharedPtr sub_reference_traj_; diff --git a/control/autoware_control_validator/msg/ControlValidatorStatus.msg b/control/autoware_control_validator/msg/ControlValidatorStatus.msg index 5db179f5a9257..c40f160510d89 100644 --- a/control/autoware_control_validator/msg/ControlValidatorStatus.msg +++ b/control/autoware_control_validator/msg/ControlValidatorStatus.msg @@ -4,10 +4,12 @@ builtin_interfaces/Time stamp bool is_valid_max_distance_deviation bool is_rolling_back bool is_over_velocity +bool is_valid_latency # values float64 max_distance_deviation float64 target_vel float64 vehicle_vel +float64 latency int64 invalid_count diff --git a/control/autoware_control_validator/package.xml b/control/autoware_control_validator/package.xml index 6c1e2bf175283..a9f62039e5c25 100644 --- a/control/autoware_control_validator/package.xml +++ b/control/autoware_control_validator/package.xml @@ -20,6 +20,7 @@ autoware_cmake rosidl_default_generators + autoware_control_msgs autoware_motion_utils autoware_planning_msgs autoware_signal_processing diff --git a/control/autoware_control_validator/src/control_validator.cpp b/control/autoware_control_validator/src/control_validator.cpp index 0f2763a53f0c9..528bfdfcc1df2 100644 --- a/control/autoware_control_validator/src/control_validator.cpp +++ b/control/autoware_control_validator/src/control_validator.cpp @@ -34,6 +34,8 @@ ControlValidator::ControlValidator(const rclcpp::NodeOptions & options) { using std::placeholders::_1; + sub_control_cmd_ = create_subscription( + "~/input/control_cmd", 1, std::bind(&ControlValidator::on_control_cmd, this, _1)); sub_predicted_traj_ = create_subscription( "~/input/predicted_trajectory", 1, std::bind(&ControlValidator::on_predicted_trajectory, this, _1)); @@ -70,6 +72,7 @@ void ControlValidator::setup_parameters() p.rolling_back_velocity = declare_parameter(t + "rolling_back_velocity"); p.over_velocity_offset = declare_parameter(t + "over_velocity_offset"); p.over_velocity_ratio = declare_parameter(t + "over_velocity_ratio"); + p.nominal_latency_threshold = declare_parameter(t + "nominal_latency"); } const auto lpf_gain = declare_parameter("vel_lpf_gain"); vehicle_vel_.setGain(lpf_gain); @@ -126,6 +129,10 @@ void ControlValidator::setup_diag() stat, !validation_status_.is_over_velocity, "The vehicle is over-speeding against the target."); }); + d.add(ns + "latency", [&](auto & stat) { + set_status( + stat, validation_status_.is_valid_latency, "The latency is larger than expected value."); + }); } bool ControlValidator::is_data_ready() @@ -147,6 +154,15 @@ bool ControlValidator::is_data_ready() return true; } +void ControlValidator::on_control_cmd(const Control::ConstSharedPtr msg) +{ + validation_status_.latency = (this->now() - msg->stamp).seconds(); + validation_status_.is_valid_latency = + validation_status_.latency < validation_params_.nominal_latency_threshold; + validation_status_.invalid_count = + is_all_valid(validation_status_) ? 0 : validation_status_.invalid_count + 1; +} + void ControlValidator::on_predicted_trajectory(const Trajectory::ConstSharedPtr msg) { stop_watch.tic(); @@ -254,7 +270,8 @@ void ControlValidator::calc_velocity_deviation_status( bool ControlValidator::is_all_valid(const ControlValidatorStatus & s) { - return s.is_valid_max_distance_deviation && !s.is_rolling_back && !s.is_over_velocity; + return s.is_valid_max_distance_deviation && !s.is_rolling_back && !s.is_over_velocity && + s.is_valid_latency; } void ControlValidator::display_status() diff --git a/launch/tier4_control_launch/launch/control.launch.xml b/launch/tier4_control_launch/launch/control.launch.xml index f35f02e1d0a80..4e83d7e421cb3 100644 --- a/launch/tier4_control_launch/launch/control.launch.xml +++ b/launch/tier4_control_launch/launch/control.launch.xml @@ -201,6 +201,7 @@ +