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

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

Author: yuki-takagi-66

control/autoware_control_validator/README.md

@@ -64,3 +64,5 @@ The input trajectory is detected as invalid if the index exceeds the following t
 | `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           |

control/autoware_control_validator/config/control_validator.param.yaml

@@ -1,19 +1,22 @@
 /**:
   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
       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,13 +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>
@@ -45,6 +47,7 @@ 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
@@ -57,6 +60,35 @@ struct ValidationParams
   double nominal_latency_threshold;
 };
 
+/**
+ * @class AccelerationValidator
+ * @brief Validates deviation between output acceleration and measured acceleration.
+ */
+class AccelerationValidator
+{
+public:
+  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");
+    const double acc_lpf_gain =
+      autoware_utils::get_or_declare_parameter<double>(node, "acc_lpf_gain");
+    desired_acc_lpf.setGain(acc_lpf_gain);
+    measured_acc_lpf.setGain(acc_lpf_gain);
+  };
+
+  void validate(
+    ControlValidatorStatus & res, const Odometry & kinematic_state, const Control & control_cmd,
+    const AccelWithCovarianceStamped & loc_acc);
+
+private:
+  double e_offset;
+  double e_scale;
+  autoware::signal_processing::LowpassFilter1d desired_acc_lpf{0.0};
+  autoware::signal_processing::LowpassFilter1d measured_acc_lpf{0.0};
+};
+
 /**
  * @class ControlValidator
  * @brief Validates control commands by comparing predicted trajectories against reference
@@ -130,7 +162,8 @@ class ControlValidator : public rclcpp::Node
    */
   void validate(
     const Trajectory & predicted_trajectory, const Trajectory & reference_trajectory,
-    const Odometry & kinematics);
+    const Odometry & kinematics, const Control & control_cmd,
+    const AccelWithCovarianceStamped & measured_acc);
 
   /**
    * @brief Publish debug information
@@ -156,6 +189,8 @@ class ControlValidator : public rclcpp::Node
   rclcpp::Subscription<Trajectory>::SharedPtr sub_predicted_traj_;
   autoware_utils::InterProcessPollingSubscriber<Odometry>::SharedPtr sub_kinematics_;
   autoware_utils::InterProcessPollingSubscriber<Trajectory>::SharedPtr sub_reference_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
@@ -171,6 +206,8 @@ class ControlValidator : public rclcpp::Node
   ValidationParams validation_params_;  // for thresholds
   autoware::signal_processing::LowpassFilter1d vehicle_vel_{0.0};
   autoware::signal_processing::LowpassFilter1d target_vel_{0.0};
+  AccelerationValidator acceleration_validator{*this};
+
   bool hold_velocity_error_until_stop_{false};
 
   vehicle_info_utils::VehicleInfo vehicle_info_;
@@ -186,6 +223,8 @@ class ControlValidator : public rclcpp::Node
   Trajectory::ConstSharedPtr current_predicted_trajectory_;
 
   Odometry::ConstSharedPtr current_kinematics_;
+  AccelWithCovarianceStamped::ConstSharedPtr acceleration_msg_;
+  Control::ConstSharedPtr control_cmd_msg_;
 
   autoware_utils::StopWatch<std::chrono::milliseconds> stop_watch;
 

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,13 +2,16 @@ 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 is_valid_latency
 
 # values
 float64 max_distance_deviation
+float64 desired_acc
+float64 measured_acc
 float64 target_vel
 float64 vehicle_vel
 float64 dist_to_stop

control/autoware_control_validator/src/control_validator.cpp

@@ -19,8 +19,6 @@
 #include "autoware/motion_utils/trajectory/trajectory.hpp"
 #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp"
 
-#include <nav_msgs/msg/odometry.hpp>
-
 #include <algorithm>
 #include <cstdint>
 #include <memory>
@@ -31,6 +29,31 @@ namespace autoware::control_validator
 {
 using diagnostic_msgs::msg::DiagnosticStatus;
 
+void AccelerationValidator::validate(
+  ControlValidatorStatus & res, const Odometry & kinematic_state, const Control & control_cmd,
+  const AccelWithCovarianceStamped & loc_acc)
+{
+  desired_acc_lpf.filter(
+    control_cmd.longitudinal.acceleration +
+    9.8 * autoware_utils::get_rpy(kinematic_state.pose.pose).y);
+  measured_acc_lpf.filter(loc_acc.accel.accel.linear.x);
+  if (std::abs(kinematic_state.twist.twist.linear.x) < 1e-3) {
+    desired_acc_lpf.reset(0.0);
+    measured_acc_lpf.reset(0.0);
+  }
+
+  res.desired_acc = desired_acc_lpf.getValue().value();
+  res.measured_acc = measured_acc_lpf.getValue().value();
+  res.is_valid_acc = [this]() {
+    const double des = desired_acc_lpf.getValue().value();
+    const double mes = measured_acc_lpf.getValue().value();
+    const int8_t des_sign = std::signbit(des) ? 1 : -1;
+
+    return mes <= des * (1 + des_sign * e_scale) + e_offset &&
+           mes >= des * (1 - des_sign * e_scale) + e_offset;
+  }();
+}
+
 ControlValidator::ControlValidator(const rclcpp::NodeOptions & options)
 : Node("control_validator", options), validation_params_(), vehicle_info_()
 {
@@ -47,6 +70,9 @@ ControlValidator::ControlValidator(const rclcpp::NodeOptions & options)
   sub_reference_traj_ =
     autoware_utils::InterProcessPollingSubscriber<Trajectory>::create_subscription(
       this, "~/input/reference_trajectory", 1);
+  sub_measured_acc_ =
+    autoware_utils::InterProcessPollingSubscriber<AccelWithCovarianceStamped>::create_subscription(
+      this, "~/input/measured_acceleration", 1);
 
   pub_status_ = create_publisher<ControlValidatorStatus>("~/output/validation_status", 1);
 
@@ -77,9 +103,10 @@ void ControlValidator::setup_parameters()
     p.overrun_stop_point_dist = declare_parameter<double>(t + "overrun_stop_point_dist");
     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);
-  target_vel_.setGain(lpf_gain);
+
+  const auto vel_lpf_gain = declare_parameter<double>("vel_lpf_gain");
+  vehicle_vel_.setGain(vel_lpf_gain);
+  target_vel_.setGain(vel_lpf_gain);
 
   hold_velocity_error_until_stop_ = declare_parameter<bool>("hold_velocity_error_until_stop");
 
@@ -122,6 +149,11 @@ void ControlValidator::setup_diag()
       stat, validation_status_.is_valid_max_distance_deviation,
       "control output is deviated from trajectory");
   });
+  d.add(ns + "acceleration_error", [&](auto & stat) {
+    set_status(
+      stat, validation_status_.is_valid_acc,
+      "Measured acceleration and desired acceleration is deviated");
+  });
   d.add(ns + "rolling_back", [&](auto & stat) {
     set_status(
       stat, !validation_status_.is_rolling_back,
@@ -159,14 +191,23 @@ bool ControlValidator::is_data_ready()
   if (!current_predicted_trajectory_) {
     return waiting(sub_predicted_traj_->get_topic_name());
   }
+  if (!acceleration_msg_) {
+    return waiting(sub_measured_acc_->subscriber()->get_topic_name());
+  }
+  if (!control_cmd_msg_) {
+    return waiting(sub_control_cmd_->get_topic_name());
+  }
   return true;
 }
 
 void ControlValidator::on_control_cmd(const Control::ConstSharedPtr msg)
 {
+  control_cmd_msg_ = 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;
 }
@@ -178,12 +219,15 @@ void ControlValidator::on_predicted_trajectory(const Trajectory::ConstSharedPtr
   current_predicted_trajectory_ = msg;
   current_reference_trajectory_ = sub_reference_traj_->take_data();
   current_kinematics_ = sub_kinematics_->take_data();
+  acceleration_msg_ = sub_measured_acc_->take_data();
 
   if (!is_data_ready()) return;
 
   debug_pose_publisher_->clear_markers();
 
-  validate(*current_predicted_trajectory_, *current_reference_trajectory_, *current_kinematics_);
+  validate(
+    *current_predicted_trajectory_, *current_reference_trajectory_, *current_kinematics_,
+    *control_cmd_msg_, *acceleration_msg_);
 
   diag_updater_.force_update();
 
@@ -213,7 +257,8 @@ void ControlValidator::publish_debug_info()
 
 void ControlValidator::validate(
   const Trajectory & predicted_trajectory, const Trajectory & reference_trajectory,
-  const Odometry & kinematics)
+  const Odometry & kinematics, const Control & control_cmd,
+  const AccelWithCovarianceStamped & measured_acc)
 {
   if (predicted_trajectory.points.size() < 2) {
     RCLCPP_DEBUG(get_logger(), "predicted_trajectory size is less than 2. Cannot validate.");
@@ -232,7 +277,7 @@ void ControlValidator::validate(
   std::tie(
     validation_status_.max_distance_deviation, validation_status_.is_valid_max_distance_deviation) =
     calc_lateral_deviation_status(predicted_trajectory, *current_reference_trajectory_);
-
+  acceleration_validator.validate(validation_status_, kinematics, control_cmd, measured_acc);
   calc_velocity_deviation_status(*current_reference_trajectory_, kinematics);
   calc_stop_point_overrun_status(*current_reference_trajectory_, kinematics);
 
@@ -310,8 +355,8 @@ void ControlValidator::calc_stop_point_overrun_status(
 
 bool ControlValidator::is_all_valid(const ControlValidatorStatus & s)
 {
-  return s.is_valid_max_distance_deviation && !s.is_rolling_back && !s.is_over_velocity &&
-         !s.has_overrun_stop_point;
+  return s.is_valid_max_distance_deviation && s.is_valid_acc && !s.is_rolling_back &&
+         !s.is_over_velocity && !s.has_overrun_stop_point;
 }
 
 void ControlValidator::display_status()

launch/tier4_control_launch/launch/control.launch.xml

@@ -203,6 +203,7 @@
           <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/measured_acceleration" to="/localization/acceleration"/>
             <remap from="~/input/reference_trajectory" to="/planning/scenario_planning/trajectory"/>
             <remap from="~/input/predicted_trajectory" to="/control/trajectory_follower/lateral/predicted_trajectory"/>
             <remap from="~/output/validation_status" to="~/validation_status"/>