diff --git a/common/goal_distance_calculator/CMakeLists.txt b/common/goal_distance_calculator/CMakeLists.txt
new file mode 100644
index 0000000000000..add32e3eabd53
--- /dev/null
+++ b/common/goal_distance_calculator/CMakeLists.txt
@@ -0,0 +1,32 @@
+cmake_minimum_required(VERSION 3.5)
+project(goal_distance_calculator)
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+
+ament_auto_add_library(goal_distance_calculator SHARED
+  src/goal_distance_calculator_node.cpp
+  src/goal_distance_calculator.cpp
+)
+
+rclcpp_components_register_node(goal_distance_calculator
+  PLUGIN "goal_distance_calculator::GoalDistanceCalculatorNode"
+  EXECUTABLE goal_distance_calculator_node
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+  config
+)
diff --git a/common/goal_distance_calculator/Readme.md b/common/goal_distance_calculator/Readme.md
new file mode 100644
index 0000000000000..a732e5153b02c
--- /dev/null
+++ b/common/goal_distance_calculator/Readme.md
@@ -0,0 +1,43 @@
+# goal_distance_calculator
+
+## Purpose
+
+This node publishes deviation of self-pose from goal pose.
+
+## Inner-workings / Algorithms
+
+## Inputs / Outputs
+
+### Input
+
+| Name                               | Type                                      | Description           |
+| ---------------------------------- | ----------------------------------------- | --------------------- |
+| `/planning/mission_planning/route` | `autoware_auto_planning_msgs::msg::Route` | Used to get goal pose |
+| `/tf`                              | `tf2_msgs/TFMessage`                      | TF (self-pose)        |
+
+### Output
+
+| Name                     | Type                                       | Description                                                   |
+| ------------------------ | ------------------------------------------ | ------------------------------------------------------------- |
+| `deviation/lateral`      | `autoware_debug_msgs::msg::Float64Stamped` | publish lateral deviation of self-pose from goal pose[m]      |
+| `deviation/longitudinal` | `autoware_debug_msgs::msg::Float64Stamped` | publish longitudinal deviation of self-pose from goal pose[m] |
+| `deviation/yaw`          | `autoware_debug_msgs::msg::Float64Stamped` | publish yaw deviation of self-pose from goal pose[rad]        |
+| `deviation/yaw_deg`      | `autoware_debug_msgs::msg::Float64Stamped` | publish yaw deviation of self-pose from goal pose[deg]        |
+
+## Parameters
+
+### Node Parameters
+
+| Name          | Type   | Default Value | Explanation                 |
+| ------------- | ------ | ------------- | --------------------------- |
+| `update_rate` | double | 10.0          | Timer callback period. [Hz] |
+
+### Core Parameters
+
+| Name      | Type | Default Value | Explanation                                |
+| --------- | ---- | ------------- | ------------------------------------------ |
+| `oneshot` | bool | true          | publish deviations just once or repeatedly |
+
+## Assumptions / Known limits
+
+TBD.
diff --git a/common/goal_distance_calculator/config/goal_distance_calculator.param.yaml b/common/goal_distance_calculator/config/goal_distance_calculator.param.yaml
new file mode 100644
index 0000000000000..5b8c019de5a20
--- /dev/null
+++ b/common/goal_distance_calculator/config/goal_distance_calculator.param.yaml
@@ -0,0 +1,3 @@
+/**:
+  ros__parameters:
+    update_rate: 10.0
diff --git a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp
new file mode 100644
index 0000000000000..618b8db7db37b
--- /dev/null
+++ b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp
@@ -0,0 +1,57 @@
+// Copyright 2020 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.
+
+#ifndef GOAL_DISTANCE_CALCULATOR__GOAL_DISTANCE_CALCULATOR_HPP_
+#define GOAL_DISTANCE_CALCULATOR__GOAL_DISTANCE_CALCULATOR_HPP_
+
+#include <autoware_utils/geometry/pose_deviation.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_planning_msgs/msg/route.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+
+#include <boost/optional.hpp>
+
+namespace goal_distance_calculator
+{
+using autoware_utils::PoseDeviation;
+
+struct Param
+{
+};
+
+struct Input
+{
+  geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose;
+  autoware_auto_planning_msgs::msg::Route::ConstSharedPtr route;
+};
+
+struct Output
+{
+  PoseDeviation goal_deviation;
+};
+
+class GoalDistanceCalculator
+{
+public:
+  Output update(const Input & input);
+
+  void setParam(const Param & param) { param_ = param; }
+
+private:
+  Param param_;
+};
+}  // namespace goal_distance_calculator
+
+#endif  // GOAL_DISTANCE_CALCULATOR__GOAL_DISTANCE_CALCULATOR_HPP_
diff --git a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp
new file mode 100644
index 0000000000000..92d6362c149c3
--- /dev/null
+++ b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp
@@ -0,0 +1,78 @@
+// Copyright 2020 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.
+
+#ifndef GOAL_DISTANCE_CALCULATOR__GOAL_DISTANCE_CALCULATOR_NODE_HPP_
+#define GOAL_DISTANCE_CALCULATOR__GOAL_DISTANCE_CALCULATOR_NODE_HPP_
+
+#include "goal_distance_calculator/goal_distance_calculator.hpp"
+
+#include <autoware_utils/ros/debug_publisher.hpp>
+#include <autoware_utils/ros/self_pose_listener.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_planning_msgs/msg/route.hpp>
+#include <autoware_debug_msgs/msg/float64_stamped.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+
+#include <tf2_ros/transform_listener.h>
+
+#include <memory>
+
+namespace goal_distance_calculator
+{
+struct NodeParam
+{
+  double update_rate;
+  bool oneshot;
+};
+
+class GoalDistanceCalculatorNode : public rclcpp::Node
+{
+public:
+  explicit GoalDistanceCalculatorNode(const rclcpp::NodeOptions & options);
+
+private:
+  // Subscriber
+  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr sub_initial_pose_;
+  autoware_utils::SelfPoseListener self_pose_listener_;
+  rclcpp::Subscription<autoware_auto_planning_msgs::msg::Route>::SharedPtr sub_route_;
+
+  // Data Buffer
+  geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_;
+  autoware_auto_planning_msgs::msg::Route::SharedPtr route_;
+
+  // Callback
+  void onRoute(const autoware_auto_planning_msgs::msg::Route::ConstSharedPtr & msg);
+
+  // Publisher
+  autoware_utils::DebugPublisher debug_publisher_;
+
+  // Timer
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  bool isDataReady();
+  bool isDataTimeout();
+  void onTimer();
+
+  // Parameter
+  NodeParam node_param_;
+  Param param_;
+
+  // Core
+  Input input_;
+  Output output_;
+  std::unique_ptr<GoalDistanceCalculator> goal_distance_calculator_;
+};
+}  // namespace goal_distance_calculator
+#endif  // GOAL_DISTANCE_CALCULATOR__GOAL_DISTANCE_CALCULATOR_NODE_HPP_
diff --git a/common/goal_distance_calculator/launch/goal_distance_calculator.launch.xml b/common/goal_distance_calculator/launch/goal_distance_calculator.launch.xml
new file mode 100644
index 0000000000000..05c361c031220
--- /dev/null
+++ b/common/goal_distance_calculator/launch/goal_distance_calculator.launch.xml
@@ -0,0 +1,8 @@
+<launch>
+  <arg name="oneshot" default="false" />
+  <arg name="config_file" default="$(find-pkg-share goal_distance_calculator)/config/goal_distance_calculator.param.yaml" />
+  <node pkg="goal_distance_calculator" exec="goal_distance_calculator_node" name="goal_distance_calculator" output="screen">
+    <param from="$(var config_file)"/>
+    <param name="oneshot" value="$(var oneshot)"/>
+  </node>
+</launch>
diff --git a/common/goal_distance_calculator/package.xml b/common/goal_distance_calculator/package.xml
new file mode 100644
index 0000000000000..1a89b0eea664a
--- /dev/null
+++ b/common/goal_distance_calculator/package.xml
@@ -0,0 +1,25 @@
+<?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>goal_distance_calculator</name>
+  <version>0.0.0</version>
+  <description>The goal_distance_calculator package </description>
+  <maintainer email="ttatcoder@outlook.jp">Taiki Tanaka</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>autoware_auto_planning_msgs</depend>
+  <depend>autoware_debug_msgs</depend>
+  <depend>autoware_utils</depend>
+  <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</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/common/goal_distance_calculator/src/goal_distance_calculator.cpp b/common/goal_distance_calculator/src/goal_distance_calculator.cpp
new file mode 100755
index 0000000000000..c0e31b24ba92b
--- /dev/null
+++ b/common/goal_distance_calculator/src/goal_distance_calculator.cpp
@@ -0,0 +1,29 @@
+// Copyright 2020 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 "goal_distance_calculator/goal_distance_calculator.hpp"
+
+namespace goal_distance_calculator
+{
+Output GoalDistanceCalculator::update(const Input & input)
+{
+  Output output{};
+
+  output.goal_deviation =
+    autoware_utils::calcPoseDeviation(input.route->goal_point.pose, input.current_pose->pose);
+
+  return output;
+}
+
+}  // namespace goal_distance_calculator
diff --git a/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp b/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp
new file mode 100644
index 0000000000000..7c86fa071c5ce
--- /dev/null
+++ b/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp
@@ -0,0 +1,139 @@
+// Copyright 2020 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 "goal_distance_calculator/goal_distance_calculator_node.hpp"
+
+#include <autoware_utils/math/unit_conversion.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/timer.hpp>
+
+#include <autoware_debug_msgs/msg/float64_stamped.hpp>
+
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <string>
+#include <utility>
+
+namespace goal_distance_calculator
+{
+GoalDistanceCalculatorNode::GoalDistanceCalculatorNode(const rclcpp::NodeOptions & options)
+: Node("goal_distance_calculator"),
+  self_pose_listener_(this),
+  debug_publisher_(this, "goal_distance_calculator")
+{
+  using std::placeholders::_1;
+
+  static constexpr std::size_t queue_size = 1;
+  rclcpp::QoS durable_qos(queue_size);
+  durable_qos.transient_local();
+
+  // Node Parameter
+  node_param_.update_rate = declare_parameter("update_rate", 10.0);
+  node_param_.oneshot = declare_parameter("oneshot", true);
+
+  // Core
+  goal_distance_calculator_ = std::make_unique<GoalDistanceCalculator>();
+  goal_distance_calculator_->setParam(param_);
+
+  // Subscriber
+  sub_route_ = create_subscription<autoware_auto_planning_msgs::msg::Route>(
+    "/planning/mission_planning/route", queue_size,
+    [&](const autoware_auto_planning_msgs::msg::Route::SharedPtr msg_ptr) { route_ = msg_ptr; });
+
+  // Wait for first self pose
+  self_pose_listener_.waitForFirstPose();
+
+  // Timer
+  double delta_time = 1.0 / static_cast<double>(node_param_.update_rate);
+  auto timer_callback_ = std::bind(&GoalDistanceCalculatorNode::onTimer, this);
+  const auto period_ns =
+    std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(delta_time));
+  timer_ = std::make_shared<rclcpp::GenericTimer<decltype(timer_callback_)>>(
+    this->get_clock(), period_ns, std::move(timer_callback_),
+    this->get_node_base_interface()->get_context());
+  this->get_node_timers_interface()->add_timer(timer_, nullptr);
+  goal_distance_calculator_ = std::make_unique<GoalDistanceCalculator>();
+}
+
+bool GoalDistanceCalculatorNode::isDataReady()
+{
+  if (!current_pose_) {
+    RCLCPP_INFO_THROTTLE(
+      this->get_logger(), *this->get_clock(), 5000, "waiting for current_pose...");
+    return false;
+  }
+
+  if (!route_) {
+    RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "waiting for route msg...");
+    return false;
+  }
+
+  return true;
+}
+
+bool GoalDistanceCalculatorNode::isDataTimeout()
+{
+  constexpr double th_pose_timeout = 1.0;
+  const auto pose_time_diff = rclcpp::Time(current_pose_->header.stamp) - now();
+  if (pose_time_diff.seconds() > th_pose_timeout) {
+    RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "pose is timeout...");
+    return true;
+  }
+  return false;
+}
+
+void GoalDistanceCalculatorNode::onTimer()
+{
+  current_pose_ = self_pose_listener_.getCurrentPose();
+
+  if (!isDataReady()) {
+    return;
+  }
+
+  if (isDataTimeout()) {
+    return;
+  }
+
+  input_.current_pose = current_pose_;
+  input_.route = route_;
+
+  output_ = goal_distance_calculator_->update(input_);
+
+  {
+    using autoware_utils::rad2deg;
+    const auto & deviation = output_.goal_deviation;
+
+    debug_publisher_.publish<autoware_debug_msgs::msg::Float64Stamped>(
+      "deviation/lateral", deviation.lateral);
+    debug_publisher_.publish<autoware_debug_msgs::msg::Float64Stamped>(
+      "deviation/longitudinal", deviation.longitudinal);
+    debug_publisher_.publish<autoware_debug_msgs::msg::Float64Stamped>(
+      "deviation/yaw", deviation.yaw);
+    debug_publisher_.publish<autoware_debug_msgs::msg::Float64Stamped>(
+      "deviation/yaw_deg", rad2deg(deviation.yaw));
+    RCLCPP_INFO_THROTTLE(
+      this->get_logger(), *this->get_clock(), 1000,
+      "lateral: %f[mm], longitudinal: %f[mm], yaw: %f[deg]", 1000 * deviation.lateral,
+      1000 * deviation.longitudinal, rad2deg(deviation.yaw));
+  }
+
+  if (node_param_.oneshot) {
+    rclcpp::shutdown();
+  }
+}
+}  // namespace goal_distance_calculator
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(goal_distance_calculator::GoalDistanceCalculatorNode)