feat(control_validator, tier4_diagnostics): sync upstream (#1983)

* feat(control_validator): add diag to check control component latency (autowarefoundation#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>

* feat(control_validator)!: add overrun check (autowarefoundation#10236)

Signed-off-by: yuki-takagi-66 <yuki.takagi@tier4.jp>

* feat(control_validator)!: add acceleration check (autowarefoundation#10326)

Signed-off-by: yuki-takagi-66 <yuki.takagi@tier4.jp>

* fix(control_validator): fix sign miss and add code test (autowarefoundation#10341)

Signed-off-by: yuki-takagi-66 <yuki.takagi@tier4.jp>

* fix(control validator): combine callback functions to fix error count increment bug (autowarefoundation#10355)

Signed-off-by: yuki-takagi-66 <yuki.takagi@tier4.jp>

* refactor(control validaor): refactor control_validator (autowarefoundation#10363)

Signed-off-by: yuki-takagi-66 <yuki.takagi@tier4.jp>

* feat(control_validator): add over run estimation feature (autowarefoundation#10422)

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

* fix(control_validator): fix less trajectory point check (autowarefoundation#10508)

Signed-off-by: yuki-takagi-66 <yuki.takagi@tier4.jp>

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
Signed-off-by: yuki-takagi-66 <yuki.takagi@tier4.jp>
Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>

Author: yuki-takagi-66

control/autoware_control_validator/README.md

@@ -13,6 +13,7 @@ The listed features below does not always correspond to the latest implementatio
 | ---------------------------------------------------------------------------------- | ----------------------------------------------------------------------------------------------- | :---------------------------------------------------: |
 | Inverse velocity: Measured velocity has a different sign from the target velocity. | measured velocity $v$, target velocity $\hat{v}$, and velocity parameter $c$                    |      $v \hat{v} < 0, \quad \lvert v \rvert > c$       |
 | Overspeed: Measured speed exceeds target speed significantly.                      | measured velocity $v$, target velocity $\hat{v}$, ratio parameter $r$, and offset parameter $c$ | $\lvert v \rvert > (1 + r) \lvert \hat{v} \rvert + c$ |
+| Overrun estimation: estimate overrun even if decelerate by assumed rate.           | assumed deceleration, assumed delay                                                             |                                                       |
 
 - **Deviation check between reference trajectory and predicted trajectory** : invalid when the largest deviation between the predicted trajectory and reference trajectory is greater than the given threshold.
 
@@ -57,9 +58,15 @@ The following parameters can be set for the `control_validator`:
 
 The input trajectory is detected as invalid if the index exceeds the following thresholds.
 
-| Name                                | Type   | Description                                                                                                 | Default value |
-| :---------------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :------------ |
-| `thresholds.max_distance_deviation` | double | invalid threshold of the max distance deviation between the predicted path and the reference trajectory [m] | 1.0           |
-| `thresholds.rolling_back_velocity`  | double | threshold velocity to valid the vehicle velocity [m/s]                                                      | 0.5           |
-| `thresholds.over_velocity_offset`   | double | threshold velocity offset to valid the vehicle velocity [m/s]                                               | 2.0           |
-| `thresholds.over_velocity_ratio`    | double | threshold ratio to valid the vehicle velocity [*]                                                           | 0.2           |
+| Name                                      | Type   | Description                                                                                                 | Default value |
+| :---------------------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :------------ |
+| `thresholds.max_distance_deviation`       | double | invalid threshold of the max distance deviation between the predicted path and the reference trajectory [m] | 1.0           |
+| `thresholds.rolling_back_velocity`        | double | threshold velocity to valid the vehicle velocity [m/s]                                                      | 0.5           |
+| `thresholds.over_velocity_offset`         | double | threshold velocity offset to valid the vehicle velocity [m/s]                                               | 2.0           |
+| `thresholds.over_velocity_ratio`          | double | threshold ratio to valid the vehicle velocity [*]                                                           | 0.2           |
+| `thresholds.overrun_stop_point_dist`      | double | threshold distance to overrun stop point [m]                                                                | 0.8           |
+| `thresholds.acc_error_offset`             | double | threshold ratio to valid the vehicle acceleration [*]                                                       | 0.8           |
+| `thresholds.acc_error_scale`              | double | threshold acceleration to valid the vehicle acceleration [m]                                                | 0.2           |
+| `thresholds.will_overrun_stop_point_dist` | double | threshold distance to overrun stop point [m]                                                                | 1.0           |
+| `thresholds.assumed_limit_acc`            | double | assumed acceleration for over run estimation [m]                                                            | 5.0           |
+| `thresholds.assumed_delay_time`           | double | assumed delay for over run estimation [m]                                                                   | 0.2           |

control/autoware_control_validator/config/control_validator.param.yaml

@@ -1,17 +1,25 @@
 /**:
   ros__parameters:
-    # If the number of consecutive invalid trajectory exceeds this threshold, the Diag will be set to ERROR.
-    # (For example, threshold = 1 means, even if the trajectory is invalid, Diag will not be ERROR if
-    #  the next trajectory is valid.)
+    # If the number of consecutive invalid predicted_path exceeds this threshold, the Diag will be set to ERROR.
+    # (For example, threshold = 1 means, even if the predicted_path is invalid, Diag will not be ERROR if
+    #  the next predicted_path is valid.)
     diag_error_count_threshold: 0
 
     display_on_terminal: false # show error msg on terminal
 
     thresholds:
       max_distance_deviation: 1.0
+      acc_error_offset: 0.8
+      acc_error_scale: 0.2
       rolling_back_velocity: 0.5
       over_velocity_offset: 2.0
       over_velocity_ratio: 0.2
+      overrun_stop_point_dist: 0.8
+      will_overrun_stop_point_dist: 1.0
+      assumed_limit_acc: 5.0
+      assumed_delay_time: 0.2
+      nominal_latency: 0.01
 
+    acc_lpf_gain: 0.97 # Time constant 1.11s
     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

@@ -22,12 +22,15 @@
 
 #include <autoware/signal_processing/lowpass_filter_1d.hpp>
 #include <autoware_control_validator/msg/control_validator_status.hpp>
+#include <autoware_utils/ros/parameter.hpp>
 #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>
+#include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 
 #include <cstdint>
@@ -38,19 +41,139 @@
 
 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;
 using diagnostic_updater::DiagnosticStatusWrapper;
 using diagnostic_updater::Updater;
+using geometry_msgs::msg::AccelWithCovarianceStamped;
 using nav_msgs::msg::Odometry;
 
-struct ValidationParams
+/**
+ * @class LatencyValidator
+ * @brief Validates latency of the control module.
+ */
+class LatencyValidator
+{
+public:
+  explicit LatencyValidator(rclcpp::Node & node)
+  : nominal_latency_threshold{
+      autoware_utils::get_or_declare_parameter<double>(node, "thresholds.nominal_latency")} {};
+
+  void validate(
+    ControlValidatorStatus & res, const Control & control_cmd, rclcpp::Node & node) const;
+
+private:
+  const double nominal_latency_threshold;
+};
+
+/**
+ * @class TrajectoryValidator
+ * @brief Calculate the maximum lateral distance between the reference trajectory and the predicted
+ * trajectory.
+ */
+class TrajectoryValidator
 {
-  double max_distance_deviation_threshold;
-  double rolling_back_velocity;
-  double over_velocity_ratio;
-  double over_velocity_offset;
+public:
+  explicit TrajectoryValidator(rclcpp::Node & node)
+  : max_distance_deviation_threshold{autoware_utils::get_or_declare_parameter<double>(
+      node, "thresholds.max_distance_deviation")} {};
+
+  void validate(
+    ControlValidatorStatus & res, const Trajectory & predicted_trajectory,
+    const Trajectory & reference_trajectory) const;
+
+private:
+  const double max_distance_deviation_threshold;
+};
+
+/**
+ * @class AccelerationValidator
+ * @brief Validates deviation between output acceleration and measured acceleration.
+ */
+class AccelerationValidator
+{
+public:
+  friend class AccelerationValidatorTest;
+  explicit AccelerationValidator(rclcpp::Node & node)
+  : e_offset{autoware_utils::get_or_declare_parameter<double>(node, "thresholds.acc_error_offset")},
+    e_scale{autoware_utils::get_or_declare_parameter<double>(node, "thresholds.acc_error_scale")},
+    desired_acc_lpf{autoware_utils::get_or_declare_parameter<double>(node, "acc_lpf_gain")},
+    measured_acc_lpf{autoware_utils::get_or_declare_parameter<double>(node, "acc_lpf_gain")} {};
+
+  void validate(
+    ControlValidatorStatus & res, const Odometry & kinematic_state, const Control & control_cmd,
+    const AccelWithCovarianceStamped & loc_acc);
+
+private:
+  bool is_in_error_range() const;
+  const double e_offset;
+  const double e_scale;
+  autoware::signal_processing::LowpassFilter1d desired_acc_lpf;
+  autoware::signal_processing::LowpassFilter1d measured_acc_lpf;
+};
+
+/**
+ * @class VelocityValidator
+ * @brief Validates deviation between target velocity and measured velocity.
+ */
+class VelocityValidator
+{
+public:
+  explicit VelocityValidator(rclcpp::Node & node)
+  : rolling_back_velocity_th{autoware_utils::get_or_declare_parameter<double>(
+      node, "thresholds.rolling_back_velocity")},
+    over_velocity_ratio_th{
+      autoware_utils::get_or_declare_parameter<double>(node, "thresholds.over_velocity_ratio")},
+    over_velocity_offset_th{
+      autoware_utils::get_or_declare_parameter<double>(node, "thresholds.over_velocity_offset")},
+    hold_velocity_error_until_stop{
+      autoware_utils::get_or_declare_parameter<bool>(node, "hold_velocity_error_until_stop")},
+    vehicle_vel_lpf{autoware_utils::get_or_declare_parameter<double>(node, "vel_lpf_gain")},
+    target_vel_lpf{autoware_utils::get_or_declare_parameter<double>(node, "vel_lpf_gain")} {};
+
+  void validate(
+    ControlValidatorStatus & res, const Trajectory & reference_trajectory,
+    const Odometry & kinematics);
+
+private:
+  const double rolling_back_velocity_th;
+  const double over_velocity_ratio_th;
+  const double over_velocity_offset_th;
+  const bool hold_velocity_error_until_stop;
+  autoware::signal_processing::LowpassFilter1d vehicle_vel_lpf;
+  autoware::signal_processing::LowpassFilter1d target_vel_lpf;
+};
+
+/**
+ * @class OverrunValidator
+ * @brief Calculate whether the vehicle has overrun a stop point in the trajectory.
+ */
+class OverrunValidator
+{
+public:
+  explicit OverrunValidator(rclcpp::Node & node)
+  : overrun_stop_point_dist_th{autoware_utils::get_or_declare_parameter<double>(
+      node, "thresholds.overrun_stop_point_dist")},
+    will_overrun_stop_point_dist_th{autoware_utils::get_or_declare_parameter<double>(
+      node, "thresholds.will_overrun_stop_point_dist")},
+    assumed_limit_acc{
+      autoware_utils::get_or_declare_parameter<double>(node, "thresholds.assumed_limit_acc")},
+    assumed_delay_time{
+      autoware_utils::get_or_declare_parameter<double>(node, "thresholds.assumed_delay_time")},
+    vehicle_vel_lpf{autoware_utils::get_or_declare_parameter<double>(node, "vel_lpf_gain")} {};
+
+  void validate(
+    ControlValidatorStatus & res, const Trajectory & reference_trajectory,
+    const Odometry & kinematics);
+
+private:
+  const double overrun_stop_point_dist_th;
+  const double will_overrun_stop_point_dist_th;
+  const double assumed_limit_acc;
+  const double assumed_delay_time;
+  autoware::signal_processing::LowpassFilter1d vehicle_vel_lpf;
 };
 
 /**
@@ -68,23 +191,10 @@ class ControlValidator : public rclcpp::Node
   explicit ControlValidator(const rclcpp::NodeOptions & options);
 
   /**
-   * @brief Callback function for the predicted trajectory.
-   * @param msg Predicted trajectory message
-   */
-  void on_predicted_trajectory(const Trajectory::ConstSharedPtr msg);
-
-  /**
-   * @brief Calculate the maximum lateral distance between the reference trajectory and predicted
-   * trajectory.
-   * @param predicted_trajectory Predicted trajectory
-   * @param reference_trajectory Reference trajectory
-   * @return A pair consisting of the maximum lateral deviation and a boolean indicating validity
+   * @brief Callback function for the control component output.
+   * @param msg Control message
    */
-  std::pair<double, bool> calc_lateral_deviation_status(
-    const Trajectory & predicted_trajectory, const Trajectory & reference_trajectory) const;
-
-  void calc_velocity_deviation_status(
-    const Trajectory & reference_trajectory, const Odometry & kinematics);
+  void on_control_cmd(const Control::ConstSharedPtr msg);
 
 private:
   /**
@@ -97,27 +207,10 @@ class ControlValidator : public rclcpp::Node
    */
   void setup_parameters();
 
-  /**
-   * @brief Check if all required data is ready for validation
-   * @return Boolean indicating readiness of data
-   */
-  bool is_data_ready();
-
-  /**
-   * @brief Validate the predicted trajectory against the reference trajectory and current
-   * kinematics
-   * @param predicted_trajectory Predicted trajectory
-   * @param reference_trajectory Reference trajectory
-   * @param kinematics Current vehicle kinematics
-   */
-  void validate(
-    const Trajectory & predicted_trajectory, const Trajectory & reference_trajectory,
-    const Odometry & kinematics);
-
   /**
    * @brief Publish debug information
    */
-  void publish_debug_info();
+  void publish_debug_info(const geometry_msgs::msg::Pose & ego_pose);
 
   /**
    * @brief Display validation status on terminal
@@ -134,9 +227,12 @@ class ControlValidator : public rclcpp::Node
     DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg) const;
 
   // Subscribers and publishers
-  rclcpp::Subscription<Trajectory>::SharedPtr sub_predicted_traj_;
+  rclcpp::Subscription<Control>::SharedPtr sub_control_cmd_;
   autoware_utils::InterProcessPollingSubscriber<Odometry>::SharedPtr sub_kinematics_;
   autoware_utils::InterProcessPollingSubscriber<Trajectory>::SharedPtr sub_reference_traj_;
+  autoware_utils::InterProcessPollingSubscriber<Trajectory>::SharedPtr sub_predicted_traj_;
+  autoware_utils::InterProcessPollingSubscriber<AccelWithCovarianceStamped>::SharedPtr
+    sub_measured_acc_;
   rclcpp::Publisher<ControlValidatorStatus>::SharedPtr pub_status_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_markers_;
   rclcpp::Publisher<autoware_internal_debug_msgs::msg::Float64Stamped>::SharedPtr
@@ -145,32 +241,26 @@ class ControlValidator : public rclcpp::Node
   // system parameters
   int64_t diag_error_count_threshold_ = 0;
   bool display_on_terminal_ = true;
-
   Updater diag_updater_{this};
-
   ControlValidatorStatus validation_status_;
-  ValidationParams validation_params_;  // for thresholds
-  autoware::signal_processing::LowpassFilter1d vehicle_vel_{0.0};
-  autoware::signal_processing::LowpassFilter1d target_vel_{0.0};
-  bool hold_velocity_error_until_stop_{false};
-
   vehicle_info_utils::VehicleInfo vehicle_info_;
-
   /**
    * @brief Check if all validation criteria are met
    * @param status Validation status
    * @return Boolean indicating if all criteria are met
    */
   static bool is_all_valid(const ControlValidatorStatus & status);
 
-  Trajectory::ConstSharedPtr current_reference_trajectory_;
-  Trajectory::ConstSharedPtr current_predicted_trajectory_;
-
-  Odometry::ConstSharedPtr current_kinematics_;
-
+  // debug
+  std::shared_ptr<ControlValidatorDebugMarkerPublisher> debug_pose_publisher_;
   autoware_utils::StopWatch<std::chrono::milliseconds> stop_watch;
 
-  std::shared_ptr<ControlValidatorDebugMarkerPublisher> debug_pose_publisher_;
+  // individual validators
+  LatencyValidator latency_validator{*this};
+  TrajectoryValidator trajectory_validator{*this};
+  AccelerationValidator acceleration_validator{*this};
+  VelocityValidator velocity_validator{*this};
+  OverrunValidator overrun_validator{*this};
 };
 }  // namespace autoware::control_validator
 

control/autoware_control_validator/launch/control_validator.launch.xml

@@ -8,9 +8,11 @@
     <param from="$(var control_validator_param_path)"/>
 
     <!-- remap topic name -->
+    <remap from="~/input/control_cmd" to="/control/command/control_cmd"/>
+    <remap from="~/input/kinematics" to="/localization/kinematic_state"/>
+    <remap from="~/input/measured_acceleration" to="/localization/acceleration"/>
     <remap from="~/input/reference_trajectory" to="$(var input_reference_trajectory)"/>
     <remap from="~/input/predicted_trajectory" to="$(var input_predicted_trajectory)"/>
-    <remap from="~/input/kinematics" to="/localization/kinematic_state"/>
     <remap from="~/output/validation_status" to="~/validation_status"/>
   </node>
 </launch>

control/autoware_control_validator/msg/ControlValidatorStatus.msg

@@ -2,12 +2,22 @@ builtin_interfaces/Time stamp
 
 # states
 bool is_valid_max_distance_deviation
+bool is_valid_acc
 bool is_rolling_back
 bool is_over_velocity
+bool has_overrun_stop_point
+bool will_overrun_stop_point
+bool is_valid_latency
 
 # values
 float64 max_distance_deviation
+float64 desired_acc
+float64 measured_acc
 float64 target_vel
 float64 vehicle_vel
+float64 dist_to_stop
+float64 pred_dist_to_stop
+float64 nearest_trajectory_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>