diff --git a/evaluator/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp
index e71b887ff3a10..04a5b758d62e1 100644
--- a/evaluator/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp
+++ b/evaluator/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp
@@ -26,6 +26,8 @@ namespace metrics
 {
 using autoware_auto_planning_msgs::msg::Trajectory;
 using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+using geometry_msgs::msg::Point;
+using geometry_msgs::msg::Pose;
 
 /**
  * @brief calculate lateral deviation of the given trajectory from the reference trajectory
@@ -51,6 +53,30 @@ Stat<double> calcYawDeviation(const Trajectory & ref, const Trajectory & traj);
  */
 Stat<double> calcVelocityDeviation(const Trajectory & ref, const Trajectory & traj);
 
+/**
+ * @brief calculate longitudinal deviation of the given ego pose from the modified goal pose
+ * @param [in] base_pose base pose
+ * @param [in] target_point target point
+ * @return calculated statistics
+ */
+Stat<double> calcLongitudinalDeviation(const Pose & base_pose, const Point & target_point);
+
+/**
+ * @brief calculate lateral deviation of the given ego pose from the modified goal pose
+ * @param [in] base_pose base pose
+ * @param [in] target_point target point
+ * @return calculated statistics
+ */
+Stat<double> calcLateralDeviation(const Pose & base_pose, const Point & target_point);
+
+/**
+ * @brief calculate yaw deviation of the given ego pose from the modified goal pose
+ * @param [in] base_pose base pose
+ * @param [in] target_pose target pose
+ * @return calculated statistics
+ */
+Stat<double> calcYawDeviation(const Pose & base_pose, const Pose & target_pose);
+
 }  // namespace metrics
 }  // namespace planning_diagnostics
 
diff --git a/evaluator/planning_evaluator/include/planning_evaluator/metrics/metric.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/metric.hpp
index 16811f91b32b1..e1ec69dbaef5c 100644
--- a/evaluator/planning_evaluator/include/planning_evaluator/metrics/metric.hpp
+++ b/evaluator/planning_evaluator/include/planning_evaluator/metrics/metric.hpp
@@ -41,6 +41,9 @@ enum class Metric {
   stability_frechet,
   obstacle_distance,
   obstacle_ttc,
+  modified_goal_longitudinal_deviation,
+  modified_goal_lateral_deviation,
+  modified_goal_yaw_deviation,
   SIZE,
 };
 
