feat(control_validator)!: add velocity check (autowarefoundation#7806)

* add velocity check
---------
Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

Author: yuki-takagi-66

control/autoware_control_validator/README.md

@@ -6,14 +6,17 @@ The `control_validator` is a module that checks the validity of the output of th
 
 ## Supported features
 
-The following features are supported for the validation and can have thresholds set by parameters:
+The following features are supported for the validation and can have thresholds set by parameters.
+The listed features below does not always correspond to the latest implementation.
+| Description | Arguments | Diagnostic equation | Implemented function name |
+| ---------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------- | :--------------------------------: | ------------------------------- |
+| Inverse velocity: Measured velocity has a different sign from the target velocity. | measured velocity $v$, target velocity $\hat{v}$, and threshold velocity parameter $k$ | $v\hat{v}<0,~\|v\|>k $       | `checkValidVelocityDeviation()` |
+| Overspeed: Measured speed exceeds target speed significantly.                      | measured velocity $v$, target velocity $\hat{v}$, and threshold ratio parameter $r$ | $\| v \| > (1 + r) \| \hat{v} \| $ | `checkValidVelocityDeviation()` |
 
 - **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.
 
 ![trajectory_deviation](./image/trajectory_deviation.drawio.svg)
 
-Other features are to be implemented.
-
 ## Inputs/Outputs
 
 ### Inputs
@@ -53,6 +56,8 @@ 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           |
+| 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.max_reverse_velocity`    | double | threshold velocity to valid the vehicle velocity [m/s]                                                      | WIP           |
+| `thresholds.max_over_velocity_ratio` | double | threshold ratio to valid the vehicle velocity [*]                                                           | WIP           |

control/autoware_control_validator/config/control_validator.param.yaml

@@ -1,6 +1,5 @@
 /**:
   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.)
@@ -10,3 +9,5 @@
 
     thresholds:
       max_distance_deviation: 1.0
+      max_reverse_velocity: 0.2
+      max_over_velocity_ratio: 0.1

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

@@ -42,6 +42,8 @@ using nav_msgs::msg::Odometry;
 struct ValidationParams
 {
   double max_distance_deviation_threshold;
+  double max_reverse_velocity_threshold;
+  double max_over_velocity_ratio_threshold;
 };
 
 class ControlValidator : public rclcpp::Node
@@ -52,6 +54,8 @@ class ControlValidator : public rclcpp::Node
   void onPredictedTrajectory(const Trajectory::ConstSharedPtr msg);
 
   bool checkValidMaxDistanceDeviation(const Trajectory & predicted_trajectory);
+  bool checkValidVelocityDeviation(
+    const Trajectory & reference_trajectory, const Odometry & kinematics);
 
 private:
   void setupDiag();
@@ -60,7 +64,9 @@ class ControlValidator : public rclcpp::Node
 
   bool isDataReady();
 
-  void validate(const Trajectory & trajectory);
+  void validate(
+    const Trajectory & predicted_trajectory, const Trajectory & reference_trajectory,
+    const Odometry & kinematics);
 
   void publishPredictedTrajectory();
   void publishDebugInfo();
@@ -85,6 +91,10 @@ class ControlValidator : public rclcpp::Node
   ControlValidatorStatus validation_status_;
   ValidationParams validation_params_;  // for thresholds
 
+  // ego nearest index search
+  double ego_nearest_dist_threshold_;
+  double ego_nearest_yaw_threshold_;
+
   autoware::vehicle_info_utils::VehicleInfo vehicle_info_;
 
   bool isAllValid(const ControlValidatorStatus & status);

control/autoware_control_validator/msg/ControlValidatorStatus.msg

@@ -2,8 +2,11 @@ builtin_interfaces/Time stamp
 
 # states
 bool is_valid_max_distance_deviation
+bool is_valid_velocity_deviation
 
 # values
 float64 max_distance_deviation
+float64 desired_velocity
+float64 current_velocity
 
 int64 invalid_count

control/autoware_control_validator/src/control_validator.cpp

@@ -15,6 +15,8 @@
 #include "autoware/control_validator/control_validator.hpp"
 
 #include "autoware/control_validator/utils.hpp"
+#include "autoware/motion_utils/trajectory/interpolation.hpp"
+#include "autoware/motion_utils/trajectory/trajectory.hpp"
 
 #include <memory>
 #include <string>
@@ -51,6 +53,8 @@ void ControlValidator::setupParameters()
     auto & p = validation_params_;
     const std::string t = "thresholds.";
     p.max_distance_deviation_threshold = declare_parameter<double>(t + "max_distance_deviation");
+    p.max_reverse_velocity_threshold = declare_parameter<double>(t + "reverse_velocity");
+    p.max_over_velocity_ratio_threshold = declare_parameter<double>(t + "over_velocity_ratio");
   }
 
   try {
@@ -88,6 +92,11 @@ void ControlValidator::setupDiag()
       stat, validation_status_.is_valid_max_distance_deviation,
       "control output is deviated from trajectory");
   });
+  d.add(ns + "velocity_deviation", [&](auto & stat) {
+    setStatus(
+      stat, validation_status_.is_valid_velocity_deviation,
+      "current velocity is deviated from the desired velocity");
+  });
 }
 
 bool ControlValidator::isDataReady()
@@ -119,7 +128,7 @@ void ControlValidator::onPredictedTrajectory(const Trajectory::ConstSharedPtr ms
 
   debug_pose_publisher_->clearMarkers();
 
-  validate(*current_predicted_trajectory_);
+  validate(*current_predicted_trajectory_, *current_reference_trajectory_, *current_kinematics_);
 
   diag_updater_.force_update();
 
@@ -142,7 +151,9 @@ void ControlValidator::publishDebugInfo()
   debug_pose_publisher_->publish();
 }
 
-void ControlValidator::validate(const Trajectory & predicted_trajectory)
+void ControlValidator::validate(
+  const Trajectory & predicted_trajectory, const Trajectory & reference_trajectory,
+  const Odometry & kinematics)
 {
   if (predicted_trajectory.points.size() < 2) {
     RCLCPP_ERROR_THROTTLE(
@@ -154,6 +165,7 @@ void ControlValidator::validate(const Trajectory & predicted_trajectory)
   auto & s = validation_status_;
 
   s.is_valid_max_distance_deviation = checkValidMaxDistanceDeviation(predicted_trajectory);
+  s.is_valid_velocity_deviation = checkValidVelocityDeviation(reference_trajectory, kinematics);
 
   s.invalid_count = isAllValid(s) ? 0 : s.invalid_count + 1;
 }
@@ -170,9 +182,32 @@ bool ControlValidator::checkValidMaxDistanceDeviation(const Trajectory & predict
   return true;
 }
 
+bool ControlValidator::checkValidVelocityDeviation(
+  const Trajectory & reference_trajectory, const Odometry & kinematics)
+{
+  const double current_vel = kinematics.twist.twist.linear.x;
+  if (reference_trajectory.points.size() < 2) return true;
+  const double desired_vel =
+    autoware::motion_utils::calcInterpolatedPoint(reference_trajectory, kinematics.pose.pose)
+      .longitudinal_velocity_mps;
+
+  validation_status_.current_velocity = current_vel;
+  validation_status_.desired_velocity = desired_vel;
+
+  const bool is_over_velocity =
+    std::abs(current_vel) >
+    std::abs(desired_vel) * (1.0 + validation_params_.max_over_velocity_ratio_threshold) +
+      validation_params_.max_reverse_velocity_threshold;
+  const bool is_reverse_velocity =
+    std::signbit(current_vel * desired_vel) &&
+    std::abs(current_vel) > validation_params_.max_reverse_velocity_threshold;
+
+  return !(is_over_velocity || is_reverse_velocity);
+}
+
 bool ControlValidator::isAllValid(const ControlValidatorStatus & s)
 {
-  return s.is_valid_max_distance_deviation;
+  return s.is_valid_max_distance_deviation && s.is_valid_velocity_deviation;
 }
 
 void ControlValidator::displayStatus()