diff --git a/common/tier4_planning_rviz_plugin/CMakeLists.txt b/common/tier4_planning_rviz_plugin/CMakeLists.txt
index 83ee54da1b748..967e010558eb2 100644
--- a/common/tier4_planning_rviz_plugin/CMakeLists.txt
+++ b/common/tier4_planning_rviz_plugin/CMakeLists.txt
@@ -27,6 +27,8 @@ ament_auto_add_library(tier4_planning_rviz_plugin SHARED
   src/path_with_lane_id/display.cpp
   include/path_with_lane_id_footprint/display.hpp
   src/path_with_lane_id_footprint/display.cpp
+  include/pose_with_uuid_stamped/display.hpp
+  src/pose_with_uuid_stamped/display.cpp
   include/trajectory/display.hpp
   src/trajectory/display.cpp
   include/trajectory_footprint/display.hpp
diff --git a/common/tier4_planning_rviz_plugin/icons/classes/PoseWithUuidStamped.png b/common/tier4_planning_rviz_plugin/icons/classes/PoseWithUuidStamped.png
new file mode 100644
index 0000000000000..6a67573717ae1
Binary files /dev/null and b/common/tier4_planning_rviz_plugin/icons/classes/PoseWithUuidStamped.png differ
diff --git a/common/tier4_planning_rviz_plugin/include/pose_with_uuid_stamped/display.hpp b/common/tier4_planning_rviz_plugin/include/pose_with_uuid_stamped/display.hpp
new file mode 100644
index 0000000000000..f1fe3c30cd457
--- /dev/null
+++ b/common/tier4_planning_rviz_plugin/include/pose_with_uuid_stamped/display.hpp
@@ -0,0 +1,84 @@
+// Copyright 2023 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 POSE_WITH_UUID_STAMPED__DISPLAY_HPP_
+#define POSE_WITH_UUID_STAMPED__DISPLAY_HPP_
+
+#include <rviz_common/message_filter_display.hpp>
+
+#include <autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp>
+
+#include <deque>
+#include <memory>
+#include <string>
+
+namespace rviz_rendering
+{
+class Axes;
+class MovableText;
+}  // namespace rviz_rendering
+namespace rviz_common::properties
+{
+class FloatProperty;
+class TfFrameProperty;
+}  // namespace rviz_common::properties
+
+namespace rviz_plugins
+{
+class AutowarePoseWithUuidStampedDisplay
+: public rviz_common::MessageFilterDisplay<autoware_planning_msgs::msg::PoseWithUuidStamped>
+{
+  Q_OBJECT
+
+public:
+  AutowarePoseWithUuidStampedDisplay();
+  ~AutowarePoseWithUuidStampedDisplay() override;
+  AutowarePoseWithUuidStampedDisplay(const AutowarePoseWithUuidStampedDisplay &) = delete;
+  AutowarePoseWithUuidStampedDisplay(const AutowarePoseWithUuidStampedDisplay &&) = delete;
+  AutowarePoseWithUuidStampedDisplay & operator=(const AutowarePoseWithUuidStampedDisplay &) =
+    delete;
+  AutowarePoseWithUuidStampedDisplay & operator=(const AutowarePoseWithUuidStampedDisplay &&) =
+    delete;
+
+protected:
+  void onInitialize() override;
+  void onEnable() override;
+  void onDisable() override;
+
+private Q_SLOTS:
+  void updateVisualization();
+
+private:
+  void subscribe() override;
+  void unsubscribe() override;
+  void processMessage(
+    const autoware_planning_msgs::msg::PoseWithUuidStamped::ConstSharedPtr meg_ptr) override;
+
+  std::unique_ptr<rviz_rendering::Axes> axes_;
+  std::unique_ptr<Ogre::SceneNode> uuid_node_;
+  rviz_rendering::MovableText * uuid_;
+
+  rviz_common::properties::FloatProperty * length_property_;
+  rviz_common::properties::FloatProperty * radius_property_;
+  rviz_common::properties::TfFrameProperty * frame_property_;
+
+  rviz_common::properties::BoolProperty * uuid_text_view_property_;
+  rviz_common::properties::FloatProperty * uuid_text_scale_property_;
+
+  autoware_planning_msgs::msg::PoseWithUuidStamped::ConstSharedPtr last_msg_ptr_;
+};
+
+}  // namespace rviz_plugins
+
+#endif  // POSE_WITH_UUID_STAMPED__DISPLAY_HPP_
diff --git a/common/tier4_planning_rviz_plugin/package.xml b/common/tier4_planning_rviz_plugin/package.xml
index a140123035acf..ebe782b8b937c 100644
--- a/common/tier4_planning_rviz_plugin/package.xml
+++ b/common/tier4_planning_rviz_plugin/package.xml
@@ -13,6 +13,7 @@
   <build_depend>autoware_cmake</build_depend>
 
   <depend>autoware_auto_planning_msgs</depend>
+  <depend>autoware_planning_msgs</depend>
   <depend>libqt5-core</depend>
   <depend>libqt5-gui</depend>
   <depend>libqt5-widgets</depend>
diff --git a/common/tier4_planning_rviz_plugin/plugins/plugin_description.xml b/common/tier4_planning_rviz_plugin/plugins/plugin_description.xml
index b61b79ba35d3f..44e4c42c24034 100644
--- a/common/tier4_planning_rviz_plugin/plugins/plugin_description.xml
+++ b/common/tier4_planning_rviz_plugin/plugins/plugin_description.xml
@@ -24,6 +24,11 @@
     base_class_type="rviz_common::Display">
     <description>Display footprint of autoware_auto_planning_msg::PathWithLaneId</description>
   </class>
+  <class name="rviz_plugins/PoseWithUuidStamped"
+    type="rviz_plugins::AutowarePoseWithUuidStampedDisplay"
+    base_class_type="rviz_common::Display">
+    <description>Display pose_with_uuid_stamped of autoware_planning_msg::PoseWithUuidStamped</description>
+  </class>
   <class name="rviz_plugins/Trajectory"
     type="rviz_plugins::AutowareTrajectoryDisplay"
     base_class_type="rviz_common::Display">
diff --git a/common/tier4_planning_rviz_plugin/src/pose_with_uuid_stamped/display.cpp b/common/tier4_planning_rviz_plugin/src/pose_with_uuid_stamped/display.cpp
new file mode 100644
index 0000000000000..76b1a7be0cf0d
--- /dev/null
+++ b/common/tier4_planning_rviz_plugin/src/pose_with_uuid_stamped/display.cpp
@@ -0,0 +1,146 @@
+// Copyright 2023 TIER IV, Inc. All rights reserved.
+//
+// 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.
+
+#include "rviz_common/properties/tf_frame_property.hpp"
+
+#include <pose_with_uuid_stamped/display.hpp>
+#include <rviz_common/properties/float_property.hpp>
+#include <rviz_common/validate_floats.hpp>
+#include <rviz_rendering/objects/axes.hpp>
+#include <rviz_rendering/objects/movable_text.hpp>
+
+namespace
+{
+std::string uuid_to_string(const unique_identifier_msgs::msg::UUID & u)
+{
+  std::stringstream ss;
+  for (auto i = 0; i < 16; ++i) {
+    ss << std::hex << std::setfill('0') << std::setw(2) << +u.uuid[i];
+  }
+  return ss.str();
+}
+}  // namespace
+
+namespace rviz_plugins
+{
+AutowarePoseWithUuidStampedDisplay::AutowarePoseWithUuidStampedDisplay()
+{
+  length_property_ = new rviz_common::properties::FloatProperty(
+    "Length", 1.5f, "Length of each axis, in meters.", this, SLOT(updateVisualization()));
+  length_property_->setMin(0.0001f);
+
+  radius_property_ = new rviz_common::properties::FloatProperty(
+    "Radius", 0.5f, "Radius of each axis, in meters.", this, SLOT(updateVisualization()));
+  radius_property_->setMin(0.0001f);
+
+  uuid_text_view_property_ = new rviz_common::properties::BoolProperty(
+    "UUID", false, "flag of visualizing uuid text", this, SLOT(updateVisualization()), this);
+  uuid_text_scale_property_ = new rviz_common::properties::FloatProperty(
+    "Scale", 0.3f, "Scale of uuid text", uuid_text_view_property_, SLOT(updateVisualization()),
+    this);
+}
+
+AutowarePoseWithUuidStampedDisplay::~AutowarePoseWithUuidStampedDisplay() = default;
+
+void AutowarePoseWithUuidStampedDisplay::onInitialize()
+{
+  MFDClass::onInitialize();
+  axes_ = std::make_unique<rviz_rendering::Axes>(
+    scene_manager_, scene_node_, length_property_->getFloat(), radius_property_->getFloat());
+  axes_->getSceneNode()->setVisible(isEnabled());
+
+  uuid_node_.reset(scene_node_->createChildSceneNode());
+  uuid_ = new rviz_rendering::MovableText("not initialized", "Liberation Sans", 0.1);
+}
+
+void AutowarePoseWithUuidStampedDisplay::onEnable()
+{
+  subscribe();
+  axes_->getSceneNode()->setVisible(true);
+}
+
+void AutowarePoseWithUuidStampedDisplay::onDisable()
+{
+  unsubscribe();
+  axes_->getSceneNode()->setVisible(false);
+}
+
+void AutowarePoseWithUuidStampedDisplay::subscribe() { MFDClass::subscribe(); }
+
+void AutowarePoseWithUuidStampedDisplay::unsubscribe() { MFDClass::unsubscribe(); }
+
+void AutowarePoseWithUuidStampedDisplay::updateVisualization()
+{
+  if (last_msg_ptr_ != nullptr) {
+    processMessage(last_msg_ptr_);
+  }
+}
+
+void AutowarePoseWithUuidStampedDisplay::processMessage(
+  const autoware_planning_msgs::msg::PoseWithUuidStamped::ConstSharedPtr msg_ptr)
+{
+  uuid_node_->detachAllObjects();
+
+  {
+    Ogre::Vector3 position;
+    Ogre::Quaternion orientation;
+
+    if (!context_->getFrameManager()->getTransform(msg_ptr->header, position, orientation)) {
+      const auto frame = msg_ptr->header.frame_id.c_str();
+      RCLCPP_DEBUG(
+        rclcpp::get_logger("AutowarePoseWithUuidStampedDisplay"),
+        "Error transforming from frame '%s' to frame '%s'", frame, qPrintable(fixed_frame_));
+      axes_->getSceneNode()->setVisible(false);
+      uuid_->setVisible(false);
+      setMissingTransformToFixedFrame(frame);
+    } else {
+      setTransformOk();
+      axes_->getSceneNode()->setVisible(true);
+      uuid_->setVisible(true);
+      scene_node_->setPosition(position);
+      scene_node_->setOrientation(orientation);
+    }
+  }
+
+  {
+    Ogre::Vector3 position;
+    position.x = msg_ptr->pose.position.x;
+    position.y = msg_ptr->pose.position.y;
+    position.z = msg_ptr->pose.position.z;
+
+    Ogre::Quaternion orientation;
+    orientation.x = msg_ptr->pose.orientation.x;
+    orientation.y = msg_ptr->pose.orientation.y;
+    orientation.z = msg_ptr->pose.orientation.z;
+    orientation.w = msg_ptr->pose.orientation.w;
+    axes_->setPosition(position);
+    axes_->setOrientation(orientation);
+    axes_->set(length_property_->getFloat(), radius_property_->getFloat());
+
+    if (uuid_text_view_property_->getBool()) {
+      uuid_node_->setPosition(position);
+      uuid_->setTextAlignment(
+        rviz_rendering::MovableText::H_CENTER, rviz_rendering::MovableText::V_ABOVE);
+      uuid_->setCaption(uuid_to_string(msg_ptr->uuid));
+      uuid_->setCharacterHeight(uuid_text_scale_property_->getFloat());
+      uuid_->setVisible(true);
+      uuid_node_->attachObject(uuid_);
+    }
+  }
+  last_msg_ptr_ = msg_ptr;
+}
+}  // namespace rviz_plugins
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowarePoseWithUuidStampedDisplay, rviz_common::Display)
diff --git a/evaluator/diagnostic_converter/CMakeLists.txt b/evaluator/diagnostic_converter/CMakeLists.txt
new file mode 100644
index 0000000000000..3f47eb0386a1c
--- /dev/null
+++ b/evaluator/diagnostic_converter/CMakeLists.txt
@@ -0,0 +1,40 @@
+cmake_minimum_required(VERSION 3.16.3) # Ubuntu 20.04 default CMake version
+
+project(diagnostic_converter)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+find_package(pluginlib REQUIRED)
+
+ament_auto_find_build_dependencies()
+
+
+ament_auto_add_library(${PROJECT_NAME}_node SHARED
+  src/converter_node.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}_node
+  PLUGIN "diagnostic_converter::DiagnosticConverter"
+  EXECUTABLE ${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  ament_add_gtest(test_${PROJECT_NAME}
+    test/test_converter_node.cpp
+  )
+  target_link_libraries(test_${PROJECT_NAME}
+    ${PROJECT_NAME}_node
+  )
+endif()
+
+ament_auto_package()
diff --git a/evaluator/diagnostic_converter/README.md b/evaluator/diagnostic_converter/README.md
new file mode 100644
index 0000000000000..adc84c3967d25
--- /dev/null
+++ b/evaluator/diagnostic_converter/README.md
@@ -0,0 +1,53 @@
+# Planning Evaluator
+
+## Purpose
+
+This package provides a node to convert `diagnostic_msgs::msg::DiagnosticArray` messages
+into `tier4_simulation_msgs::msg::UserDefinedValue` messages.
+
+## Inner-workings / Algorithms
+
+The node subscribes to all topics listed in the parameters and assumes they publish
+`DiagnosticArray` messages.
+Each time such message is received,
+it is converted into as many `UserDefinedValue` messages as the number of `KeyValue` objects.
+The format of the output topic is detailed in the _output_ section.
+
+## Inputs / Outputs
+
+### Inputs
+
+The node listens to `DiagnosticArray` messages on the topics specified in the parameters.
+
+### Outputs
+
+The node outputs `UserDefinedValue` messages that are converted from the received `DiagnosticArray`.
+
+The name of the output topics are generated from the corresponding input topic, the name of the diagnostic status, and the key of the diagnostic.
+For example, we might listen to topic `/diagnostic_topic` and receive a `DiagnosticArray` with 2 status:
+
+- Status with `name: "x"`.
+  - Key: `a`.
+  - Key: `b`.
+- Status with `name: "y"`.
+  - Key: `a`.
+  - Key: `c`.
+
+The resulting topics to publish the `UserDefinedValue` are as follows:
+
+- `/metrics_x_a`.
+- `/metrics_x_b`.
+- `/metrics_y_a`.
+- `/metrics_y_c`.
+
+## Parameters
+
+| Name                | Type             | Description                                                   |
+| :------------------ | :--------------- | :------------------------------------------------------------ |
+| `diagnostic_topics` | list of `string` | list of DiagnosticArray topics to convert to UserDefinedValue |
+
+## Assumptions / Known limits
+
+Values in the `KeyValue` objects of a `DiagnosticStatus` are assumed to be of type `double`.
+
+## Future extensions / Unimplemented parts
diff --git a/evaluator/diagnostic_converter/include/converter_node.hpp b/evaluator/diagnostic_converter/include/converter_node.hpp
new file mode 100644
index 0000000000000..53c762dac0ffe
--- /dev/null
+++ b/evaluator/diagnostic_converter/include/converter_node.hpp
@@ -0,0 +1,66 @@
+// Copyright 2023 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 CONVERTER_NODE_HPP_
+#define CONVERTER_NODE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include "diagnostic_msgs/msg/diagnostic_array.hpp"
+#include "tier4_simulation_msgs/msg/user_defined_value.hpp"
+#include "tier4_simulation_msgs/msg/user_defined_value_type.hpp"
+
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace diagnostic_converter
+{
+using diagnostic_msgs::msg::DiagnosticArray;
+using diagnostic_msgs::msg::DiagnosticStatus;
+using diagnostic_msgs::msg::KeyValue;
+using tier4_simulation_msgs::msg::UserDefinedValue;
+using tier4_simulation_msgs::msg::UserDefinedValueType;
+
+/**
+ * @brief Node for converting from DiagnosticArray to UserDefinedValue
+ */
+class DiagnosticConverter : public rclcpp::Node
+{
+public:
+  explicit DiagnosticConverter(const rclcpp::NodeOptions & node_options);
+
+  /**
+   * @brief callback for DiagnosticArray msgs that publishes equivalent UserDefinedValue msgs
+   * @param [in] diag_msg received diagnostic message
+   */
+  void onDiagnostic(
+    const DiagnosticArray::ConstSharedPtr diag_msg, const size_t diag_idx,
+    const std::string & topic);
+
+  UserDefinedValue createUserDefinedValue(const KeyValue & key_value) const;
+
+  rclcpp::Publisher<UserDefinedValue>::SharedPtr getPublisher(
+    const std::string & topic, const size_t pub_idx);
+
+private:
+  // ROS
+  std::vector<rclcpp::Subscription<DiagnosticArray>::SharedPtr> diagnostics_sub_;
+  std::vector<std::unordered_map<std::string, rclcpp::Publisher<UserDefinedValue>::SharedPtr>>
+    params_pub_;
+};
+}  // namespace diagnostic_converter
+
+#endif  // CONVERTER_NODE_HPP_
diff --git a/evaluator/diagnostic_converter/package.xml b/evaluator/diagnostic_converter/package.xml
new file mode 100644
index 0000000000000..c561cfcac6c65
--- /dev/null
+++ b/evaluator/diagnostic_converter/package.xml
@@ -0,0 +1,29 @@
+<?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>diagnostic_converter</name>
+  <version>0.5.6</version>
+  <description>Node for converting diagnostic messages into ParameterDeclaration messages</description>
+  <maintainer email="kyoichi.sugahara@tier4.jp">Kyoichi Sugahara</maintainer>
+  <maintainer email="maxime.clement@tier4.jp">Maxime CLEMENT</maintainer>
+  <maintainer email="takamasa.horibe@tier4.jp">Takamasa Horibe</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_depend>
+
+  <depend>diagnostic_msgs</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tier4_simulation_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>
diff --git a/evaluator/diagnostic_converter/src/converter_node.cpp b/evaluator/diagnostic_converter/src/converter_node.cpp
new file mode 100644
index 0000000000000..798515b8667bd
--- /dev/null
+++ b/evaluator/diagnostic_converter/src/converter_node.cpp
@@ -0,0 +1,70 @@
+// Copyright 2023 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.
+
+#include "converter_node.hpp"
+
+namespace diagnostic_converter
+{
+DiagnosticConverter::DiagnosticConverter(const rclcpp::NodeOptions & node_options)
+: Node("diagnostic_converter", node_options)
+{
+  using std::placeholders::_1;
+
+  size_t sub_counter = 0;
+  std::vector<std::string> diagnostic_topics;
+  declare_parameter<std::vector<std::string>>("diagnostic_topics", std::vector<std::string>());
+  get_parameter<std::vector<std::string>>("diagnostic_topics", diagnostic_topics);
+  for (const std::string & diagnostic_topic : diagnostic_topics) {
+    // std::function required with multiple arguments https://answers.ros.org/question/289207
+    const std::function<void(const DiagnosticArray::ConstSharedPtr)> fn =
+      std::bind(&DiagnosticConverter::onDiagnostic, this, _1, sub_counter++, diagnostic_topic);
+    diagnostics_sub_.push_back(create_subscription<DiagnosticArray>(diagnostic_topic, 1, fn));
+  }
+  params_pub_.resize(diagnostics_sub_.size());
+}
+
+void DiagnosticConverter::onDiagnostic(
+  const DiagnosticArray::ConstSharedPtr diag_msg, const size_t diag_idx,
+  const std::string & base_topic)
+{
+  for (const auto & status : diag_msg->status) {
+    std::string status_topic = base_topic + (status.name.empty() ? "" : "_" + status.name);
+    for (const auto & key_value : status.values) {
+      getPublisher(status_topic + "_" + key_value.key, diag_idx)
+        ->publish(createUserDefinedValue(key_value));
+    }
+  }
+}
+
+UserDefinedValue DiagnosticConverter::createUserDefinedValue(const KeyValue & key_value) const
+{
+  UserDefinedValue param_msg;
+  param_msg.type.data = UserDefinedValueType::DOUBLE;
+  param_msg.value = key_value.value;
+  return param_msg;
+}
+
+rclcpp::Publisher<UserDefinedValue>::SharedPtr DiagnosticConverter::getPublisher(
+  const std::string & topic, const size_t pub_idx)
+{
+  auto & pubs = params_pub_[pub_idx];
+  if (pubs.count(topic) == 0) {
+    pubs[topic] = create_publisher<UserDefinedValue>(topic, 1);
+  }
+  return pubs.at(topic);
+}
+}  // namespace diagnostic_converter
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(diagnostic_converter::DiagnosticConverter)
diff --git a/evaluator/diagnostic_converter/test/test_converter_node.cpp b/evaluator/diagnostic_converter/test/test_converter_node.cpp
new file mode 100644
index 0000000000000..47f42d018ea71
--- /dev/null
+++ b/evaluator/diagnostic_converter/test/test_converter_node.cpp
@@ -0,0 +1,127 @@
+// Copyright 2023 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.
+
+#include "converter_node.hpp"
+#include "gtest/gtest.h"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include "diagnostic_msgs/msg/diagnostic_array.hpp"
+#include "tier4_simulation_msgs/msg/user_defined_value.hpp"
+#include "tier4_simulation_msgs/msg/user_defined_value_type.hpp"
+
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+using ConverterNode = diagnostic_converter::DiagnosticConverter;
+using diagnostic_msgs::msg::DiagnosticArray;
+using diagnostic_msgs::msg::DiagnosticStatus;
+using diagnostic_msgs::msg::KeyValue;
+using tier4_simulation_msgs::msg::UserDefinedValue;
+
+void waitForMsg(
+  bool & flag, const rclcpp::Node::SharedPtr node1, const rclcpp::Node::SharedPtr node2)
+{
+  while (!flag) {
+    rclcpp::spin_some(node1);
+    rclcpp::spin_some(node2);
+    rclcpp::sleep_for(std::chrono::milliseconds(100));
+  }
+  flag = false;
+}
+
+std::function<void(UserDefinedValue::ConstSharedPtr)> generateCallback(
+  bool & flag, UserDefinedValue & msg)
+{
+  return [&](UserDefinedValue::ConstSharedPtr received_msg) {
+    flag = true;
+    msg = *received_msg;
+  };
+}
+
+TEST(ConverterNode, ConvertDiagnostics)
+{
+  const std::vector<std::string> input_topics = {"/test1/diag", "/test2/diag"};
+
+  rclcpp::init(0, nullptr);
+  rclcpp::Node::SharedPtr dummy_node = std::make_shared<rclcpp::Node>("converter_test_node");
+
+  rclcpp::NodeOptions options;
+  options.append_parameter_override("diagnostic_topics", rclcpp::ParameterValue(input_topics));
+  auto node = std::make_shared<ConverterNode>(options);
+
+  {  // Simple case with 1 resulting UserDefinedValue
+    bool msg_received = false;
+    UserDefinedValue param;
+    // DiagnosticArray publishers
+    const auto diag_pub = dummy_node->create_publisher<DiagnosticArray>(input_topics[0], 1);
+    // UserDefinedValue subscribers
+    const auto param_sub_a = dummy_node->create_subscription<UserDefinedValue>(
+      input_topics[0] + "_a", 1, generateCallback(msg_received, param));
+    DiagnosticArray diag;
+    DiagnosticStatus status;
+    status.name = "";
+    KeyValue key_value = KeyValue().set__key("a").set__value("1");
+    status.values.push_back(key_value);
+    diag.status.push_back(status);
+    diag_pub->publish(diag);
+    waitForMsg(msg_received, node, dummy_node);
+    EXPECT_EQ(param.value, "1");
+  }
+  {  // Case with multiple UserDefinedValue converted from one DiagnosticArray
+    bool msg_received_xa = false;
+    bool msg_received_xb = false;
+    bool msg_received_ya = false;
+    bool msg_received_yb = false;
+    UserDefinedValue param_xa;
+    UserDefinedValue param_xb;
+    UserDefinedValue param_ya;
+    UserDefinedValue param_yb;
+    // DiagnosticArray publishers
+    const auto diag_pub = dummy_node->create_publisher<DiagnosticArray>(input_topics[1], 1);
+    // UserDefinedValue subscribers
+    const auto param_sub_xa = dummy_node->create_subscription<UserDefinedValue>(
+      input_topics[1] + "_x_a", 1, generateCallback(msg_received_xa, param_xa));
+    const auto param_sub_xb = dummy_node->create_subscription<UserDefinedValue>(
+      input_topics[1] + "_x_b", 1, generateCallback(msg_received_xb, param_xb));
+    const auto param_sub_ya = dummy_node->create_subscription<UserDefinedValue>(
+      input_topics[1] + "_y_a", 1, generateCallback(msg_received_ya, param_ya));
+    const auto param_sub_yb = dummy_node->create_subscription<UserDefinedValue>(
+      input_topics[1] + "_y_b", 1, generateCallback(msg_received_yb, param_yb));
+    DiagnosticArray diag;
+    DiagnosticStatus status_x;
+    status_x.name = "x";
+    status_x.values.push_back(KeyValue().set__key("a").set__value("1"));
+    status_x.values.push_back(KeyValue().set__key("b").set__value("10"));
+    diag.status.push_back(status_x);
+    DiagnosticStatus status_y;
+    status_y.name = "y";
+    status_y.values.push_back(KeyValue().set__key("a").set__value("9"));
+    status_y.values.push_back(KeyValue().set__key("b").set__value("6"));
+    diag.status.push_back(status_y);
+    diag_pub->publish(diag);
+    waitForMsg(msg_received_xa, node, dummy_node);
+    EXPECT_EQ(param_xa.value, "1");
+    waitForMsg(msg_received_xb, node, dummy_node);
+    EXPECT_EQ(param_xb.value, "10");
+    waitForMsg(msg_received_ya, node, dummy_node);
+    EXPECT_EQ(param_ya.value, "9");
+    waitForMsg(msg_received_yb, node, dummy_node);
+    EXPECT_EQ(param_yb.value, "6");
+  }
+
+  rclcpp::shutdown();
+}
diff --git a/planning/planning_evaluator/CMakeLists.txt b/evaluator/planning_evaluator/CMakeLists.txt
similarity index 96%
rename from planning/planning_evaluator/CMakeLists.txt
rename to evaluator/planning_evaluator/CMakeLists.txt
index 6865f8eef7005..4b643b3c85e44 100644
--- a/planning/planning_evaluator/CMakeLists.txt
+++ b/evaluator/planning_evaluator/CMakeLists.txt
@@ -4,6 +4,8 @@ project(planning_evaluator)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
+find_package(pluginlib REQUIRED)
+
 ament_auto_add_library(${PROJECT_NAME}_node SHARED
   src/metrics_calculator.cpp
   src/${PROJECT_NAME}_node.cpp
diff --git a/planning/planning_evaluator/README.md b/evaluator/planning_evaluator/README.md
similarity index 100%
rename from planning/planning_evaluator/README.md
rename to evaluator/planning_evaluator/README.md
diff --git a/planning/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp
similarity index 68%
rename from planning/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp
rename to evaluator/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp
index e71b887ff3a10..04a5b758d62e1 100644
--- a/planning/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/planning/planning_evaluator/include/planning_evaluator/metrics/metric.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/metric.hpp
similarity index 81%
rename from planning/planning_evaluator/include/planning_evaluator/metrics/metric.hpp
rename to evaluator/planning_evaluator/include/planning_evaluator/metrics/metric.hpp
index 16811f91b32b1..e1ec69dbaef5c 100644
--- a/planning/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/planning/planning_evaluator/include/planning_evaluator/metrics/metrics_utils.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/metrics_utils.hpp
similarity index 100%
rename from planning/planning_evaluator/include/planning_evaluator/metrics/metrics_utils.hpp
rename to evaluator/planning_evaluator/include/planning_evaluator/metrics/metrics_utils.hpp
diff --git a/planning/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp
similarity index 100%
rename from planning/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp
rename to evaluator/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp
diff --git a/planning/planning_evaluator/include/planning_evaluator/metrics/stability_metrics.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/stability_metrics.hpp
similarity index 100%
rename from planning/planning_evaluator/include/planning_evaluator/metrics/stability_metrics.hpp
rename to evaluator/planning_evaluator/include/planning_evaluator/metrics/stability_metrics.hpp
diff --git a/planning/planning_evaluator/include/planning_evaluator/metrics/trajectory_metrics.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/trajectory_metrics.hpp
similarity index 100%
rename from planning/planning_evaluator/include/planning_evaluator/metrics/trajectory_metrics.hpp
rename to evaluator/planning_evaluator/include/planning_evaluator/metrics/trajectory_metrics.hpp
diff --git a/planning/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp
similarity index 85%
rename from planning/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp
rename to evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp
index 0ed2af7b3862e..851678e55d755 100644
--- a/planning/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/planning/planning_evaluator/include/planning_evaluator/motion_evaluator_node.hpp b/evaluator/planning_evaluator/include/planning_evaluator/motion_evaluator_node.hpp
similarity index 100%
rename from planning/planning_evaluator/include/planning_evaluator/motion_evaluator_node.hpp
rename to evaluator/planning_evaluator/include/planning_evaluator/motion_evaluator_node.hpp
diff --git a/planning/planning_evaluator/include/planning_evaluator/parameters.hpp b/evaluator/planning_evaluator/include/planning_evaluator/parameters.hpp
similarity index 100%
rename from planning/planning_evaluator/include/planning_evaluator/parameters.hpp
rename to evaluator/planning_evaluator/include/planning_evaluator/parameters.hpp
diff --git a/planning/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp b/evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp
similarity index 79%
rename from planning/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp
rename to evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp
index e9fd82c85cf7b..888eea855295c 100644
--- a/planning/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/planning/planning_evaluator/include/planning_evaluator/stat.hpp b/evaluator/planning_evaluator/include/planning_evaluator/stat.hpp
similarity index 100%
rename from planning/planning_evaluator/include/planning_evaluator/stat.hpp
rename to evaluator/planning_evaluator/include/planning_evaluator/stat.hpp
diff --git a/planning/planning_evaluator/launch/motion_evaluator.launch.xml b/evaluator/planning_evaluator/launch/motion_evaluator.launch.xml
similarity index 100%
rename from planning/planning_evaluator/launch/motion_evaluator.launch.xml
rename to evaluator/planning_evaluator/launch/motion_evaluator.launch.xml
diff --git a/evaluator/planning_evaluator/launch/planning_evaluator.launch.xml b/evaluator/planning_evaluator/launch/planning_evaluator.launch.xml
new file mode 100644
index 0000000000000..1b7510c2ced69
--- /dev/null
+++ b/evaluator/planning_evaluator/launch/planning_evaluator.launch.xml
@@ -0,0 +1,20 @@
+<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>
+</launch>
diff --git a/planning/planning_evaluator/package.xml b/evaluator/planning_evaluator/package.xml
similarity index 88%
rename from planning/planning_evaluator/package.xml
rename to evaluator/planning_evaluator/package.xml
index b288d911b4c3c..ef2bca288c8b0 100644
--- a/planning/planning_evaluator/package.xml
+++ b/evaluator/planning_evaluator/package.xml
@@ -5,6 +5,7 @@
   <version>0.1.0</version>
   <description>ROS2 node for evaluating planners</description>
   <maintainer email="maxime.clement@tier4.jp">Maxime CLEMENT</maintainer>
+  <maintainer email="kyoichi.sugahara@tier4.jp">Kyoichi Sugahara</maintainer>
   <license>Apache License 2.0</license>
 
   <author email="maxime.clement@tier4.jp">Maxime CLEMENT</author>
@@ -15,11 +16,13 @@
 
   <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>
   <depend>motion_utils</depend>
   <depend>nav_msgs</depend>
+  <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>tf2</depend>
diff --git a/planning/planning_evaluator/param/planning_evaluator.defaults.yaml b/evaluator/planning_evaluator/param/planning_evaluator.defaults.yaml
similarity index 88%
rename from planning/planning_evaluator/param/planning_evaluator.defaults.yaml
rename to evaluator/planning_evaluator/param/planning_evaluator.defaults.yaml
index bd47deedb282f..2e92a174ca79f 100644
--- a/planning/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/planning/planning_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp
similarity index 79%
rename from planning/planning_evaluator/src/metrics/deviation_metrics.cpp
rename to evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp
index d6d8aa661e5ea..5146fcd3ec0f2 100644
--- a/planning/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/planning/planning_evaluator/src/metrics/metrics_utils.cpp b/evaluator/planning_evaluator/src/metrics/metrics_utils.cpp
similarity index 100%
rename from planning/planning_evaluator/src/metrics/metrics_utils.cpp
rename to evaluator/planning_evaluator/src/metrics/metrics_utils.cpp
diff --git a/planning/planning_evaluator/src/metrics/obstacle_metrics.cpp b/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp
similarity index 100%
rename from planning/planning_evaluator/src/metrics/obstacle_metrics.cpp
rename to evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp
diff --git a/planning/planning_evaluator/src/metrics/stability_metrics.cpp b/evaluator/planning_evaluator/src/metrics/stability_metrics.cpp
similarity index 100%
rename from planning/planning_evaluator/src/metrics/stability_metrics.cpp
rename to evaluator/planning_evaluator/src/metrics/stability_metrics.cpp
diff --git a/planning/planning_evaluator/src/metrics/trajectory_metrics.cpp b/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp
similarity index 100%
rename from planning/planning_evaluator/src/metrics/trajectory_metrics.cpp
rename to evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp
diff --git a/planning/planning_evaluator/src/metrics_calculator.cpp b/evaluator/planning_evaluator/src/metrics_calculator.cpp
similarity index 84%
rename from planning/planning_evaluator/src/metrics_calculator.cpp
rename to evaluator/planning_evaluator/src/metrics_calculator.cpp
index 802e1dde94fd1..adfdddd7d2c5e 100644
--- a/planning/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/planning/planning_evaluator/src/motion_evaluator_node.cpp b/evaluator/planning_evaluator/src/motion_evaluator_node.cpp
similarity index 97%
rename from planning/planning_evaluator/src/motion_evaluator_node.cpp
rename to evaluator/planning_evaluator/src/motion_evaluator_node.cpp
index 56b08fca81f9f..9a103890d53ac 100644
--- a/planning/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/planning/planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/planning_evaluator/src/planning_evaluator_node.cpp
similarity index 75%
rename from planning/planning_evaluator/src/planning_evaluator_node.cpp
rename to evaluator/planning_evaluator/src/planning_evaluator_node.cpp
index cca9bbf7c62e3..346b029922e3e 100644
--- a/planning/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()) {
@@ -156,7 +151,52 @@ void PlanningEvaluatorNode::onTrajectory(const Trajectory::ConstSharedPtr traj_m
   }
   metrics_calculator_.setPreviousTrajectory(*traj_msg);
   auto runtime = (now() - start).seconds();
-  RCLCPP_INFO(get_logger(), "Calculation time: %2.2f ms", runtime * 1e3);
+  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)
diff --git a/planning/planning_evaluator/test/test_planning_evaluator_node.cpp b/evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp
similarity index 81%
rename from planning/planning_evaluator/test/test_planning_evaluator_node.cpp
rename to evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp
index 8e9bcdd734222..4f2ab014a79d6 100644
--- a/planning/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);
+}
diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml
index e179d410e345e..f2226b57673f5 100644
--- a/launch/tier4_planning_launch/launch/planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/planning.launch.xml
@@ -34,5 +34,10 @@
       <push-ros-namespace namespace="planning_diagnostics"/>
       <include file="$(find-pkg-share tier4_planning_launch)/launch/planning_diagnostics/planning_error_monitor.launch.xml"/>
     </group>