@@ -63,7 +66,10 @@ static const std::unordered_map<std::string, Metric> str_to_metric = {
   {"stability_frechet", Metric::stability_frechet},
   {"obstacle_distance", Metric::obstacle_distance},
   {"obstacle_ttc", Metric::obstacle_ttc},
-};
+  {"modified_goal_longitudinal_deviation", Metric::modified_goal_longitudinal_deviation},
+  {"modified_goal_lateral_deviation", Metric::modified_goal_lateral_deviation},
+  {"modified_goal_yaw_deviation", Metric::modified_goal_yaw_deviation}};
+
 static const std::unordered_map<Metric, std::string> metric_to_str = {
   {Metric::curvature, "curvature"},
   {Metric::point_interval, "point_interval"},
@@ -79,7 +85,10 @@ static const std::unordered_map<Metric, std::string> metric_to_str = {
   {Metric::stability, "stability"},
   {Metric::stability_frechet, "stability_frechet"},
   {Metric::obstacle_distance, "obstacle_distance"},
-  {Metric::obstacle_ttc, "obstacle_ttc"}};
+  {Metric::obstacle_ttc, "obstacle_ttc"},
+  {Metric::modified_goal_longitudinal_deviation, "modified_goal_longitudinal_deviation"},
+  {Metric::modified_goal_lateral_deviation, "modified_goal_lateral_deviation"},
+  {Metric::modified_goal_yaw_deviation, "modified_goal_yaw_deviation"}};
 
 // Metrics descriptions
 static const std::unordered_map<Metric, std::string> metric_descriptions = {
@@ -97,7 +106,10 @@ static const std::unordered_map<Metric, std::string> metric_descriptions = {
   {Metric::stability, "Stability[m]"},
   {Metric::stability_frechet, "StabilityFrechet[m]"},
   {Metric::obstacle_distance, "Obstacle_distance[m]"},
-  {Metric::obstacle_ttc, "Obstacle_time_to_collision[s]"}};
+  {Metric::obstacle_ttc, "Obstacle_time_to_collision[s]"},
+  {Metric::modified_goal_longitudinal_deviation, "Modified_goal_longitudinal_deviation[m]"},
+  {Metric::modified_goal_lateral_deviation, "Modified_goal_lateral_deviation[m]"},
+  {Metric::modified_goal_yaw_deviation, "Modified_goal_yaw_deviation[rad]"}};
 
 namespace details
 {
diff --git a/evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp
index 0ed2af7b3862e..851678e55d755 100644
--- a/evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp
+++ b/evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp
@@ -14,7 +14,6 @@
 
 #ifndef PLANNING_EVALUATOR__METRICS_CALCULATOR_HPP_
 #define PLANNING_EVALUATOR__METRICS_CALCULATOR_HPP_
-
 #include "planning_evaluator/metrics/metric.hpp"
 #include "planning_evaluator/parameters.hpp"
 #include "planning_evaluator/stat.hpp"
@@ -22,13 +21,19 @@
 #include "autoware_auto_perception_msgs/msg/predicted_objects.hpp"
 #include "autoware_auto_planning_msgs/msg/trajectory.hpp"
 #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
+#include "autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp"
 #include "geometry_msgs/msg/pose.hpp"
 
+#include <optional>
+
 namespace planning_diagnostics
 {
 using autoware_auto_perception_msgs::msg::PredictedObjects;
 using autoware_auto_planning_msgs::msg::Trajectory;
 using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+using autoware_planning_msgs::msg::PoseWithUuidStamped;
+using geometry_msgs::msg::Point;
+using geometry_msgs::msg::Pose;
 
 class MetricsCalculator
 {
@@ -42,7 +47,9 @@ class MetricsCalculator
    * @param [in] metric Metric enum value
    * @return string describing the requested metric
    */
-  Stat<double> calculate(const Metric metric, const Trajectory & traj) const;
+  std::optional<Stat<double>> calculate(const Metric metric, const Trajectory & traj) const;
+  std::optional<Stat<double>> calculate(
+    const Metric metric, const Pose & base_pose, const Pose & target_pose) const;
 
   /**
    * @brief set the reference trajectory used to calculate the deviation metrics
@@ -68,6 +75,12 @@ class MetricsCalculator
    */
   void setEgoPose(const geometry_msgs::msg::Pose & pose);
 
+  /**
+   * @brief get the ego pose
+   * @return ego pose
+   */
+  Pose getEgoPose();
+
 private:
   /**
    * @brief trim a trajectory from the current ego pose to some fixed time or distance
@@ -86,6 +99,7 @@ class MetricsCalculator
   Trajectory previous_trajectory_lookahead_;
   PredictedObjects dynamic_objects_;
   geometry_msgs::msg::Pose ego_pose_;
+  PoseWithUuidStamped modified_goal_;
 };  // class MetricsCalculator
 
 }  // namespace planning_diagnostics
diff --git a/evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp b/evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp
index e9fd82c85cf7b..888eea855295c 100644
--- a/evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp
+++ b/evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp
@@ -24,7 +24,9 @@
 #include "autoware_auto_perception_msgs/msg/predicted_objects.hpp"
 #include "autoware_auto_planning_msgs/msg/trajectory.hpp"
 #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
+#include "autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp"
 #include "diagnostic_msgs/msg/diagnostic_array.hpp"
+#include "nav_msgs/msg/odometry.hpp"
 
 #include <array>
 #include <deque>
@@ -37,8 +39,10 @@ namespace planning_diagnostics
 using autoware_auto_perception_msgs::msg::PredictedObjects;
 using autoware_auto_planning_msgs::msg::Trajectory;
 using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+using autoware_planning_msgs::msg::PoseWithUuidStamped;
 using diagnostic_msgs::msg::DiagnosticArray;
 using diagnostic_msgs::msg::DiagnosticStatus;
+using nav_msgs::msg::Odometry;
 
 /**
  * @brief Node for planning evaluation
@@ -49,6 +53,12 @@ class PlanningEvaluatorNode : public rclcpp::Node
   explicit PlanningEvaluatorNode(const rclcpp::NodeOptions & node_options);
   ~PlanningEvaluatorNode();
 
+  /**
+   * @brief callback on receiving an odometry
+   * @param [in] odometry_msg received odometry message
+   */
+  void onOdometry(const Odometry::ConstSharedPtr odometry_msg);
+
   /**
    * @brief callback on receiving a trajectory
    * @param [in] traj_msg received trajectory message
@@ -68,9 +78,10 @@ class PlanningEvaluatorNode : public rclcpp::Node
   void onObjects(const PredictedObjects::ConstSharedPtr objects_msg);
 
   /**
-   * @brief update the ego pose stored in the MetricsCalculator
+   * @brief callback on receiving a modified goal
+   * @param [in] modified_goal_msg received modified goal message
    */
-  void updateCalculatorEgoPose(const std::string & target_frame);
+  void onModifiedGoal(const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg);
 
   /**
    * @brief publish the given metric statistic
@@ -80,11 +91,15 @@ class PlanningEvaluatorNode : public rclcpp::Node
 
 private:
   static bool isFinite(const TrajectoryPoint & p);
+  void publishModifiedGoalDeviationMetrics();
 
   // ROS
   rclcpp::Subscription<Trajectory>::SharedPtr traj_sub_;
   rclcpp::Subscription<Trajectory>::SharedPtr ref_sub_;
   rclcpp::Subscription<PredictedObjects>::SharedPtr objects_sub_;
+  rclcpp::Subscription<PoseWithUuidStamped>::SharedPtr modified_goal_sub_;
+  rclcpp::Subscription<Odometry>::SharedPtr odom_sub_;
+
   rclcpp::Publisher<DiagnosticArray>::SharedPtr metrics_pub_;
   std::shared_ptr<tf2_ros::TransformListener> transform_listener_{nullptr};
   std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
@@ -99,6 +114,9 @@ class PlanningEvaluatorNode : public rclcpp::Node
   std::vector<Metric> metrics_;
   std::deque<rclcpp::Time> stamps_;
   std::array<std::deque<Stat<double>>, static_cast<size_t>(Metric::SIZE)> metric_stats_;
+
+  Odometry::ConstSharedPtr ego_state_ptr_;
+  PoseWithUuidStamped::ConstSharedPtr modified_goal_ptr_;
 };
 }  // namespace planning_diagnostics
 
diff --git a/evaluator/planning_evaluator/launch/planning_evaluator.launch.xml b/evaluator/planning_evaluator/launch/planning_evaluator.launch.xml
index af7c5369f7422..1b7510c2ced69 100644
--- a/evaluator/planning_evaluator/launch/planning_evaluator.launch.xml
+++ b/evaluator/planning_evaluator/launch/planning_evaluator.launch.xml
@@ -1,15 +1,19 @@
 <launch>
+  <arg name="input/odometry" default="/localization/kinematic_state"/>
   <arg name="input/trajectory" default="/planning/scenario_planning/trajectory"/>
   <arg name="input/reference_trajectory" default="/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/trajectory"/>
   <arg name="input/objects" default="/perception/object_recognition/objects"/>
+  <arg name="input/modified_goal" default="/planning/scenario_planning/modified_goal"/>
 
   <!-- planning evaluator -->
   <group>
     <node name="planning_evaluator" exec="planning_evaluator" pkg="planning_evaluator">
       <param from="$(find-pkg-share planning_evaluator)/param/planning_evaluator.defaults.yaml"/>
+      <remap from="~/input/odometry" to="$(var input/odometry)"/>
       <remap from="~/input/trajectory" to="$(var input/trajectory)"/>
       <remap from="~/input/reference_trajectory" to="$(var input/reference_trajectory)"/>
       <remap from="~/input/objects" to="$(var input/objects)"/>
+      <remap from="~/input/modified_goal" to="$(var input/modified_goal)"/>
       <remap from="~/metrics" to="/diagnostic/planning_evaluator/metrics"/>
     </node>
   </group>
diff --git a/evaluator/planning_evaluator/package.xml b/evaluator/planning_evaluator/package.xml
index 73186f6b20e88..ef2bca288c8b0 100644
--- a/evaluator/planning_evaluator/package.xml
+++ b/evaluator/planning_evaluator/package.xml
@@ -16,6 +16,7 @@
 
   <depend>autoware_auto_perception_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>
+  <depend>autoware_planning_msgs</depend>
   <depend>diagnostic_msgs</depend>
   <depend>eigen</depend>
   <depend>geometry_msgs</depend>
diff --git a/evaluator/planning_evaluator/param/planning_evaluator.defaults.yaml b/evaluator/planning_evaluator/param/planning_evaluator.defaults.yaml
index bd47deedb282f..2e92a174ca79f 100644
--- a/evaluator/planning_evaluator/param/planning_evaluator.defaults.yaml
+++ b/evaluator/planning_evaluator/param/planning_evaluator.defaults.yaml
@@ -19,6 +19,9 @@
       - stability_frechet
       - obstacle_distance
       - obstacle_ttc
+      - modified_goal_longitudinal_deviation
+      - modified_goal_lateral_deviation
+      - modified_goal_yaw_deviation
 
     trajectory:
       min_point_dist_m: 0.1 # [m] minimum distance between two successive points to use for angle calculation
diff --git a/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp
index d6d8aa661e5ea..5146fcd3ec0f2 100644
--- a/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp
+++ b/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp
@@ -77,5 +77,25 @@ Stat<double> calcVelocityDeviation(const Trajectory & ref, const Trajectory & tr
   return stat;
 }
 
+Stat<double> calcLongitudinalDeviation(const Pose & base_pose, const Point & target_point)
+{
+  Stat<double> stat;
+  stat.add(std::abs(tier4_autoware_utils::calcLongitudinalDeviation(base_pose, target_point)));
+  return stat;
+}
+
+Stat<double> calcLateralDeviation(const Pose & base_pose, const Point & target_point)
+{
+  Stat<double> stat;
+  stat.add(std::abs(tier4_autoware_utils::calcLateralDeviation(base_pose, target_point)));
+  return stat;
+}
+
+Stat<double> calcYawDeviation(const Pose & base_pose, const Pose & target_pose)
+{
+  Stat<double> stat;
+  stat.add(std::abs(tier4_autoware_utils::calcYawDeviation(base_pose, target_pose)));
+  return stat;
+}
 }  // namespace metrics
 }  // namespace planning_diagnostics
diff --git a/evaluator/planning_evaluator/src/metrics_calculator.cpp b/evaluator/planning_evaluator/src/metrics_calculator.cpp
index 802e1dde94fd1..adfdddd7d2c5e 100644
--- a/evaluator/planning_evaluator/src/metrics_calculator.cpp
+++ b/evaluator/planning_evaluator/src/metrics_calculator.cpp
@@ -23,9 +23,10 @@
 
 namespace planning_diagnostics
 {
-Stat<double> MetricsCalculator::calculate(const Metric metric, const Trajectory & traj) const
+std::optional<Stat<double>> MetricsCalculator::calculate(
+  const Metric metric, const Trajectory & traj) const
 {
-  // Functions to calculate metrics
+  // Functions to calculate trajectory metrics
   switch (metric) {
     case Metric::curvature:
       return metrics::calcTrajectoryCurvature(traj);
@@ -70,9 +71,23 @@ Stat<double> MetricsCalculator::calculate(const Metric metric, const Trajectory
     case Metric::obstacle_ttc:
       return metrics::calcTimeToCollision(dynamic_objects_, traj, parameters.obstacle.dist_thr_m);
     default:
-      throw std::runtime_error(
-        "[MetricsCalculator][calculate()] unknown Metric " +
-        std::to_string(static_cast<int>(metric)));
+      return {};
+  }
+}
+
+std::optional<Stat<double>> MetricsCalculator::calculate(
+  const Metric metric, const Pose & base_pose, const Pose & target_pose) const
+{
+  // Functions to calculate pose metrics
+  switch (metric) {
+    case Metric::modified_goal_longitudinal_deviation:
+      return metrics::calcLongitudinalDeviation(base_pose, target_pose.position);
+    case Metric::modified_goal_lateral_deviation:
+      return metrics::calcLateralDeviation(base_pose, target_pose.position);
+    case Metric::modified_goal_yaw_deviation:
+      return metrics::calcYawDeviation(base_pose, target_pose);
+    default:
+      return {};
   }
 }
 
@@ -93,6 +108,8 @@ void MetricsCalculator::setPredictedObjects(const PredictedObjects & objects)
 
 void MetricsCalculator::setEgoPose(const geometry_msgs::msg::Pose & pose) { ego_pose_ = pose; }
 
+Pose MetricsCalculator::getEgoPose() { return ego_pose_; }
+
 Trajectory MetricsCalculator::getLookaheadTrajectory(
   const Trajectory & traj, const double max_dist_m, const double max_time_s) const
 {
diff --git a/evaluator/planning_evaluator/src/motion_evaluator_node.cpp b/evaluator/planning_evaluator/src/motion_evaluator_node.cpp
index 56b08fca81f9f..9a103890d53ac 100644
--- a/evaluator/planning_evaluator/src/motion_evaluator_node.cpp
+++ b/evaluator/planning_evaluator/src/motion_evaluator_node.cpp
@@ -55,7 +55,9 @@ MotionEvaluatorNode::~MotionEvaluatorNode()
   f << std::endl;
   for (Metric metric : metrics_) {
     const auto & stat = metrics_calculator_.calculate(metric, accumulated_trajectory_);
-    f /* << std::setw(3 * column_width) */ << stat << " ";
+    if (stat) {
+      f /* << std::setw(3 * column_width) */ << *stat << " ";
+    }
   }
   f.close();
 }
diff --git a/evaluator/planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/planning_evaluator/src/planning_evaluator_node.cpp
index 663f0482cf77b..346b029922e3e 100644
--- a/evaluator/planning_evaluator/src/planning_evaluator_node.cpp
+++ b/evaluator/planning_evaluator/src/planning_evaluator_node.cpp
@@ -40,6 +40,13 @@ PlanningEvaluatorNode::PlanningEvaluatorNode(const rclcpp::NodeOptions & node_op
 
   objects_sub_ = create_subscription<PredictedObjects>(
     "~/input/objects", 1, std::bind(&PlanningEvaluatorNode::onObjects, this, _1));
+
+  modified_goal_sub_ = create_subscription<PoseWithUuidStamped>(
+    "~/input/modified_goal", 1, std::bind(&PlanningEvaluatorNode::onModifiedGoal, this, _1));
+
+  odom_sub_ = create_subscription<Odometry>(
+    "~/input/odometry", 1, std::bind(&PlanningEvaluatorNode::onOdometry, this, _1));
+
   tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
   transform_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
 
@@ -98,24 +105,6 @@ PlanningEvaluatorNode::~PlanningEvaluatorNode()
   }
 }
 
