test(autoware_control_evaluator): add unit test for utils autoware_co…

…ntrol_evaluator (autowarefoundation#9307)

* update unit test of control_evaluator.

Signed-off-by: xtk8532704 <1041084556@qq.com>

* manual pre-commit.

Signed-off-by: xtk8532704 <1041084556@qq.com>

---------

Signed-off-by: xtk8532704 <1041084556@qq.com>

evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp

@@ -37,6 +37,7 @@ using EvalNode = control_diagnostics::ControlEvaluatorNode;
 using Trajectory = autoware_planning_msgs::msg::Trajectory;
 using TrajectoryPoint = autoware_planning_msgs::msg::TrajectoryPoint;
 using MetricArrayMsg = tier4_metric_msgs::msg::MetricArray;
+using geometry_msgs::msg::AccelWithCovarianceStamped;
 using nav_msgs::msg::Odometry;
 
 constexpr double epsilon = 1e-6;
@@ -69,6 +70,8 @@ class EvalTest : public ::testing::Test
       rclcpp::create_publisher<Trajectory>(dummy_node, "/control_evaluator/input/trajectory", 1);
     odom_pub_ =
       rclcpp::create_publisher<Odometry>(dummy_node, "/control_evaluator/input/odometry", 1);
+    acc_pub_ = rclcpp::create_publisher<AccelWithCovarianceStamped>(
+      dummy_node, "/control_evaluator/input/acceleration", 1);
     tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(dummy_node);
     publishEgoPose(0.0, 0.0, 0.0);
   }
@@ -107,19 +110,15 @@ class EvalTest : public ::testing::Test
   void publishTrajectory(const Trajectory & traj)
   {
     traj_pub_->publish(traj);
-    rclcpp::spin_some(eval_node);
-    rclcpp::spin_some(dummy_node);
-    rclcpp::sleep_for(std::chrono::milliseconds(100));
+    spin_some();
   }
 
   double publishTrajectoryAndGetMetric(const Trajectory & traj)
   {
     metric_updated_ = false;
     traj_pub_->publish(traj);
     while (!metric_updated_) {
-      rclcpp::spin_some(eval_node);
-      rclcpp::spin_some(dummy_node);
-      rclcpp::sleep_for(std::chrono::milliseconds(100));
+      spin_some();
     }
     return metric_value_;
   }
@@ -138,8 +137,36 @@ class EvalTest : public ::testing::Test
     odom.pose.pose.orientation.y = q.y();
     odom.pose.pose.orientation.z = q.z();
     odom.pose.pose.orientation.w = q.w();
-
     odom_pub_->publish(odom);
+    spin_some();
+  }
+
+  void publishEgoAcc(const double acc1)
+  {
+    AccelWithCovarianceStamped acc_msg;
+    acc_msg.header.frame_id = "baselink";
+    acc_msg.header.stamp = dummy_node->now();
+    acc_msg.accel.accel.linear.x = acc1;
+    acc_pub_->publish(acc_msg);
+    spin_some();
+  }
+
+  double publishEgoAccAndGetMetric(const double acc)
+  {
+    metric_updated_ = false;
+    AccelWithCovarianceStamped acc_msg;
+    acc_msg.header.frame_id = "baselink";
+
+    acc_msg.header.stamp = dummy_node->now();
+    acc_msg.accel.accel.linear.x = acc;
+    acc_pub_->publish(acc_msg);
+    while (!metric_updated_) {
+      spin_some();
+    }
+    return metric_value_;
+  }
+  void spin_some()
+  {
     rclcpp::spin_some(eval_node);
     rclcpp::spin_some(dummy_node);
     rclcpp::sleep_for(std::chrono::milliseconds(100));
@@ -151,9 +178,10 @@ class EvalTest : public ::testing::Test
   // Node
   rclcpp::Node::SharedPtr dummy_node;
   EvalNode::SharedPtr eval_node;
-  // Trajectory publishers
+  // publishers
   rclcpp::Publisher<Trajectory>::SharedPtr traj_pub_;
   rclcpp::Publisher<Odometry>::SharedPtr odom_pub_;
+  rclcpp::Publisher<AccelWithCovarianceStamped>::SharedPtr acc_pub_;
   rclcpp::Subscription<MetricArrayMsg>::SharedPtr metric_sub_;
   // TF broadcaster
   std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
@@ -196,3 +224,23 @@ TEST_F(EvalTest, TestLateralDeviation)
   publishEgoPose(1.0, 1.0, 0.0);
   EXPECT_NEAR(publishTrajectoryAndGetMetric(t), 1.0, epsilon);
 }
+
+TEST_F(EvalTest, TestKinematicStateAcc)
+{
+  setTargetMetric("kinematic_state/acc");
+  Trajectory t = makeTrajectory({{0.0, 0.0}, {1.0, 0.0}});
+  publishTrajectory(t);
+  publishEgoPose(0.0, 0.0, 0.0);
+  EXPECT_NEAR(publishEgoAccAndGetMetric(2.0), 2.0, epsilon);
+}
+
+TEST_F(EvalTest, TestKinematicStateJerk)
+{
+  setTargetMetric("kinematic_state/jerk");
+  Trajectory t = makeTrajectory({{0.0, 0.0}, {1.0, 0.0}});
+  publishTrajectory(t);
+  publishEgoPose(0.0, 0.0, 0.0);
+  // there is about 0.1 sec delay in spin_some
+  publishEgoAcc(0.0);
+  EXPECT_NEAR(publishEgoAccAndGetMetric(1), 10.0, 0.5);
+}