+
+    <!-- planning evaluator -->
+    <group>
+      <include file="$(find-pkg-share planning_evaluator)/launch/planning_evaluator.launch.xml"/>
+    </group>
   </group>
 </launch>
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py
index 25c23f9189087..23bb66fec103b 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py
+++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py
@@ -158,6 +158,7 @@ def launch_setup(context, *args, **kwargs):
             ("~/output/path", "path_with_lane_id"),
             ("~/output/turn_indicators_cmd", "/planning/turn_indicators_cmd"),
             ("~/output/hazard_lights_cmd", "/planning/hazard_lights_cmd"),
+            ("~/output/modified_goal", "/planning/scenario_planning/modified_goal"),
         ],
         parameters=[
             nearest_search_param,
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
index c4c7b3bab7597..65edfb14306bc 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
@@ -12,6 +12,7 @@
     <remap from="~/output/path" to="path_with_lane_id"/>
     <remap from="~/output/turn_indicators_cmd" to="/planning/turn_signal_cmd"/>
     <remap from="~/output/hazard_lights_cmd" to="/planning/hazard_signal_cmd"/>
+    <remap from="~/output/modified_goal" to="/planning/scenario_planning/modified_goal"/>
     <param from="$(find-pkg-share tier4_planning_launch)/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml"/>
     <param from="$(find-pkg-share tier4_planning_launch)/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml"/>
     <param from="$(find-pkg-share tier4_planning_launch)/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml"/>
diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml
index 8705ba30a9350..7029de7c1095e 100644
--- a/launch/tier4_simulator_launch/launch/simulator.launch.xml
+++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml
@@ -5,6 +5,7 @@
   <arg name="launch_dummy_perception"/>
   <arg name="launch_dummy_vehicle"/>
   <arg name="launch_dummy_localization"/>
+  <arg name="launch_diagnostic_converter"/>
   <arg name="vehicle_info_param_file"/>
 
   <arg name="perception/enable_detection_failure" default="true" description="enable to simulate detection failure when using dummy perception"/>
@@ -102,6 +103,13 @@
     </include>
   </group>
 
+  <!-- Diagnostic Converter -->
+  <group if="$(var launch_diagnostic_converter)">
+    <node name="diagnostic_converter" exec="diagnostic_converter" pkg="diagnostic_converter" output="screen">
+      <param name="diagnostic_topics" value="[/diagnostic/planning_evaluator/metrics]"/>
+    </node>
+  </group>
+
   <!-- Simulator model -->
   <group if="$(var launch_dummy_vehicle)">
     <arg name="simulator_model" default="$(var vehicle_model_pkg)/config/simulator_model.param.yaml" description="path to the file of simulator model"/>
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp
index 48e7fbaf71a65..25d4bb6d570e2 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp
@@ -37,6 +37,7 @@
 #include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
 #include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
 #include <autoware_planning_msgs/msg/lanelet_route.hpp>
+#include <autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp>
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <tier4_planning_msgs/msg/approval.hpp>
@@ -74,6 +75,7 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
 using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
 using autoware_planning_msgs::msg::LaneletRoute;
+using autoware_planning_msgs::msg::PoseWithUuidStamped;
 using geometry_msgs::msg::TwistStamped;
 using nav_msgs::msg::OccupancyGrid;
 using nav_msgs::msg::Odometry;
@@ -103,6 +105,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   rclcpp::Publisher<TurnIndicatorsCommand>::SharedPtr turn_signal_publisher_;
   rclcpp::Publisher<HazardLightsCommand>::SharedPtr hazard_signal_publisher_;
   rclcpp::Publisher<MarkerArray>::SharedPtr bound_publisher_;
+  rclcpp::Publisher<PoseWithUuidStamped>::SharedPtr modified_goal_publisher_;
   rclcpp::TimerBase::SharedPtr timer_;
 
   std::map<std::string, rclcpp::Publisher<Path>::SharedPtr> path_candidate_publishers_;
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp
index f86f592f0560f..4d463c9c3fa09 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp
@@ -105,11 +105,11 @@ class PullOverModule : public SceneModuleInterface
   vehicle_info_util::VehicleInfo vehicle_info_;
 
   rclcpp::Subscription<OccupancyGrid>::SharedPtr occupancy_grid_sub_;
-  rclcpp::Publisher<PoseStamped>::SharedPtr goal_pose_pub_;
 
   PUllOverStatus status_;
   std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map_;
-  Pose modified_goal_pose_;
+  std::optional<GoalCandidate> modified_goal_pose_;
+  std::optional<size_t> prev_goal_id_;
   Pose refined_goal_pose_;
   GoalCandidates goal_candidates_;
   std::vector<PullOverPath> pull_over_path_candidates_;
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp
index 65be40b9fb603..ba03589cc769e 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp
@@ -29,6 +29,7 @@
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
 #include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
+#include <autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp>
 #include <tier4_planning_msgs/msg/avoidance_debug_msg_array.hpp>
 #include <unique_identifier_msgs/msg/uuid.hpp>
 
@@ -48,6 +49,7 @@ using autoware_adapi_v1_msgs::msg::SteeringFactor;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
 using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
+using autoware_planning_msgs::msg::PoseWithUuidStamped;
 using rtc_interface::RTCInterface;
 using steering_factor_interface::SteeringFactorInterface;
 using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
@@ -63,6 +65,8 @@ struct BehaviorModuleOutput
   PlanResult path{};
 
   TurnSignalInfo turn_signal_info{};
+
+  std::optional<PoseWithUuidStamped> modified_goal{};
 };
 
 struct CandidateOutput
diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
index c4ba49d81c056..4e330d2390b26 100644
--- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
+++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -64,6 +64,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
   turn_signal_publisher_ =
     create_publisher<TurnIndicatorsCommand>("~/output/turn_indicators_cmd", 1);
   hazard_signal_publisher_ = create_publisher<HazardLightsCommand>("~/output/hazard_lights_cmd", 1);
+  modified_goal_publisher_ = create_publisher<PoseWithUuidStamped>("~/output/modified_goal", 1);
   debug_avoidance_msg_array_publisher_ =
     create_publisher<AvoidanceDebugMsgArray>("~/debug/avoidance_debug_message_array", 1);
   debug_lane_change_msg_array_publisher_ =
@@ -705,6 +706,12 @@ void BehaviorPathPlannerNode::run()
 
   publishSceneModuleDebugMsg();
 
+  if (output.modified_goal) {
+    PoseWithUuidStamped modified_goal = *(output.modified_goal);
+    modified_goal.header.stamp = path->header.stamp;
+    modified_goal_publisher_->publish(modified_goal);
+  }
+
   if (planner_data->parameters.visualize_maximum_drivable_area) {
     const auto maximum_drivable_area =
       marker_utils::createFurthestLineStringMarkerArray(util::getMaximumDrivableArea(planner_data));
diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp
index 0bf00e9326a66..70437be483e97 100644
--- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp
@@ -52,8 +52,6 @@ PullOverModule::PullOverModule(
 {
   rtc_interface_ptr_ = std::make_shared<RTCInterface>(&node, "pull_over");
   steering_factor_interface_ptr_ = std::make_unique<SteeringFactorInterface>(&node, "pull_over");
-  goal_pose_pub_ =
-    node.create_publisher<PoseStamped>("/planning/scenario_planning/modified_goal", 1);
 
   LaneDepartureChecker lane_departure_checker{};
   lane_departure_checker.setVehicleInfo(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo());
@@ -107,6 +105,7 @@ void PullOverModule::resetStatus()
   pull_over_path_candidates_.clear();
   closest_start_pose_.reset();
   goal_candidates_.clear();
+  prev_goal_id_.reset();
 }
 
 // This function is needed for waiting for planner_data_
@@ -466,7 +465,7 @@ BehaviorModuleOutput PullOverModule::plan()
 
       status_.is_safe = true;
       status_.pull_over_path = pull_over_path;
-      modified_goal_pose_ = pull_over_path.getFullPath().points.back().point.pose;
+      modified_goal_pose_ = *goal_candidate_it;
       break;
     }
 
@@ -534,12 +533,16 @@ BehaviorModuleOutput PullOverModule::plan()
 
   setDebugData();
 
-  // Publish the modified goal only when its path is safe.
-  if (status_.is_safe) {
-    PoseStamped goal_pose_stamped;
-    goal_pose_stamped.header = planner_data_->route_handler->getRouteHeader();
-    goal_pose_stamped.pose = modified_goal_pose_;
-    goal_pose_pub_->publish(goal_pose_stamped);
+  // Publish the modified goal only when it is updated
+  if (
+    status_.is_safe && modified_goal_pose_ &&
+    (!prev_goal_id_ || *prev_goal_id_ != modified_goal_pose_->id)) {
+    PoseWithUuidStamped modified_goal{};
+    modified_goal.uuid = planner_data_->route_handler->getRouteUuid();
+    modified_goal.pose = modified_goal_pose_->goal_pose;
+    modified_goal.header = planner_data_->route_handler->getRouteHeader();
+    output.modified_goal = modified_goal;
+    prev_goal_id_ = modified_goal_pose_->id;
   }
 
   const uint16_t steering_factor_direction = std::invoke([this]() {
@@ -554,7 +557,7 @@ BehaviorModuleOutput PullOverModule::plan()
 
   // TODO(tkhmy) add handle status TRYING
   steering_factor_interface_ptr_->updateSteeringFactor(
-    {status_.pull_over_path.start_pose, modified_goal_pose_},
+    {status_.pull_over_path.start_pose, modified_goal_pose_->goal_pose},
     {distance_to_path_change.first, distance_to_path_change.second}, SteeringFactor::PULL_OVER,
     steering_factor_direction, SteeringFactor::TURNING, "");
 
@@ -575,7 +578,7 @@ BehaviorModuleOutput PullOverModule::planWaitingApproval()
 {
   updateOccupancyGrid();
   BehaviorModuleOutput out;
-  plan();  // update status_
+  out.modified_goal = plan().modified_goal;  // update status_
   out.path = std::make_shared<PathWithLaneId>(generateStopPath());
   path_candidate_ = status_.is_safe
                       ? std::make_shared<PathWithLaneId>(status_.pull_over_path.getFullPath())
@@ -594,7 +597,7 @@ BehaviorModuleOutput PullOverModule::planWaitingApproval()
   });
 
   steering_factor_interface_ptr_->updateSteeringFactor(
-    {status_.pull_over_path.start_pose, modified_goal_pose_},
+    {status_.pull_over_path.start_pose, modified_goal_pose_->goal_pose},
     {distance_to_path_change.first, distance_to_path_change.second}, SteeringFactor::PULL_OVER,
     steering_factor_direction, SteeringFactor::APPROACHING, "");
   waitApproval();
@@ -609,7 +612,8 @@ std::pair<double, double> PullOverModule::calcDistanceToPathChange() const
     full_path.points, planner_data_->self_pose->pose, status_.pull_over_path.start_pose.position,
     std::numeric_limits<double>::max(), M_PI_2);
   const double dist_to_parking_finish_pose = calcSignedArcLength(
-    full_path.points, planner_data_->self_pose->pose.position, modified_goal_pose_.position);
+    full_path.points, planner_data_->self_pose->pose.position,
+    modified_goal_pose_->goal_pose.position);
   const double start_distance_to_path_change =
     dist_to_parking_start_pose ? *dist_to_parking_start_pose : std::numeric_limits<double>::max();
   return {start_distance_to_path_change, dist_to_parking_finish_pose};
@@ -798,7 +802,7 @@ bool PullOverModule::hasFinishedPullOver()
   // check ego car is close enough to goal pose
   const auto current_pose = planner_data_->self_pose->pose;
   const bool car_is_on_goal =
-    calcDistance2d(current_pose, modified_goal_pose_) < parameters_.th_arrived_distance;
+    calcDistance2d(current_pose, modified_goal_pose_->goal_pose) < parameters_.th_arrived_distance;
 
   return car_is_on_goal && isStopped();
 }
