feat(control_validator): add diag to check control component latency (#…

…10240)

* feat(control_validator): add diag to check control component latency

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix: missing param

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

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: 0.01
 
     vel_lpf_gain: 0.9 # Time constant 0.33
     hold_velocity_error_until_stop: true

control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp

@@ -25,6 +25,7 @@
 #include <autoware_utils/system/stop_watch.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <autoware_control_msgs/msg/control.hpp>
 #include <autoware_internal_debug_msgs/msg/float64_stamped.hpp>
 #include <autoware_planning_msgs/msg/trajectory.hpp>
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
@@ -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<Control>::SharedPtr sub_control_cmd_;
   rclcpp::Subscription<Trajectory>::SharedPtr sub_predicted_traj_;
   autoware_utils::InterProcessPollingSubscriber<Odometry>::SharedPtr sub_kinematics_;
   autoware_utils::InterProcessPollingSubscriber<Trajectory>::SharedPtr sub_reference_traj_;

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

control/autoware_control_validator/package.xml

@@ -20,6 +20,7 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
   <build_depend>rosidl_default_generators</build_depend>
 
+  <depend>autoware_control_msgs</depend>
   <depend>autoware_motion_utils</depend>
   <depend>autoware_planning_msgs</depend>
   <depend>autoware_signal_processing</depend>

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<Control>(
+    "~/input/control_cmd", 1, std::bind(&ControlValidator::on_control_cmd, this, _1));
   sub_predicted_traj_ = create_subscription<Trajectory>(
     "~/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<double>(t + "rolling_back_velocity");
     p.over_velocity_offset = declare_parameter<double>(t + "over_velocity_offset");
     p.over_velocity_ratio = declare_parameter<double>(t + "over_velocity_ratio");
+    p.nominal_latency_threshold = declare_parameter<double>(t + "nominal_latency");
   }
   const auto lpf_gain = declare_parameter<double>("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()

launch/tier4_control_launch/launch/control.launch.xml

@@ -201,6 +201,7 @@
       <group if="$(var launch_control_validator)">
         <load_composable_node target="/control/control_check_container">
           <composable_node pkg="autoware_control_validator" plugin="autoware::control_validator::ControlValidator" name="control_validator">
+            <remap from="~/input/control_cmd" to="/control/command/control_cmd"/>
             <remap from="~/input/kinematics" to="/localization/kinematic_state"/>
             <remap from="~/input/reference_trajectory" to="/planning/scenario_planning/trajectory"/>
             <remap from="~/input/predicted_trajectory" to="/control/trajectory_follower/lateral/predicted_trajectory"/>