feat(evaluator): Add Trajectory Evaluator

evaluator/autoware_trajectory_evaluator/CMakeLists.txt

@@ -0,0 +1,43 @@
+cmake_minimum_required(VERSION 3.22)
+project(autoware_trajectory_evaluator)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(pluginlib REQUIRED)
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(trajectory_evaluator_node SHARED
+  src/trajectory_evaluator_node.cpp
+)
+
+rclcpp_components_register_node(trajectory_evaluator_node
+  PLUGIN "trajectory_evaluator::TrajectoryEvaluatorNode"
+  EXECUTABLE trajectory_evaluator
+)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    config
+    launch
+)
+
+if(BUILD_TESTING)
+  ament_add_gtest(test_trajectory_evaluator
+    test/test_trajectory_evaluator_node.cpp
+  )
+  target_link_libraries(test_trajectory_evaluator
+    trajectory_evaluator_node
+  )
+endif()

evaluator/autoware_trajectory_evaluator/config/trajectory_evaluator.param.yaml

@@ -0,0 +1,5 @@
+/**:
+  ros__parameters:
+    # output_file: ""
+    # trajectory_history_limit: ""
+    ego_frame: base_link # reference frame of ego

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

@@ -0,0 +1,110 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__TRAJECTORY_EVALUATOR__TRAJECTORY_EVALUATOR_NODE_HPP_
+#define AUTOWARE__TRAJECTORY_EVALUATOR__TRAJECTORY_EVALUATOR_NODE_HPP_
+
+#include "autoware/motion_utils/trajectory/trajectory.hpp"
+#include "autoware/universe_utils/geometry/geometry.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include <autoware/universe_utils/ros/polling_subscriber.hpp>
+
+#include "autoware_planning_msgs/msg/trajectory.hpp"
+#include "autoware_planning_msgs/msg/trajectory_point.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+
+#include <chrono>
+#include <cmath>
+#include <deque>
+#include <fstream>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace trajectory_evaluator
+{
+
+struct TrajectoryPointWithTime
+{
+  autoware_planning_msgs::msg::TrajectoryPoint trajectory_point;
+  rclcpp::Duration time_from_start;
+  rclcpp::Time time_stamp;
+
+  TrajectoryPointWithTime(
+    const autoware_planning_msgs::msg::TrajectoryPoint & p, const rclcpp::Duration & time_start,
+    const rclcpp::Time & ts)
+  : trajectory_point(p), time_from_start(time_start), time_stamp(ts)
+  {
+  }
+};
+
+struct TrajectoryWithTimestamp
+{
+  std::vector<TrajectoryPointWithTime> trajectory_points;
+  rclcpp::Time time_stamp;
+};
+
+struct TimeErrorData
+{
+  rclcpp::Time stamp;
+  size_t trajectory_index;
+  geometry_msgs::msg::Point position;
+  double expected_time;
+  double actual_time;
+  double time_error;
+};
+
+std::vector<TrajectoryWithTimestamp> trajectory_history_;
+
+class TrajectoryEvaluatorNode : public rclcpp::Node
+{
+public:
+  explicit TrajectoryEvaluatorNode(const rclcpp::NodeOptions & node_options);
+  ~TrajectoryEvaluatorNode();
+  void store_trajectory(
+    const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr traj_msg,
+    const nav_msgs::msg::Odometry::ConstSharedPtr odom_msg);
+  void on_kinematic_state(const nav_msgs::msg::Odometry::ConstSharedPtr odom_msg);
+  bool is_pose_equal(
+    const geometry_msgs::msg::Pose & pose1, const geometry_msgs::msg::Pose & pose2);
+
+private:
+  std::shared_ptr<rclcpp::TimerBase> traj_timer_;
+  std::shared_ptr<rclcpp::TimerBase> odom_timer_;
+  void on_timer();
+
+  // void onKinematicState(const Odometry::ConstSharedPtr odom_msg);
+  void calculate_time_from_start(
+    TrajectoryWithTimestamp & trajectory, const geometry_msgs::msg::Point & current_ego_point,
+    const float min_velocity);
+
+  // Parameter
+  std::string output_file_str_;
+
+  // Subscribers
+  autoware::universe_utils::InterProcessPollingSubscriber<autoware_planning_msgs::msg::Trajectory>
+    traj_sub_{this, "/planning/scenario_planning/trajectory"};
+  autoware::universe_utils::InterProcessPollingSubscriber<nav_msgs::msg::Odometry>
+    kinematic_state_sub_{this, "/localization/kinematic_state"};
+
+  // Store past trajectory messages
+  std::deque<TrajectoryWithTimestamp> trajectory_history_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  std::vector<TimeErrorData> time_errors_;
+  std::deque<rclcpp::Time> stamps_;
+};
+}  // namespace trajectory_evaluator
+
+#endif  // AUTOWARE__TRAJECTORY_EVALUATOR__TRAJECTORY_EVALUATOR_NODE_HPP_

evaluator/autoware_trajectory_evaluator/launch/trajectory_evaluator.launch.xml

@@ -0,0 +1,5 @@
+<launch>
+  <group>
+    <node name="trajectory_evaluator" exec="trajectory_evaluator" pkg="autoware_trajectory_evaluator"/>
+  </group>
+</launch>

evaluator/autoware_trajectory_evaluator/package.xml

@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autoware_trajectory_evaluator</name>
+  <version>0.0.0</version>
+  <description>trajectory evaluator ROS 2 node</description>
+  <maintainer email="[email protected]">Muhammad Raditya GIOVANNI</maintainer>
+  <maintainer email="[email protected]">Maxime CLEMENT</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <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>
+  <depend>diagnostic_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tier4_metric_msgs</depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>