@@ -937,7 +941,7 @@ void PullOverModule::printParkingPositionError() const
   const auto current_pose = planner_data_->self_pose->pose;
   const double real_shoulder_to_map_shoulder = 0.0;
 
-  const Pose goal_to_ego = inverseTransformPose(current_pose, modified_goal_pose_);
+  const Pose goal_to_ego = inverseTransformPose(current_pose, modified_goal_pose_->goal_pose);
   const double dx = goal_to_ego.position.x;
   const double dy = goal_to_ego.position.y;
   const double distance_from_real_shoulder =
@@ -945,7 +949,8 @@ void PullOverModule::printParkingPositionError() const
   RCLCPP_INFO(
     getLogger(), "current pose to goal, dx:%f dy:%f dyaw:%f from_real_shoulder:%f", dx, dy,
     tier4_autoware_utils::rad2deg(
-      tf2::getYaw(current_pose.orientation) - tf2::getYaw(modified_goal_pose_.orientation)),
+      tf2::getYaw(current_pose.orientation) -
+      tf2::getYaw(modified_goal_pose_->goal_pose.orientation)),
     distance_from_real_shoulder);
 }
 }  // namespace behavior_path_planner
diff --git a/planning/mission_planner/src/mission_planner/arrival_checker.cpp b/planning/mission_planner/src/mission_planner/arrival_checker.cpp
index e44d6d772a5d9..be37d94b0a943 100644
--- a/planning/mission_planner/src/mission_planner/arrival_checker.cpp
+++ b/planning/mission_planner/src/mission_planner/arrival_checker.cpp
@@ -18,9 +18,6 @@
 
 #include <tf2/utils.h>
 
