po

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

Author: yuki-takagi-66

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
@@ -130,7 +133,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
@@ -142,6 +146,32 @@ class ControlValidator : public rclcpp::Node
    */
   void display_status();
 
+  class AccelerationValidator
+  {
+  public:
+    void setup_parameters(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 auto 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);
+    };
+
+    bool validate(
+      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};
+  } acceleration_validator;
+
   /**
    * @brief Set the diagnostic status
    * @param stat Diagnostic status wrapper
@@ -156,6 +186,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 +203,7 @@ 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};
+
   bool hold_velocity_error_until_stop_{false};
 
   vehicle_info_utils::VehicleInfo vehicle_info_;
@@ -186,6 +219,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,6 +2,7 @@ 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

control/autoware_control_validator/src/control_validator.cpp

@@ -19,7 +19,7 @@
 #include "autoware/motion_utils/trajectory/trajectory.hpp"
 #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp"
 
-#include <nav_msgs/msg/odometry.hpp>
+// #include <nav_msgs/msg/odometry.hpp>
 
 #include <algorithm>
 #include <cstdint>
@@ -47,6 +47,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 +80,11 @@ 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);
+  acceleration_validator.setup_parameters(*this);
+
+  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 +127,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,11 +169,18 @@ 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;
@@ -178,12 +195,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 +233,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.");
@@ -235,6 +256,9 @@ void ControlValidator::validate(
 
   calc_velocity_deviation_status(*current_reference_trajectory_, kinematics);
   calc_stop_point_overrun_status(*current_reference_trajectory_, kinematics);
+  validation_status_.is_valid_acc =
+    acceleration_validator.validate(kinematics, control_cmd, measured_acc);
+  calc_stop_point_overrun_status(*current_reference_trajectory_, kinematics);
 
   validation_status_.invalid_count =
     is_all_valid(validation_status_) ? 0 : validation_status_.invalid_count + 1;
@@ -250,6 +274,30 @@ std::pair<double, bool> ControlValidator::calc_lateral_deviation_status(
     max_distance_deviation <= validation_params_.max_distance_deviation_threshold};
 }
 
+bool ControlValidator::AccelerationValidator::validate(
+  const Odometry & kinematic_state, const Control & control_cmd,
+  const AccelWithCovarianceStamped & loc_acc)
+{
+  if (std::abs(kinematic_state.twist.twist.linear.x) < 1e-3) {
+    desired_acc_lpf.reset();
+    measured_acc_lpf.reset();
+    return true;
+  }
+
+  const double des = desired_acc_lpf.filter(
+    control_cmd.longitudinal.acceleration + autoware_utils::get_rpy(kinematic_state.pose.pose).y);
+  const int8_t des_sign = std::signbit(des) ? 1 : -1;
+  const double mes = measured_acc_lpf.filter(loc_acc.accel.accel.linear.x);
+
+  if (
+    mes < des * (1 + des_sign * e_scale) + e_offset &&
+    mes > des * (1 - des_sign * e_scale) - e_offset) {
+    return true;
+  }
+
+  return false;
+}
+
 void ControlValidator::calc_velocity_deviation_status(
   const Trajectory & reference_trajectory, const Odometry & kinematics)
 {
@@ -310,8 +358,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"/>