diff --git a/.github/workflows/openai-pr-reviewer.yaml b/.github/workflows/openai-pr-reviewer.yaml
index 8445b0debb17e..ecd8d85769a95 100644
--- a/.github/workflows/openai-pr-reviewer.yaml
+++ b/.github/workflows/openai-pr-reviewer.yaml
@@ -6,6 +6,10 @@ permissions:
 
 on:
   pull_request:
+    types:
+      - opened
+      - synchronize
+      - labeled
   pull_request_review_comment:
     types: [created]
 
@@ -16,7 +20,13 @@ concurrency:
   cancel-in-progress: ${{ github.event_name != 'pull_request_review_comment' }}
 
 jobs:
+  prevent-no-label-execution:
+    uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1
+    with:
+      label: openai-pr-reviewer
   review:
+    needs: prevent-no-label-execution
+    if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }}
     runs-on: ubuntu-latest
     steps:
       - uses: fluxninja/openai-pr-reviewer@latest
diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/perception.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/perception.hpp
new file mode 100644
index 0000000000000..5711c9a86d12a
--- /dev/null
+++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/perception.hpp
@@ -0,0 +1,36 @@
+// Copyright 2022 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE_AD_API_SPECS__PERCEPTION_HPP_
+#define AUTOWARE_AD_API_SPECS__PERCEPTION_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/dynamic_object_array.hpp>
+
+namespace autoware_ad_api::perception
+{
+
+struct DynamicObjectArray
+{
+  using Message = autoware_adapi_v1_msgs::msg::DynamicObjectArray;
+  static constexpr char name[] = "/api/perception/objects";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
+}  // namespace autoware_ad_api::perception
+
+#endif  // AUTOWARE_AD_API_SPECS__PERCEPTION_HPP_
diff --git a/common/component_interface_specs/include/component_interface_specs/perception.hpp b/common/component_interface_specs/include/component_interface_specs/perception.hpp
new file mode 100644
index 0000000000000..8665b9c35157d
--- /dev/null
+++ b/common/component_interface_specs/include/component_interface_specs/perception.hpp
@@ -0,0 +1,36 @@
+// Copyright 2022 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 COMPONENT_INTERFACE_SPECS__PERCEPTION_HPP_
+#define COMPONENT_INTERFACE_SPECS__PERCEPTION_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
+
+namespace perception_interface
+{
+
+struct ObjectRecognition
+{
+  using Message = autoware_auto_perception_msgs::msg::PredictedObjects;
+  static constexpr char name[] = "/perception/object_recognition/objects";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
+}  // namespace perception_interface
+
+#endif  // COMPONENT_INTERFACE_SPECS__PERCEPTION_HPP_
diff --git a/launch/tier4_localization_launch/launch/localization.launch.xml b/launch/tier4_localization_launch/launch/localization.launch.xml
index 550c9b5ff7fff..8538179253480 100644
--- a/launch/tier4_localization_launch/launch/localization.launch.xml
+++ b/launch/tier4_localization_launch/launch/localization.launch.xml
@@ -14,7 +14,7 @@
   <arg name="pose_initializer_param_path"/>
   <arg name="eagleye_param_path"/>
 
-  <arg name="input/pointcloud" default="/sensing/lidar/top/outlier_filtered/pointcloud" description="The topic will be used in the localization util module"/>
+  <arg name="input_pointcloud" default="/sensing/lidar/top/outlier_filtered/pointcloud"/>
   <arg name="use_pointcloud_container" default="true" description="launch pointcloud container"/>
   <arg name="pointcloud_container_name" default="pointcloud_container"/>
 
diff --git a/launch/tier4_localization_launch/launch/util/util.launch.py b/launch/tier4_localization_launch/launch/util/util.launch.py
index ef9f25e43cf90..1a34429f438ed 100644
--- a/launch/tier4_localization_launch/launch/util/util.launch.py
+++ b/launch/tier4_localization_launch/launch/util/util.launch.py
@@ -34,7 +34,7 @@ def load_composable_node_param(param_path):
         plugin="pointcloud_preprocessor::CropBoxFilterComponent",
         name="crop_box_filter_measurement_range",
         remappings=[
-            ("input", LaunchConfiguration("input/pointcloud")),
+            ("input", LaunchConfiguration("input_pointcloud")),
             ("output", "measurement_range/pointcloud"),
         ],
         parameters=[
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py
index dc3e33faafb94..a5d8d2113d2db 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py
@@ -65,7 +65,7 @@ def create_normal_pipeline(self):
                     ("map", "/map/pointcloud_map"),
                     ("output", LaunchConfiguration("output_topic")),
                     ("map_loader_service", "/map/get_differential_pointcloud_map"),
-                    ("pose_with_covariance", "/localization/pose_estimator/pose_with_covariance"),
+                    ("kinematic_state", "/localization/kinematic_state"),
                 ],
                 parameters=[
                     {
@@ -122,7 +122,7 @@ def create_down_sample_pipeline(self):
                     ("map", "/map/pointcloud_map"),
                     ("output", LaunchConfiguration("output_topic")),
                     ("map_loader_service", "/map/get_differential_pointcloud_map"),
-                    ("pose_with_covariance", "/localization/pose_estimator/pose_with_covariance"),
+                    ("kinematic_state", "/localization/kinematic_state"),
                 ],
                 parameters=[
                     {
diff --git a/perception/compare_map_segmentation/CMakeLists.txt b/perception/compare_map_segmentation/CMakeLists.txt
index 2d65f75a098a3..4655cf4c2f73a 100644
--- a/perception/compare_map_segmentation/CMakeLists.txt
+++ b/perception/compare_map_segmentation/CMakeLists.txt
@@ -46,8 +46,7 @@ ament_target_dependencies(compare_map_segmentation
   sensor_msgs
   tier4_autoware_utils
   autoware_map_msgs
-  component_interface_specs
-  component_interface_utils
+  nav_msgs
 )
 
 if(OPENMP_FOUND)
diff --git a/perception/compare_map_segmentation/README.md b/perception/compare_map_segmentation/README.md
index ad891b9ad6afa..0566b5af806db 100644
--- a/perception/compare_map_segmentation/README.md
+++ b/perception/compare_map_segmentation/README.md
@@ -61,12 +61,11 @@ This filter is a combination of the distance_based_compare_map_filter and voxel_
 
 #### Input
 
-| Name                                 | Type                                            | Description                                                    |
-| ------------------------------------ | ----------------------------------------------- | -------------------------------------------------------------- |
-| `~/input/points`                     | `sensor_msgs::msg::PointCloud2`                 | reference points                                               |
-| `~/input/map`                        | `sensor_msgs::msg::PointCloud2`                 | map (in case static map loading)                               |
-| `~/pose_with_covariance`             | `geometry_msgs::msg::PoseWithCovarianceStamped` | current ego-vehicle pose (in case dynamic map loading)         |
-| `/localization/initialization_state` | `localization_interface::InitializationState`   | Ego-vehicle pose initialization state (in dynamic map loading) |
+| Name                            | Type                            | Description                                            |
+| ------------------------------- | ------------------------------- | ------------------------------------------------------ |
+| `~/input/points`                | `sensor_msgs::msg::PointCloud2` | reference points                                       |
+| `~/input/map`                   | `sensor_msgs::msg::PointCloud2` | map (in case static map loading)                       |
+| `/localization/kinematic_state` | `nav_msgs::msg::Odometry`       | current ego-vehicle pose (in case dynamic map loading) |
 
 #### Output
 
diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp
index 9a9c0d1534a5e..8e3bb21cc4486 100644
--- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp
+++ b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp
@@ -15,12 +15,11 @@
 #ifndef COMPARE_MAP_SEGMENTATION__VOXEL_GRID_MAP_LOADER_HPP_
 #define COMPARE_MAP_SEGMENTATION__VOXEL_GRID_MAP_LOADER_HPP_
 
-#include <component_interface_specs/localization.hpp>
-#include <component_interface_utils/rclcpp.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_map_msgs/srv/get_differential_point_cloud_map.hpp>
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 
 #include <pcl/filters/voxel_grid.h>
@@ -154,16 +153,11 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
   };
 
   typedef typename std::map<std::string, struct MapGridVoxelInfo> VoxelGridDict;
-  using InitializationState = localization_interface::InitializationState;
 
   /** \brief Map to hold loaded map grid id and it's voxel filter */
   VoxelGridDict current_voxel_grid_dict_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_kinematic_state_;
 
-  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
-    sub_estimated_pose_;
-  component_interface_utils::Subscription<InitializationState>::SharedPtr
-    sub_pose_initializer_state_;
-  InitializationState::Message initialization_state_;
   std::optional<geometry_msgs::msg::Point> current_position_ = std::nullopt;
   std::optional<geometry_msgs::msg::Point> last_updated_position_ = std::nullopt;
   rclcpp::TimerBase::SharedPtr map_update_timer_;
@@ -200,8 +194,7 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
     rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
     std::string * tf_map_input_frame, std::mutex * mutex,
     rclcpp::CallbackGroup::SharedPtr main_callback_group);
-  void onEstimatedPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose);
-  void onPoseInitializerCallback(const InitializationState::Message::ConstSharedPtr msg);
+  void onEstimatedPoseCallback(nav_msgs::msg::Odometry::ConstSharedPtr pose);
 
   void timer_callback();
   bool should_update_map() const;
diff --git a/perception/compare_map_segmentation/package.xml b/perception/compare_map_segmentation/package.xml
index fc6806ddaa843..42fad723b5d57 100644
--- a/perception/compare_map_segmentation/package.xml
+++ b/perception/compare_map_segmentation/package.xml
@@ -18,10 +18,9 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
   <depend>autoware_map_msgs</depend>
-  <depend>component_interface_specs</depend>
-  <depend>component_interface_utils</depend>
   <depend>grid_map_pcl</depend>
   <depend>grid_map_ros</depend>
+  <depend>nav_msgs</depend>
   <depend>pcl_conversions</depend>
   <depend>pointcloud_preprocessor</depend>
   <depend>rclcpp</depend>
diff --git a/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp b/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp
index 426dc8e706dc9..06bd89af38259 100644
--- a/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp
+++ b/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp
@@ -301,13 +301,8 @@ VoxelGridDynamicMapLoader::VoxelGridDynamicMapLoader(
   map_loader_radius_ = node->declare_parameter<double>("map_loader_radius");
   auto main_sub_opt = rclcpp::SubscriptionOptions();
   main_sub_opt.callback_group = main_callback_group;
-
-  const auto localization_node = component_interface_utils::NodeAdaptor(node);
-  localization_node.init_sub(
-    sub_pose_initializer_state_, this, &VoxelGridDynamicMapLoader::onPoseInitializerCallback);
-
-  sub_estimated_pose_ = node->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
-    "pose_with_covariance", rclcpp::QoS{1},
+  sub_kinematic_state_ = node->create_subscription<nav_msgs::msg::Odometry>(
+    "kinematic_state", rclcpp::QoS{1},
     std::bind(&VoxelGridDynamicMapLoader::onEstimatedPoseCallback, this, std::placeholders::_1),
     main_sub_opt);
   RCLCPP_INFO(logger_, "VoxelGridDynamicMapLoader initialized.\n");
@@ -327,18 +322,7 @@ VoxelGridDynamicMapLoader::VoxelGridDynamicMapLoader(
     node, node->get_clock(), period_ns, std::bind(&VoxelGridDynamicMapLoader::timer_callback, this),
     timer_callback_group_);
 }
-void VoxelGridDynamicMapLoader::onPoseInitializerCallback(
-  const InitializationState::Message::ConstSharedPtr msg)
-{
-  initialization_state_.state = msg->state;
-  if (msg->state != InitializationState::Message::INITIALIZED) {
-    current_position_ = std::nullopt;
-    last_updated_position_ = std::nullopt;
-    RCLCPP_INFO(logger_, "Initializing pose... Reset the position of Vehicle");
-  }
-}
-void VoxelGridDynamicMapLoader::onEstimatedPoseCallback(
-  geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg)
+void VoxelGridDynamicMapLoader::onEstimatedPoseCallback(nav_msgs::msg::Odometry::ConstSharedPtr msg)
 {
   current_position_ = msg->pose.pose.position;
 }
@@ -417,9 +401,7 @@ bool VoxelGridDynamicMapLoader::is_close_to_map(
 }
 void VoxelGridDynamicMapLoader::timer_callback()
 {
-  if (
-    current_position_ == std::nullopt ||
-    initialization_state_.state != InitializationState::Message::INITIALIZED) {
+  if (current_position_ == std::nullopt) {
     return;
   }
   if (last_updated_position_ == std::nullopt) {
diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py
index f4256422e6c8b..db86bbf80d250 100644
--- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py
+++ b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py
@@ -57,7 +57,7 @@ def load_composable_node_param(param_path):
             ("map", LaunchConfiguration("input_map")),
             ("output", "compare_map_filtered/pointcloud"),
             ("map_loader_service", "/map/get_differential_pointcloud_map"),
-            ("pose_with_covariance", "/localization/pose_estimator/pose_with_covariance"),
+            ("kinematic_state", "/localization/kinematic_state"),
         ],
         parameters=[
             {
diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py
index a2c7c5966da62..82ce5605b67cd 100644
--- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py
+++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py
@@ -47,7 +47,7 @@ def load_composable_node_param(param_path):
             ("map", LaunchConfiguration("input_map")),
             ("output", "map_filter/pointcloud"),
             ("map_loader_service", "/map/get_differential_pointcloud_map"),
-            ("pose_with_covariance", "/localization/pose_estimator/pose_with_covariance"),
+            ("kinematic_state", "/localization/kinematic_state"),
         ],
         parameters=[load_composable_node_param("compare_map_param_path")],
     )
diff --git a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp
index e0c346c3108a5..9180b18adeed8 100644
--- a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp
+++ b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp
@@ -191,38 +191,39 @@ void RoiClusterFusionNode::fuseOnSingleImage(
         max_iou = iou + iou_x + iou_y;
       }
     }
-    bool is_roi_label_known = feature_obj.object.classification.front().label !=
-                              autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN;
-    bool is_roi_existence_prob_higher =
-      output_cluster_msg.feature_objects.at(index).object.existence_probability <=
-      feature_obj.object.existence_probability;
-    if (iou_threshold_ < max_iou && is_roi_existence_prob_higher && is_roi_label_known) {
-      output_cluster_msg.feature_objects.at(index).object.classification =
-        feature_obj.object.classification;
-
-      // Update existence_probability for fused objects
-      if (
-        output_cluster_msg.feature_objects.at(index).object.existence_probability <
-        min_roi_existence_prob_) {
-        output_cluster_msg.feature_objects.at(index).object.existence_probability =
-          min_roi_existence_prob_;
+    if (!output_cluster_msg.feature_objects.empty()) {
+      bool is_roi_label_known = feature_obj.object.classification.front().label !=
+                                autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN;
+      bool is_roi_existence_prob_higher =
+        output_cluster_msg.feature_objects.at(index).object.existence_probability <=
+        feature_obj.object.existence_probability;
+      if (iou_threshold_ < max_iou && is_roi_existence_prob_higher && is_roi_label_known) {
+        output_cluster_msg.feature_objects.at(index).object.classification =
+          feature_obj.object.classification;
+
+        // Update existence_probability for fused objects
+        if (
+          output_cluster_msg.feature_objects.at(index).object.existence_probability <
+          min_roi_existence_prob_) {
+          output_cluster_msg.feature_objects.at(index).object.existence_probability =
+            min_roi_existence_prob_;
+        }
       }
-    }
 
-    // fuse with unknown roi
-
-    if (unknown_iou_threshold_ < max_iou && is_roi_existence_prob_higher && !is_roi_label_known) {
-      output_cluster_msg.feature_objects.at(index).object.classification =
-        feature_obj.object.classification;
-      // Update existence_probability for fused objects
-      if (
-        output_cluster_msg.feature_objects.at(index).object.existence_probability <
-        min_roi_existence_prob_) {
-        output_cluster_msg.feature_objects.at(index).object.existence_probability =
-          min_roi_existence_prob_;
+      // fuse with unknown roi
+
+      if (unknown_iou_threshold_ < max_iou && is_roi_existence_prob_higher && !is_roi_label_known) {
+        output_cluster_msg.feature_objects.at(index).object.classification =
+          feature_obj.object.classification;
+        // Update existence_probability for fused objects
+        if (
+          output_cluster_msg.feature_objects.at(index).object.existence_probability <
+          min_roi_existence_prob_) {
+          output_cluster_msg.feature_objects.at(index).object.existence_probability =
+            min_roi_existence_prob_;
+        }
       }
     }
-
     debug_image_rois.push_back(feature_obj.feature.roi);
   }
 
diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp
index a7762815f9e69..9292476e80c8e 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -227,7 +227,7 @@ lanelet::ConstLanelets getRightLineSharingLanelets(
   for (auto & candidate : right_lane_candidates) {
     // exclude self lanelet
     if (candidate == current_lanelet) continue;
-    // if candidate has linestring as leftbound, assign it to output
+    // if candidate has linestring as left bound, assign it to output
     if (candidate.leftBound() == current_lanelet.rightBound()) {
       output_lanelets.push_back(candidate);
     }
@@ -254,7 +254,7 @@ lanelet::ConstLanelets getLeftLineSharingLanelets(
   for (auto & candidate : left_lane_candidates) {
     // exclude self lanelet
     if (candidate == current_lanelet) continue;
-    // if candidate has linestring as rightbound, assign it to output
+    // if candidate has linestring as right bound, assign it to output
     if (candidate.rightBound() == current_lanelet.leftBound()) {
       output_lanelets.push_back(candidate);
     }
@@ -287,7 +287,7 @@ lanelet::routing::LaneletPaths getPossiblePathsForIsolatedLanelet(
   lanelet::ConstLanelets possible_lanelets;
   possible_lanelets.push_back(lanelet);
   lanelet::routing::LaneletPaths possible_paths;
-  // need to init path with constlanelets
+  // need to initialize path with constant lanelets
   lanelet::routing::LaneletPath possible_path(possible_lanelets);
   possible_paths.push_back(possible_path);
   return possible_paths;
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp
index f8830212a8222..7f18c847af43e 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp
@@ -82,13 +82,13 @@ class DynamicAvoidanceModule : public SceneModuleInterface
   {
     DynamicAvoidanceObject(
       const PredictedObject & predicted_object, const double arg_vel, const double arg_lat_vel,
-      const bool arg_is_left, const double arg_time_to_collision)
+      const bool arg_is_collision_left, const double arg_time_to_collision)
     : uuid(tier4_autoware_utils::toHexString(predicted_object.object_id)),
       pose(predicted_object.kinematics.initial_pose_with_covariance.pose),
       shape(predicted_object.shape),
       vel(arg_vel),
       lat_vel(arg_lat_vel),
-      is_left(arg_is_left),
+      is_collision_left(arg_is_collision_left),
       time_to_collision(arg_time_to_collision)
     {
       for (const auto & path : predicted_object.kinematics.predicted_paths) {
@@ -101,7 +101,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface
     autoware_auto_perception_msgs::msg::Shape shape;
     double vel;
     double lat_vel;
-    bool is_left;
+    bool is_collision_left;
     double time_to_collision;
     std::vector<autoware_auto_perception_msgs::msg::PredictedPath> predicted_paths{};
   };
@@ -162,7 +162,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface
     const std::vector<PathPointWithLaneId> & ego_path, const PredictedPath & predicted_path,
     const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const;
   bool willObjectCutOut(
-    const double obj_tangent_vel, const double obj_normal_vel, const bool is_left) const;
+    const double obj_tangent_vel, const double obj_normal_vel, const bool is_collision_left) const;
   bool isObjectFarFromPath(
     const PredictedObject & predicted_object, const double obj_dist_to_path) const;
   double calcTimeToCollision(
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp
index 7928938ac4d5b..2042593064c51 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp
@@ -45,10 +45,6 @@ class ShiftPullOut : public PullOutPlannerBase
     const BehaviorPathPlannerParameters & common_parameter,
     const behavior_path_planner::StartPlannerParameters & parameter);
 
-  bool hasEnoughDistance(
-    const double pull_out_total_distance, const lanelet::ConstLanelets & current_lanes,
-    const Pose & current_pose, const bool isInGoalRouteSection, const Pose & goal_pose);
-
   double calcBeforeShiftedArcLength(
     const PathWithLaneId & path, const double target_after_arc_length, const double dr);
 
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp
index 07ec7c5db6605..d1a69fa0e6cce 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp
@@ -383,7 +383,7 @@ lanelet::ConstLanelets getExtendedCurrentLanes(
 
 lanelet::ConstLanelets getExtendedCurrentLanes(
   const std::shared_ptr<const PlannerData> & planner_data, const double backward_length,
-  const double forward_length);
+  const double forward_length, const bool until_goal_lane);
 
 lanelet::ConstLanelets calcLaneAroundPose(
   const std::shared_ptr<RouteHandler> route_handler, const geometry_msgs::msg::Pose & pose,
diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp
index b1aa372c42ac5..a90ded11d2dfe 100644
--- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp
@@ -56,7 +56,7 @@ void appendObjectMarker(MarkerArray & marker_array, const geometry_msgs::msg::Po
     "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "dynamic_objects_to_avoid",
     marker_array.markers.size(), visualization_msgs::msg::Marker::CUBE,
     tier4_autoware_utils::createMarkerScale(3.0, 1.0, 1.0),
-    tier4_autoware_utils::createMarkerColor(0.7, 0.15, 0.9, 0.8));
+    tier4_autoware_utils::createMarkerColor(1.0, 0.5, 0.6, 0.8));
   marker.pose = obj_pose;
 
   marker_array.markers.push_back(marker);
@@ -68,8 +68,8 @@ void appendExtractedPolygonMarker(
   auto marker = tier4_autoware_utils::createDefaultMarker(
     "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "extracted_polygons", marker_array.markers.size(),
     visualization_msgs::msg::Marker::LINE_STRIP,
-    tier4_autoware_utils::createMarkerScale(0.1, 0.0, 0.0),
-    tier4_autoware_utils::createMarkerColor(0.7, 0.15, 0.9, 0.8));
+    tier4_autoware_utils::createMarkerScale(0.05, 0.0, 0.0),
+    tier4_autoware_utils::createMarkerColor(1.0, 0.5, 0.6, 0.8));
 
   // NOTE: obj_poly.outer() has already duplicated points to close the polygon.
   for (size_t i = 0; i < obj_poly.outer().size(); ++i) {
@@ -262,7 +262,8 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan()
   for (const auto & object : target_objects_) {
     const auto obstacle_poly = calcDynamicObstaclePolygon(object);
     if (obstacle_poly) {
-      obstacles_for_drivable_area.push_back({object.pose, obstacle_poly.value(), object.is_left});
+      obstacles_for_drivable_area.push_back(
+        {object.pose, obstacle_poly.value(), object.is_collision_left});
 
       appendObjectMarker(info_marker_, object.pose);
       appendExtractedPolygonMarker(debug_marker_, obstacle_poly.value());
@@ -396,16 +397,15 @@ DynamicAvoidanceModule::calcTargetObjectsCandidate()
     }
 
     // 6. calculate which side object exists against ego's path
-    // TODO(murooka) use obj_path.back() instead of obj_pose for the case where the object crosses
-    // the ego's path
-    const bool is_left = isLeft(prev_module_path->points, obj_path.path.back().position);
+    const bool is_object_left = isLeft(prev_module_path->points, obj_pose.position);
     const auto lat_lon_offset =
       getLateralLongitudinalOffset(prev_module_path->points, predicted_object);
 
     // 6. check if object will not cut in or cut out
     const bool will_object_cut_in =
       willObjectCutIn(prev_module_path->points, obj_path, obj_tangent_vel, lat_lon_offset);
-    const bool will_object_cut_out = willObjectCutOut(obj_tangent_vel, obj_normal_vel, is_left);
+    const bool will_object_cut_out =
+      willObjectCutOut(obj_tangent_vel, obj_normal_vel, is_object_left);
     if (will_object_cut_in) {
       RCLCPP_INFO_EXPRESSION(
         getLogger(), parameters_->enable_debug_info,
@@ -434,6 +434,13 @@ DynamicAvoidanceModule::calcTargetObjectsCandidate()
       continue;
     }
 
+    // calculate which side object will be against ego's path
+    const auto future_obj_pose =
+      object_recognition_utils::calcInterpolatedPose(obj_path, time_to_collision);
+    const bool is_collision_left = future_obj_pose
+                                     ? isLeft(prev_module_path->points, future_obj_pose->position)
+                                     : is_object_left;
+
     // 8. calculate alive counter for filtering objects
     const auto prev_target_object_candidate =
       DynamicAvoidanceObjectCandidate::getObjectFromUuid(prev_target_objects_candidate_, obj_uuid);
@@ -445,7 +452,7 @@ DynamicAvoidanceModule::calcTargetObjectsCandidate()
         : 0;
 
     const auto target_object = DynamicAvoidanceObject(
-      predicted_object, obj_tangent_vel, obj_normal_vel, is_left, time_to_collision);
+      predicted_object, obj_tangent_vel, obj_normal_vel, is_collision_left, time_to_collision);
     const auto target_object_candidate =
       DynamicAvoidanceObjectCandidate{target_object, alive_counter};
     output_objects_candidate.push_back(target_object_candidate);
@@ -552,7 +559,7 @@ bool DynamicAvoidanceModule::willObjectCutIn(
 }
 
 bool DynamicAvoidanceModule::willObjectCutOut(
-  const double obj_tangent_vel, const double obj_normal_vel, const bool is_left) const
+  const double obj_tangent_vel, const double obj_normal_vel, const bool is_collision_left) const
 {
   // Ignore oncoming object
   if (obj_tangent_vel < 0) {
@@ -560,7 +567,7 @@ bool DynamicAvoidanceModule::willObjectCutOut(
   }
 
   constexpr double object_lat_vel_thresh = 0.3;
-  if (is_left) {
+  if (is_collision_left) {
     if (object_lat_vel_thresh < obj_normal_vel) {
       return true;
     }
@@ -689,8 +696,10 @@ std::optional<tier4_autoware_utils::Polygon2d> DynamicAvoidanceModule::calcDynam
     }
     return getMinMaxValues(obj_lat_abs_offset_vec);
   }();
-  const double min_obj_lat_offset = min_obj_lat_abs_offset * (object.is_left ? 1.0 : -1.0);
-  const double max_obj_lat_offset = max_obj_lat_abs_offset * (object.is_left ? 1.0 : -1.0);
+  const double min_obj_lat_offset =
+    min_obj_lat_abs_offset * (object.is_collision_left ? 1.0 : -1.0);
+  const double max_obj_lat_offset =
+    max_obj_lat_abs_offset * (object.is_collision_left ? 1.0 : -1.0);
 
   // calculate min/max longitudinal offset from object to path
   const auto obj_lon_offset = [&]() -> std::optional<std::pair<double, double>> {
@@ -708,9 +717,15 @@ std::optional<tier4_autoware_utils::Polygon2d> DynamicAvoidanceModule::calcDynam
       return std::nullopt;
     }
 
-    return std::make_pair(
-      raw_min_obj_lon_offset + object.vel * object.time_to_collision,
-      raw_max_obj_lon_offset + object.vel * object.time_to_collision);
+    if (object.vel < 0) {
+      return std::make_pair(
+        raw_min_obj_lon_offset + object.vel * object.time_to_collision, raw_max_obj_lon_offset);
+    }
+
+    // NOTE: If time to collision is considered here, the ego is close to the object when starting
+    // avoidance.
+    //       The ego should start avoidance before overtaking.
+    return std::make_pair(raw_min_obj_lon_offset, raw_max_obj_lon_offset);
   }();
 
   if (!obj_lon_offset) {
@@ -752,10 +767,11 @@ std::optional<tier4_autoware_utils::Polygon2d> DynamicAvoidanceModule::calcDynam
   // calculate bound min and max lateral offset
   const double min_bound_lat_offset = [&]() {
     const double raw_min_bound_lat_offset =
-      min_obj_lat_offset - parameters_->lat_offset_from_obstacle * (object.is_left ? 1.0 : -1.0);
+      min_obj_lat_offset -
+      parameters_->lat_offset_from_obstacle * (object.is_collision_left ? 1.0 : -1.0);
     const double min_bound_lat_abs_offset_limit =
       planner_data_->parameters.vehicle_width / 2.0 - parameters_->max_lat_offset_to_avoid;
-    if (object.is_left) {
+    if (object.is_collision_left) {
       if (raw_min_bound_lat_offset < min_bound_lat_abs_offset_limit) {
         return min_bound_lat_abs_offset_limit;
       }
@@ -767,7 +783,8 @@ std::optional<tier4_autoware_utils::Polygon2d> DynamicAvoidanceModule::calcDynam
     return raw_min_bound_lat_offset;
   }();
   const double max_bound_lat_offset =
-    max_obj_lat_offset + parameters_->lat_offset_from_obstacle * (object.is_left ? 1.0 : -1.0);
+    max_obj_lat_offset +
+    parameters_->lat_offset_from_obstacle * (object.is_collision_left ? 1.0 : -1.0);
 
   // filter min_bound_lat_offset
   const auto prev_min_bound_lat_offset = prev_objects_min_bound_lat_offset_.get(object.uuid);
diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp
index 6915b2be16819..61322025b46f2 100644
--- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp
@@ -153,7 +153,8 @@ void GoalPlannerModule::onTimer()
   // generate valid pull over path candidates and calculate closest start pose
   const auto current_lanes = utils::getExtendedCurrentLanes(
     planner_data_, parameters_->backward_goal_search_length,
-    parameters_->forward_goal_search_length);
+    parameters_->forward_goal_search_length,
+    /*until_goal_lane*/ false);
   std::vector<PullOverPath> path_candidates{};
   std::optional<Pose> closest_start_pose{};
   double min_start_arc_length = std::numeric_limits<double>::max();
@@ -590,7 +591,8 @@ void GoalPlannerModule::setLanes()
 {
   status_.current_lanes = utils::getExtendedCurrentLanes(
     planner_data_, parameters_->backward_goal_search_length,
-    parameters_->forward_goal_search_length);
+    parameters_->forward_goal_search_length,
+    /*until_goal_lane*/ false);
   status_.pull_over_lanes =
     goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_);
   status_.lanes =
diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp
index bc3ce480476ee..fbc81d4a2e890 100644
--- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp
@@ -115,7 +115,8 @@ bool StartPlannerModule::isExecutionRequested() const
   const double backward_path_length =
     planner_data_->parameters.backward_path_length + parameters_->max_back_distance;
   const auto current_lanes = utils::getExtendedCurrentLanes(
-    planner_data_, backward_path_length, std::numeric_limits<double>::max());
+    planner_data_, backward_path_length, std::numeric_limits<double>::max(),
+    /*until_goal_lane*/ true);
   const auto pull_out_lanes =
     start_planner_utils::getPullOutLanes(planner_data_, backward_path_length);
   auto lanes = current_lanes;
@@ -326,7 +327,8 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
   const double backward_path_length =
     planner_data_->parameters.backward_path_length + parameters_->max_back_distance;
   const auto current_lanes = utils::getExtendedCurrentLanes(
-    planner_data_, backward_path_length, std::numeric_limits<double>::max());
+    planner_data_, backward_path_length, std::numeric_limits<double>::max(),
+    /*until_goal_lane*/ true);
 
   const auto pull_out_lanes =
     start_planner_utils::getPullOutLanes(planner_data_, backward_path_length);
@@ -562,7 +564,8 @@ void StartPlannerModule::updatePullOutStatus()
   const double backward_path_length =
     planner_data_->parameters.backward_path_length + parameters_->max_back_distance;
   status_.current_lanes = utils::getExtendedCurrentLanes(
-    planner_data_, backward_path_length, std::numeric_limits<double>::max());
+    planner_data_, backward_path_length, std::numeric_limits<double>::max(),
+    /*until_goal_lane*/ true);
   status_.pull_out_lanes =
     start_planner_utils::getPullOutLanes(planner_data_, backward_path_length);
 
diff --git a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp
index 24c6d98ed09e5..ca251e625a5b0 100644
--- a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp
+++ b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp
@@ -45,7 +45,8 @@ boost::optional<PullOverPath> GeometricPullOver::plan(const Pose & goal_pose)
 
   // prepare road nad shoulder lanes
   const auto road_lanes = utils::getExtendedCurrentLanes(
-    planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length);
+    planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length,
+    /*until_goal_lane*/ false);
   const auto shoulder_lanes =
     goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_);
   if (road_lanes.empty() || shoulder_lanes.empty()) {
diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp
index 9123beca0d95c..a3bd96bddb3d7 100644
--- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp
+++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp
@@ -54,7 +54,9 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose)
 
   const auto pull_over_lanes =
     goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_);
-  auto lanes = utils::getExtendedCurrentLanes(planner_data_, backward_length, forward_length);
+  auto lanes = utils::getExtendedCurrentLanes(
+    planner_data_, backward_length, forward_length,
+    /*until_goal_lane*/ false);
   lanes.insert(lanes.end(), pull_over_lanes.begin(), pull_over_lanes.end());
 
   const auto goal_arc_coords =
diff --git a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp
index 8bf20b212d51d..71ebed11cbd63 100644
--- a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp
+++ b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp
@@ -46,8 +46,9 @@ boost::optional<PullOverPath> ShiftPullOver::plan(const Pose & goal_pose)
   const double jerk_resolution = std::abs(max_jerk - min_jerk) / shift_sampling_num;
 
   // get road and shoulder lanes
-  const auto road_lanes =
-    utils::getExtendedCurrentLanes(planner_data_, backward_search_length, forward_search_length);
+  const auto road_lanes = utils::getExtendedCurrentLanes(
+    planner_data_, backward_search_length, forward_search_length,
+    /*until_goal_lane*/ false);
   const auto shoulder_lanes =
     goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_);
   if (road_lanes.empty() || shoulder_lanes.empty()) {
diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp
index 9ab3101cdc4a5..5561411dfbf39 100644
--- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp
+++ b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp
@@ -43,7 +43,8 @@ boost::optional<PullOutPath> GeometricPullOut::plan(Pose start_pose, Pose goal_p
   const double backward_path_length =
     planner_data_->parameters.backward_path_length + parameters_.max_back_distance;
   const auto road_lanes = utils::getExtendedCurrentLanes(
-    planner_data_, backward_path_length, std::numeric_limits<double>::max());
+    planner_data_, backward_path_length, std::numeric_limits<double>::max(),
+    /*until_goal_lane*/ true);
   const auto pull_out_lanes = getPullOutLanes(planner_data_, backward_path_length);
   auto lanes = road_lanes;
   for (const auto & pull_out_lane : pull_out_lanes) {
diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp
index a34d9f536c682..ef6d55a7af4c6 100644
--- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp
+++ b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp
@@ -53,8 +53,8 @@ boost::optional<PullOutPath> ShiftPullOut::plan(Pose start_pose, Pose goal_pose)
   }
 
   const auto road_lanes = utils::getExtendedCurrentLanes(
-    planner_data_, backward_path_length, std::numeric_limits<double>::max());
-
+    planner_data_, backward_path_length, std::numeric_limits<double>::max(),
+    /*until_goal_lane*/ true);
   // find candidate paths
   auto pull_out_paths = calcPullOutPaths(
     *route_handler, road_lanes, start_pose, goal_pose, common_parameters, parameters_);
@@ -216,14 +216,6 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
     const double before_shifted_pull_out_distance =
       std::max(pull_out_distance, pull_out_distance_converted);
 
-    // check has enough distance
-    const bool is_in_goal_route_section = route_handler.isInGoalRouteSection(road_lanes.back());
-    if (!hasEnoughDistance(
-          before_shifted_pull_out_distance, road_lanes, start_pose, is_in_goal_route_section,
-          goal_pose)) {
-      continue;
-    }
-
     // if before_shifted_pull_out_distance is too short, shifting path fails, so add non shifted
     if (before_shifted_pull_out_distance < RESAMPLE_INTERVAL && !has_non_shifted_path) {
       candidate_paths.push_back(non_shifted_path);
@@ -319,25 +311,6 @@ double ShiftPullOut::calcPullOutLongitudinalDistance(
   return min_pull_out_distance;
 }
 
-bool ShiftPullOut::hasEnoughDistance(
-  const double pull_out_total_distance, const lanelet::ConstLanelets & road_lanes,
-  const Pose & current_pose, const bool is_in_goal_route_section, const Pose & goal_pose)
-{
-  // the goal is far so current_lanes do not include goal's lane
-  if (pull_out_total_distance > utils::getDistanceToEndOfLane(current_pose, road_lanes)) {
-    return false;
-  }
-
-  // current_lanes include goal's lane
-  if (
-    is_in_goal_route_section &&
-    pull_out_total_distance > utils::getSignedDistance(current_pose, goal_pose, road_lanes)) {
-    return false;
-  }
-
-  return true;
-}
-
 double ShiftPullOut::calcBeforeShiftedArcLength(
   const PathWithLaneId & path, const double target_after_arc_length, const double dr)
 {
diff --git a/planning/behavior_path_planner/src/utils/start_planner/util.cpp b/planning/behavior_path_planner/src/utils/start_planner/util.cpp
index 2a9ab676211fb..856385be8d970 100644
--- a/planning/behavior_path_planner/src/utils/start_planner/util.cpp
+++ b/planning/behavior_path_planner/src/utils/start_planner/util.cpp
@@ -114,6 +114,8 @@ lanelet::ConstLanelets getPullOutLanes(
 
   // pull out from road lane
   return utils::getExtendedCurrentLanes(
-    planner_data, backward_length, /*forward_length*/ std::numeric_limits<double>::max());
+    planner_data, backward_length,
+    /*forward_length*/ std::numeric_limits<double>::max(),
+    /*until_goal_lane*/ true);
 }
 }  // namespace behavior_path_planner::start_planner_utils
diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp
index 44b575e2cdf44..55beafa8a3ea1 100644
--- a/planning/behavior_path_planner/src/utils/utils.cpp
+++ b/planning/behavior_path_planner/src/utils/utils.cpp
@@ -2864,7 +2864,14 @@ lanelet::ConstLanelets extendNextLane(
   // Add next lane
   const auto next_lanes = route_handler->getNextLanelets(extended_lanes.back());
   if (!next_lanes.empty()) {
-    extended_lanes.push_back(next_lanes.front());
+    // use the next lane in route if it exists
+    auto target_next_lane = next_lanes.front();
+    for (const auto & next_lane : next_lanes) {
+      if (route_handler->isRouteLanelet(next_lane)) {
+        target_next_lane = next_lane;
+      }
+    }
+    extended_lanes.push_back(target_next_lane);
   }
 
   return extended_lanes;
@@ -2878,9 +2885,15 @@ lanelet::ConstLanelets extendPrevLane(
   // Add previous lane
   const auto prev_lanes = route_handler->getPreviousLanelets(extended_lanes.front());
   if (!prev_lanes.empty()) {
-    extended_lanes.insert(extended_lanes.begin(), prev_lanes.front());
+    // use the previous lane in route if it exists
+    auto target_prev_lane = prev_lanes.front();
+    for (const auto & prev_lane : prev_lanes) {
+      if (route_handler->isRouteLanelet(prev_lane)) {
+        target_prev_lane = prev_lane;
+      }
+    }
+    extended_lanes.insert(extended_lanes.begin(), target_prev_lane);
   }
-
   return extended_lanes;
 }
 
@@ -2895,26 +2908,24 @@ lanelet::ConstLanelets extendLanes(
 
 lanelet::ConstLanelets getExtendedCurrentLanes(
   const std::shared_ptr<const PlannerData> & planner_data, const double backward_length,
-  const double forward_length)
+  const double forward_length, const bool until_goal_lane)
 {
   auto lanes = getCurrentLanes(planner_data);
   if (lanes.empty()) return lanes;
+  const auto start_lane = lanes.front();
 
   double forward_length_sum = 0.0;
   double backward_length_sum = 0.0;
 
-  const auto is_loop = [&](const auto & target_lane) {
-    auto it = std::find_if(lanes.begin(), lanes.end(), [&](const lanelet::ConstLanelet & lane) {
-      return lane.id() == target_lane.id();
-    });
-
-    return it != lanes.end();
-  };
-
   while (backward_length_sum < backward_length) {
     auto extended_lanes = extendPrevLane(planner_data->route_handler, lanes);
-
-    if (extended_lanes.empty() || is_loop(extended_lanes.front())) {
+    if (extended_lanes.empty()) {
+      return lanes;
+    }
+    // loop check
+    // if current map lanes is looping and has a very large value for backward_length,
+    // the extending process will not finish.
+    if (extended_lanes.front().id() == start_lane.id()) {
       return lanes;
     }
 
@@ -2927,9 +2938,25 @@ lanelet::ConstLanelets getExtendedCurrentLanes(
   }
 
   while (forward_length_sum < forward_length) {
-    auto extended_lanes = extendNextLane(planner_data->route_handler, lanes);
+    // stop extending if the goal lane is included
+    // if forward_length is a very large value, set it to true,
+    // as it may continue to extend lanes outside the route ahead of goal forever.
+    if (until_goal_lane) {
+      lanelet::ConstLanelet goal_lane;
+      planner_data->route_handler->getGoalLanelet(&goal_lane);
+      if (lanes.back().id() == goal_lane.id()) {
+        return lanes;
+      }
+    }
 
-    if (extended_lanes.empty() || is_loop(extended_lanes.back())) {
+    auto extended_lanes = extendNextLane(planner_data->route_handler, lanes);
+    if (extended_lanes.empty()) {
+      return lanes;
+    }
+    // loop check
+    // if current map lanes is looping and has a very large value for forward_length,
+    // the extending process will not finish.
+    if (extended_lanes.back().id() == start_lane.id()) {
       return lanes;
     }
 
diff --git a/planning/behavior_velocity_intersection_module/CMakeLists.txt b/planning/behavior_velocity_intersection_module/CMakeLists.txt
index 3e6d5bc81ff92..8ff78dac06716 100644
--- a/planning/behavior_velocity_intersection_module/CMakeLists.txt
+++ b/planning/behavior_velocity_intersection_module/CMakeLists.txt
@@ -5,6 +5,8 @@ find_package(autoware_cmake REQUIRED)
 autoware_package()
 pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml)
 
+find_package(OpenCV REQUIRED)
+
 ament_auto_add_library(${PROJECT_NAME} SHARED
   src/debug.cpp
   src/manager.cpp
@@ -13,4 +15,8 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/util.cpp
 )
 
+target_link_libraries(${PROJECT_NAME}
+  ${OpenCV_LIBRARIES}
+)
+
 ament_auto_package(INSTALL_TO_SHARE config)
diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml
index efe0efa3d7310..cbd4d4879753b 100644
--- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml
+++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml
@@ -46,7 +46,7 @@
         min_vehicle_brake_for_rss: -2.5 # [m/s^2]
         max_vehicle_velocity_for_rss: 16.66 # [m/s] == 60kmph
         denoise_kernel: 1.0 # [m]
-        pub_debug_grid: false
+        possible_object_bbox: [1.0, 2.0] # [m x m]
 
       enable_rtc: # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval
         intersection: true
diff --git a/planning/behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_intersection_module/package.xml
index 25df9f2385d74..1d6067d4be322 100644
--- a/planning/behavior_velocity_intersection_module/package.xml
+++ b/planning/behavior_velocity_intersection_module/package.xml
@@ -20,14 +20,9 @@
   <depend>autoware_auto_perception_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>
   <depend>behavior_velocity_planner_common</depend>
-  <depend>cv_bridge</depend>
   <depend>geometry_msgs</depend>
-  <depend>grid_map_cv</depend>
-  <depend>grid_map_ros</depend>
   <depend>interpolation</depend>
   <depend>lanelet2_extension</depend>
-  <depend>libboost-dev</depend>
-  <depend>libopencv-dev</depend>
   <depend>magic_enum</depend>
   <depend>motion_utils</depend>
   <depend>nav_msgs</depend>
diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp
index dc5b2b8373f04..ae8eee39d6556 100644
--- a/planning/behavior_velocity_intersection_module/src/debug.cpp
+++ b/planning/behavior_velocity_intersection_module/src/debug.cpp
@@ -221,6 +221,14 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray(
       &debug_marker_array, now);
   }
 
+  for (size_t j = 0; j < debug_data_.occlusion_polygons.size(); ++j) {
+    const auto & p = debug_data_.occlusion_polygons.at(j);
+    appendMarkerArray(
+      debug::createPolygonMarkerArray(
+        p, "occlusion_polygons", lane_id_ + j, now, 0.3, 0.0, 0.0, 1.0, 0.0, 0.0),
+      &debug_marker_array, now);
+  }
+
   return debug_marker_array;
 }
 
diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp
index 8064ccbd33430..336087751d9aa 100644
--- a/planning/behavior_velocity_intersection_module/src/manager.cpp
+++ b/planning/behavior_velocity_intersection_module/src/manager.cpp
@@ -113,7 +113,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
   ip.occlusion.max_vehicle_velocity_for_rss =
     node.declare_parameter<double>(ns + ".occlusion.max_vehicle_velocity_for_rss");
   ip.occlusion.denoise_kernel = node.declare_parameter<double>(ns + ".occlusion.denoise_kernel");
-  ip.occlusion.pub_debug_grid = node.declare_parameter<bool>(ns + ".occlusion.pub_debug_grid");
+  ip.occlusion.possible_object_bbox =
+    node.declare_parameter<std::vector<double>>(ns + ".occlusion.possible_object_bbox");
 }
 
 void IntersectionModuleManager::launchNewModules(
diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
index 086e844cd5e81..d4db6e83861a2 100644
--- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
+++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
@@ -12,26 +12,17 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <grid_map_cv/grid_map_cv.hpp>
-#include <grid_map_ros/grid_map_ros.hpp>
-#include <lanelet2_extension/regulatory_elements/road_marking.hpp>
-#include <lanelet2_extension/utility/message_conversion.hpp>
-#include <lanelet2_extension/utility/query.hpp>
-#include <lanelet2_extension/utility/utilities.hpp>
-
-#if __has_include(<cv_bridge/cv_bridge.hpp>)
-#include <cv_bridge/cv_bridge.hpp>
-#else
-#include <cv_bridge/cv_bridge.h>
-#endif
-// #include <sensor_msgs/image_encodings.h>
-// #include <opencv2/highgui/highgui.hpp>
 #include "scene_intersection.hpp"
+
 #include "util.hpp"
 
 #include <behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp>
 #include <behavior_velocity_planner_common/utilization/path_utilization.hpp>
 #include <behavior_velocity_planner_common/utilization/util.hpp>
+#include <lanelet2_extension/regulatory_elements/road_marking.hpp>
+#include <lanelet2_extension/utility/message_conversion.hpp>
+#include <lanelet2_extension/utility/query.hpp>
+#include <lanelet2_extension/utility/utilities.hpp>
 #include <magic_enum.hpp>
 #include <opencv2/imgproc.hpp>
 
@@ -93,10 +84,6 @@ IntersectionModule::IntersectionModule(
     planner_param_.collision_detection.state_transit_margin_time);
   before_creep_state_machine_.setMarginTime(planner_param_.occlusion.before_creep_stop_time);
   before_creep_state_machine_.setState(StateMachine::State::STOP);
-  if (enable_occlusion_detection) {
-    occlusion_grid_pub_ = node_.create_publisher<grid_map_msgs::msg::GridMap>(
-      "~/debug/intersection/occlusion_grid", rclcpp::QoS(1).transient_local());
-  }
   stuck_private_area_timeout_.setMarginTime(planner_param_.stuck_vehicle.timeout_private_area);
   stuck_private_area_timeout_.setState(StateMachine::State::STOP);
 }
@@ -1176,7 +1163,7 @@ bool IntersectionModule::isOcclusionCleared(
     }
   }
   for (const auto & poly : attention_area_cv_polygons) {
-    cv::fillPoly(attention_mask, poly, cv::Scalar(255), cv::LINE_AA);
+    cv::fillPoly(attention_mask, poly, cv::Scalar(255), cv::LINE_4);
   }
   // (1.1)
   // reset adjacent_lanelets area to 0 on attention_mask
@@ -1201,8 +1188,8 @@ bool IntersectionModule::isOcclusionCleared(
     for (const auto & common_area : common_areas) {
       std::vector<cv::Point> adjacent_lane_cv_polygon;
       for (const auto & p : common_area.outer()) {
-        const int idx_x = static_cast<int>((p.x() - origin.x) / resolution);
-        const int idx_y = static_cast<int>((p.y() - origin.y) / resolution);
+        const int idx_x = std::floor<int>((p.x() - origin.x) / resolution);
+        const int idx_y = std::floor<int>((p.y() - origin.y) / resolution);
         adjacent_lane_cv_polygon.emplace_back(idx_x, height - 1 - idx_y);
       }
       adjacent_lane_cv_polygons.push_back(adjacent_lane_cv_polygon);
@@ -1231,7 +1218,7 @@ bool IntersectionModule::isOcclusionCleared(
   cv::bitwise_and(attention_mask, unknown_mask, occlusion_mask_raw);
   // (3.1) apply morphologyEx
   cv::Mat occlusion_mask;
-  const int morph_size = std::ceil(planner_param_.occlusion.denoise_kernel / resolution);
+  const int morph_size = static_cast<int>(planner_param_.occlusion.denoise_kernel / resolution);
   cv::morphologyEx(
     occlusion_mask_raw, occlusion_mask, cv::MORPH_OPEN,
     cv::getStructuringElement(cv::MORPH_RECT, cv::Size(morph_size, morph_size)));
@@ -1337,59 +1324,70 @@ bool IntersectionModule::isOcclusionCleared(
     }
   }
 
-  // clean-up and find nearest risk
+  const auto & possible_object_bbox = planner_param_.occlusion.possible_object_bbox;
+  const double possible_object_bbox_x = possible_object_bbox.at(0) / resolution;
+  const double possible_object_bbox_y = possible_object_bbox.at(1) / resolution;
+  const double possible_object_area = possible_object_bbox_x * possible_object_bbox_y;
+  std::vector<std::vector<cv::Point>> contours;
+  cv::findContours(occlusion_mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
+  std::vector<std::vector<cv::Point>> valid_contours;
+  for (const auto & contour : contours) {
+    std::vector<cv::Point> valid_contour;
+    for (const auto & p : contour) {
+      if (distance_grid.at<unsigned char>(p.y, p.x) == undef_pixel) {
+        continue;
+      }
+      valid_contour.push_back(p);
+    }
+    if (valid_contour.size() <= 2) {
+      continue;
+    }
+    std::vector<cv::Point> approx_contour;
+    cv::approxPolyDP(
+      valid_contour, approx_contour,
+      std::round(std::min(possible_object_bbox_x, possible_object_bbox_y) / std::sqrt(2.0)), true);
+    if (approx_contour.size() <= 1) continue;
+    // check area
+    const double poly_area = cv::contourArea(approx_contour);
+    if (poly_area < possible_object_area) continue;
+
+    valid_contours.push_back(approx_contour);
+    geometry_msgs::msg::Polygon polygon_msg;
+    geometry_msgs::msg::Point32 point_msg;
+    for (const auto & p : approx_contour) {
+      const double glob_x = (p.x + 0.5) * resolution + origin.x;
+      const double glob_y = (height - 0.5 - p.y) * resolution + origin.y;
+      point_msg.x = glob_x;
+      point_msg.y = glob_y;
+      point_msg.z = origin.z;
+      polygon_msg.points.push_back(point_msg);
+    }
+    debug_data_.occlusion_polygons.push_back(polygon_msg);
+  }
+
   const int min_cost_thr = dist2pixel(occlusion_dist_thr);
   int min_cost = undef_pixel - 1;
-  int max_cost = 0;
-  std::optional<int> min_cost_projection_ind = std::nullopt;
   geometry_msgs::msg::Point nearest_occlusion_point;
-  for (int i = 0; i < width; ++i) {
-    for (int j = 0; j < height; ++j) {
-      const int pixel = static_cast<int>(distance_grid.at<unsigned char>(height - 1 - j, i));
-      const bool occluded = (occlusion_mask.at<unsigned char>(height - 1 - j, i) == 255);
+  for (const auto & occlusion_contour : valid_contours) {
+    for (const auto & p : occlusion_contour) {
+      const int pixel = static_cast<int>(distance_grid.at<unsigned char>(p.y, p.x));
+      const bool occluded = (occlusion_mask.at<unsigned char>(p.y, p.x) == 255);
       if (pixel == undef_pixel || !occluded) {
-        // ignore outside of cropped
-        // some cell maybe undef still
-        distance_grid.at<unsigned char>(height - 1 - j, i) = 0;
         continue;
       }
-      if (max_cost < pixel) {
-        max_cost = pixel;
-      }
-      const int projection_ind = projection_ind_grid.at<int>(height - 1 - j, i);
       if (pixel < min_cost) {
-        assert(projection_ind >= 0);
         min_cost = pixel;
-        min_cost_projection_ind = projection_ind;
-        nearest_occlusion_point.x = origin.x + i * resolution;
-        nearest_occlusion_point.y = origin.y + j * resolution;
-        nearest_occlusion_point.z = origin.z + distance_max * pixel / max_cost_pixel;
+        nearest_occlusion_point.x = origin.x + p.x * resolution;
+        nearest_occlusion_point.y = origin.y + (height - 1 - p.y) * resolution;
+        nearest_occlusion_point.z = origin.z;
       }
     }
   }
-  debug_data_.nearest_occlusion_point = nearest_occlusion_point;
-
-  if (planner_param_.occlusion.pub_debug_grid) {
-    cv::Mat distance_grid_heatmap;
-    cv::applyColorMap(distance_grid, distance_grid_heatmap, cv::COLORMAP_JET);
-    grid_map::GridMap occlusion_grid({"elevation"});
-    occlusion_grid.setFrameId("map");
-    occlusion_grid.setGeometry(
-      grid_map::Length(width * resolution, height * resolution), resolution,
-      grid_map::Position(origin.x + width * resolution / 2, origin.y + height * resolution / 2));
-    cv::rotate(distance_grid, distance_grid, cv::ROTATE_90_COUNTERCLOCKWISE);
-    cv::rotate(distance_grid_heatmap, distance_grid_heatmap, cv::ROTATE_90_COUNTERCLOCKWISE);
-    grid_map::GridMapCvConverter::addLayerFromImage<unsigned char, 1>(
-      distance_grid, "elevation", occlusion_grid, origin.z /* elevation for 0 */,
-      origin.z + distance_max /* elevation for 255 */);
-    grid_map::GridMapCvConverter::addColorLayerFromImage<unsigned char, 3>(
-      distance_grid_heatmap, "color", occlusion_grid);
-    occlusion_grid_pub_->publish(grid_map::GridMapRosConverter::toMessage(occlusion_grid));
-  }
 
-  if (min_cost > min_cost_thr || !min_cost_projection_ind.has_value()) {
+  if (min_cost > min_cost_thr) {
     return true;
   } else {
+    debug_data_.nearest_occlusion_point = nearest_occlusion_point;
     return false;
   }
 }
@@ -1418,9 +1416,8 @@ bool IntersectionModule::checkFrontVehicleDeceleration(
   // get the nearest centerpoint to object
   std::vector<double> dist_obj_center_points;
   for (const auto & p : center_points)
-    dist_obj_center_points.push_back(tier4_autoware_utils::calcDistance2d(object_pose.position, p));
-  const int obj_closest_centerpoint_idx = std::distance(
-    dist_obj_center_points.begin(),
+    dist_obj_center_points.push_back(tier4_autoware_utils::calcDistance2d(object_pose.position,
+p)); const int obj_closest_centerpoint_idx = std::distance( dist_obj_center_points.begin(),
     std::min_element(dist_obj_center_points.begin(), dist_obj_center_points.end()));
   // find two center_points whose distances from `closest_centerpoint` cross stopping_distance
   double acc_dist_prev = 0.0, acc_dist = 0.0;
diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
index 893761e834f3a..950cd6006cbe4 100644
--- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
+++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
@@ -20,13 +20,11 @@
 #include <behavior_velocity_planner_common/scene_module_interface.hpp>
 #include <behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp>
 #include <behavior_velocity_planner_common/utilization/state_machine.hpp>
-#include <grid_map_core/grid_map_core.hpp>
 #include <motion_utils/motion_utils.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
-#include <grid_map_msgs/msg/grid_map.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_routing/RoutingGraph.h>
@@ -108,7 +106,7 @@ class IntersectionModule : public SceneModuleInterface
       double min_vehicle_brake_for_rss;
       double max_vehicle_velocity_for_rss;
       double denoise_kernel;
-      bool pub_debug_grid;
+      std::vector<double> possible_object_bbox;
     } occlusion;
   };
 
@@ -274,7 +272,6 @@ class IntersectionModule : public SceneModuleInterface
   */
 
   util::DebugData debug_data_;
-  rclcpp::Publisher<grid_map_msgs::msg::GridMap>::SharedPtr occlusion_grid_pub_;
 };
 
 }  // namespace behavior_velocity_planner
diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp
index 4a87b993dbe73..55e6308d7e67b 100644
--- a/planning/behavior_velocity_intersection_module/src/util_type.hpp
+++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp
@@ -49,6 +49,7 @@ struct DebugData
   autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets;
   std::optional<geometry_msgs::msg::Point> nearest_occlusion_point = std::nullopt;
   std::optional<geometry_msgs::msg::Point> nearest_occlusion_projection_point = std::nullopt;
+  std::vector<geometry_msgs::msg::Polygon> occlusion_polygons;
 };
 
 struct InterpolatedPathInfo
diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt
index 7bb80164bb9a0..7b9ad47d3782c 100644
--- a/system/default_ad_api/CMakeLists.txt
+++ b/system/default_ad_api/CMakeLists.txt
@@ -10,6 +10,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/localization.cpp
   src/motion.cpp
   src/operation_mode.cpp
+  src/perception.cpp
   src/planning.cpp
   src/routing.cpp
   src/vehicle.cpp
@@ -25,6 +26,7 @@ rclcpp_components_register_nodes(${PROJECT_NAME}
   "default_ad_api::LocalizationNode"
   "default_ad_api::MotionNode"
   "default_ad_api::OperationModeNode"
+  "default_ad_api::PerceptionNode"
   "default_ad_api::PlanningNode"
   "default_ad_api::RoutingNode"
   "default_ad_api::VehicleNode"
diff --git a/system/default_ad_api/launch/default_ad_api.launch.py b/system/default_ad_api/launch/default_ad_api.launch.py
index fdb79d517206d..57a8557eb7e0a 100644
--- a/system/default_ad_api/launch/default_ad_api.launch.py
+++ b/system/default_ad_api/launch/default_ad_api.launch.py
@@ -47,6 +47,7 @@ def generate_launch_description():
         create_api_node("localization", "LocalizationNode"),
         create_api_node("motion", "MotionNode"),
         create_api_node("operation_mode", "OperationModeNode"),
+        create_api_node("perception", "PerceptionNode"),
         create_api_node("planning", "PlanningNode"),
         create_api_node("routing", "RoutingNode"),
         create_api_node("vehicle", "VehicleNode"),
diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml
index 032335fdddfd2..dfceeabe41122 100644
--- a/system/default_ad_api/package.xml
+++ b/system/default_ad_api/package.xml
@@ -28,6 +28,7 @@
   <depend>nav_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
+  <depend>shape_msgs</depend>
   <depend>std_srvs</depend>
   <depend>tier4_api_msgs</depend>
   <depend>tier4_control_msgs</depend>
diff --git a/system/default_ad_api/src/perception.cpp b/system/default_ad_api/src/perception.cpp
new file mode 100644
index 0000000000000..61d09444c70a4
--- /dev/null
+++ b/system/default_ad_api/src/perception.cpp
@@ -0,0 +1,93 @@
+// Copyright 2022 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 "perception.hpp"
+
+#include <vector>
+
+namespace default_ad_api
+{
+
+using DynamicObjectArray = autoware_ad_api::perception::DynamicObjectArray;
+using ObjectClassification = autoware_adapi_v1_msgs::msg::ObjectClassification;
+using DynamicObject = autoware_adapi_v1_msgs::msg::DynamicObject;
+using DynamicObjectPath = autoware_adapi_v1_msgs::msg::DynamicObjectPath;
+using API_Shape = shape_msgs::msg::SolidPrimitive;
+using Shape = autoware_auto_perception_msgs::msg::Shape;
+
+std::unordered_map<uint8_t, uint8_t> shape_type_ = {
+  {Shape::BOUNDING_BOX, API_Shape::BOX},
+  {Shape::CYLINDER, API_Shape::CYLINDER},
+  {Shape::POLYGON, API_Shape::PRISM},
+};
+
+PerceptionNode::PerceptionNode(const rclcpp::NodeOptions & options) : Node("perception", options)
+{
+  const auto adaptor = component_interface_utils::NodeAdaptor(this);
+  adaptor.init_pub(pub_object_recognized_);
+  adaptor.init_sub(sub_object_recognized_, this, &PerceptionNode::object_recognize);
+}
+
+uint8_t PerceptionNode::mapping(
+  std::unordered_map<uint8_t, uint8_t> hash_map, uint8_t input, uint8_t default_value)
+{
+  if (hash_map.find(input) == hash_map.end()) {
+    return default_value;
+  } else {
+    return hash_map[input];
+  }
+}
+
+void PerceptionNode::object_recognize(
+  const perception_interface::ObjectRecognition::Message::ConstSharedPtr msg)
+{
+  DynamicObjectArray::Message objects;
+  objects.header = msg->header;
+  for (const auto & msg_object : msg->objects) {
+    DynamicObject object;
+    object.id = msg_object.object_id;
+    object.existence_probability = msg_object.existence_probability;
+    for (const auto & msg_classification : msg_object.classification) {
+      ObjectClassification classification;
+      classification.label = msg_classification.label;
+      classification.probability = msg_classification.probability;
+      object.classification.insert(object.classification.begin(), classification);
+    }
+    object.kinematics.pose = msg_object.kinematics.initial_pose_with_covariance.pose;
+    object.kinematics.twist = msg_object.kinematics.initial_twist_with_covariance.twist;
+    object.kinematics.accel = msg_object.kinematics.initial_acceleration_with_covariance.accel;
+    for (const auto & msg_predicted_path : msg_object.kinematics.predicted_paths) {
+      DynamicObjectPath predicted_path;
+      for (const auto & msg_path : msg_predicted_path.path) {
+        predicted_path.path.insert(predicted_path.path.begin(), msg_path);
+      }
+      predicted_path.time_step = msg_predicted_path.time_step;
+      predicted_path.confidence = msg_predicted_path.confidence;
+      object.kinematics.predicted_paths.insert(
+        object.kinematics.predicted_paths.begin(), predicted_path);
+    }
+    object.shape.type = mapping(shape_type_, msg_object.shape.type, API_Shape::PRISM);
+    object.shape.dimensions = {
+      msg_object.shape.dimensions.x, msg_object.shape.dimensions.y, msg_object.shape.dimensions.z};
+    object.shape.polygon = msg_object.shape.footprint;
+    objects.objects.insert(objects.objects.begin(), object);
+  }
+
+  pub_object_recognized_->publish(objects);
+}
+
+}  // namespace default_ad_api
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::PerceptionNode)
diff --git a/system/default_ad_api/src/perception.hpp b/system/default_ad_api/src/perception.hpp
new file mode 100644
index 0000000000000..d1c71fdb08025
--- /dev/null
+++ b/system/default_ad_api/src/perception.hpp
@@ -0,0 +1,52 @@
+// Copyright 2022 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 PERCEPTION_HPP_
+#define PERCEPTION_HPP_
+
+#include <autoware_ad_api_specs/perception.hpp>
+#include <component_interface_specs/perception.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/dynamic_object.hpp>
+#include <autoware_adapi_v1_msgs/msg/dynamic_object_path.hpp>
+#include <autoware_adapi_v1_msgs/msg/object_classification.hpp>
+#include <autoware_auto_perception_msgs/msg/shape.hpp>
+#include <shape_msgs/msg/solid_primitive.hpp>
+
+#include <unordered_map>
+#include <vector>
+
+// This file should be included after messages.
+#include "utils/types.hpp"
+
+namespace default_ad_api
+{
+
+class PerceptionNode : public rclcpp::Node
+{
+public:
+  explicit PerceptionNode(const rclcpp::NodeOptions & options);
+
+private:
+  Pub<autoware_ad_api::perception::DynamicObjectArray> pub_object_recognized_;
+  Sub<perception_interface::ObjectRecognition> sub_object_recognized_;
+  void object_recognize(const perception_interface::ObjectRecognition::Message::ConstSharedPtr msg);
+  uint8_t mapping(
+    std::unordered_map<uint8_t, uint8_t> hash_map, uint8_t input, uint8_t default_value);
+};
+
+}  // namespace default_ad_api
+
+#endif  // PERCEPTION_HPP_