add test

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

@@ -39,6 +39,8 @@
 #include <tuple>
 #include <utility>
 
+class AccelerationValidatorTest;
+
 namespace autoware::control_validator
 {
 using autoware_control_msgs::msg::Control;
@@ -67,6 +69,7 @@ struct ValidationParams
 class AccelerationValidator
 {
 public:
+  friend class ::AccelerationValidatorTest;
   explicit AccelerationValidator(rclcpp::Node & node)
   {
     e_offset =
@@ -83,6 +86,7 @@ class AccelerationValidator
     const AccelWithCovarianceStamped & loc_acc);
 
 private:
+  bool is_in_error_range() const;
   double e_offset;
   double e_scale;
   autoware::signal_processing::LowpassFilter1d desired_acc_lpf{0.0};

control/autoware_control_validator/src/control_validator.cpp

@@ -37,21 +37,23 @@ void AccelerationValidator::validate(
     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) {
+  if (std::abs(kinematic_state.twist.twist.linear.x) < 0.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;
-  }();
+  res.is_valid_acc = is_in_error_range();
+}
+
+bool AccelerationValidator::is_in_error_range() const
+{
+  const double des = desired_acc_lpf.getValue().value();
+  const double mes = measured_acc_lpf.getValue().value();
+
+  return mes <= des + std::abs(e_scale * des) + e_offset &&
+         mes >= des - std::abs(e_scale * des) - e_offset;
 }
 
 ControlValidator::ControlValidator(const rclcpp::NodeOptions & options)

control/autoware_control_validator/test/test_control_validator.cpp

@@ -72,7 +72,7 @@ TrajectoryPoint make_trajectory_point(double x, double y)
   return point;
 }
 
-class ControlValidatorTest
+class TrajectoryDeviationTest
 : public ::testing::TestWithParam<std::tuple<Trajectory, Trajectory, double, bool>>
 {
 protected:
@@ -96,7 +96,7 @@ class ControlValidatorTest
   std::shared_ptr<autoware::control_validator::ControlValidator> node;
 };
 
-TEST_P(ControlValidatorTest, test_calc_lateral_deviation_status)
+TEST_P(TrajectoryDeviationTest, test_calc_lateral_deviation_status)
 {
   auto [reference_trajectory, predicted_trajectory, expected_deviation, expected_condition] =
     GetParam();
@@ -108,7 +108,7 @@ TEST_P(ControlValidatorTest, test_calc_lateral_deviation_status)
 }
 
 INSTANTIATE_TEST_SUITE_P(
-  ControlValidatorTests, ControlValidatorTest,
+  TrajectoryDeviationTests, TrajectoryDeviationTest,
   ::testing::Values(
 
     std::make_tuple(
@@ -169,3 +169,51 @@ INSTANTIATE_TEST_SUITE_P(
       1.0, true))
 
 );
+
+class AccelerationValidatorTest : public ::testing::TestWithParam<std::tuple<bool, double, double>>
+{
+public:
+  bool is_in_error_range() { return acceleration_validator_->is_in_error_range(); }
+  void set_desired(double x) { acceleration_validator_->desired_acc_lpf.reset(x); }
+  void set_measured(double x) { acceleration_validator_->measured_acc_lpf.reset(x); }
+
+protected:
+  void SetUp() override
+  {
+    rclcpp::init(0, nullptr);
+    rclcpp::NodeOptions options;
+    options.arguments(
+      {"--ros-args", "--params-file",
+       ament_index_cpp::get_package_share_directory("autoware_control_validator") +
+         "/config/control_validator.param.yaml",
+       "--params-file",
+       ament_index_cpp::get_package_share_directory("autoware_test_utils") +
+         "/config/test_vehicle_info.param.yaml"});
+
+    node_ = std::make_shared<autoware::control_validator::ControlValidator>(options);
+    acceleration_validator_ =
+      std::make_shared<autoware::control_validator::AccelerationValidator>(*node_);
+  }
+  void TearDown() override { rclcpp::shutdown(); }
+
+  std::shared_ptr<rclcpp::Node> node_;
+  std::shared_ptr<autoware::control_validator::AccelerationValidator> acceleration_validator_;
+};
+
+TEST_P(AccelerationValidatorTest, test_is_in_error_range)
+{
+  auto [expected, des, mes] = GetParam();
+  set_desired(des);
+  set_measured(mes);
+
+  ASSERT_EQ(expected, is_in_error_range());
+};
+
+INSTANTIATE_TEST_SUITE_P(
+  AccelerationValidatorTests, AccelerationValidatorTest,
+  ::testing::Values(
+    std::make_tuple(true, 0.0, 0.0), std::make_tuple(false, 0.0, 5.0),
+    std::make_tuple(false, 0.0, -5.0), std::make_tuple(true, 1.0, 1.0),
+    std::make_tuple(false, 1.0, 5.0), std::make_tuple(false, 1.0, -5.0),
+    std::make_tuple(true, -1.0, -1.0), std::make_tuple(false, -1.0, -5.0),
+    std::make_tuple(false, -1.0, 5.0)));