-void PlanningEvaluatorNode::updateCalculatorEgoPose(const std::string & target_frame)
-{
-  try {
-    const geometry_msgs::msg::TransformStamped transform =
-      tf_buffer_->lookupTransform(target_frame, ego_frame_str_, tf2::TimePointZero);
-    geometry_msgs::msg::Pose ego_pose;
-    ego_pose.position.x = transform.transform.translation.x;
-    ego_pose.position.y = transform.transform.translation.y;
-    ego_pose.position.z = transform.transform.translation.z;
-    ego_pose.orientation = transform.transform.rotation;
-    metrics_calculator_.setEgoPose(ego_pose);
-  } catch (tf2::TransformException & ex) {
-    RCLCPP_INFO(
-      this->get_logger(), "Cannot set ego pose: could not transform %s to %s: %s",
-      target_frame.c_str(), ego_frame_str_.c_str(), ex.what());
-  }
-}
-
 DiagnosticStatus PlanningEvaluatorNode::generateDiagnosticStatus(
   const Metric & metric, const Stat<double> & metric_stat) const
 {
@@ -137,18 +126,24 @@ DiagnosticStatus PlanningEvaluatorNode::generateDiagnosticStatus(
 
 void PlanningEvaluatorNode::onTrajectory(const Trajectory::ConstSharedPtr traj_msg)
 {
+  if (!ego_state_ptr_) {
+    return;
+  }
+
   auto start = now();
   stamps_.push_back(traj_msg->header.stamp);
 
-  updateCalculatorEgoPose(traj_msg->header.frame_id);
-
   DiagnosticArray metrics_msg;
   metrics_msg.header.stamp = now();
   for (Metric metric : metrics_) {
-    const Stat metric_stat = metrics_calculator_.calculate(Metric(metric), *traj_msg);
-    metric_stats_[static_cast<size_t>(metric)].push_back(metric_stat);
-    if (metric_stat.count() > 0) {
-      metrics_msg.status.push_back(generateDiagnosticStatus(metric, metric_stat));
+    const auto metric_stat = metrics_calculator_.calculate(Metric(metric), *traj_msg);
+    if (!metric_stat) {
+      continue;
+    }
+
+    metric_stats_[static_cast<size_t>(metric)].push_back(*metric_stat);
+    if (metric_stat->count() > 0) {
+      metrics_msg.status.push_back(generateDiagnosticStatus(metric, *metric_stat));
     }
   }
   if (!metrics_msg.status.empty()) {
@@ -159,6 +154,51 @@ void PlanningEvaluatorNode::onTrajectory(const Trajectory::ConstSharedPtr traj_m
   RCLCPP_DEBUG(get_logger(), "Planning evaluation calculation time: %2.2f ms", runtime * 1e3);
 }
 
+void PlanningEvaluatorNode::onModifiedGoal(
+  const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg)
+{
+  modified_goal_ptr_ = modified_goal_msg;
+  if (ego_state_ptr_) {
+    publishModifiedGoalDeviationMetrics();
+  }
+}
+
+void PlanningEvaluatorNode::onOdometry(const Odometry::ConstSharedPtr odometry_msg)
+{
+  ego_state_ptr_ = odometry_msg;
+  metrics_calculator_.setEgoPose(odometry_msg->pose.pose);
+
+  if (modified_goal_ptr_) {
+    publishModifiedGoalDeviationMetrics();
+  }
+}
+
+void PlanningEvaluatorNode::publishModifiedGoalDeviationMetrics()
+{
+  auto start = now();
+
+  DiagnosticArray metrics_msg;
+  metrics_msg.header.stamp = now();
+  for (Metric metric : metrics_) {
+    const auto metric_stat = metrics_calculator_.calculate(
+      Metric(metric), modified_goal_ptr_->pose, ego_state_ptr_->pose.pose);
+    if (!metric_stat) {
+      continue;
+    }
+    metric_stats_[static_cast<size_t>(metric)].push_back(*metric_stat);
+    if (metric_stat->count() > 0) {
+      metrics_msg.status.push_back(generateDiagnosticStatus(metric, *metric_stat));
+    }
+  }
+  if (!metrics_msg.status.empty()) {
+    metrics_pub_->publish(metrics_msg);
+  }
+  auto runtime = (now() - start).seconds();
+  RCLCPP_DEBUG(
+    get_logger(), "Planning evaluation modified goal deviation calculation time: %2.2f ms",
+    runtime * 1e3);
+}
+
 void PlanningEvaluatorNode::onReferenceTrajectory(const Trajectory::ConstSharedPtr traj_msg)
 {
   metrics_calculator_.setReferenceTrajectory(*traj_msg);
diff --git a/evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp b/evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp
index 8e9bcdd734222..4f2ab014a79d6 100644
--- a/evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp
+++ b/evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp
@@ -36,7 +36,11 @@ using EvalNode = planning_diagnostics::PlanningEvaluatorNode;
 using Trajectory = autoware_auto_planning_msgs::msg::Trajectory;
 using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint;
 using Objects = autoware_auto_perception_msgs::msg::PredictedObjects;
+using autoware_planning_msgs::msg::PoseWithUuidStamped;
 using diagnostic_msgs::msg::DiagnosticArray;
+using nav_msgs::msg::Odometry;
+
+constexpr double epsilon = 1e-6;
 
 class EvalTest : public ::testing::Test
 {
@@ -70,6 +74,10 @@ class EvalTest : public ::testing::Test
       dummy_node, "/planning_evaluator/input/reference_trajectory", 1);
     objects_pub_ =
       rclcpp::create_publisher<Objects>(dummy_node, "/planning_evaluator/input/objects", 1);
+    odom_pub_ =
+      rclcpp::create_publisher<Odometry>(dummy_node, "/planning_evaluator/input/odometry", 1);
+    modified_goal_pub_ = rclcpp::create_publisher<PoseWithUuidStamped>(
+      dummy_node, "/planning_evaluator/input/modified_goal", 1);
 
     tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(dummy_node);
     publishEgoPose(0.0, 0.0, 0.0);
@@ -139,28 +147,51 @@ class EvalTest : public ::testing::Test
     return metric_value_;
   }
 
-  void publishEgoPose(const double x, const double y, const double yaw)
+  double publishModifiedGoalAndGetMetric(const double x, const double y, const double yaw)
   {
-    geometry_msgs::msg::TransformStamped t;
+    metric_updated_ = false;
 
-    // Read message content and assign it to
-    // corresponding tf variables
-    t.header.stamp = dummy_node->now();
-    t.header.frame_id = "map";
-    t.child_frame_id = "base_link";
+    PoseWithUuidStamped goal;
+    goal.header.frame_id = "map";
+    goal.header.stamp = dummy_node->now();
+    goal.pose.position.x = x;
+    goal.pose.position.y = y;
+    goal.pose.position.z = 0.0;
+    tf2::Quaternion q;
+    q.setRPY(0.0, 0.0, yaw);
+    goal.pose.orientation.x = q.x();
+    goal.pose.orientation.y = q.y();
+    goal.pose.orientation.z = q.z();
+    goal.pose.orientation.w = q.w();
+    modified_goal_pub_->publish(goal);
 
-    t.transform.translation.x = x;
-    t.transform.translation.y = y;
-    t.transform.translation.z = 0.0;
+    while (!metric_updated_) {
+      rclcpp::spin_some(eval_node);
+      rclcpp::spin_some(dummy_node);
+      rclcpp::sleep_for(std::chrono::milliseconds(100));
+    }
+    return metric_value_;
+  }
 
+  void publishEgoPose(const double x, const double y, const double yaw)
+  {
+    Odometry odom;
+    odom.header.frame_id = "map";
+    odom.header.stamp = dummy_node->now();
+    odom.pose.pose.position.x = x;
+    odom.pose.pose.position.y = y;
+    odom.pose.pose.position.z = 0.0;
     tf2::Quaternion q;
     q.setRPY(0.0, 0.0, yaw);
-    t.transform.rotation.x = q.x();
-    t.transform.rotation.y = q.y();
-    t.transform.rotation.z = q.z();
-    t.transform.rotation.w = q.w();
+    odom.pose.pose.orientation.x = q.x();
+    odom.pose.pose.orientation.y = q.y();
+    odom.pose.pose.orientation.z = q.z();
+    odom.pose.pose.orientation.w = q.w();
 
-    tf_broadcaster_->sendTransform(t);
+    odom_pub_->publish(odom);
+    rclcpp::spin_some(eval_node);
+    rclcpp::spin_some(dummy_node);
+    rclcpp::sleep_for(std::chrono::milliseconds(100));
   }
 
   // Latest metric value
@@ -173,6 +204,8 @@ class EvalTest : public ::testing::Test
   rclcpp::Publisher<Trajectory>::SharedPtr traj_pub_;
   rclcpp::Publisher<Trajectory>::SharedPtr ref_traj_pub_;
   rclcpp::Publisher<Objects>::SharedPtr objects_pub_;
+  rclcpp::Publisher<PoseWithUuidStamped>::SharedPtr modified_goal_pub_;
+  rclcpp::Publisher<Odometry>::SharedPtr odom_pub_;
   rclcpp::Subscription<DiagnosticArray>::SharedPtr metric_sub_;
   // TF broadcaster
   std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
@@ -407,3 +440,29 @@ TEST_F(EvalTest, TestObstacleTTC)
   t.points[1].pose.position.x = 1.0;
   EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), 2.0);
 }
+
+TEST_F(EvalTest, TestModifiedGoalLongitudinalDeviation)
+{
+  setTargetMetric(planning_diagnostics::Metric::modified_goal_longitudinal_deviation);
+  EXPECT_NEAR(publishModifiedGoalAndGetMetric(1.0, 0.0, 0.0), 1.0, epsilon);
+  EXPECT_NEAR(publishModifiedGoalAndGetMetric(1.0, 0.0, M_PI_2), 0.0, epsilon);
+  EXPECT_NEAR(publishModifiedGoalAndGetMetric(0.0, 1.0, 0.0), 0.0, epsilon);
+  EXPECT_NEAR(publishModifiedGoalAndGetMetric(0.0, 1.0, M_PI_2), 1.0, epsilon);
+}
+
+TEST_F(EvalTest, TestModifiedGoalLateralDeviation)
+{
+  setTargetMetric(planning_diagnostics::Metric::modified_goal_lateral_deviation);
+  EXPECT_NEAR(publishModifiedGoalAndGetMetric(1.0, 0.0, 0.0), 0.0, epsilon);
+  EXPECT_NEAR(publishModifiedGoalAndGetMetric(1.0, 0.0, M_PI_2), 1.0, epsilon);
+  EXPECT_NEAR(publishModifiedGoalAndGetMetric(0.0, 1.0, 0.0), 1.0, epsilon);
+  EXPECT_NEAR(publishModifiedGoalAndGetMetric(0.0, 1.0, M_PI_2), 0.0, epsilon);
+}
+
+TEST_F(EvalTest, TestModifiedGoalYawDeviation)
+{
+  setTargetMetric(planning_diagnostics::Metric::modified_goal_yaw_deviation);
+  EXPECT_NEAR(publishModifiedGoalAndGetMetric(0.0, 0.0, M_PI_2), M_PI_2, epsilon);
+  EXPECT_NEAR(publishModifiedGoalAndGetMetric(1.0, 1.0, -M_PI_2), M_PI_2, epsilon);
+  EXPECT_NEAR(publishModifiedGoalAndGetMetric(1.0, 1.0, -M_PI_4), M_PI_4, epsilon);
+}