add metrics

evaluator/autoware_trajectory_evaluator/include/autoware/trajectory_evaluator/trajectory_evaluator_node.hpp

@@ -29,6 +29,8 @@
 #include <cmath>
 #include <deque>
 #include <fstream>
+#include <memory>
+#include <string>
 #include <vector>
 
 namespace trajectory_evaluator

evaluator/autoware_trajectory_evaluator/package.xml

@@ -12,7 +12,6 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
-
   <depend>autoware_motion_utils</depend>
   <depend>autoware_planning_msgs</depend>
   <depend>autoware_universe_utils</depend>

evaluator/autoware_trajectory_evaluator/src/trajectory_evaluator_node.cpp

@@ -183,6 +183,38 @@ void TrajectoryEvaluatorNode::on_kinematic_state(
         time_error_metric.value = time_error;
         metrics_msg_.metric_array.push_back(time_error_metric);
 
+        MetricMsg closest_traj_x;
+        closest_traj_x.name = "trajectory_metrics/closest_traj_x";
+        closest_traj_x.value = closest_point.trajectory_point.pose.position.x;
+        metrics_msg_.metric_array.push_back(closest_traj_x);
+
+        MetricMsg closest_traj_y;
+        closest_traj_y.name = "trajectory_metrics/closest_traj_y";
+        closest_traj_y.value = closest_point.trajectory_point.pose.position.y;
+        metrics_msg_.metric_array.push_back(closest_traj_y);
+
+        MetricMsg closest_traj_z;
+        closest_traj_z.name = "trajectory_metrics/closest_traj_z";
+        closest_traj_z.value = closest_point.trajectory_point.pose.position.z;
+        metrics_msg_.metric_array.push_back(closest_traj_z);
+
+        MetricMsg current_pose_x;
+        current_pose_x.name = "trajectory_metrics/current_pose_x";
+        current_pose_x.value = pose.position.x;
+        metrics_msg_.metric_array.push_back(closest_traj_x);
+
+        MetricMsg current_pose_y;
+        current_pose_y.name = "trajectory_metrics/current_pose_y";
+        current_pose_y.value = pose.position.y;
+        metrics_msg_.metric_array.push_back(closest_traj_y);
+
+        MetricMsg current_pose_z;
+        current_pose_z.name = "trajectory_metrics/current_pose_z";
+        current_pose_z.value = pose.position.z;
+        metrics_msg_.metric_array.push_back(current_pose_z);
+
+        metrics_pub_->publish(metrics_msg_);
+
         metrics_pub_->publish(metrics_msg_);
 
         RCLCPP_INFO(

evaluator/autoware_trajectory_evaluator/test/test_trajectory_evaluator_node.cpp

@@ -58,7 +58,7 @@ class TrajectoryEvaluatorTests : public ::testing::Test
 
   virtual void TearDown() { rclcpp::shutdown(); }
 
-  void publishTrajectoryAndWait(
+  void publish_trajectory_and_wait(
     const autoware_planning_msgs::msg::Trajectory & trajectory, std::chrono::milliseconds timeout)
   {
     // received_metrics.reset();
@@ -72,7 +72,7 @@ class TrajectoryEvaluatorTests : public ::testing::Test
     }
   }
 
-  void simulateEgoVehicle(
+  void simulate_ego_vehicle(
     const autoware_planning_msgs::msg::Trajectory & trajectory, double interval_seconds)
   {
     nav_msgs::msg::Odometry odometry_msg;
@@ -116,7 +116,7 @@ class TrajectoryEvaluatorTests : public ::testing::Test
     }
   }
 
-  double extractMetricValue(const MetricArrayMsg & metrics, const std::string & name)
+  double extract_metric_value(const MetricArrayMsg & metrics, const std::string & name)
   {
     for (const auto & metric : metrics.metric_array) {
       if (metric.name == name) {
@@ -126,7 +126,7 @@ class TrajectoryEvaluatorTests : public ::testing::Test
     throw std::runtime_error("Metric not found: " + name);
   }
 
-  autoware_planning_msgs::msg::Trajectory generateLinearTrajectory(
+  autoware_planning_msgs::msg::Trajectory generate_linear_trajectory(
     double length, int num_points, double velocity)
   {
     autoware_planning_msgs::msg::Trajectory trajectory;
@@ -148,15 +148,16 @@ TEST_F(TrajectoryEvaluatorTests, TestLinearTrajectoryWithEgoVehicleOdometry)
   int num_points = 20;
   double velocity = 5.0;
 
-  auto trajectory = generateLinearTrajectory(length, num_points, velocity);
-  publishTrajectoryAndWait(trajectory, std::chrono::milliseconds(5000));
+  auto trajectory = generate_linear_trajectory(length, num_points, velocity);
+  publish_trajectory_and_wait(trajectory, std::chrono::milliseconds(5000));
 
   double interval_seconds = 0.1;
-  simulateEgoVehicle(trajectory, interval_seconds);
+  simulate_ego_vehicle(trajectory, interval_seconds);
 
-  double expected_time = extractMetricValue(*received_metrics, "trajectory_metrics/expected_time");
-  double actual_time = extractMetricValue(*received_metrics, "trajectory_metrics/actual_time");
-  double time_error = extractMetricValue(*received_metrics, "trajectory_metrics/time_error");
+  double expected_time =
+    extract_metric_value(*received_metrics, "trajectory_metrics/expected_time");
+  double actual_time = extract_metric_value(*received_metrics, "trajectory_metrics/actual_time");
+  double time_error = extract_metric_value(*received_metrics, "trajectory_metrics/time_error");
 
   double tolerance = 0.1;
   EXPECT_NEAR(actual_time, expected_time, tolerance);