-// TODO(Takagi, Isamu): remove when modified goal is always published
-#include <memory>
-
 namespace mission_planner
 {
 
@@ -31,40 +28,48 @@ ArrivalChecker::ArrivalChecker(rclcpp::Node * node) : vehicle_stop_checker_(node
   distance_ = node->declare_parameter<double>("arrival_check_distance");
   duration_ = node->declare_parameter<double>("arrival_check_duration");
 
-  sub_goal_ = node->create_subscription<geometry_msgs::msg::PoseStamped>(
+  sub_goal_ = node->create_subscription<PoseWithUuidStamped>(
     "input/modified_goal", 1,
-    [this](const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg) { goal_pose_ = msg; });
+    [this](const PoseWithUuidStamped::ConstSharedPtr msg) { modify_goal(*msg); });
+}
+
+void ArrivalChecker::set_goal()
+{
+  // Ignore the modified goal after the route is cleared.
+  goal_with_uuid_ = std::nullopt;
 }
 
-void ArrivalChecker::reset_goal()
+void ArrivalChecker::set_goal(const PoseWithUuidStamped & goal)
 {
-  // Disable checking until the modified goal is received.
-  goal_pose_.reset();
+  // Ignore the modified goal for the previous route using uuid.
+  goal_with_uuid_ = goal;
 }
 
-// TODO(Takagi, Isamu): remove when modified goal is always published
-void ArrivalChecker::reset_goal(const geometry_msgs::msg::PoseStamped & goal)
+void ArrivalChecker::modify_goal(const PoseWithUuidStamped & modified_goal)
 {
-  const auto pose = std::make_shared<geometry_msgs::msg::PoseStamped>();
-  *pose = goal;
-  goal_pose_ = pose;
+  if (!goal_with_uuid_) {
+    return;
+  }
+  if (goal_with_uuid_.value().uuid.uuid != modified_goal.uuid.uuid) {
+    return;
+  }
+  set_goal(modified_goal);
 }
 
-bool ArrivalChecker::is_arrived(const geometry_msgs::msg::PoseStamped & pose) const
+bool ArrivalChecker::is_arrived(const PoseStamped & pose) const
 {
-  // Check if the modified goal is received.
-  if (goal_pose_ == nullptr) {
+  if (!goal_with_uuid_) {
     return false;
   }
-  const auto & goal = *goal_pose_;
+  const auto goal = goal_with_uuid_.value();
 
-  // Check frame_id.
+  // Check frame id
   if (goal.header.frame_id != pose.header.frame_id) {
     return false;
   }
 
   // Check distance.
-  if (distance_ < tier4_autoware_utils::calcDistance2d(pose, goal)) {
+  if (distance_ < tier4_autoware_utils::calcDistance2d(pose.pose, goal.pose)) {
     return false;
   }
 
diff --git a/planning/mission_planner/src/mission_planner/arrival_checker.hpp b/planning/mission_planner/src/mission_planner/arrival_checker.hpp
index c800128d51fcf..0d58b1e01d9ed 100644
--- a/planning/mission_planner/src/mission_planner/arrival_checker.hpp
+++ b/planning/mission_planner/src/mission_planner/arrival_checker.hpp
@@ -18,6 +18,7 @@
 #include <motion_utils/vehicle/vehicle_state_checker.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 
@@ -27,18 +28,21 @@ namespace mission_planner
 class ArrivalChecker
 {
 public:
+  using PoseWithUuidStamped = autoware_planning_msgs::msg::PoseWithUuidStamped;
+  using PoseStamped = geometry_msgs::msg::PoseStamped;
   explicit ArrivalChecker(rclcpp::Node * node);
-  void reset_goal();
-  void reset_goal(const geometry_msgs::msg::PoseStamped & goal);
-  bool is_arrived(const geometry_msgs::msg::PoseStamped & pose) const;
+  void set_goal();
+  void set_goal(const PoseWithUuidStamped & goal);
+  bool is_arrived(const PoseStamped & pose) const;
 
 private:
   double distance_;
   double angle_;
   double duration_;
-  geometry_msgs::msg::PoseStamped::ConstSharedPtr goal_pose_;
-  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr sub_goal_;
+  std::optional<PoseWithUuidStamped> goal_with_uuid_;
+  rclcpp::Subscription<PoseWithUuidStamped>::SharedPtr sub_goal_;
   motion_utils::VehicleStopChecker vehicle_stop_checker_;
+  void modify_goal(const PoseWithUuidStamped & modified_goal);
 };
 
 }  // namespace mission_planner
diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp
index 2d97a06cec51c..56cb1fd004b7d 100644
--- a/planning/mission_planner/src/mission_planner/mission_planner.cpp
+++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp
@@ -16,12 +16,10 @@
 
 #include <autoware_adapi_v1_msgs/srv/set_route.hpp>
 #include <autoware_adapi_v1_msgs/srv/set_route_points.hpp>
-
-#ifdef ROS_DISTRO_GALACTIC
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#else
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
-#endif
+
+#include <array>
+#include <random>
 
 namespace
 {
@@ -48,6 +46,14 @@ LaneletSegment convert(const LaneletSegment & s)
   return segment;
 }
 
+std::array<uint8_t, 16> generate_random_id()
+{
+  static std::independent_bits_engine<std::mt19937, 8, uint8_t> engine(std::random_device{}());
+  std::array<uint8_t, 16> id;
+  std::generate(id.begin(), id.end(), std::ref(engine));
+  return id;
+}
+
 }  // namespace
 
 namespace mission_planner
@@ -113,19 +119,18 @@ PoseStamped MissionPlanner::transform_pose(const PoseStamped & input)
 
 void MissionPlanner::change_route()
 {
-  arrival_checker_.reset_goal();
+  arrival_checker_.set_goal();
   // TODO(Takagi, Isamu): publish an empty route here
 }
 
 void MissionPlanner::change_route(const LaneletRoute & route)
 {
-  // TODO(Takagi, Isamu): replace when modified goal is always published
-  // arrival_checker_.reset_goal();
-  PoseStamped goal;
+  PoseWithUuidStamped goal;
   goal.header = route.header;
   goal.pose = route.goal_pose;
-  arrival_checker_.reset_goal(goal);
+  goal.uuid = route.uuid;
 
+  arrival_checker_.set_goal(goal);
   pub_route_->publish(route);
   pub_marker_->publish(planner_->visualize(route));
 }
@@ -161,20 +166,21 @@ void MissionPlanner::on_set_route(
       ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received.");
   }
 
-  // Use temporary pose stapmed for transform.
+  // Use temporary pose stamped for transform.
   PoseStamped pose;
   pose.header = req->header;
   pose.pose = req->goal;
 
   // Convert route.
   LaneletRoute route;
-  route.header.stamp = req->header.stamp;
-  route.header.frame_id = map_frame_;
   route.start_pose = odometry_->pose.pose;
   route.goal_pose = transform_pose(pose).pose;
   for (const auto & segment : req->segments) {
     route.segments.push_back(convert(segment));
   }
+  route.header.stamp = req->header.stamp;
+  route.header.frame_id = map_frame_;
+  route.uuid.uuid = generate_random_id();
 
   // Update route.
   change_route(route);
@@ -202,7 +208,7 @@ void MissionPlanner::on_set_route_points(
       ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received.");
   }
 
-  // Use temporary pose stapmed for transform.
+  // Use temporary pose stamped for transform.
   PoseStamped pose;
   pose.header = req->header;
 
@@ -224,6 +230,7 @@ void MissionPlanner::on_set_route_points(
   }
   route.header.stamp = req->header.stamp;
   route.header.frame_id = map_frame_;
+  route.uuid.uuid = generate_random_id();
 
   // Update route.
   change_route(route);
diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp
index 238411ad63b1e..2907b4698e156 100644
--- a/planning/mission_planner/src/mission_planner/mission_planner.hpp
+++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp
@@ -38,6 +38,7 @@ namespace mission_planner
 {
 
 using PoseStamped = geometry_msgs::msg::PoseStamped;
+using PoseWithUuidStamped = autoware_planning_msgs::msg::PoseWithUuidStamped;
 using LaneletRoute = autoware_planning_msgs::msg::LaneletRoute;
 using MarkerArray = visualization_msgs::msg::MarkerArray;
 using SetRoutePoints = planning_interface::SetRoutePoints;
diff --git a/planning/planning_evaluator/launch/planning_evaluator.launch.xml b/planning/planning_evaluator/launch/planning_evaluator.launch.xml
deleted file mode 100644
index b3956359c1fd9..0000000000000
--- a/planning/planning_evaluator/launch/planning_evaluator.launch.xml
+++ /dev/null
@@ -1,12 +0,0 @@
-<launch>
-  <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"/>
-
-  <node name="planning_evaluator" exec="planning_evaluator" pkg="planning_evaluator" output="screen">
-    <param from="$(find-pkg-share planning_evaluator)/param/planning_evaluator.defaults.yaml"/>
-    <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)"/>
-  </node>
-</launch>
diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp
index 78b2ef55c0ae8..591b436151cf4 100644
--- a/planning/route_handler/include/route_handler/route_handler.hpp
+++ b/planning/route_handler/include/route_handler/route_handler.hpp
@@ -28,6 +28,7 @@
 #include <autoware_planning_msgs/msg/lanelet_route.hpp>
 #include <autoware_planning_msgs/msg/lanelet_segment.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
+#include <unique_identifier_msgs/msg/uuid.hpp>
 
 #include <lanelet2_routing/Route.h>
 #include <lanelet2_routing/RoutingCost.h>
@@ -50,6 +51,7 @@ using autoware_planning_msgs::msg::LaneletSegment;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::PoseStamped;
 using std_msgs::msg::Header;
+using unique_identifier_msgs::msg::UUID;
 using RouteSections = std::vector<autoware_planning_msgs::msg::LaneletSegment>;
 
 enum class LaneChangeDirection { NONE, LEFT, RIGHT };
@@ -73,6 +75,7 @@ class RouteHandler
   bool isHandlerReady() const;
   lanelet::ConstPolygon3d getExtraDrivableAreaById(const lanelet::Id id) const;
   Header getRouteHeader() const;
+  UUID getRouteUuid() const;
 
   // for routing graph
   bool isMapMsgReady() const;
diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp
index b0d4fe247d862..b88cf06d2d3cc 100644
--- a/planning/route_handler/src/route_handler.cpp
+++ b/planning/route_handler/src/route_handler.cpp
@@ -336,6 +336,8 @@ lanelet::ConstPolygon3d RouteHandler::getIntersectionAreaById(const lanelet::Id
 
 Header RouteHandler::getRouteHeader() const { return route_msg_.header; }
 
+UUID RouteHandler::getRouteUuid() const { return route_msg_.uuid; }
+
 std::vector<lanelet::ConstLanelet> RouteHandler::getLanesBeforePose(
   const geometry_msgs::msg::Pose & pose, const double length) const
 {