diff --git a/.github/workflows/cppcheck-differential.yaml b/.github/workflows/cppcheck-differential.yaml
index 99f8aa1311b5e..7fe37ea006596 100644
--- a/.github/workflows/cppcheck-differential.yaml
+++ b/.github/workflows/cppcheck-differential.yaml
@@ -31,37 +31,33 @@ jobs:
         run: git fetch origin ${{ github.base_ref }}
         shell: bash
 
-      - name: Get changed files
-        id: changed-files
+      - name: Get changed files (existing files only)
+        id: get-changed-files
         run: |
-          git diff --name-only "origin/${{ github.base_ref }}"...HEAD > changed_files.txt
-          cat changed_files.txt
+          echo "changed-files=$(git diff --name-only "origin/${{ github.base_ref }}"...HEAD | grep -E '\.(cpp|hpp)$' | while read -r file; do [ -e "$file" ] && echo -n "$file "; done)" >> $GITHUB_OUTPUT
         shell: bash
 
       - name: Run Cppcheck on changed files
+        if: ${{ steps.get-changed-files.outputs.changed-files != '' }}
         continue-on-error: true
         id: cppcheck
         run: |
-          files=$(cat changed_files.txt | grep -E '\.(cpp|hpp)$' || true)
-          if [ -n "$files" ]; then
-            echo "Running Cppcheck on changed files: $files"
-            cppcheck --enable=all --inconclusive --check-level=exhaustive --error-exitcode=1 --suppressions-list=.cppcheck_suppressions $files 2> cppcheck-report.txt
-          else
-            echo "No C++ files changed."
-            touch cppcheck-report.txt
-          fi
+          echo "Running Cppcheck on changed files: ${{ steps.get-changed-files.outputs.changed-files }}"
+          cppcheck --enable=all --inconclusive --check-level=exhaustive --error-exitcode=1 --suppressions-list=.cppcheck_suppressions ${{ steps.get-changed-files.outputs.changed-files }} 2> cppcheck-report.txt
         shell: bash
 
       - name: Show cppcheck-report result
+        if: ${{ steps.get-changed-files.outputs.changed-files != '' }}
         run: |
           cat cppcheck-report.txt
 
       - name: Upload Cppcheck report
+        if: ${{ steps.get-changed-files.outputs.changed-files != '' }}
         uses: actions/upload-artifact@v2
         with:
           name: cppcheck-report
           path: cppcheck-report.txt
 
       - name: Fail the job if Cppcheck failed
-        if: steps.cppcheck.outcome == 'failure'
+        if: ${{ steps.get-changed-files.outputs.changed-files != '' && steps.cppcheck.outcome == 'failure' }}
         run: exit 1
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml
index 228b4c89f95d3..373e343ad64fa 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml
@@ -1,5 +1,8 @@
 <?xml version="1.0"?>
 <launch>
+  <!-- Current namespace -->
+  <let name="ns" value="/perception/object_recognition/detection"/>
+
   <!-- Pipeline junctions -->
   <arg name="mode" description="options: `camera_lidar_radar_fusion`, `camera_lidar_fusion`, `lidar_radar_fusion`, `lidar` or `radar`"/>
   <arg name="lidar_detection_model" description="options: `transfusion`, `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
@@ -10,15 +13,13 @@
   <arg name="use_validator" description="use obstacle_pointcloud based validator"/>
   <arg name="objects_validation_method" description="options: `obstacle_pointcloud` or `occupancy_grid`"/>
   <arg name="use_low_intensity_cluster_filter"/>
+  <arg name="use_multi_channel_tracker_merger"/>
 
-  <!-- Lidar interfaces -->
+  <!-- External interfaces -->
+  <arg name="number_of_cameras"/>
+  <arg name="node/pointcloud_container"/>
   <arg name="input/pointcloud"/>
-  <arg name="input/pointcloud_map/pointcloud" default="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
   <arg name="input/obstacle_segmentation/pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
-  <arg name="node/pointcloud_container"/>
-
-  <!-- Camera interfaces -->
-  <arg name="number_of_cameras"/>
   <arg name="input/camera0/image"/>
   <arg name="input/camera0/info"/>
   <arg name="input/camera0/rois"/>
@@ -43,10 +44,21 @@
   <arg name="input/camera7/image"/>
   <arg name="input/camera7/info"/>
   <arg name="input/camera7/rois"/>
-
-  <!-- Radar interfaces -->
   <arg name="input/radar"/>
 
+  <arg name="output/objects" default="objects"/>
+
+  <!-- internal interfaces -->
+  <arg name="pointcloud_filter/output/pointcloud" default="$(var ns)/pointcloud_map_filtered/pointcloud"/>
+  <arg name="lidar_ml_detector/output/objects" default="$(var ns)/$(var lidar_detection_model)/objects"/>
+  <arg name="lidar_rule_detector/output/objects" default="$(var ns)/clustering/objects"/>
+  <arg name="camera_lidar_rule_detector/output/objects" default="$(var ns)/clustering/camera_lidar_fusion/objects"/>
+  <arg name="tracker_based_detector/output/objects" default="$(var ns)/detection_by_tracker/objects"/>
+  <arg name="lidar_object_filter/output/objects" default="$(var ns)/$(var lidar_detection_model)/validation/objects"/>
+  <arg name="camera_lidar_object_filter/output/objects" default="$(var ns)/clustering/camera_lidar_fusion/filtered/objects"/>
+  <arg name="radar_pipeline/output/objects" default="$(var ns)/radar/far_objects"/>
+  <arg name="radar_object_filter/output/objects" default="$(var ns)/radar/far_objects"/>
+
   <!-- Module switching -->
   <arg name="switch/detector/camera_lidar" default="false"/>
   <arg name="switch/detector/lidar_dnn" default="false"/>
@@ -56,35 +68,58 @@
   <arg name="switch/detector/tracker_based" default="false"/>
   <let name="switch/detector/tracker_based" value="true" if="$(var use_detection_by_tracker)"/>
 
+  <arg name="switch/filter/pointcloud" default="false"/>
+  <arg name="switch/filter/camera_lidar_object" default="false"/>
+  <arg name="switch/filter/lidar_object" default="false"/>
+
   <arg name="switch/merger/camera_lidar_radar" default="false"/>
   <arg name="switch/merger/camera_lidar" default="false"/>
   <arg name="switch/merger/lidar_radar" default="false"/>
   <arg name="switch/merger/lidar" default="false"/>
 
   <group scoped="false" if="$(eval '&quot;$(var mode)&quot;==&quot;camera_lidar_radar_fusion&quot;')">
+    <let name="switch/filter/pointcloud" value="true"/>
     <let name="switch/detector/camera_lidar" value="true"/>
     <let name="switch/detector/lidar_dnn" value="true"/>
     <let name="switch/detector/radar" value="true"/>
-    <let name="switch/merger/camera_lidar_radar" value="true"/>
+    <let name="switch/merger/camera_lidar_radar" value="true" unless="$(var use_multi_channel_tracker_merger)"/>
+    <group scoped="false" if="$(var use_multi_channel_tracker_merger)">
+      <let name="switch/filter/camera_lidar_object" value="true"/>
+      <let name="switch/filter/lidar_object" value="true"/>
+      <let name="camera_lidar_rule_detector/output/objects" value="$(var ns)/clustering/camera_lidar_fusion/unfiltered/objects"/>
+      <let name="camera_lidar_object_filter/output/objects" value="$(var ns)/clustering/camera_lidar_fusion/objects"/>
+    </group>
   </group>
   <group scoped="false" if="$(eval '&quot;$(var mode)&quot;==&quot;camera_lidar_fusion&quot;')">
+    <let name="switch/filter/pointcloud" value="true"/>
     <let name="switch/detector/camera_lidar" value="true"/>
     <let name="switch/detector/lidar_dnn" value="true"/>
-    <let name="switch/merger/camera_lidar" value="true"/>
+    <let name="switch/merger/camera_lidar" value="true" unless="$(var use_multi_channel_tracker_merger)"/>
+    <group scoped="false" if="$(var use_multi_channel_tracker_merger)">
+      <let name="switch/filter/camera_lidar_object" value="true"/>
+      <let name="switch/filter/lidar_object" value="true"/>
+      <let name="camera_lidar_rule_detector/output/objects" value="$(var ns)/clustering/camera_lidar_fusion/unfiltered/objects"/>
+      <let name="camera_lidar_object_filter/output/objects" value="$(var ns)/clustering/camera_lidar_fusion/objects"/>
+    </group>
   </group>
   <group scoped="false" if="$(eval '&quot;$(var mode)&quot;==&quot;lidar_radar_fusion&quot;')">
+    <let name="switch/filter/pointcloud" value="true"/>
     <let name="switch/detector/lidar_dnn" value="true"/>
     <let name="switch/detector/lidar_rule" value="true"/>
     <let name="switch/detector/radar" value="true"/>
-    <let name="switch/merger/lidar_radar" value="true"/>
+    <let name="switch/filter/lidar_object" value="true" if="$(var use_multi_channel_tracker_merger)"/>
+    <let name="switch/merger/lidar_radar" value="true" unless="$(var use_multi_channel_tracker_merger)"/>
   </group>
   <group scoped="false" if="$(eval '&quot;$(var mode)&quot;==&quot;lidar&quot;')">
+    <let name="switch/filter/pointcloud" value="true"/>
     <let name="switch/detector/lidar_dnn" value="true"/>
     <let name="switch/detector/lidar_rule" value="true"/>
-    <let name="switch/merger/lidar" value="true"/>
+    <let name="switch/filter/lidar_object" value="true" if="$(var use_multi_channel_tracker_merger)"/>
+    <let name="switch/merger/lidar" value="true" unless="$(var use_multi_channel_tracker_merger)"/>
   </group>
   <group scoped="false" if="$(eval '&quot;$(var mode)&quot;==&quot;radar&quot;')">
-    <let name="switch/detector/radar_only" value="true"/>
+    <let name="switch/detector/radar" value="true" if="$(var use_multi_channel_tracker_merger)"/>
+    <let name="switch/detector/radar_only" value="true" unless="$(var use_multi_channel_tracker_merger)"/>
     <let name="switch/detector/tracker_based" value="false"/>
   </group>
 
@@ -118,11 +153,12 @@
       <arg name="input/camera7/info" value="$(var input/camera7/info)"/>
       <arg name="input/camera7/rois" value="$(var input/camera7/rois)"/>
       <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
-      <arg name="input/pointcloud_map/pointcloud" value="$(var input/pointcloud_map/pointcloud)"/>
+      <arg name="input/pointcloud_map/pointcloud" value="$(var pointcloud_filter/output/pointcloud)"/>
       <arg name="input/obstacle_segmentation/pointcloud" value="$(var input/obstacle_segmentation/pointcloud)"/>
+      <arg name="output/ml_detector/objects" value="$(var lidar_ml_detector/output/objects)"/>
+      <arg name="output/rule_detector/objects" value="$(var camera_lidar_rule_detector/output/objects)"/>
       <arg name="node/pointcloud_container" value="$(var node/pointcloud_container)"/>
       <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
-      <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
       <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
       <arg name="use_roi_based_cluster" value="$(var use_roi_based_cluster)"/>
       <arg name="detection_by_tracker_param_path" value="$(var detection_by_tracker_param_path)"/>
@@ -133,6 +169,7 @@
     <!-- Lidar dnn-based detectors-->
     <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml">
       <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
+      <arg name="output/objects" value="$(var lidar_ml_detector/output/objects)"/>
       <arg name="node/pointcloud_container" value="$(var node/pointcloud_container)"/>
       <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
     </include>
@@ -141,9 +178,9 @@
     <!-- Lidar rule-based detectors-->
     <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml">
       <arg name="node/pointcloud_container" value="$(var node/pointcloud_container)"/>
-      <arg name="input/pointcloud_map/pointcloud" value="$(var input/pointcloud_map/pointcloud)"/>
+      <arg name="input/pointcloud_map/pointcloud" value="$(var pointcloud_filter/output/pointcloud)"/>
       <arg name="input/obstacle_segmentation/pointcloud" value="$(var input/obstacle_segmentation/pointcloud)"/>
-      <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
+      <arg name="output/objects" value="$(var lidar_rule_detector/output/objects)"/>
       <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
     </include>
   </group>
@@ -152,7 +189,7 @@
     <push-ros-namespace namespace="radar"/>
     <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/filter/radar_filter.launch.xml">
       <arg name="input/radar" value="$(var input/radar)"/>
-      <arg name="output/objects" value="far_objects"/>
+      <arg name="output/objects" value="$(var radar_pipeline/output/objects)"/>
       <arg name="radar_lanelet_filtering_range_param_path" value="$(var radar_lanelet_filtering_range_param_path)"/>
       <arg name="radar_crossing_objects_noise_filter_param_path" value="$(var object_recognition_detection_radar_crossing_objects_noise_filter_param_path)"/>
       <arg name="object_velocity_splitter_param_path" value="$(var object_recognition_detection_object_velocity_splitter_radar_fusion_param_path)"/>
@@ -175,10 +212,38 @@
   <!-- DetectionByTracker -->
   <group if="$(var switch/detector/tracker_based)">
     <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/detector/tracker_based_detector.launch.xml">
+      <arg name="output/objects" value="$(var tracker_based_detector/output/objects)"/>
       <arg name="detection_by_tracker_param_path" value="$(var object_recognition_detection_detection_by_tracker_param)"/>
     </include>
   </group>
 
+  <!-- Filter -->
+  <group if="$(var switch/filter/pointcloud)">
+    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/filter/pointcloud_map_filter.launch.py">
+      <arg name="input_topic" value="$(var input/obstacle_segmentation/pointcloud)"/>
+      <arg name="output_topic" value="$(var pointcloud_filter/output/pointcloud)"/>
+      <arg name="use_intra_process" value="true"/>
+      <arg name="use_multithread" value="true"/>
+      <arg name="pointcloud_container_name" value="$(var node/pointcloud_container)"/>
+      <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
+    </include>
+  </group>
+  <group if="$(var switch/filter/lidar_object)">
+    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/filter/object_validator.launch.xml">
+      <arg name="input/obstacle_pointcloud" value="$(var pointcloud_filter/output/pointcloud)"/>
+      <arg name="input/objects" value="$(var lidar_ml_detector/output/objects)"/>
+      <arg name="output/objects" value="$(var lidar_object_filter/output/objects)"/>
+      <arg name="objects_validation_method" value="obstacle_pointcloud"/>
+    </include>
+  </group>
+  <group if="$(var switch/filter/camera_lidar_object)">
+    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/filter/object_filter.launch.xml" if="$(var use_object_filter)">
+      <arg name="objects_filter_method" value="lanelet_filter"/>
+      <arg name="input/objects" value="$(var camera_lidar_rule_detector/output/objects)"/>
+      <arg name="output/objects" value="$(var camera_lidar_object_filter/output/objects)"/>
+    </include>
+  </group>
+
   <!-- Merger -->
   <group if="$(var switch/merger/camera_lidar_radar)">
     <!-- Camera-Lidar-Radar merger -->
@@ -208,8 +273,14 @@
       <arg name="input/camera7/image" value="$(var input/camera7/image)"/>
       <arg name="input/camera7/info" value="$(var input/camera7/info)"/>
       <arg name="input/camera7/rois" value="$(var input/camera7/rois)"/>
-      <arg name="input/pointcloud_map/pointcloud" value="$(var input/pointcloud_map/pointcloud)"/>
+      <arg name="input/pointcloud_map/pointcloud" value="$(var pointcloud_filter/output/pointcloud)"/>
       <arg name="input/obstacle_segmentation/pointcloud" value="$(var input/obstacle_segmentation/pointcloud)"/>
+      <arg name="input/lidar_ml/objects" value="$(var lidar_ml_detector/output/objects)"/>
+      <arg name="input/lidar_rule/objects" value="$(var camera_lidar_rule_detector/output/objects)"/>
+      <arg name="input/detection_by_tracker/objects" value="$(var tracker_based_detector/output/objects)"/>
+      <arg name="input/radar/objects" value="radar/noise_filtered_objects"/>
+      <arg name="input/radar_far/objects" value="$(var radar_pipeline/output/objects)"/>
+      <arg name="output/objects" value="$(var output/objects)"/>
       <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
       <arg name="use_radar_tracking_fusion" value="$(var use_radar_tracking_fusion)"/>
       <arg name="use_detection_by_tracker" value="$(var use_detection_by_tracker)"/>
@@ -250,8 +321,12 @@
       <arg name="input/camera7/image" value="$(var input/camera7/image)"/>
       <arg name="input/camera7/info" value="$(var input/camera7/info)"/>
       <arg name="input/camera7/rois" value="$(var input/camera7/rois)"/>
-      <arg name="input/pointcloud_map/pointcloud" value="$(var input/pointcloud_map/pointcloud)"/>
+      <arg name="input/pointcloud_map/pointcloud" value="$(var pointcloud_filter/output/pointcloud)"/>
       <arg name="input/obstacle_segmentation/pointcloud" value="$(var input/obstacle_segmentation/pointcloud)"/>
+      <arg name="input/lidar_ml/objects" value="$(var lidar_ml_detector/output/objects)"/>
+      <arg name="input/lidar_rule/objects" value="$(var camera_lidar_rule_detector/output/objects)"/>
+      <arg name="input/detection_by_tracker/objects" value="$(var tracker_based_detector/output/objects)"/>
+      <arg name="output/objects" value="$(var output/objects)"/>
       <arg name="use_object_filter" value="$(var use_object_filter)"/>
       <arg name="objects_filter_method" value="$(var objects_filter_method)"/>
       <arg name="use_validator" value="$(var use_validator)"/>
@@ -264,32 +339,41 @@
 
   <group if="$(var switch/merger/lidar_radar)">
     <!-- Lidar object merger-->
-    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/merger/lidar_merger.launch.xml">
-      <arg name="input/pointcloud_map/pointcloud" value="$(var input/pointcloud_map/pointcloud)"/>
-      <arg name="input/obstacle_segmentation/pointcloud" value="$(var input/obstacle_segmentation/pointcloud)"/>
-      <arg name="output/objects" value="lidar/objects"/>
-      <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
-      <arg name="use_detection_by_tracker" value="$(var use_detection_by_tracker)"/>
-      <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
-      <arg name="use_object_filter" value="$(var use_object_filter)"/>
-      <arg name="objects_filter_method" value="$(var objects_filter_method)"/>
-      <arg name="use_validator" value="$(var use_validator)"/>
-      <arg name="objects_validation_method" value="$(var objects_validation_method)"/>
-    </include>
+    <group>
+      <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/merger/lidar_merger.launch.xml">
+        <arg name="input/pointcloud_map/pointcloud" value="$(var pointcloud_filter/output/pointcloud)"/>
+        <arg name="input/obstacle_segmentation/pointcloud" value="$(var input/obstacle_segmentation/pointcloud)"/>
+        <arg name="input/lidar_ml/objects" value="$(var lidar_ml_detector/output/objects)"/>
+        <arg name="input/lidar_rule/objects" value="$(var lidar_rule_detector/output/objects)"/>
+        <arg name="output/objects" value="lidar/objects"/>
+        <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
+        <arg name="use_detection_by_tracker" value="$(var use_detection_by_tracker)"/>
+        <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
+        <arg name="use_object_filter" value="$(var use_object_filter)"/>
+        <arg name="objects_filter_method" value="$(var objects_filter_method)"/>
+        <arg name="use_validator" value="$(var use_validator)"/>
+        <arg name="objects_validation_method" value="$(var objects_validation_method)"/>
+      </include>
+    </group>
     <!-- Lidar-Radar object merger-->
-    <include file="$(find-pkg-share radar_fusion_to_detected_object)/launch/radar_object_fusion_to_detected_object.launch.xml">
-      <arg name="input/objects" value="lidar/objects"/>
-      <arg name="input/radars" value="radar/noise_filtered_objects"/>
-      <arg name="output/objects" value="objects"/>
-    </include>
+    <group>
+      <include file="$(find-pkg-share radar_fusion_to_detected_object)/launch/radar_object_fusion_to_detected_object.launch.xml">
+        <arg name="input/objects" value="lidar/objects"/>
+        <arg name="input/radars" value="radar/noise_filtered_objects"/>
+        <arg name="output/objects" value="$(var output/objects)"/>
+      </include>
+    </group>
   </group>
 
   <group if="$(var switch/merger/lidar)">
     <!-- Lidar object merger-->
     <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/merger/lidar_merger.launch.xml">
-      <arg name="input/pointcloud_map/pointcloud" value="$(var input/pointcloud_map/pointcloud)"/>
+      <arg name="input/pointcloud_map/pointcloud" value="$(var pointcloud_filter/output/pointcloud)"/>
       <arg name="input/obstacle_segmentation/pointcloud" value="$(var input/obstacle_segmentation/pointcloud)"/>
-      <arg name="output/objects" value="objects"/>
+      <arg name="input/lidar_ml/objects " value="$(var lidar_ml_detector/output/objects)"/>
+      <arg name="input/lidar_rule/objects" value="$(var lidar_rule_detector/output/objects)"/>
+      <arg name="input/detection_by_tracker/objects" value="$(var tracker_based_detector/output/objects)"/>
+      <arg name="output/objects" value="$(var output/objects)"/>
       <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
       <arg name="use_detection_by_tracker" value="$(var use_detection_by_tracker)"/>
       <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml
index 3c8480038f320..4c288aa632182 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml
@@ -1,17 +1,11 @@
 <?xml version="1.0"?>
 <launch>
-  <!-- Lidar parameters -->
-  <arg name="input/pointcloud"/>
-  <arg name="input/pointcloud_map/pointcloud"/>
-  <arg name="input/obstacle_segmentation/pointcloud"/>
-  <arg name="node/pointcloud_container"/>
-
   <!-- Lidar + Camera detector parameters -->
   <arg name="lidar_detection_model" description="options: `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
   <arg name="use_roi_based_cluster"/>
   <arg name="use_low_intensity_cluster_filter"/>
 
-  <!-- Camera parameters -->
+  <!-- External interfaces -->
   <arg name="number_of_cameras"/>
   <arg name="input/camera0/image"/>
   <arg name="input/camera0/info"/>
@@ -38,6 +32,14 @@
   <arg name="input/camera7/info"/>
   <arg name="input/camera7/rois"/>
 
+  <arg name="node/pointcloud_container"/>
+  <arg name="input/pointcloud"/>
+  <arg name="input/pointcloud_map/pointcloud"/>
+  <arg name="input/obstacle_segmentation/pointcloud"/>
+
+  <arg name="output/ml_detector/objects"/>
+  <arg name="output/rule_detector/objects"/>
+
   <!-- Jetson AGX -->
   <!-- <include file="$(find-pkg-share tensorrt_yolox)/launch/multiple_yolox.launch.xml">
     <arg name="image_raw0" value="$(var input/camera0/image)"/>
@@ -81,7 +83,7 @@
       <arg name="input/image6" value="$(var input/camera6/image)"/>
       <arg name="input/image7" value="$(var input/camera7/image)"/>
       <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
-      <arg name="output/objects" value="objects"/>
+      <arg name="output/objects" value="$(var output/ml_detector/objects)"/>
 
       <arg name="model_name" value="$(var lidar_detection_model)"/>
       <arg name="model_path" value="$(var pointpainting_model_path)"/>
@@ -93,18 +95,6 @@
     </include>
   </group>
 
-  <!-- Pointcloud filter -->
-  <group>
-    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/filter/pointcloud_map_filter.launch.py">
-      <arg name="input_topic" value="$(var input/obstacle_segmentation/pointcloud)"/>
-      <arg name="output_topic" value="pointcloud_map_filtered/pointcloud"/>
-      <arg name="use_intra_process" value="true"/>
-      <arg name="use_multithread" value="true"/>
-      <arg name="pointcloud_container_name" value="$(var node/pointcloud_container)"/>
-      <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
-    </include>
-  </group>
-
   <!-- Clustering -->
   <group>
     <push-ros-namespace namespace="clustering"/>
@@ -230,7 +220,7 @@
       <group>
         <include file="$(find-pkg-share detected_object_feature_remover)/launch/detected_object_feature_remover.launch.xml">
           <arg name="input" value="objects_with_feature"/>
-          <arg name="output" value="objects"/>
+          <arg name="output" value="$(var output/rule_detector/objects)"/>
         </include>
       </group>
     </group>
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml
index ab34cc49dd2f6..98e8ec7c8ed4b 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml
@@ -1,18 +1,19 @@
 <?xml version="1.0"?>
 <launch>
   <!-- Lidar parameters -->
-  <arg name="input/pointcloud"/>
-  <arg name="node/pointcloud_container"/>
   <arg name="lidar_detection_model" default="centerpoint" description="options: `transfusion`, `centerpoint`, `apollo`, `clustering`"/>
-
   <!-- Lidar detector centerpoint parameters -->
   <arg name="centerpoint_model_name" default="centerpoint_tiny" description="options: `centerpoint`, `centerpoint_tiny` or `centerpoint_sigma`"/>
   <arg name="centerpoint_model_path" default="$(var data_path)/lidar_centerpoint"/>
-
   <!-- Lidar detector transfusion parameters -->
   <arg name="transfusion_model_name" default="transfusion" description="options: `transfusion`"/>
   <arg name="transfusion_model_path" default="$(var data_path)/lidar_transfusion"/>
 
+  <!-- External interfaces -->
+  <arg name="node/pointcloud_container"/>
+  <arg name="input/pointcloud"/>
+  <arg name="output/objects"/>
+
   <!-- TransFusion -->
   <group if="$(eval &quot;'$(var lidar_detection_model)'=='transfusion'&quot;)">
     <push-ros-namespace namespace="transfusion"/>
@@ -21,7 +22,7 @@
     <group>
       <include file="$(find-pkg-share lidar_transfusion)/launch/lidar_transfusion.launch.xml">
         <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
-        <arg name="output/objects" value="objects"/>
+        <arg name="output/objects" value="$(var output/objects)"/>
         <arg name="model_name" value="$(var transfusion_model_name)"/>
         <arg name="model_path" value="$(var transfusion_model_path)"/>
         <arg name="model_param_path" value="$(var lidar_model_param_path)/$(var transfusion_model_name).param.yaml"/>
@@ -41,7 +42,7 @@
     <group>
       <include file="$(find-pkg-share lidar_centerpoint)/launch/lidar_centerpoint.launch.xml">
         <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
-        <arg name="output/objects" value="objects"/>
+        <arg name="output/objects" value="$(var output/objects)"/>
         <arg name="model_name" value="$(var centerpoint_model_name)"/>
         <arg name="model_path" value="$(var centerpoint_model_path)"/>
         <arg name="model_param_path" value="$(var lidar_model_param_path)/$(var centerpoint_model_name).param.yaml"/>
@@ -77,7 +78,7 @@
     <group>
       <include file="$(find-pkg-share detected_object_feature_remover)/launch/detected_object_feature_remover.launch.xml">
         <arg name="input" value="objects_with_feature"/>
-        <arg name="output" value="objects"/>
+        <arg name="output" value="$(var output/objects)"/>
       </include>
     </group>
   </group>
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml
index 819a6337138a1..38ee946201f7e 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml
@@ -1,22 +1,10 @@
 <?xml version="1.0"?>
 <launch>
-  <!-- Lidar parameters -->
+  <!-- External interfaces -->
+  <arg name="node/pointcloud_container"/>
   <arg name="input/pointcloud_map/pointcloud"/>
   <arg name="input/obstacle_segmentation/pointcloud"/>
-  <arg name="node/pointcloud_container"/>
-  <arg name="use_pointcloud_map"/>
-
-  <!-- Pointcloud filter -->
-  <group>
-    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/filter/pointcloud_map_filter.launch.py">
-      <arg name="input_topic" value="$(var input/obstacle_segmentation/pointcloud)"/>
-      <arg name="output_topic" value="pointcloud_map_filtered/pointcloud"/>
-      <arg name="use_intra_process" value="true"/>
-      <arg name="use_multithread" value="true"/>
-      <arg name="pointcloud_container_name" value="$(var node/pointcloud_container)"/>
-      <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
-    </include>
-  </group>
+  <arg name="output/objects"/>
 
   <!-- Clustering -->
   <group>
@@ -43,7 +31,7 @@
     <group>
       <include file="$(find-pkg-share detected_object_feature_remover)/launch/detected_object_feature_remover.launch.xml">
         <arg name="input" value="objects_with_feature"/>
-        <arg name="output" value="objects"/>
+        <arg name="output" value="$(var output/objects)"/>
       </include>
     </group>
   </group>
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/tracker_based_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/tracker_based_detector.launch.xml
index 0efe7444fcba5..b85d4b02847b3 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/tracker_based_detector.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/tracker_based_detector.launch.xml
@@ -1,10 +1,12 @@
 <?xml version="1.0"?>
 <launch>
+  <arg name="output/objects"/>
   <!-- DetectionByTracker -->
   <group>
     <push-ros-namespace namespace="detection_by_tracker"/>
     <include file="$(find-pkg-share detection_by_tracker)/launch/detection_by_tracker.launch.xml">
       <arg name="detection_by_tracker_param_path" value="$(var object_recognition_detection_detection_by_tracker_param)"/>
+      <arg name="output" value="$(var output/objects)"/>
     </include>
   </group>
 </launch>
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml
index 85b404ba89b1f..d4a9e40f29d97 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml
@@ -44,9 +44,9 @@
   <arg name="input/camera7/rois"/>
   <arg name="input/pointcloud_map/pointcloud"/>
   <arg name="input/obstacle_segmentation/pointcloud"/>
-  <arg name="input/lidar_ml/objects" default="$(var lidar_detection_model)/objects"/>
-  <arg name="input/lidar_rule/objects" default="clustering/camera_lidar_fusion/objects"/>
-  <arg name="input/detection_by_tracker/objects" default="detection_by_tracker/objects"/>
+  <arg name="input/lidar_ml/objects"/>
+  <arg name="input/lidar_rule/objects"/>
+  <arg name="input/detection_by_tracker/objects"/>
   <arg name="output/objects" default="objects"/>
 
   <!-- internal interfaces -->
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml
index 16ec8f8fbd573..6ea745a8bae2c 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml
@@ -47,16 +47,16 @@
   <arg name="input/camera7/rois"/>
   <arg name="input/pointcloud_map/pointcloud"/>
   <arg name="input/obstacle_segmentation/pointcloud"/>
-  <arg name="input/lidar_ml/objects" default="$(var lidar_detection_model)/objects"/>
-  <arg name="input/lidar_rule/objects" default="clustering/camera_lidar_fusion/objects"/>
-  <arg name="input/radar_near/objects" default="radar/noise_filtered_objects"/>
-  <arg name="input/radar_far/objects" default="radar/far_objects"/>
-  <arg name="input/detection_by_tracker/objects" default="detection_by_tracker/objects"/>
+  <arg name="input/lidar_ml/objects"/>
+  <arg name="input/lidar_rule/objects"/>
+  <arg name="input/radar/objects"/>
+  <arg name="input/radar_far/objects"/>
+  <arg name="input/detection_by_tracker/objects"/>
   <arg name="output/objects" default="objects"/>
 
   <!-- internal interfaces -->
   <let name="radar_objects_fusion/input/objects" value="$(var input/lidar_ml/objects)"/>
-  <let name="radar_objects_fusion/input/radars" value="$(var input/radar_near/objects)"/>
+  <let name="radar_objects_fusion/input/radars" value="$(var input/radar/objects)"/>
   <let name="radar_objects_fusion/output/objects" value="radar_fusion/objects"/>
 
   <let name="camera_roi_fusion/input/objects" value="$(var radar_objects_fusion/output/objects)" if="$(var use_near_radar_fusion)"/>
diff --git a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml
index b8172daa616fa..1975ad2d15eb9 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml
@@ -1,5 +1,8 @@
 <?xml version="1.0"?>
 <launch>
+  <!-- Current namespace -->
+  <let name="ns" value="/perception/object_recognition/tracking"/>
+
   <!-- Radar Tracking and Merger parameters -->
   <arg name="object_recognition_tracking_radar_object_tracker_data_association_matrix_param_path" description="association param file for radar far object tracking"/>
   <arg name="object_recognition_tracking_radar_object_tracker_tracking_setting_param_path" description="tracking setting param file for radar far object tracking"/>
@@ -11,52 +14,87 @@
   <arg name="mode" default="lidar" description="options: `camera_lidar_radar_fusion`, `camera_lidar_fusion`, `lidar_radar_fusion`, `lidar` or `radar`"/>
   <arg name="use_radar_tracking_fusion" default="false" description="use radar tracking fusion"/>
   <let name="use_radar_tracking_fusion" value="false" if="$(eval '&quot;$(var mode)&quot;!=&quot;camera_lidar_radar_fusion&quot;')"/>
+  <arg name="use_multi_channel_tracker_merger"/>
 
   <!-- External interface -->
-  <arg name="input/detection_input_channels" default="['detected_objects']"/>
-  <!-- detection_input_channels: Input channels for multi object tracker, reference: multi_object_tracker/config/input_channels.param.yaml -->
+  <let name="input/detection_input_channels" value="['detected_objects']"/>
   <arg name="input/radar/objects" default="/perception/object_recognition/detection/radar/far_objects"/>
-  <arg name="output/objects" default="/perception/object_recognition/tracking/objects"/>
-
-  <!-- Internal interface -->
-  <let name="radar_tracker/input/objects" value="$(var input/radar/objects)"/>
-  <let name="radar_tracker/output/objects" value="/perception/object_recognition/tracking/radar/far_objects"/>
-
-  <let name="multi_object_tracker/input/selected_input_channels" value="$(var input/detection_input_channels)"/>
-  <let name="multi_object_tracker/output/objects" value="$(var output/objects)" unless="$(var use_radar_tracking_fusion)"/>
-  <let name="multi_object_tracker/output/objects" value="/perception/object_recognition/tracking/near_objects" if="$(var use_radar_tracking_fusion)"/>
-
-  <let name="tracker_merger/input/main_objects" value="$(var multi_object_tracker/output/objects)"/>
-  <let name="tracker_merger/input/sub_objects" value="$(var radar_tracker/output/objects)"/>
-  <let name="tracker_merger/output/objects" value="$(var output/objects)"/>
-
-  <!--Multi object tracking-->
-  <include file="$(find-pkg-share multi_object_tracker)/launch/multi_object_tracker.launch.xml">
-    <arg name="selected_input_channels" value="$(var multi_object_tracker/input/selected_input_channels)"/>
-    <arg name="output" value="$(var multi_object_tracker/output/objects)"/>
-    <arg name="data_association_matrix_path" value="$(var object_recognition_tracking_multi_object_tracker_data_association_matrix_param_path)"/>
-    <arg name="input_channels_path" value="$(var object_recognition_tracking_multi_object_tracker_input_channels_param_path)"/>
-    <arg name="tracker_setting_path" value="$(var object_recognition_tracking_multi_object_tracker_node_param_path)"/>
-  </include>
-
-  <!-- Run with tracking merger to add far radar information -->
-  <group if="$(var use_radar_tracking_fusion)">
-    <!--radar long range dynamic object tracking-->
-    <include file="$(find-pkg-share radar_object_tracker)/launch/radar_object_tracker.launch.xml">
-      <arg name="input" value="$(var radar_tracker/input/objects)"/>
-      <arg name="output" value="$(var radar_tracker/output/objects)"/>
-      <arg name="data_association_matrix_path" value="$(var object_recognition_tracking_radar_object_tracker_data_association_matrix_param_path)"/>
-      <arg name="tracker_setting_path" value="$(var object_recognition_tracking_radar_object_tracker_tracking_setting_param_path)"/>
-      <arg name="radar_object_tracker_param_path" value="$(var object_recognition_tracking_radar_object_tracker_node_param_path)"/>
+  <arg name="output/objects" default="$(var ns)/objects"/>
+
+  <group unless="$(var use_multi_channel_tracker_merger)">
+    <!-- Internal interface -->
+    <let name="radar_tracker/input/objects" value="$(var input/radar/objects)"/>
+    <let name="radar_tracker/output/objects" value="$(var ns)/radar/far_objects"/>
+
+    <let name="multi_object_tracker/input/selected_input_channels" value="$(var input/detection_input_channels)"/>
+    <let name="multi_object_tracker/output/objects" value="$(var output/objects)" unless="$(var use_radar_tracking_fusion)"/>
+    <let name="multi_object_tracker/output/objects" value="$(var ns)/near_objects" if="$(var use_radar_tracking_fusion)"/>
+
+    <let name="tracker_merger/input/main_objects" value="$(var multi_object_tracker/output/objects)"/>
+    <let name="tracker_merger/input/sub_objects" value="$(var radar_tracker/output/objects)"/>
+    <let name="tracker_merger/output/objects" value="$(var output/objects)"/>
+
+    <!-- Multi object tracking -->
+    <include file="$(find-pkg-share multi_object_tracker)/launch/multi_object_tracker.launch.xml">
+      <arg name="selected_input_channels" value="$(var multi_object_tracker/input/selected_input_channels)"/>
+      <arg name="output" value="$(var multi_object_tracker/output/objects)"/>
+      <arg name="data_association_matrix_path" value="$(var object_recognition_tracking_multi_object_tracker_data_association_matrix_param_path)"/>
+      <arg name="input_channels_path" value="$(var object_recognition_tracking_multi_object_tracker_input_channels_param_path)"/>
+      <arg name="tracker_setting_path" value="$(var object_recognition_tracking_multi_object_tracker_node_param_path)"/>
     </include>
 
-    <!--tracking object merger to merge near objects and far objects -->
-    <include file="$(find-pkg-share tracking_object_merger)/launch/decorative_tracker_merger.launch.xml">
-      <arg name="input/main_object" value="$(var tracker_merger/input/main_objects)"/>
-      <arg name="input/sub_object" value="$(var tracker_merger/input/sub_objects)"/>
-      <arg name="output" value="$(var tracker_merger/output/objects)"/>
-      <arg name="data_association_matrix_path" value="$(var object_recognition_tracking_object_merger_data_association_matrix_param_path)"/>
-      <arg name="node_param_file_path" value="$(var object_recognition_tracking_object_merger_node_param_path)"/>
+    <!-- Run with tracking merger to add far radar information -->
+    <group if="$(var use_radar_tracking_fusion)">
+      <!-- radar long range dynamic object tracking -->
+      <include file="$(find-pkg-share radar_object_tracker)/launch/radar_object_tracker.launch.xml">
+        <arg name="input" value="$(var radar_tracker/input/objects)"/>
+        <arg name="output" value="$(var radar_tracker/output/objects)"/>
+        <arg name="data_association_matrix_path" value="$(var object_recognition_tracking_radar_object_tracker_data_association_matrix_param_path)"/>
+        <arg name="tracker_setting_path" value="$(var object_recognition_tracking_radar_object_tracker_tracking_setting_param_path)"/>
+        <arg name="radar_object_tracker_param_path" value="$(var object_recognition_tracking_radar_object_tracker_node_param_path)"/>
+      </include>
+
+      <!-- tracking object merger to merge near objects and far objects -->
+      <include file="$(find-pkg-share tracking_object_merger)/launch/decorative_tracker_merger.launch.xml">
+        <arg name="input/main_object" value="$(var tracker_merger/input/main_objects)"/>
+        <arg name="input/sub_object" value="$(var tracker_merger/input/sub_objects)"/>
+        <arg name="output" value="$(var tracker_merger/output/objects)"/>
+        <arg name="data_association_matrix_path" value="$(var object_recognition_tracking_object_merger_data_association_matrix_param_path)"/>
+        <arg name="node_param_file_path" value="$(var object_recognition_tracking_object_merger_node_param_path)"/>
+      </include>
+    </group>
+  </group>
+
+  <!-- Object list for multi-channel tracker merger -->
+  <group if="$(var use_multi_channel_tracker_merger)">
+    <let name="detection_enabled_channels" value="$(var input/detection_input_channels)"/>
+    <group scoped="false" if="$(eval '&quot;$(var mode)&quot;==&quot;camera_lidar_radar_fusion&quot;')">
+      <let name="detection_enabled_channels" value="['lidar_$(var lidar_detection_model)_validated','camera_lidar_fusion','detection_by_tracker','radar_far']" if="$(var use_detection_by_tracker)"/>
+      <let name="detection_enabled_channels" value="['lidar_$(var lidar_detection_model)_validated','camera_lidar_fusion','radar_far']" unless="$(var use_detection_by_tracker)"/>
+    </group>
+    <group scoped="false" if="$(eval '&quot;$(var mode)&quot;==&quot;camera_lidar_fusion&quot;')">
+      <let name="detection_enabled_channels" value="['lidar_$(var lidar_detection_model)_validated','camera_lidar_fusion','detection_by_tracker']" if="$(var use_detection_by_tracker)"/>
+      <let name="detection_enabled_channels" value="['lidar_$(var lidar_detection_model)_validated','camera_lidar_fusion']" unless="$(var use_detection_by_tracker)"/>
+    </group>
+    <group scoped="false" if="$(eval '&quot;$(var mode)&quot;==&quot;lidar_radar_fusion&quot;')">
+      <let name="detection_enabled_channels" value="['lidar_$(var lidar_detection_model)_validated','detection_by_tracker','radar_far']" if="$(var use_detection_by_tracker)"/>
+      <let name="detection_enabled_channels" value="['lidar_$(var lidar_detection_model)_validated','radar_far']" unless="$(var use_detection_by_tracker)"/>
+    </group>
+    <group scoped="false" if="$(eval '&quot;$(var mode)&quot;==&quot;lidar&quot;')">
+      <let name="detection_enabled_channels" value="['lidar_$(var lidar_detection_model)_validated','detection_by_tracker']" if="$(var use_detection_by_tracker)"/>
+      <let name="detection_enabled_channels" value="['lidar_$(var lidar_detection_model)_validated']" unless="$(var use_detection_by_tracker)"/>
+    </group>
+    <group scoped="false" if="$(eval '&quot;$(var mode)&quot;==&quot;radar&quot;')">
+      <let name="detection_enabled_channels" value="['radar_far']"/>
+    </group>
+
+    <!-- Multi object tracking -->
+    <include file="$(find-pkg-share multi_object_tracker)/launch/multi_object_tracker.launch.xml">
+      <arg name="selected_input_channels" value="$(var detection_enabled_channels)"/>
+      <arg name="output" value="$(var output/objects)"/>
+      <arg name="data_association_matrix_path" value="$(var object_recognition_tracking_multi_object_tracker_data_association_matrix_param_path)"/>
+      <arg name="input_channels_path" value="$(var object_recognition_tracking_multi_object_tracker_input_channels_param_path)"/>
+      <arg name="tracker_setting_path" value="$(var object_recognition_tracking_multi_object_tracker_node_param_path)"/>
     </include>
   </group>
 </launch>
diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
index 8f0513bcd5446..5d8d7563e7a7c 100644
--- a/launch/tier4_perception_launch/launch/perception.launch.xml
+++ b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -124,6 +124,9 @@
   />
   <arg name="input/radar" default="/sensing/radar/detected_objects"/>
 
+  <!-- Object merger method: whether to enable multi-channel tracker merger -->
+  <arg name="use_multi_channel_tracker_merger" default="false" description="if it is true, replace the multi step merger to multi-channel tracker merger."/>
+
   <!-- Downsample pointcloud for perception usage -->
   <arg name="downsample_perception_common_pointcloud" default="false"/>
   <arg name="common_downsample_voxel_size_x" default="0.05"/>
@@ -189,6 +192,7 @@
         <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/detection.launch.xml">
           <arg name="mode" value="$(var mode)"/>
           <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
+          <arg name="use_multi_channel_tracker_merger" value="$(var use_multi_channel_tracker_merger)"/>
           <arg name="number_of_cameras" value="$(var image_number)"/>
           <arg name="input/camera0/image" value="$(var image_raw0)"/>
           <arg name="input/camera0/info" value="$(var camera_info0)"/>
@@ -255,7 +259,9 @@
           <arg name="object_recognition_tracking_radar_object_tracker_node_param_path" value="$(var object_recognition_tracking_radar_object_tracker_node_param_path)"/>
           <arg name="object_recognition_tracking_object_merger_data_association_matrix_param_path" value="$(var object_recognition_tracking_object_merger_data_association_matrix_param_path)"/>
           <arg name="object_recognition_tracking_object_merger_node_param_path" value="$(var object_recognition_tracking_object_merger_node_param_path)"/>
+          <arg name="use_multi_channel_tracker_merger" value="$(var use_multi_channel_tracker_merger)"/>
           <arg name="use_radar_tracking_fusion" value="$(var use_radar_tracking_fusion)"/>
+          <arg name="use_detection_by_tracker" value="$(var use_detection_by_tracker)"/>
         </include>
       </group>
 
diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml
index 97b07410d108a..7dc33e0e82ea9 100644
--- a/launch/tier4_simulator_launch/launch/simulator.launch.xml
+++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml
@@ -78,6 +78,7 @@
             <arg name="object_recognition_tracking_radar_object_tracker_node_param_path" value="$(var object_recognition_tracking_radar_object_tracker_node_param_path)"/>
             <arg name="object_recognition_tracking_object_merger_data_association_matrix_param_path" value="$(var object_recognition_tracking_object_merger_data_association_matrix_param_path)"/>
             <arg name="object_recognition_tracking_object_merger_node_param_path" value="$(var object_recognition_tracking_object_merger_node_param_path)"/>
+            <arg name="use_multi_channel_tracker_merger" value="false"/>
           </include>
         </group>
         <!-- prediction module -->
diff --git a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/README.md b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/README.md
index 37596ee9820b9..14501c689dbe6 100644
--- a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/README.md
+++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/README.md
@@ -46,7 +46,7 @@ ros2 launch autoware_launch ... \
 
 ### Rosbag
 
-#### [Sample rosbag and map (AWSIM data)](https://drive.google.com/file/d/1uMVwQQFcfs8JOqfoA1FqfH_fLPwQ71jK/view)
+#### [Sample rosbag and map (AWSIM data)](https://drive.google.com/file/d/1ZPsfDvOXFrMxtx7fb1W5sOXdAK1e71hY/view)
 
 This data is simulated data created by [AWSIM](https://tier4.github.io/AWSIM/).
 Essentially, AR tag-based self-localization is not intended for such public road driving, but for driving in a smaller area, so the max driving speed is set at 15 km/h.
@@ -55,7 +55,7 @@ It is a known problem that the timing of when each AR tag begins to be detected
 
 ![sample_result_in_awsim](./doc_image/sample_result_in_awsim.png)
 
-#### [Sample rosbag and map (Real world data)](https://drive.google.com/file/d/1wiCQjyjRnYbb0dg8G6mRecdSGh8tv3zR/view)
+#### [Sample rosbag and map (Real world data)](https://drive.google.com/file/d/1VQCQ_qiEZpCMI3-z6SNs__zJ-4HJFQjx/view)
 
 Please remap the topic names and play it.
 
diff --git a/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp b/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp
index a178a305f6d12..0f293e4d31cac 100644
--- a/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp
+++ b/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp
@@ -15,6 +15,8 @@
 #ifndef LOCALIZATION_ERROR_MONITOR__LOCALIZATION_ERROR_MONITOR_HPP_
 #define LOCALIZATION_ERROR_MONITOR__LOCALIZATION_ERROR_MONITOR_HPP_
 
+#include "localization_util/covariance_ellipse.hpp"
+
 #include <Eigen/Dense>
 #include <autoware/universe_utils/ros/logger_level_configure.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -25,15 +27,6 @@
 
 #include <memory>
 
-struct Ellipse
-{
-  double long_radius;
-  double short_radius;
-  double yaw;
-  Eigen::Matrix2d P;
-  double size_lateral_direction;
-};
-
 class LocalizationErrorMonitor : public rclcpp::Node
 {
 private:
@@ -50,12 +43,9 @@ class LocalizationErrorMonitor : public rclcpp::Node
   double warn_ellipse_size_;
   double error_ellipse_size_lateral_direction_;
   double warn_ellipse_size_lateral_direction_;
-  Ellipse ellipse_;
+  autoware::localization_util::Ellipse ellipse_;
 
   void on_odom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg);
-  visualization_msgs::msg::Marker create_ellipse_marker(
-    const Ellipse & ellipse, nav_msgs::msg::Odometry::ConstSharedPtr odom);
-  static double measure_size_ellipse_along_body_frame(const Eigen::Matrix2d & Pinv, double theta);
 
 public:
   explicit LocalizationErrorMonitor(const rclcpp::NodeOptions & options);
diff --git a/localization/localization_error_monitor/package.xml b/localization/localization_error_monitor/package.xml
index 681da7e57c6c5..b8a466f95dfab 100644
--- a/localization/localization_error_monitor/package.xml
+++ b/localization/localization_error_monitor/package.xml
@@ -23,6 +23,7 @@
   <depend>autoware_universe_utils</depend>
   <depend>diagnostic_msgs</depend>
   <depend>diagnostic_updater</depend>
+  <depend>localization_util</depend>
   <depend>nav_msgs</depend>
   <depend>rclcpp_components</depend>
   <depend>tf2</depend>
diff --git a/localization/localization_error_monitor/src/localization_error_monitor.cpp b/localization/localization_error_monitor/src/localization_error_monitor.cpp
index 204afa7bdec2f..44223b0fd1670 100644
--- a/localization/localization_error_monitor/src/localization_error_monitor.cpp
+++ b/localization/localization_error_monitor/src/localization_error_monitor.cpp
@@ -58,61 +58,12 @@ LocalizationErrorMonitor::LocalizationErrorMonitor(const rclcpp::NodeOptions & o
   logger_configure_ = std::make_unique<autoware::universe_utils::LoggerLevelConfigure>(this);
 }
 
-visualization_msgs::msg::Marker LocalizationErrorMonitor::create_ellipse_marker(
-  const Ellipse & ellipse, nav_msgs::msg::Odometry::ConstSharedPtr odom)
-{
-  tf2::Quaternion quat;
-  quat.setEuler(0, 0, ellipse.yaw);
-
-  const double ellipse_long_radius = std::min(ellipse.long_radius, 30.0);
-  const double ellipse_short_radius = std::min(ellipse.short_radius, 30.0);
-  visualization_msgs::msg::Marker marker;
-  marker.header = odom->header;
-  marker.header.stamp = this->now();
-  marker.ns = "error_ellipse";
-  marker.id = 0;
-  marker.type = visualization_msgs::msg::Marker::SPHERE;
-  marker.action = visualization_msgs::msg::Marker::ADD;
-  marker.pose = odom->pose.pose;
-  marker.pose.orientation = tf2::toMsg(quat);
-  marker.scale.x = ellipse_long_radius * 2;
-  marker.scale.y = ellipse_short_radius * 2;
-  marker.scale.z = 0.01;
-  marker.color.a = 0.1;
-  marker.color.r = 0.0;
-  marker.color.g = 0.0;
-  marker.color.b = 1.0;
-  return marker;
-}
-
 void LocalizationErrorMonitor::on_odom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg)
 {
-  // create xy covariance (2x2 matrix)
-  // input geometry_msgs::PoseWithCovariance contain 6x6 matrix
-  Eigen::Matrix2d xy_covariance;
-  const auto cov = input_msg->pose.covariance;
-  xy_covariance(0, 0) = cov[0 * 6 + 0];
-  xy_covariance(0, 1) = cov[0 * 6 + 1];
-  xy_covariance(1, 0) = cov[1 * 6 + 0];
-  xy_covariance(1, 1) = cov[1 * 6 + 1];
-
-  Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigensolver(xy_covariance);
-
-  // eigen values and vectors are sorted in ascending order
-  ellipse_.long_radius = scale_ * std::sqrt(eigensolver.eigenvalues()(1));
-  ellipse_.short_radius = scale_ * std::sqrt(eigensolver.eigenvalues()(0));
-
-  // principal component vector
-  const Eigen::Vector2d pc_vector = eigensolver.eigenvectors().col(1);
-  ellipse_.yaw = std::atan2(pc_vector.y(), pc_vector.x());
-
-  // ellipse size along lateral direction (body-frame)
-  ellipse_.P = xy_covariance;
-  const double yaw_vehicle = tf2::getYaw(input_msg->pose.pose.orientation);
-  ellipse_.size_lateral_direction =
-    scale_ * measure_size_ellipse_along_body_frame(ellipse_.P.inverse(), yaw_vehicle);
-
-  const auto ellipse_marker = create_ellipse_marker(ellipse_, input_msg);
+  ellipse_ = autoware::localization_util::calculate_xy_ellipse(input_msg->pose, scale_);
+
+  const auto ellipse_marker = autoware::localization_util::create_ellipse_marker(
+    ellipse_, input_msg->header, input_msg->pose);
   ellipse_marker_pub_->publish(ellipse_marker);
 
   // diagnostics
@@ -134,16 +85,5 @@ void LocalizationErrorMonitor::on_odom(nav_msgs::msg::Odometry::ConstSharedPtr i
   diag_pub_->publish(diag_msg);
 }
 
-double LocalizationErrorMonitor::measure_size_ellipse_along_body_frame(
-  const Eigen::Matrix2d & Pinv, const double theta)
-{
-  Eigen::MatrixXd e(2, 1);
-  e(0, 0) = std::cos(theta);
-  e(1, 0) = std::sin(theta);
-
-  double d = std::sqrt((e.transpose() * Pinv * e)(0, 0) / Pinv.determinant());
-  return d;
-}
-
 #include <rclcpp_components/register_node_macro.hpp>
 RCLCPP_COMPONENTS_REGISTER_NODE(LocalizationErrorMonitor)
diff --git a/localization/localization_util/CMakeLists.txt b/localization/localization_util/CMakeLists.txt
index 9a9086314f42e..ebf2b6dc10878 100644
--- a/localization/localization_util/CMakeLists.txt
+++ b/localization/localization_util/CMakeLists.txt
@@ -8,6 +8,7 @@ ament_auto_add_library(localization_util SHARED
   src/util_func.cpp
   src/smart_pose_buffer.cpp
   src/tree_structured_parzen_estimator.cpp
+  src/covariance_ellipse.cpp
 )
 
 if(BUILD_TESTING)
diff --git a/localization/localization_util/include/localization_util/covariance_ellipse.hpp b/localization/localization_util/include/localization_util/covariance_ellipse.hpp
new file mode 100644
index 0000000000000..1f187d411dc63
--- /dev/null
+++ b/localization/localization_util/include/localization_util/covariance_ellipse.hpp
@@ -0,0 +1,44 @@
+// Copyright 2024 Autoware Foundation
+//
+// 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 LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_
+#define LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_
+
+#include <Eigen/Dense>
+
+#include <geometry_msgs/msg/pose_with_covariance.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+namespace autoware::localization_util
+{
+
+struct Ellipse
+{
+  double long_radius;
+  double short_radius;
+  double yaw;
+  Eigen::Matrix2d P;
+  double size_lateral_direction;
+};
+
+Ellipse calculate_xy_ellipse(
+  const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double scale);
+
+visualization_msgs::msg::Marker create_ellipse_marker(
+  const Ellipse & ellipse, const std_msgs::msg::Header & header,
+  const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance);
+
+}  // namespace autoware::localization_util
+
+#endif  // LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_
diff --git a/localization/localization_util/src/covariance_ellipse.cpp b/localization/localization_util/src/covariance_ellipse.cpp
new file mode 100644
index 0000000000000..885ce2dcee19c
--- /dev/null
+++ b/localization/localization_util/src/covariance_ellipse.cpp
@@ -0,0 +1,90 @@
+// Copyright 2024 Autoware Foundation
+//
+// 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 "localization_util/covariance_ellipse.hpp"
+
+#include <tf2/utils.h>
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
+namespace autoware::localization_util
+{
+
+Ellipse calculate_xy_ellipse(
+  const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double scale)
+{
+  // input geometry_msgs::PoseWithCovariance contain 6x6 matrix
+  Eigen::Matrix2d xy_covariance;
+  const auto cov = pose_with_covariance.covariance;
+  xy_covariance(0, 0) = cov[0 * 6 + 0];
+  xy_covariance(0, 1) = cov[0 * 6 + 1];
+  xy_covariance(1, 0) = cov[1 * 6 + 0];
+  xy_covariance(1, 1) = cov[1 * 6 + 1];
+
+  Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigensolver(xy_covariance);
+
+  Ellipse ellipse;
+
+  // eigen values and vectors are sorted in ascending order
+  ellipse.long_radius = scale * std::sqrt(eigensolver.eigenvalues()(1));
+  ellipse.short_radius = scale * std::sqrt(eigensolver.eigenvalues()(0));
+
+  // principal component vector
+  const Eigen::Vector2d pc_vector = eigensolver.eigenvectors().col(1);
+  ellipse.yaw = std::atan2(pc_vector.y(), pc_vector.x());
+
+  // ellipse size along lateral direction (body-frame)
+  ellipse.P = xy_covariance;
+  const double yaw_vehicle = tf2::getYaw(pose_with_covariance.pose.orientation);
+  const Eigen::Matrix2d & p_inv = ellipse.P.inverse();
+  Eigen::MatrixXd e(2, 1);
+  e(0, 0) = std::cos(yaw_vehicle);
+  e(1, 0) = std::sin(yaw_vehicle);
+  const double d = std::sqrt((e.transpose() * p_inv * e)(0, 0) / p_inv.determinant());
+  ellipse.size_lateral_direction = scale * d;
+
+  return ellipse;
+}
+
+visualization_msgs::msg::Marker create_ellipse_marker(
+  const Ellipse & ellipse, const std_msgs::msg::Header & header,
+  const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance)
+{
+  tf2::Quaternion quat;
+  quat.setEuler(0, 0, ellipse.yaw);
+
+  const double ellipse_long_radius = std::min(ellipse.long_radius, 30.0);
+  const double ellipse_short_radius = std::min(ellipse.short_radius, 30.0);
+  visualization_msgs::msg::Marker marker;
+  marker.header = header;
+  marker.ns = "error_ellipse";
+  marker.id = 0;
+  marker.type = visualization_msgs::msg::Marker::SPHERE;
+  marker.action = visualization_msgs::msg::Marker::ADD;
+  marker.pose = pose_with_covariance.pose;
+  marker.pose.orientation = tf2::toMsg(quat);
+  marker.scale.x = ellipse_long_radius * 2;
+  marker.scale.y = ellipse_short_radius * 2;
+  marker.scale.z = 0.01;
+  marker.color.a = 0.1;
+  marker.color.r = 0.0;
+  marker.color.g = 0.0;
+  marker.color.b = 1.0;
+  return marker;
+}
+
+}  // namespace autoware::localization_util
diff --git a/localization/pose_estimator_arbiter/README.md b/localization/pose_estimator_arbiter/README.md
index a28da699ad926..7b9397dfdba47 100644
--- a/localization/pose_estimator_arbiter/README.md
+++ b/localization/pose_estimator_arbiter/README.md
@@ -45,7 +45,7 @@ The following video demonstrates the switching of four different pose estimators
 
 Users can reproduce the demonstration using the following data and launch command:
 
-[sample data (rosbag & map)](https://drive.google.com/file/d/1MxLo1Sw6PdvfkyOYf_9A5dZ9uli1vPvS/view)
+[sample data (rosbag & map)](https://drive.google.com/file/d/1ZNlkyCtwe04iKFREdeZ5xuMU_jWpwM3W/view)
 The rosbag is simulated data created by [AWSIM](https://tier4.github.io/AWSIM/).
 The map is an edited version of the [original map data](https://github.com/tier4/AWSIM/releases/download/v1.1.0/nishishinjuku_autoware_map.zip) published on the AWSIM documentation page to make it suitable for multiple pose_estimators.
 
diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp
index 65e03c6bc6316..d61ced593de78 100644
--- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp
+++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp
@@ -18,6 +18,8 @@
 #include "autoware/universe_utils/ros/debug_publisher.hpp"
 #include "image_projection_based_fusion/fusion_node.hpp"
 
+#include <image_projection_based_fusion/utils/utils.hpp>
+
 #include "autoware_perception_msgs/msg/object_classification.hpp"
 
 #include <map>
@@ -45,7 +47,8 @@ class RoiDetectedObjectFusionNode
 
   std::map<std::size_t, DetectedObjectWithFeature> generateDetectedObjectRoIs(
     const DetectedObjects & input_object_msg, const double image_width, const double image_height,
-    const Eigen::Affine3d & object2camera_affine, const Eigen::Matrix4d & camera_projection);
+    const Eigen::Affine3d & object2camera_affine,
+    const image_geometry::PinholeCameraModel & pinhole_camera_model);
 
   void fuseObjectsOnImage(
     const DetectedObjects & input_object_msg,
diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp
index 4cbc0990352e6..4d4fd8d2dac42 100644
--- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp
+++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp
@@ -17,6 +17,8 @@
 
 #include "image_projection_based_fusion/fusion_node.hpp"
 
+#include <image_projection_based_fusion/utils/utils.hpp>
+
 #include <string>
 #include <vector>
 namespace image_projection_based_fusion
diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp
index c458c17bed793..26bcfd5ed802a 100644
--- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp
+++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp
@@ -17,6 +17,8 @@
 
 #include "image_projection_based_fusion/fusion_node.hpp"
 
+#include <image_projection_based_fusion/utils/utils.hpp>
+
 #include <string>
 #include <vector>
 
diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp
index ffa1666396a1d..107e0d6c179a8 100644
--- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp
+++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp
@@ -42,6 +42,7 @@
 #include "autoware_perception_msgs/msg/shape.hpp"
 #include "tier4_perception_msgs/msg/detected_object_with_feature.hpp"
 
+#include <image_geometry/pinhole_camera_model.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl_conversions/pcl_conversions.h>
@@ -60,6 +61,10 @@ struct PointData
   float distance;
   size_t orig_index;
 };
+
+Eigen::Vector2d calcRawImageProjectedPoint(
+  const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d);
+
 std::optional<geometry_msgs::msg::TransformStamped> getTransformStamped(
   const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id,
   const std::string & source_frame_id, const rclcpp::Time & time);
diff --git a/perception/image_projection_based_fusion/package.xml b/perception/image_projection_based_fusion/package.xml
index 8d5a2ef1fe519..00ffdc8fd5528 100644
--- a/perception/image_projection_based_fusion/package.xml
+++ b/perception/image_projection_based_fusion/package.xml
@@ -21,6 +21,7 @@
   <depend>autoware_universe_utils</depend>
   <depend>cv_bridge</depend>
   <depend>euclidean_cluster</depend>
+  <depend>image_geometry</depend>
   <depend>image_transport</depend>
   <depend>lidar_centerpoint</depend>
   <depend>message_filters</depend>
diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp
index 7973c22cc1d78..a7c83ac7614a8 100644
--- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp
+++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp
@@ -307,9 +307,9 @@ void PointPaintingFusionNode::fuseOnSingleImage(
   const auto class_offset = painted_pointcloud_msg.fields.at(4).offset;
   const auto p_step = painted_pointcloud_msg.point_step;
   // projection matrix
-  Eigen::Matrix3f camera_projection;  // use only x,y,z
-  camera_projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2),
-    camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6);
+  image_geometry::PinholeCameraModel pinhole_camera_model;
+  pinhole_camera_model.fromCameraInfo(camera_info);
+
   Eigen::Vector3f point_lidar, point_camera;
   /** dc : don't care
 
@@ -342,15 +342,15 @@ dc   | dc dc dc  dc ||zc|
       continue;
     }
     // project
-    Eigen::Vector3f normalized_projected_point = camera_projection * Eigen::Vector3f(p_x, p_y, p_z);
+    Eigen::Vector2d projected_point =
+      calcRawImageProjectedPoint(pinhole_camera_model, cv::Point3d(p_x, p_y, p_z));
+
     // iterate 2d bbox
     for (const auto & feature_object : objects) {
       sensor_msgs::msg::RegionOfInterest roi = feature_object.feature.roi;
       // paint current point if it is inside bbox
       int label2d = feature_object.object.classification.front().label;
-      if (
-        !isUnknown(label2d) &&
-        isInsideBbox(normalized_projected_point.x(), normalized_projected_point.y(), roi, p_z)) {
+      if (!isUnknown(label2d) && isInsideBbox(projected_point.x(), projected_point.y(), roi, p_z)) {
         data = &painted_pointcloud_msg.data[0];
         auto p_class = reinterpret_cast<float *>(&output[stride + class_offset]);
         for (const auto & cls : isClassTable_) {
@@ -361,7 +361,7 @@ dc   | dc dc dc  dc ||zc|
 #if 0
       // Parallelizing loop don't support push_back
       if (debugger_) {
-        debug_image_points.push_back(normalized_projected_point);
+        debug_image_points.push_back(projected_point);
       }
 #endif
     }
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 bc786dd460d12..51987cbbcab84 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
@@ -83,10 +83,8 @@ void RoiClusterFusionNode::fuseOnSingleImage(
   const DetectedObjectsWithFeature & input_roi_msg,
   const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjectsWithFeature & output_cluster_msg)
 {
-  Eigen::Matrix4d projection;
-  projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), camera_info.p.at(3),
-    camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6), camera_info.p.at(7),
-    camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), camera_info.p.at(11);
+  image_geometry::PinholeCameraModel pinhole_camera_model;
+  pinhole_camera_model.fromCameraInfo(camera_info);
 
   // get transform from cluster frame id to camera optical frame id
   geometry_msgs::msg::TransformStamped transform_stamped;
@@ -133,23 +131,19 @@ void RoiClusterFusionNode::fuseOnSingleImage(
         continue;
       }
 
-      Eigen::Vector4d projected_point =
-        projection * Eigen::Vector4d(*iter_x, *iter_y, *iter_z, 1.0);
-      Eigen::Vector2d normalized_projected_point = Eigen::Vector2d(
-        projected_point.x() / projected_point.z(), projected_point.y() / projected_point.z());
+      Eigen::Vector2d projected_point =
+        calcRawImageProjectedPoint(pinhole_camera_model, cv::Point3d(*iter_x, *iter_y, *iter_z));
       if (
-        0 <= static_cast<int>(normalized_projected_point.x()) &&
-        static_cast<int>(normalized_projected_point.x()) <=
-          static_cast<int>(camera_info.width) - 1 &&
-        0 <= static_cast<int>(normalized_projected_point.y()) &&
-        static_cast<int>(normalized_projected_point.y()) <=
-          static_cast<int>(camera_info.height) - 1) {
-        min_x = std::min(static_cast<int>(normalized_projected_point.x()), min_x);
-        min_y = std::min(static_cast<int>(normalized_projected_point.y()), min_y);
-        max_x = std::max(static_cast<int>(normalized_projected_point.x()), max_x);
-        max_y = std::max(static_cast<int>(normalized_projected_point.y()), max_y);
-        projected_points.push_back(normalized_projected_point);
-        if (debugger_) debugger_->obstacle_points_.push_back(normalized_projected_point);
+        0 <= static_cast<int>(projected_point.x()) &&
+        static_cast<int>(projected_point.x()) <= static_cast<int>(camera_info.width) - 1 &&
+        0 <= static_cast<int>(projected_point.y()) &&
+        static_cast<int>(projected_point.y()) <= static_cast<int>(camera_info.height) - 1) {
+        min_x = std::min(static_cast<int>(projected_point.x()), min_x);
+        min_y = std::min(static_cast<int>(projected_point.y()), min_y);
+        max_x = std::max(static_cast<int>(projected_point.x()), max_x);
+        max_y = std::max(static_cast<int>(projected_point.y()), max_y);
+        projected_points.push_back(projected_point);
+        if (debugger_) debugger_->obstacle_points_.push_back(projected_point);
       }
     }
     if (projected_points.empty()) {
diff --git a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp
index 762d3e0ee51b1..7d85ecb2f9200 100644
--- a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp
+++ b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp
@@ -86,15 +86,12 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage(
     object2camera_affine = transformToEigen(transform_stamped_optional.value().transform);
   }
 
-  Eigen::Matrix4d camera_projection;
-  camera_projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2),
-    camera_info.p.at(3), camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6),
-    camera_info.p.at(7), camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10),
-    camera_info.p.at(11);
+  image_geometry::PinholeCameraModel pinhole_camera_model;
+  pinhole_camera_model.fromCameraInfo(camera_info);
 
   const auto object_roi_map = generateDetectedObjectRoIs(
     input_object_msg, static_cast<double>(camera_info.width),
-    static_cast<double>(camera_info.height), object2camera_affine, camera_projection);
+    static_cast<double>(camera_info.height), object2camera_affine, pinhole_camera_model);
   fuseObjectsOnImage(input_object_msg, input_roi_msg.feature_objects, object_roi_map);
 
   if (debugger_) {
@@ -109,7 +106,8 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage(
 std::map<std::size_t, DetectedObjectWithFeature>
 RoiDetectedObjectFusionNode::generateDetectedObjectRoIs(
   const DetectedObjects & input_object_msg, const double image_width, const double image_height,
-  const Eigen::Affine3d & object2camera_affine, const Eigen::Matrix4d & camera_projection)
+  const Eigen::Affine3d & object2camera_affine,
+  const image_geometry::PinholeCameraModel & pinhole_camera_model)
 {
   std::map<std::size_t, DetectedObjectWithFeature> object_roi_map;
   int64_t timestamp_nsec =
@@ -148,13 +146,8 @@ RoiDetectedObjectFusionNode::generateDetectedObjectRoIs(
         continue;
       }
 
-      Eigen::Vector2d proj_point;
-      {
-        Eigen::Vector4d proj_point_hom =
-          camera_projection * Eigen::Vector4d(point.x(), point.y(), point.z(), 1.0);
-        proj_point = Eigen::Vector2d(
-          proj_point_hom.x() / (proj_point_hom.z()), proj_point_hom.y() / (proj_point_hom.z()));
-      }
+      Eigen::Vector2d proj_point = calcRawImageProjectedPoint(
+        pinhole_camera_model, cv::Point3d(point.x(), point.y(), point.z()));
 
       min_x = std::min(proj_point.x(), min_x);
       min_y = std::min(proj_point.y(), min_y);
diff --git a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp
index ea5c2005bdd46..7f19402d9e565 100644
--- a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp
+++ b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp
@@ -101,10 +101,9 @@ void RoiPointCloudFusionNode::fuseOnSingleImage(
   }
 
   // transform pointcloud to camera optical frame id
-  Eigen::Matrix4d projection;
-  projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), camera_info.p.at(3),
-    camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6), camera_info.p.at(7),
-    camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), camera_info.p.at(11);
+  image_geometry::PinholeCameraModel pinhole_camera_model;
+  pinhole_camera_model.fromCameraInfo(camera_info);
+
   geometry_msgs::msg::TransformStamped transform_stamped;
   {
     const auto transform_stamped_optional = getTransformStamped(
@@ -143,10 +142,8 @@ void RoiPointCloudFusionNode::fuseOnSingleImage(
     if (transformed_z <= 0.0) {
       continue;
     }
-    Eigen::Vector4d projected_point =
-      projection * Eigen::Vector4d(transformed_x, transformed_y, transformed_z, 1.0);
-    Eigen::Vector2d normalized_projected_point = Eigen::Vector2d(
-      projected_point.x() / projected_point.z(), projected_point.y() / projected_point.z());
+    Eigen::Vector2d projected_point = calcRawImageProjectedPoint(
+      pinhole_camera_model, cv::Point3d(transformed_x, transformed_y, transformed_z));
     for (std::size_t i = 0; i < output_objs.size(); ++i) {
       auto & feature_obj = output_objs.at(i);
       const auto & check_roi = feature_obj.feature.roi;
@@ -156,10 +153,9 @@ void RoiPointCloudFusionNode::fuseOnSingleImage(
         continue;
       }
       if (
-        check_roi.x_offset <= normalized_projected_point.x() &&
-        check_roi.y_offset <= normalized_projected_point.y() &&
-        check_roi.x_offset + check_roi.width >= normalized_projected_point.x() &&
-        check_roi.y_offset + check_roi.height >= normalized_projected_point.y()) {
+        check_roi.x_offset <= projected_point.x() && check_roi.y_offset <= projected_point.y() &&
+        check_roi.x_offset + check_roi.width >= projected_point.x() &&
+        check_roi.y_offset + check_roi.height >= projected_point.y()) {
         std::memcpy(
           &cluster.data[clusters_data_size.at(i)], &input_pointcloud_msg.data[offset], point_step);
         clusters_data_size.at(i) += point_step;
diff --git a/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp
index 0096b91f7a3b6..06cc0fd2c61f2 100644
--- a/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp
+++ b/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp
@@ -65,11 +65,9 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage(
   if (mask.cols == 0 || mask.rows == 0) {
     return;
   }
-  Eigen::Matrix4d projection;
-  projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), camera_info.p.at(3),
-    camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6), camera_info.p.at(7),
-    camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), camera_info.p.at(11), 0.0, 0.0,
-    0.0, 1.0;
+  image_geometry::PinholeCameraModel pinhole_camera_model;
+  pinhole_camera_model.fromCameraInfo(camera_info);
+
   geometry_msgs::msg::TransformStamped transform_stamped;
   // transform pointcloud from frame id to camera optical frame id
   {
@@ -113,14 +111,11 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage(
       continue;
     }
 
-    Eigen::Vector4d projected_point =
-      projection * Eigen::Vector4d(transformed_x, transformed_y, transformed_z, 1.0);
-    Eigen::Vector2d normalized_projected_point = Eigen::Vector2d(
-      projected_point.x() / projected_point.z(), projected_point.y() / projected_point.z());
+    Eigen::Vector2d projected_point = calcRawImageProjectedPoint(
+      pinhole_camera_model, cv::Point3d(transformed_x, transformed_y, transformed_z));
 
-    bool is_inside_image =
-      normalized_projected_point.x() > 0 && normalized_projected_point.x() < camera_info.width &&
-      normalized_projected_point.y() > 0 && normalized_projected_point.y() < camera_info.height;
+    bool is_inside_image = projected_point.x() > 0 && projected_point.x() < camera_info.width &&
+                           projected_point.y() > 0 && projected_point.y() < camera_info.height;
     if (!is_inside_image) {
       copyPointCloud(
         input_pointcloud_msg, point_step, global_offset, output_cloud, output_pointcloud_size);
@@ -129,8 +124,7 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage(
 
     // skip filtering pointcloud where semantic id out of the defined list
     uint8_t semantic_id = mask.at<uint8_t>(
-      static_cast<uint16_t>(normalized_projected_point.y()),
-      static_cast<uint16_t>(normalized_projected_point.x()));
+      static_cast<uint16_t>(projected_point.y()), static_cast<uint16_t>(projected_point.x()));
     if (static_cast<size_t>(semantic_id) >= filter_semantic_label_target_.size()) {
       copyPointCloud(
         input_pointcloud_msg, point_step, global_offset, output_cloud, output_pointcloud_size);
diff --git a/perception/image_projection_based_fusion/src/utils/utils.cpp b/perception/image_projection_based_fusion/src/utils/utils.cpp
index 2e997f3e8e60f..aeec2886a801a 100644
--- a/perception/image_projection_based_fusion/src/utils/utils.cpp
+++ b/perception/image_projection_based_fusion/src/utils/utils.cpp
@@ -13,8 +13,18 @@
 // limitations under the License.
 
 #include "image_projection_based_fusion/utils/utils.hpp"
+
 namespace image_projection_based_fusion
 {
+Eigen::Vector2d calcRawImageProjectedPoint(
+  const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d)
+{
+  const cv::Point2d rectified_image_point = pinhole_camera_model.project3dToPixel(point3d);
+
+  const cv::Point2d raw_image_point = pinhole_camera_model.unrectifyPoint(rectified_image_point);
+
+  return Eigen::Vector2d(raw_image_point.x, raw_image_point.y);
+}
 
 std::optional<geometry_msgs::msg::TransformStamped> getTransformStamped(
   const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id,
diff --git a/perception/object_range_splitter/CMakeLists.txt b/perception/object_range_splitter/CMakeLists.txt
index 92c6c0668a085..1da71a31b26d0 100644
--- a/perception/object_range_splitter/CMakeLists.txt
+++ b/perception/object_range_splitter/CMakeLists.txt
@@ -4,12 +4,12 @@ project(object_range_splitter)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
-ament_auto_add_library(object_range_splitter SHARED
-  src/node.cpp
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/object_range_splitter_node.cpp
 )
 
-rclcpp_components_register_node(object_range_splitter
-  PLUGIN "object_range_splitter::ObjectRangeSplitterNode"
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "autoware::object_range_splitter::ObjectRangeSplitterNode"
   EXECUTABLE object_range_splitter_node
 )
 
diff --git a/perception/object_range_splitter/package.xml b/perception/object_range_splitter/package.xml
index bef3ea76354d1..7fc9245ee894f 100644
--- a/perception/object_range_splitter/package.xml
+++ b/perception/object_range_splitter/package.xml
@@ -14,7 +14,6 @@
   <depend>autoware_perception_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
-  <depend>sensor_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
diff --git a/perception/object_range_splitter/src/node.cpp b/perception/object_range_splitter/src/object_range_splitter_node.cpp
similarity index 91%
rename from perception/object_range_splitter/src/node.cpp
rename to perception/object_range_splitter/src/object_range_splitter_node.cpp
index 978c40275fb9b..5d5a4aad70011 100644
--- a/perception/object_range_splitter/src/node.cpp
+++ b/perception/object_range_splitter/src/object_range_splitter_node.cpp
@@ -12,9 +12,9 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "object_range_splitter/node.hpp"
+#include "object_range_splitter_node.hpp"
 
-namespace object_range_splitter
+namespace autoware::object_range_splitter
 {
 ObjectRangeSplitterNode::ObjectRangeSplitterNode(const rclcpp::NodeOptions & node_options)
 : Node("object_range_splitter_node", node_options)
@@ -59,7 +59,7 @@ void ObjectRangeSplitterNode::objectCallback(
   long_range_object_pub_->publish(output_long_range_object_msg);
   short_range_object_pub_->publish(output_short_range_object_msg);
 }
-}  // namespace object_range_splitter
+}  // namespace autoware::object_range_splitter
 
 #include <rclcpp_components/register_node_macro.hpp>
-RCLCPP_COMPONENTS_REGISTER_NODE(object_range_splitter::ObjectRangeSplitterNode)
+RCLCPP_COMPONENTS_REGISTER_NODE(autoware::object_range_splitter::ObjectRangeSplitterNode)
diff --git a/perception/object_range_splitter/include/object_range_splitter/node.hpp b/perception/object_range_splitter/src/object_range_splitter_node.hpp
similarity index 81%
rename from perception/object_range_splitter/include/object_range_splitter/node.hpp
rename to perception/object_range_splitter/src/object_range_splitter_node.hpp
index a4dbb2fb634aa..19e5924272eb9 100644
--- a/perception/object_range_splitter/include/object_range_splitter/node.hpp
+++ b/perception/object_range_splitter/src/object_range_splitter_node.hpp
@@ -12,16 +12,16 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef OBJECT_RANGE_SPLITTER__NODE_HPP_
-#define OBJECT_RANGE_SPLITTER__NODE_HPP_
+#ifndef OBJECT_RANGE_SPLITTER_NODE_HPP_
+#define OBJECT_RANGE_SPLITTER_NODE_HPP_
 
 #include <rclcpp/rclcpp.hpp>
 
-#include <autoware_perception_msgs/msg/detected_objects.hpp>
+#include "autoware_perception_msgs/msg/detected_objects.hpp"
 
 #include <memory>
 
-namespace object_range_splitter
+namespace autoware::object_range_splitter
 {
 class ObjectRangeSplitterNode : public rclcpp::Node
 {
@@ -42,6 +42,6 @@ class ObjectRangeSplitterNode : public rclcpp::Node
   float spilt_range_;
 };
 
-}  // namespace object_range_splitter
+}  // namespace autoware::object_range_splitter
 
-#endif  // OBJECT_RANGE_SPLITTER__NODE_HPP_
+#endif  // OBJECT_RANGE_SPLITTER_NODE_HPP_
diff --git a/perception/object_velocity_splitter/CMakeLists.txt b/perception/object_velocity_splitter/CMakeLists.txt
index 68e87a3355f86..afad278e383d8 100644
--- a/perception/object_velocity_splitter/CMakeLists.txt
+++ b/perception/object_velocity_splitter/CMakeLists.txt
@@ -6,12 +6,12 @@ find_package(autoware_cmake REQUIRED)
 autoware_package()
 
 # Targets
-ament_auto_add_library(object_velocity_splitter_node_component SHARED
-  src/object_velocity_splitter_node/object_velocity_splitter_node.cpp
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/object_velocity_splitter_node.cpp
 )
 
-rclcpp_components_register_node(object_velocity_splitter_node_component
-  PLUGIN "object_velocity_splitter::ObjectVelocitySplitterNode"
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "autoware::object_velocity_splitter::ObjectVelocitySplitterNode"
   EXECUTABLE object_velocity_splitter_node
 )
 
diff --git a/perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp b/perception/object_velocity_splitter/src/object_velocity_splitter_node.cpp
similarity index 93%
rename from perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp
rename to perception/object_velocity_splitter/src/object_velocity_splitter_node.cpp
index e15871d18840f..a10692faeff1e 100644
--- a/perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp
+++ b/perception/object_velocity_splitter/src/object_velocity_splitter_node.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "object_velocity_splitter/object_velocity_splitter_node.hpp"
+#include "object_velocity_splitter_node.hpp"
 
 #include "autoware/universe_utils/geometry/geometry.hpp"
 
@@ -40,7 +40,7 @@ bool update_param(
 }
 }  // namespace
 
-namespace object_velocity_splitter
+namespace autoware::object_velocity_splitter
 {
 using autoware_perception_msgs::msg::DetectedObject;
 using autoware_perception_msgs::msg::DetectedObjects;
@@ -110,7 +110,7 @@ rcl_interfaces::msg::SetParametersResult ObjectVelocitySplitterNode::onSetParam(
   return result;
 }
 
-}  // namespace object_velocity_splitter
+}  // namespace autoware::object_velocity_splitter
 
 #include "rclcpp_components/register_node_macro.hpp"
-RCLCPP_COMPONENTS_REGISTER_NODE(object_velocity_splitter::ObjectVelocitySplitterNode)
+RCLCPP_COMPONENTS_REGISTER_NODE(autoware::object_velocity_splitter::ObjectVelocitySplitterNode)
diff --git a/perception/object_velocity_splitter/include/object_velocity_splitter/object_velocity_splitter_node.hpp b/perception/object_velocity_splitter/src/object_velocity_splitter_node.hpp
similarity index 85%
rename from perception/object_velocity_splitter/include/object_velocity_splitter/object_velocity_splitter_node.hpp
rename to perception/object_velocity_splitter/src/object_velocity_splitter_node.hpp
index 2180b467dbbcf..ca0a9a9e65f3c 100644
--- a/perception/object_velocity_splitter/include/object_velocity_splitter/object_velocity_splitter_node.hpp
+++ b/perception/object_velocity_splitter/src/object_velocity_splitter_node.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef OBJECT_VELOCITY_SPLITTER__OBJECT_VELOCITY_SPLITTER_NODE_HPP_
-#define OBJECT_VELOCITY_SPLITTER__OBJECT_VELOCITY_SPLITTER_NODE_HPP_
+#ifndef OBJECT_VELOCITY_SPLITTER_NODE_HPP_
+#define OBJECT_VELOCITY_SPLITTER_NODE_HPP_
 
 #include "rclcpp/rclcpp.hpp"
 
@@ -24,7 +24,7 @@
 #include <string>
 #include <vector>
 
-namespace object_velocity_splitter
+namespace autoware::object_velocity_splitter
 {
 using autoware_perception_msgs::msg::DetectedObject;
 using autoware_perception_msgs::msg::DetectedObjects;
@@ -62,6 +62,6 @@ class ObjectVelocitySplitterNode : public rclcpp::Node
   NodeParam node_param_{};
 };
 
-}  // namespace object_velocity_splitter
+}  // namespace autoware::object_velocity_splitter
 
-#endif  // OBJECT_VELOCITY_SPLITTER__OBJECT_VELOCITY_SPLITTER_NODE_HPP_
+#endif  // OBJECT_VELOCITY_SPLITTER_NODE_HPP_
diff --git a/perception/radar_crossing_objects_noise_filter/CMakeLists.txt b/perception/radar_crossing_objects_noise_filter/CMakeLists.txt
index bf4a46cda7ae6..6414739413d5c 100644
--- a/perception/radar_crossing_objects_noise_filter/CMakeLists.txt
+++ b/perception/radar_crossing_objects_noise_filter/CMakeLists.txt
@@ -6,12 +6,12 @@ find_package(autoware_cmake REQUIRED)
 autoware_package()
 
 # Targets
-ament_auto_add_library(radar_crossing_objects_noise_filter_node_component SHARED
-  src/radar_crossing_objects_noise_filter_node/radar_crossing_objects_noise_filter_node.cpp
+ament_auto_add_library(${PROJECT_NAME}_node_component SHARED
+  src/radar_crossing_objects_noise_filter_node.cpp
 )
 
-rclcpp_components_register_node(radar_crossing_objects_noise_filter_node_component
-  PLUGIN "radar_crossing_objects_noise_filter::RadarCrossingObjectsNoiseFilterNode"
+rclcpp_components_register_node(${PROJECT_NAME}_node_component
+  PLUGIN "autoware::radar_crossing_objects_noise_filter::RadarCrossingObjectsNoiseFilterNode"
   EXECUTABLE radar_crossing_objects_noise_filter_node
 )
 
@@ -23,10 +23,10 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 
   file(GLOB_RECURSE test_files test/**/*.cpp)
-  ament_add_ros_isolated_gtest(radar_crossing_objects_noise_filter ${test_files})
+  ament_add_ros_isolated_gtest(${PROJECT_NAME} ${test_files})
 
-  target_link_libraries(radar_crossing_objects_noise_filter
-    radar_crossing_objects_noise_filter_node_component
+  target_link_libraries(${PROJECT_NAME}
+    ${PROJECT_NAME}_node_component
   )
 endif()
 
diff --git a/perception/radar_crossing_objects_noise_filter/package.xml b/perception/radar_crossing_objects_noise_filter/package.xml
index 0db40b24e809c..d0245dfe62a40 100644
--- a/perception/radar_crossing_objects_noise_filter/package.xml
+++ b/perception/radar_crossing_objects_noise_filter/package.xml
@@ -16,7 +16,6 @@
 
   <depend>autoware_perception_msgs</depend>
   <depend>autoware_universe_utils</depend>
-  <depend>geometry_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>tf2</depend>
diff --git a/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node/radar_crossing_objects_noise_filter_node.cpp b/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node.cpp
similarity index 94%
rename from perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node/radar_crossing_objects_noise_filter_node.cpp
rename to perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node.cpp
index 2f5884c04ce37..13231b83bf27b 100644
--- a/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node/radar_crossing_objects_noise_filter_node.cpp
+++ b/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp"
+#include "radar_crossing_objects_noise_filter_node.hpp"
 
 #include "autoware/universe_utils/geometry/geometry.hpp"
 #include "autoware/universe_utils/math/normalization.hpp"
@@ -50,7 +50,7 @@ bool update_param(
 }
 }  // namespace
 
-namespace radar_crossing_objects_noise_filter
+namespace autoware::radar_crossing_objects_noise_filter
 {
 using autoware_perception_msgs::msg::DetectedObject;
 using autoware_perception_msgs::msg::DetectedObjects;
@@ -142,8 +142,8 @@ bool RadarCrossingObjectsNoiseFilterNode::isNoise(const DetectedObject & object)
   }
 }
 
-}  // namespace radar_crossing_objects_noise_filter
+}  // namespace autoware::radar_crossing_objects_noise_filter
 
 #include "rclcpp_components/register_node_macro.hpp"
 RCLCPP_COMPONENTS_REGISTER_NODE(
-  radar_crossing_objects_noise_filter::RadarCrossingObjectsNoiseFilterNode)
+  autoware::radar_crossing_objects_noise_filter::RadarCrossingObjectsNoiseFilterNode)
diff --git a/perception/radar_crossing_objects_noise_filter/include/radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp b/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node.hpp
similarity index 82%
rename from perception/radar_crossing_objects_noise_filter/include/radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp
rename to perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node.hpp
index fbd64c884d396..6e389f563a138 100644
--- a/perception/radar_crossing_objects_noise_filter/include/radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp
+++ b/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RADAR_CROSSING_OBJECTS_NOISE_FILTER__RADAR_CROSSING_OBJECTS_NOISE_FILTER_NODE_HPP_
-#define RADAR_CROSSING_OBJECTS_NOISE_FILTER__RADAR_CROSSING_OBJECTS_NOISE_FILTER_NODE_HPP_
+#ifndef RADAR_CROSSING_OBJECTS_NOISE_FILTER_NODE_HPP_
+#define RADAR_CROSSING_OBJECTS_NOISE_FILTER_NODE_HPP_
 
 #include "rclcpp/rclcpp.hpp"
 
@@ -23,7 +23,7 @@
 #include <memory>
 #include <vector>
 
-namespace radar_crossing_objects_noise_filter
+namespace autoware::radar_crossing_objects_noise_filter
 {
 using autoware_perception_msgs::msg::DetectedObject;
 using autoware_perception_msgs::msg::DetectedObjects;
@@ -63,6 +63,6 @@ class RadarCrossingObjectsNoiseFilterNode : public rclcpp::Node
   bool isNoise(const DetectedObject & object);
 };
 
-}  // namespace radar_crossing_objects_noise_filter
+}  // namespace autoware::radar_crossing_objects_noise_filter
 
-#endif  // RADAR_CROSSING_OBJECTS_NOISE_FILTER__RADAR_CROSSING_OBJECTS_NOISE_FILTER_NODE_HPP_
+#endif  // RADAR_CROSSING_OBJECTS_NOISE_FILTER_NODE_HPP_
diff --git a/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp b/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp
index 44ccaae8ee7fc..93475af7b3628 100644
--- a/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp
+++ b/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp
@@ -12,24 +12,24 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include "../../src/radar_crossing_objects_noise_filter_node.hpp"
 #include "autoware/universe_utils/geometry/geometry.hpp"
 #include "autoware/universe_utils/math/unit_conversion.hpp"
-#include "radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp"
 
 #include <geometry_msgs/msg/point32.hpp>
 
 #include <gtest/gtest.h>
 
-std::shared_ptr<radar_crossing_objects_noise_filter::RadarCrossingObjectsNoiseFilterNode> get_node(
-  double angle_threshold, double velocity_threshold)
+std::shared_ptr<autoware::radar_crossing_objects_noise_filter::RadarCrossingObjectsNoiseFilterNode>
+get_node(double angle_threshold, double velocity_threshold)
 {
   rclcpp::NodeOptions node_options;
 
   node_options.parameter_overrides(
     {{"angle_threshold", angle_threshold}, {"velocity_threshold", velocity_threshold}});
-  auto node =
-    std::make_shared<radar_crossing_objects_noise_filter::RadarCrossingObjectsNoiseFilterNode>(
-      node_options);
+  auto node = std::make_shared<
+    autoware::radar_crossing_objects_noise_filter::RadarCrossingObjectsNoiseFilterNode>(
+    node_options);
   return node;
 }
 
diff --git a/perception/radar_fusion_to_detected_object/CMakeLists.txt b/perception/radar_fusion_to_detected_object/CMakeLists.txt
index c8297567511dd..76c27d3958841 100644
--- a/perception/radar_fusion_to_detected_object/CMakeLists.txt
+++ b/perception/radar_fusion_to_detected_object/CMakeLists.txt
@@ -6,14 +6,14 @@ find_package(autoware_cmake REQUIRED)
 autoware_package()
 
 # Targets
-ament_auto_add_library(radar_object_fusion_to_detected_object_node_component SHARED
-  src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/node.cpp
   src/radar_fusion_to_detected_object.cpp
 )
 
-rclcpp_components_register_node(radar_object_fusion_to_detected_object_node_component
-  PLUGIN "radar_fusion_to_detected_object::RadarObjectFusionToDetectedObjectNode"
-  EXECUTABLE radar_object_fusion_to_detected_object_node
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "autoware::radar_fusion_to_detected_object::RadarObjectFusionToDetectedObjectNode"
+  EXECUTABLE radar_fusion_to_detected_object_node
 )
 
 # Tests
diff --git a/perception/radar_fusion_to_detected_object/launch/radar_object_fusion_to_detected_object.launch.xml b/perception/radar_fusion_to_detected_object/launch/radar_object_fusion_to_detected_object.launch.xml
index d43f6b9655ae5..e5505ad87d9ec 100644
--- a/perception/radar_fusion_to_detected_object/launch/radar_object_fusion_to_detected_object.launch.xml
+++ b/perception/radar_fusion_to_detected_object/launch/radar_object_fusion_to_detected_object.launch.xml
@@ -8,7 +8,7 @@
   <arg name="config_file" default="$(find-pkg-share radar_fusion_to_detected_object)/config/radar_object_fusion_to_detected_object.param.yaml"/>
 
   <!-- Node -->
-  <node pkg="radar_fusion_to_detected_object" exec="radar_object_fusion_to_detected_object_node" name="radar_object_fusion_to_detected_object" output="screen">
+  <node pkg="radar_fusion_to_detected_object" exec="radar_fusion_to_detected_object_node" name="radar_object_fusion_to_detected_object" output="screen">
     <remap from="~/input/objects" to="$(var input/objects)"/>
     <remap from="~/input/radars" to="$(var input/radars)"/>
     <remap from="~/output/objects" to="$(var output/objects)"/>
diff --git a/perception/radar_fusion_to_detected_object/package.xml b/perception/radar_fusion_to_detected_object/package.xml
index aa0711f1cc6ce..2094ff7770555 100644
--- a/perception/radar_fusion_to_detected_object/package.xml
+++ b/perception/radar_fusion_to_detected_object/package.xml
@@ -22,7 +22,6 @@
   <depend>message_filters</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
-  <depend>std_msgs</depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>autoware_lint_common</test_depend>
diff --git a/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object_node.hpp b/perception/radar_fusion_to_detected_object/src/include/node.hpp
similarity index 86%
rename from perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object_node.hpp
rename to perception/radar_fusion_to_detected_object/src/include/node.hpp
index 96794509bdfc3..3f181da0041cb 100644
--- a/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object_node.hpp
+++ b/perception/radar_fusion_to_detected_object/src/include/node.hpp
@@ -13,13 +13,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RADAR_FUSION_TO_DETECTED_OBJECT__RADAR_FUSION_TO_DETECTED_OBJECT_NODE_HPP_
-#define RADAR_FUSION_TO_DETECTED_OBJECT__RADAR_FUSION_TO_DETECTED_OBJECT_NODE_HPP_
+#ifndef NODE_HPP_
+#define NODE_HPP_
 
 #include "message_filters/subscriber.h"
 #include "message_filters/sync_policies/approximate_time.h"
 #include "message_filters/synchronizer.h"
-#include "radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp"
+#include "radar_fusion_to_detected_object.hpp"
 #include "rclcpp/rclcpp.hpp"
 
 #include "autoware_perception_msgs/msg/detected_objects.hpp"
@@ -29,7 +29,7 @@
 #include <string>
 #include <vector>
 
-namespace radar_fusion_to_detected_object
+namespace autoware::radar_fusion_to_detected_object
 {
 using autoware_perception_msgs::msg::DetectedObject;
 using autoware_perception_msgs::msg::DetectedObjects;
@@ -87,6 +87,6 @@ class RadarObjectFusionToDetectedObjectNode : public rclcpp::Node
     const DetectedObject & radar_object, const std_msgs::msg::Header & header_);
 };
 
-}  // namespace radar_fusion_to_detected_object
+}  // namespace autoware::radar_fusion_to_detected_object
 
-#endif  // RADAR_FUSION_TO_DETECTED_OBJECT__RADAR_FUSION_TO_DETECTED_OBJECT_NODE_HPP_
+#endif  // NODE_HPP_
diff --git a/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp b/perception/radar_fusion_to_detected_object/src/include/radar_fusion_to_detected_object.hpp
similarity index 91%
rename from perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp
rename to perception/radar_fusion_to_detected_object/src/include/radar_fusion_to_detected_object.hpp
index cd7e7de6b2ff4..545b7d7e41b2a 100644
--- a/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp
+++ b/perception/radar_fusion_to_detected_object/src/include/radar_fusion_to_detected_object.hpp
@@ -12,25 +12,26 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RADAR_FUSION_TO_DETECTED_OBJECT__RADAR_FUSION_TO_DETECTED_OBJECT_HPP_
-#define RADAR_FUSION_TO_DETECTED_OBJECT__RADAR_FUSION_TO_DETECTED_OBJECT_HPP_
+#ifndef RADAR_FUSION_TO_DETECTED_OBJECT_HPP_
+#define RADAR_FUSION_TO_DETECTED_OBJECT_HPP_
+
+#define EIGEN_MPL2_ONLY
 
 #include "autoware/universe_utils/geometry/boost_geometry.hpp"
 #include "rclcpp/logger.hpp"
-#define EIGEN_MPL2_ONLY
+
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 
 #include "autoware_perception_msgs/msg/detected_objects.hpp"
 #include "geometry_msgs/msg/pose_with_covariance.hpp"
 #include "geometry_msgs/msg/twist_with_covariance.hpp"
-// #include "std_msgs/msg/header.hpp"
 
 #include <memory>
 #include <string>
 #include <vector>
 
-namespace radar_fusion_to_detected_object
+namespace autoware::radar_fusion_to_detected_object
 {
 using autoware::universe_utils::LinearRing2d;
 using autoware::universe_utils::Point2d;
@@ -113,6 +114,6 @@ class RadarFusionToDetectedObject
   double getTwistNorm(const Twist & twist);
   LinearRing2d createObject2dWithMargin(const Point2d object_size, const double margin);
 };
-}  // namespace radar_fusion_to_detected_object
+}  // namespace autoware::radar_fusion_to_detected_object
 
-#endif  // RADAR_FUSION_TO_DETECTED_OBJECT__RADAR_FUSION_TO_DETECTED_OBJECT_HPP_
+#endif  // RADAR_FUSION_TO_DETECTED_OBJECT_HPP_
diff --git a/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp b/perception/radar_fusion_to_detected_object/src/node.cpp
similarity index 96%
rename from perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp
rename to perception/radar_fusion_to_detected_object/src/node.cpp
index 90851674ef9eb..58e893032340c 100644
--- a/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp
+++ b/perception/radar_fusion_to_detected_object/src/node.cpp
@@ -13,7 +13,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "radar_fusion_to_detected_object/radar_fusion_to_detected_object_node.hpp"
+#include "include/node.hpp"
+
 #include "rclcpp/rclcpp.hpp"
 
 #include <memory>
@@ -46,7 +47,7 @@ bool update_param(
 }
 }  // namespace
 
-namespace radar_fusion_to_detected_object
+namespace autoware::radar_fusion_to_detected_object
 {
 using autoware_perception_msgs::msg::DetectedObject;
 using autoware_perception_msgs::msg::DetectedObjects;
@@ -217,8 +218,8 @@ RadarFusionToDetectedObject::RadarInput RadarObjectFusionToDetectedObjectNode::s
   return output;
 }
 
-}  // namespace radar_fusion_to_detected_object
+}  // namespace autoware::radar_fusion_to_detected_object
 
 #include "rclcpp_components/register_node_macro.hpp"
 RCLCPP_COMPONENTS_REGISTER_NODE(
-  radar_fusion_to_detected_object::RadarObjectFusionToDetectedObjectNode)
+  autoware::radar_fusion_to_detected_object::RadarObjectFusionToDetectedObjectNode)
diff --git a/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp b/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp
index 5a0c1b5412312..0c90a468f2ebd 100644
--- a/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp
+++ b/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp
@@ -13,10 +13,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp"
+#include "include/radar_fusion_to_detected_object.hpp"
 
-#include <autoware/universe_utils/geometry/geometry.hpp>
-#include <autoware/universe_utils/math/normalization.hpp>
+#include "autoware/universe_utils/geometry/geometry.hpp"
+#include "autoware/universe_utils/math/normalization.hpp"
 
 #include <boost/geometry.hpp>
 
@@ -27,7 +27,7 @@
 #include <string>
 #include <vector>
 
-namespace radar_fusion_to_detected_object
+namespace autoware::radar_fusion_to_detected_object
 {
 using autoware::universe_utils::LinearRing2d;
 using autoware::universe_utils::Point2d;
@@ -358,4 +358,4 @@ LinearRing2d RadarFusionToDetectedObject::createObject2dWithMargin(
 
   return box;
 }
-}  // namespace radar_fusion_to_detected_object
+}  // namespace autoware::radar_fusion_to_detected_object
diff --git a/perception/radar_object_clustering/CMakeLists.txt b/perception/radar_object_clustering/CMakeLists.txt
index ea25353d3e1d7..9a54bbb0dae43 100644
--- a/perception/radar_object_clustering/CMakeLists.txt
+++ b/perception/radar_object_clustering/CMakeLists.txt
@@ -6,12 +6,12 @@ find_package(autoware_cmake REQUIRED)
 autoware_package()
 
 # Targets
-ament_auto_add_library(radar_object_clustering_node_component SHARED
-  src/radar_object_clustering_node/radar_object_clustering_node.cpp
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/radar_object_clustering_node.cpp
 )
 
-rclcpp_components_register_node(radar_object_clustering_node_component
-  PLUGIN "radar_object_clustering::RadarObjectClusteringNode"
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "autoware::radar_object_clustering::RadarObjectClusteringNode"
   EXECUTABLE radar_object_clustering_node
 )
 
diff --git a/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp b/perception/radar_object_clustering/src/radar_object_clustering_node.cpp
similarity index 97%
rename from perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp
rename to perception/radar_object_clustering/src/radar_object_clustering_node.cpp
index 339ab21741d29..b867d6a193ed7 100644
--- a/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp
+++ b/perception/radar_object_clustering/src/radar_object_clustering_node.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "radar_object_clustering/radar_object_clustering_node.hpp"
+#include "radar_object_clustering_node.hpp"
 
 #include "autoware/universe_utils/geometry/geometry.hpp"
 #include "autoware/universe_utils/math/unit_conversion.hpp"
@@ -20,17 +20,18 @@
 
 #include <tf2/utils.h>
 
-#include <cmath>
-#include <memory>
-#include <string>
-#include <vector>
-
 #ifdef ROS_DISTRO_GALACTIC
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #else
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #endif
 
+#include <algorithm>
+#include <cmath>
+#include <memory>
+#include <string>
+#include <vector>
+
 namespace
 {
 template <class T>
@@ -58,7 +59,7 @@ double get_distance(const autoware_perception_msgs::msg::DetectedObject & object
 
 }  // namespace
 
-namespace radar_object_clustering
+namespace autoware::radar_object_clustering
 {
 using autoware_perception_msgs::msg::DetectedObject;
 using autoware_perception_msgs::msg::DetectedObjects;
@@ -229,7 +230,7 @@ rcl_interfaces::msg::SetParametersResult RadarObjectClusteringNode::onSetParam(
   result.reason = "success";
   return result;
 }
-}  // namespace radar_object_clustering
+}  // namespace autoware::radar_object_clustering
 
 #include "rclcpp_components/register_node_macro.hpp"
-RCLCPP_COMPONENTS_REGISTER_NODE(radar_object_clustering::RadarObjectClusteringNode)
+RCLCPP_COMPONENTS_REGISTER_NODE(autoware::radar_object_clustering::RadarObjectClusteringNode)
diff --git a/perception/radar_object_clustering/include/radar_object_clustering/radar_object_clustering_node.hpp b/perception/radar_object_clustering/src/radar_object_clustering_node.hpp
similarity index 87%
rename from perception/radar_object_clustering/include/radar_object_clustering/radar_object_clustering_node.hpp
rename to perception/radar_object_clustering/src/radar_object_clustering_node.hpp
index 38a2235a117eb..13284a4151f49 100644
--- a/perception/radar_object_clustering/include/radar_object_clustering/radar_object_clustering_node.hpp
+++ b/perception/radar_object_clustering/src/radar_object_clustering_node.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RADAR_OBJECT_CLUSTERING__RADAR_OBJECT_CLUSTERING_NODE_HPP_
-#define RADAR_OBJECT_CLUSTERING__RADAR_OBJECT_CLUSTERING_NODE_HPP_
+#ifndef RADAR_OBJECT_CLUSTERING_NODE_HPP_
+#define RADAR_OBJECT_CLUSTERING_NODE_HPP_
 
 #include "rclcpp/rclcpp.hpp"
 
@@ -24,7 +24,7 @@
 #include <string>
 #include <vector>
 
-namespace radar_object_clustering
+namespace autoware::radar_object_clustering
 {
 using autoware_perception_msgs::msg::DetectedObject;
 using autoware_perception_msgs::msg::DetectedObjects;
@@ -72,6 +72,6 @@ class RadarObjectClusteringNode : public rclcpp::Node
   bool isSameObject(const DetectedObject & object_1, const DetectedObject & object_2);
 };
 
-}  // namespace radar_object_clustering
+}  // namespace autoware::radar_object_clustering
 
-#endif  // RADAR_OBJECT_CLUSTERING__RADAR_OBJECT_CLUSTERING_NODE_HPP_
+#endif  // RADAR_OBJECT_CLUSTERING_NODE_HPP_
diff --git a/perception/radar_object_tracker/CMakeLists.txt b/perception/radar_object_tracker/CMakeLists.txt
index ade45847f8a9f..b732ef6cc863c 100644
--- a/perception/radar_object_tracker/CMakeLists.txt
+++ b/perception/radar_object_tracker/CMakeLists.txt
@@ -20,31 +20,31 @@ include_directories(
     ${EIGEN3_INCLUDE_DIR}
 )
 
-ament_auto_add_library(radar_object_tracker_utils SHARED
+ament_auto_add_library(${PROJECT_NAME}_utils SHARED
   src/utils/utils.cpp
   src/utils/radar_object_tracker_utils.cpp
 )
 
-ament_auto_add_library(radar_object_tracker_node SHARED
-  src/radar_object_tracker_node/radar_object_tracker_node.cpp
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/radar_object_tracker_node.cpp
   src/tracker/model/tracker_base.cpp
   src/tracker/model/linear_motion_tracker.cpp
   src/tracker/model/constant_turn_rate_motion_tracker.cpp
-  src/data_association/data_association.cpp
-  src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp
+  src/association/data_association.cpp
+  src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp
 )
 
-target_link_libraries(radar_object_tracker_node
+target_link_libraries(${PROJECT_NAME}
   Eigen3::Eigen
   yaml-cpp
   nlohmann_json::nlohmann_json # for debug
   glog::glog
-  radar_object_tracker_utils
+  ${PROJECT_NAME}_utils
 )
 
-rclcpp_components_register_node(radar_object_tracker_node
-  PLUGIN "RadarObjectTrackerNode"
-  EXECUTABLE radar_object_tracker
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "autoware::radar_object_tracker::RadarObjectTrackerNode"
+  EXECUTABLE radar_object_tracker_node
 )
 
 # testing
@@ -52,15 +52,15 @@ if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
 
-  ament_add_gtest(test_radar_object_tracker_utils
+  ament_add_gtest(test_${PROJECT_NAME}_utils
     test/test_radar_object_tracker_utils.cpp
     src/utils/radar_object_tracker_utils.cpp
   )
-  target_include_directories(test_radar_object_tracker_utils PRIVATE
+  target_include_directories(test_${PROJECT_NAME}_utils PRIVATE
     ${CMAKE_CURRENT_SOURCE_DIR}/include
   )
-  target_link_libraries(test_radar_object_tracker_utils
-    radar_object_tracker_utils
+  target_link_libraries(test_${PROJECT_NAME}_utils
+    ${PROJECT_NAME}_utils
   )
 endif()
 
diff --git a/perception/radar_object_tracker/include/radar_object_tracker/data_association/data_association.hpp b/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/data_association.hpp
similarity index 78%
rename from perception/radar_object_tracker/include/radar_object_tracker/data_association/data_association.hpp
rename to perception/radar_object_tracker/include/autoware_radar_object_tracker/association/data_association.hpp
index 434fe66f13790..efde1e6763cdd 100644
--- a/perception/radar_object_tracker/include/radar_object_tracker/data_association/data_association.hpp
+++ b/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/data_association.hpp
@@ -16,25 +16,27 @@
 // Author: v1.0 Yukihiro Saito
 //
 
-#ifndef RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_
-#define RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_
-
-#include <list>
-#include <memory>
-#include <unordered_map>
-#include <vector>
+#ifndef AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__DATA_ASSOCIATION_HPP_
+#define AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__DATA_ASSOCIATION_HPP_
 
 #define EIGEN_MPL2_ONLY
+
+#include "autoware_radar_object_tracker/association/solver/gnn_solver.hpp"
+#include "autoware_radar_object_tracker/tracker/tracker.hpp"
 #include "object_recognition_utils/object_recognition_utils.hpp"
-#include "radar_object_tracker/data_association/solver/gnn_solver.hpp"
-#include "radar_object_tracker/tracker/tracker.hpp"
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 
-#include <autoware_perception_msgs/msg/detected_objects.hpp>
+#include "autoware_perception_msgs/msg/detected_objects.hpp"
 
+#include <list>
+#include <memory>
 #include <string>
+#include <unordered_map>
+#include <vector>
+namespace autoware::radar_object_tracker
+{
 class DataAssociation
 {
 private:
@@ -62,5 +64,5 @@ class DataAssociation
     const std::string & file_name);
   virtual ~DataAssociation() {}
 };
-
-#endif  // RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_
+}  // namespace autoware::radar_object_tracker
+#endif  // AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__DATA_ASSOCIATION_HPP_
diff --git a/perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/gnn_solver.hpp b/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver.hpp
similarity index 55%
rename from perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/gnn_solver.hpp
rename to perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver.hpp
index 6d531ac2ee8c2..2aa4a577ef91c 100644
--- a/perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/gnn_solver.hpp
+++ b/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver.hpp
@@ -12,11 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_
-#define RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_
+#ifndef AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_
+#define AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_
 
-#include "radar_object_tracker/data_association/solver/gnn_solver_interface.hpp"
-#include "radar_object_tracker/data_association/solver/mu_successive_shortest_path.hpp"
-#include "radar_object_tracker/data_association/solver/successive_shortest_path.hpp"
+#include "autoware_radar_object_tracker/association/solver/gnn_solver_interface.hpp"
+#include "autoware_radar_object_tracker/association/solver/mu_ssp.hpp"
+#include "autoware_radar_object_tracker/association/solver/ssp.hpp"
 
-#endif  // RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_
+#endif  // AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_
diff --git a/perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/gnn_solver_interface.hpp b/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver_interface.hpp
similarity index 71%
rename from perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/gnn_solver_interface.hpp
rename to perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver_interface.hpp
index 9b633f04ca190..ee2e98ba47278 100644
--- a/perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/gnn_solver_interface.hpp
+++ b/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver_interface.hpp
@@ -12,13 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
-#define RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
+#ifndef AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
+#define AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
 
 #include <unordered_map>
 #include <vector>
 
-namespace gnn_solver
+namespace autoware::radar_object_tracker::gnn_solver
 {
 class GnnSolverInterface
 {
@@ -30,6 +30,6 @@ class GnnSolverInterface
     const std::vector<std::vector<double>> & cost, std::unordered_map<int, int> * direct_assignment,
     std::unordered_map<int, int> * reverse_assignment) = 0;
 };
-}  // namespace gnn_solver
+}  // namespace autoware::radar_object_tracker::gnn_solver
 
-#endif  // RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
+#endif  // AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
diff --git a/perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/mu_successive_shortest_path.hpp b/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/mu_ssp.hpp
similarity index 69%
rename from perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/mu_successive_shortest_path.hpp
rename to perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/mu_ssp.hpp
index d4bdd72248670..2a5a336a3eb87 100644
--- a/perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/mu_successive_shortest_path.hpp
+++ b/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/mu_ssp.hpp
@@ -12,15 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_
-#define RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_
+#ifndef AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__MU_SSP_HPP_
+#define AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__MU_SSP_HPP_
 
-#include "radar_object_tracker/data_association/solver/gnn_solver_interface.hpp"
+#include "autoware_radar_object_tracker/association/solver/gnn_solver_interface.hpp"
 
 #include <unordered_map>
 #include <vector>
 
-namespace gnn_solver
+namespace autoware::radar_object_tracker::gnn_solver
 {
 class MuSSP : public GnnSolverInterface
 {
@@ -32,6 +32,6 @@ class MuSSP : public GnnSolverInterface
     const std::vector<std::vector<double>> & cost, std::unordered_map<int, int> * direct_assignment,
     std::unordered_map<int, int> * reverse_assignment) override;
 };
-}  // namespace gnn_solver
+}  // namespace autoware::radar_object_tracker::gnn_solver
 
-#endif  // RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_
+#endif  // AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__MU_SSP_HPP_
diff --git a/perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/successive_shortest_path.hpp b/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/ssp.hpp
similarity index 69%
rename from perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/successive_shortest_path.hpp
rename to perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/ssp.hpp
index b08e3c0d302a7..c956e1ebd2f6d 100644
--- a/perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/successive_shortest_path.hpp
+++ b/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/ssp.hpp
@@ -12,15 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_
-#define RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_
+#ifndef AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__SSP_HPP_
+#define AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__SSP_HPP_
 
-#include "radar_object_tracker/data_association/solver/gnn_solver_interface.hpp"
+#include "autoware_radar_object_tracker/association/solver/gnn_solver_interface.hpp"
 
 #include <unordered_map>
 #include <vector>
 
-namespace gnn_solver
+namespace autoware::radar_object_tracker::gnn_solver
 {
 class SSP : public GnnSolverInterface
 {
@@ -32,6 +32,6 @@ class SSP : public GnnSolverInterface
     const std::vector<std::vector<double>> & cost, std::unordered_map<int, int> * direct_assignment,
     std::unordered_map<int, int> * reverse_assignment) override;
 };
-}  // namespace gnn_solver
+}  // namespace autoware::radar_object_tracker::gnn_solver
 
-#endif  // RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_
+#endif  // AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__SSP_HPP_
diff --git a/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp b/perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp
similarity index 86%
rename from perception/radar_object_tracker/include/radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp
rename to perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp
index 7294f8745bec4..dab4d3f8efa24 100644
--- a/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp
+++ b/perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp
@@ -12,15 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_
-#define RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_
+#ifndef AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_
+#define AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_
 
-#include "radar_object_tracker/tracker/model/tracker_base.hpp"
-
-#include <kalman_filter/kalman_filter.hpp>
+#include "autoware_radar_object_tracker/tracker/model/tracker_base.hpp"
+#include "kalman_filter/kalman_filter.hpp"
 
 #include <string>
-
+namespace autoware::radar_object_tracker
+{
 using Label = autoware_perception_msgs::msg::ObjectClassification;
 class ConstantTurnRateMotionTracker : public Tracker  // means constant turn rate motion tracker
 {
@@ -108,5 +108,5 @@ class ConstantTurnRateMotionTracker : public Tracker  // means constant turn rat
     autoware_perception_msgs::msg::TrackedObject & object) const override;
   virtual ~ConstantTurnRateMotionTracker() {}
 };
-
-#endif  // RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_
+}  // namespace autoware::radar_object_tracker
+#endif  // AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_
diff --git a/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/linear_motion_tracker.hpp b/perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/linear_motion_tracker.hpp
similarity index 87%
rename from perception/radar_object_tracker/include/radar_object_tracker/tracker/model/linear_motion_tracker.hpp
rename to perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/linear_motion_tracker.hpp
index 39b0eaa417c88..7c96aa8fbaa36 100644
--- a/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/linear_motion_tracker.hpp
+++ b/perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/linear_motion_tracker.hpp
@@ -12,15 +12,17 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RADAR_OBJECT_TRACKER__TRACKER__MODEL__LINEAR_MOTION_TRACKER_HPP_
-#define RADAR_OBJECT_TRACKER__TRACKER__MODEL__LINEAR_MOTION_TRACKER_HPP_
+#ifndef AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__MODEL__LINEAR_MOTION_TRACKER_HPP_
+#define AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__MODEL__LINEAR_MOTION_TRACKER_HPP_
 
-#include "radar_object_tracker/tracker/model/tracker_base.hpp"
-
-#include <kalman_filter/kalman_filter.hpp>
+#include "autoware_radar_object_tracker/tracker/model/tracker_base.hpp"
+#include "kalman_filter/kalman_filter.hpp"
 
 #include <string>
 
+namespace autoware::radar_object_tracker
+{
+
 using Label = autoware_perception_msgs::msg::ObjectClassification;
 class LinearMotionTracker : public Tracker
 {
@@ -111,5 +113,5 @@ class LinearMotionTracker : public Tracker
     autoware_perception_msgs::msg::TrackedObject & object) const override;
   virtual ~LinearMotionTracker() {}
 };
-
-#endif  // RADAR_OBJECT_TRACKER__TRACKER__MODEL__LINEAR_MOTION_TRACKER_HPP_
+}  // namespace autoware::radar_object_tracker
+#endif  // AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__MODEL__LINEAR_MOTION_TRACKER_HPP_
diff --git a/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/tracker_base.hpp b/perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/tracker_base.hpp
similarity index 89%
rename from perception/radar_object_tracker/include/radar_object_tracker/tracker/model/tracker_base.hpp
rename to perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/tracker_base.hpp
index 3135aa45b87b4..caea725ef8f81 100644
--- a/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/tracker_base.hpp
+++ b/perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/tracker_base.hpp
@@ -16,12 +16,12 @@
 // Author: v1.0 Yukihiro Saito
 //
 
-#ifndef RADAR_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_
-#define RADAR_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_
+#ifndef AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_
+#define AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_
 
 #define EIGEN_MPL2_ONLY
+#include "autoware_radar_object_tracker/utils/utils.hpp"
 #include "object_recognition_utils/object_recognition_utils.hpp"
-#include "radar_object_tracker/utils/utils.hpp"
 
 #include <Eigen/Core>
 #include <rclcpp/rclcpp.hpp>
@@ -33,6 +33,8 @@
 
 #include <vector>
 
+namespace autoware::radar_object_tracker
+{
 class Tracker
 {
 protected:
@@ -92,5 +94,5 @@ class Tracker
     const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const = 0;
   virtual bool predict(const rclcpp::Time & time) = 0;
 };
-
-#endif  // RADAR_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_
+}  // namespace autoware::radar_object_tracker
+#endif  // AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_
diff --git a/perception/radar_object_tracker/include/radar_object_tracker/tracker/tracker.hpp b/perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/tracker.hpp
similarity index 79%
rename from perception/radar_object_tracker/include/radar_object_tracker/tracker/tracker.hpp
rename to perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/tracker.hpp
index 70045e6b16a07..26222f4c65679 100644
--- a/perception/radar_object_tracker/include/radar_object_tracker/tracker/tracker.hpp
+++ b/perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/tracker.hpp
@@ -12,11 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RADAR_OBJECT_TRACKER__TRACKER__TRACKER_HPP_
-#define RADAR_OBJECT_TRACKER__TRACKER__TRACKER_HPP_
+#ifndef AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__TRACKER_HPP_
+#define AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__TRACKER_HPP_
 
 #include "model/constant_turn_rate_motion_tracker.hpp"
 #include "model/linear_motion_tracker.hpp"
 #include "model/tracker_base.hpp"
 
-#endif  // RADAR_OBJECT_TRACKER__TRACKER__TRACKER_HPP_
+#endif  // AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__TRACKER_HPP_
diff --git a/perception/radar_object_tracker/include/radar_object_tracker/utils/radar_object_tracker_utils.hpp b/perception/radar_object_tracker/include/autoware_radar_object_tracker/utils/radar_object_tracker_utils.hpp
similarity index 81%
rename from perception/radar_object_tracker/include/radar_object_tracker/utils/radar_object_tracker_utils.hpp
rename to perception/radar_object_tracker/include/autoware_radar_object_tracker/utils/radar_object_tracker_utils.hpp
index e1c33c022e427..c3aaeb92ee951 100644
--- a/perception/radar_object_tracker/include/radar_object_tracker/utils/radar_object_tracker_utils.hpp
+++ b/perception/radar_object_tracker/include/autoware_radar_object_tracker/utils/radar_object_tracker_utils.hpp
@@ -12,12 +12,17 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RADAR_OBJECT_TRACKER__UTILS__RADAR_OBJECT_TRACKER_UTILS_HPP_
-#define RADAR_OBJECT_TRACKER__UTILS__RADAR_OBJECT_TRACKER_UTILS_HPP_
+#ifndef AUTOWARE_RADAR_OBJECT_TRACKER__UTILS__RADAR_OBJECT_TRACKER_UTILS_HPP_
+#define AUTOWARE_RADAR_OBJECT_TRACKER__UTILS__RADAR_OBJECT_TRACKER_UTILS_HPP_
+
+#include "autoware/universe_utils/geometry/geometry.hpp"
+#include "autoware/universe_utils/math/unit_conversion.hpp"
 
 #include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
+#include <rclcpp/rclcpp.hpp>
 
+#include "autoware_perception_msgs/msg/tracked_object.hpp"
 #include <geometry_msgs/msg/transform.hpp>
 
 #include <boost/optional.hpp>
@@ -33,13 +38,6 @@
 #else
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #endif
-
-#include <autoware/universe_utils/geometry/geometry.hpp>
-#include <autoware/universe_utils/math/unit_conversion.hpp>
-#include <rclcpp/rclcpp.hpp>
-
-#include <autoware_perception_msgs/msg/tracked_object.hpp>
-
 #include <tf2/LinearMath/Transform.h>
 #include <tf2/convert.h>
 #include <tf2/transform_datatypes.h>
@@ -47,7 +45,8 @@
 
 #include <string>
 #include <utility>
-namespace radar_object_tracker_utils
+
+namespace autoware::radar_object_tracker::utils
 {
 
 boost::optional<geometry_msgs::msg::Transform> getTransformAnonymous(
@@ -72,6 +71,6 @@ bool hasValidVelocityDirectionToLanelet(
   const autoware_perception_msgs::msg::TrackedObject & object,
   const lanelet::ConstLanelets & lanelets, const double max_lateral_velocity);
 
-}  // namespace radar_object_tracker_utils
+}  // namespace autoware::radar_object_tracker::utils
 
-#endif  // RADAR_OBJECT_TRACKER__UTILS__RADAR_OBJECT_TRACKER_UTILS_HPP_
+#endif  // AUTOWARE_RADAR_OBJECT_TRACKER__UTILS__RADAR_OBJECT_TRACKER_UTILS_HPP_
diff --git a/perception/radar_object_tracker/include/radar_object_tracker/utils/utils.hpp b/perception/radar_object_tracker/include/autoware_radar_object_tracker/utils/utils.hpp
similarity index 53%
rename from perception/radar_object_tracker/include/radar_object_tracker/utils/utils.hpp
rename to perception/radar_object_tracker/include/autoware_radar_object_tracker/utils/utils.hpp
index 019d684e8bd46..dd011ac274b64 100644
--- a/perception/radar_object_tracker/include/radar_object_tracker/utils/utils.hpp
+++ b/perception/radar_object_tracker/include/autoware_radar_object_tracker/utils/utils.hpp
@@ -16,16 +16,16 @@
 // Author: v1.0 Yukihiro Saito
 //
 
-#ifndef RADAR_OBJECT_TRACKER__UTILS__UTILS_HPP_
-#define RADAR_OBJECT_TRACKER__UTILS__UTILS_HPP_
+#ifndef AUTOWARE_RADAR_OBJECT_TRACKER__UTILS__UTILS_HPP_
+#define AUTOWARE_RADAR_OBJECT_TRACKER__UTILS__UTILS_HPP_
 
 #include <Eigen/Core>
 #include <Eigen/Dense>
 #include <Eigen/Geometry>
 
-#include <autoware_perception_msgs/msg/detected_object.hpp>
-#include <autoware_perception_msgs/msg/shape.hpp>
-#include <autoware_perception_msgs/msg/tracked_object.hpp>
+#include "autoware_perception_msgs/msg/detected_object.hpp"
+#include "autoware_perception_msgs/msg/shape.hpp"
+#include "autoware_perception_msgs/msg/tracked_object.hpp"
 #include <geometry_msgs/msg/polygon.hpp>
 #include <geometry_msgs/msg/vector3.hpp>
 
@@ -33,52 +33,12 @@
 #include <cmath>
 #include <tuple>
 #include <vector>
-
-namespace utils
+namespace autoware::radar_object_tracker::utils
 {
-enum MSG_COV_IDX {
-  X_X = 0,
-  X_Y = 1,
-  X_Z = 2,
-  X_ROLL = 3,
-  X_PITCH = 4,
-  X_YAW = 5,
-  Y_X = 6,
-  Y_Y = 7,
-  Y_Z = 8,
-  Y_ROLL = 9,
-  Y_PITCH = 10,
-  Y_YAW = 11,
-  Z_X = 12,
-  Z_Y = 13,
-  Z_Z = 14,
-  Z_ROLL = 15,
-  Z_PITCH = 16,
-  Z_YAW = 17,
-  ROLL_X = 18,
-  ROLL_Y = 19,
-  ROLL_Z = 20,
-  ROLL_ROLL = 21,
-  ROLL_PITCH = 22,
-  ROLL_YAW = 23,
-  PITCH_X = 24,
-  PITCH_Y = 25,
-  PITCH_Z = 26,
-  PITCH_ROLL = 27,
-  PITCH_PITCH = 28,
-  PITCH_YAW = 29,
-  YAW_X = 30,
-  YAW_Y = 31,
-  YAW_Z = 32,
-  YAW_ROLL = 33,
-  YAW_PITCH = 34,
-  YAW_YAW = 35
-};
-
 // matrix concatenate
 Eigen::MatrixXd stackMatricesVertically(const std::vector<Eigen::MatrixXd> & matrices);
 Eigen::MatrixXd stackMatricesDiagonally(const std::vector<Eigen::MatrixXd> & matrices);
 
-}  // namespace utils
+}  // namespace autoware::radar_object_tracker::utils
 
-#endif  // RADAR_OBJECT_TRACKER__UTILS__UTILS_HPP_
+#endif  // AUTOWARE_RADAR_OBJECT_TRACKER__UTILS__UTILS_HPP_
diff --git a/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml b/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml
index dfebf334456ea..313ef4b0f9fcd 100644
--- a/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml
+++ b/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml
@@ -7,7 +7,7 @@
   <arg name="radar_object_tracker_param_path" default="$(find-pkg-share radar_object_tracker)/config/radar_object_tracker.param.yaml"/>
   <arg name="vector_map_topic" default="/map/vector_map"/>
 
-  <node pkg="radar_object_tracker" exec="radar_object_tracker" name="radar_object_tracker" output="screen">
+  <node pkg="radar_object_tracker" exec="radar_object_tracker_node" name="radar_object_tracker" output="screen">
     <remap from="input" to="$(var input)"/>
     <remap from="output" to="$(var output)"/>
     <remap from="/vector_map" to="$(var vector_map_topic)"/>
diff --git a/perception/radar_object_tracker/package.xml b/perception/radar_object_tracker/package.xml
index c3c4b33946b56..00b9b19113048 100644
--- a/perception/radar_object_tracker/package.xml
+++ b/perception/radar_object_tracker/package.xml
@@ -27,7 +27,6 @@
   <depend>rclcpp_components</depend>
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
-  <depend>tier4_perception_msgs</depend>
   <depend>unique_identifier_msgs</depend>
   <depend>yaml-cpp</depend>
 
diff --git a/perception/radar_object_tracker/src/data_association/data_association.cpp b/perception/radar_object_tracker/src/association/data_association.cpp
similarity index 98%
rename from perception/radar_object_tracker/src/data_association/data_association.cpp
rename to perception/radar_object_tracker/src/association/data_association.cpp
index b308f62a24dfa..ac57a3f0a87c0 100644
--- a/perception/radar_object_tracker/src/data_association/data_association.cpp
+++ b/perception/radar_object_tracker/src/association/data_association.cpp
@@ -12,14 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "radar_object_tracker/data_association/data_association.hpp"
+#include "autoware_radar_object_tracker/association/data_association.hpp"
 
-#include "radar_object_tracker/data_association/solver/gnn_solver.hpp"
+#include "autoware_radar_object_tracker/association/solver/gnn_solver.hpp"
 
 #include <nlohmann/json.hpp>
 
-// #include "multi_object_tracker/utils/utils.hpp"
-
 #include <algorithm>
 #include <fstream>
 #include <list>
@@ -71,6 +69,8 @@ double getFormedYawAngle(
 }
 }  // namespace
 
+namespace autoware::radar_object_tracker
+{
 DataAssociation::DataAssociation(
   std::vector<int> can_assign_vector, std::vector<double> max_dist_vector,
   std::vector<double> max_area_vector, std::vector<double> min_area_vector,
@@ -294,3 +294,5 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix(
 
   return score_matrix;
 }
+
+}  // namespace autoware::radar_object_tracker
diff --git a/perception/radar_object_tracker/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp b/perception/radar_object_tracker/src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp
similarity index 86%
rename from perception/radar_object_tracker/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp
rename to perception/radar_object_tracker/src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp
index 43f974c2621e1..2f47e3b8147d5 100644
--- a/perception/radar_object_tracker/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp
+++ b/perception/radar_object_tracker/src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "radar_object_tracker/data_association/solver/mu_successive_shortest_path.hpp"
+#include "autoware_radar_object_tracker/association/solver/mu_ssp.hpp"
 
 #include <mussp/mussp.h>
 
@@ -24,7 +24,7 @@
 #include <unordered_map>
 #include <vector>
 
-namespace gnn_solver
+namespace autoware::radar_object_tracker::gnn_solver
 {
 void MuSSP::maximizeLinearAssignment(
   const std::vector<std::vector<double>> & cost, std::unordered_map<int, int> * direct_assignment,
@@ -38,4 +38,4 @@ void MuSSP::maximizeLinearAssignment(
   // Solve DA by muSSP
   solve_muSSP(cost, direct_assignment, reverse_assignment);
 }
-}  // namespace gnn_solver
+}  // namespace autoware::radar_object_tracker::gnn_solver
diff --git a/perception/radar_object_tracker/src/data_association/successive_shortest_path/successive_shortest_path.cpp b/perception/radar_object_tracker/src/association/ssp/successive_shortest_path.cpp
similarity index 98%
rename from perception/radar_object_tracker/src/data_association/successive_shortest_path/successive_shortest_path.cpp
rename to perception/radar_object_tracker/src/association/ssp/successive_shortest_path.cpp
index c904a9005b27e..7877fe5451d8d 100644
--- a/perception/radar_object_tracker/src/data_association/successive_shortest_path/successive_shortest_path.cpp
+++ b/perception/radar_object_tracker/src/association/ssp/successive_shortest_path.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "radar_object_tracker/data_association/solver/successive_shortest_path.hpp"
+#include "autoware_radar_object_tracker/association/solver/ssp.hpp"
 
 #include <algorithm>
 #include <cassert>
@@ -22,7 +22,7 @@
 #include <utility>
 #include <vector>
 
-namespace gnn_solver
+namespace autoware::radar_object_tracker::gnn_solver
 {
 struct ResidualEdge
 {
@@ -367,4 +367,4 @@ void SSP::maximizeLinearAssignment(
   }
 #endif
 }
-}  // namespace gnn_solver
+}  // namespace autoware::radar_object_tracker::gnn_solver
diff --git a/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp b/perception/radar_object_tracker/src/radar_object_tracker_node.cpp
similarity index 95%
rename from perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp
rename to perception/radar_object_tracker/src/radar_object_tracker_node.cpp
index 7a868b8b11118..10bf11e46abb9 100644
--- a/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp
+++ b/perception/radar_object_tracker/src/radar_object_tracker_node.cpp
@@ -12,15 +12,16 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp"
+#define EIGEN_MPL2_ONLY
+
+#include "radar_object_tracker_node.hpp"
 
-#include "radar_object_tracker/utils/radar_object_tracker_utils.hpp"
-#include "radar_object_tracker/utils/utils.hpp"
+#include "autoware_radar_object_tracker/utils/radar_object_tracker_utils.hpp"
+#include "autoware_radar_object_tracker/utils/utils.hpp"
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 #include <ament_index_cpp/get_package_share_directory.hpp>
-#include <rclcpp_components/register_node_macro.hpp>
 
 #include <boost/optional.hpp>
 
@@ -35,8 +36,9 @@
 #include <unordered_map>
 #include <utility>
 #include <vector>
-#define EIGEN_MPL2_ONLY
 
+namespace autoware::radar_object_tracker
+{
 using Label = autoware_perception_msgs::msg::ObjectClassification;
 
 RadarObjectTrackerNode::RadarObjectTrackerNode(const rclcpp::NodeOptions & node_options)
@@ -140,7 +142,7 @@ void RadarObjectTrackerNode::onMap(const LaneletMapBin::ConstSharedPtr msg)
 void RadarObjectTrackerNode::onMeasurement(
   const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg)
 {
-  const auto self_transform = radar_object_tracker_utils::getTransformAnonymous(
+  const auto self_transform = autoware::radar_object_tracker::utils::getTransformAnonymous(
     tf_buffer_, "base_link", world_frame_id_, input_objects_msg->header.stamp);
   if (!self_transform) {
     return;
@@ -236,7 +238,7 @@ std::shared_ptr<Tracker> RadarObjectTrackerNode::createNewTracker(
 void RadarObjectTrackerNode::onTimer()
 {
   rclcpp::Time current_time = this->now();
-  const auto self_transform = radar_object_tracker_utils::getTransformAnonymous(
+  const auto self_transform = autoware::radar_object_tracker::utils::getTransformAnonymous(
     tf_buffer_, world_frame_id_, "base_link", current_time);
   if (!self_transform) {
     return;
@@ -283,14 +285,14 @@ void RadarObjectTrackerNode::mapBasedNoiseFilter(
   for (auto itr = list_tracker.begin(); itr != list_tracker.end(); ++itr) {
     autoware_perception_msgs::msg::TrackedObject object;
     (*itr)->getTrackedObject(time, object);
-    const auto closest_lanelets = radar_object_tracker_utils::getClosestValidLanelets(
+    const auto closest_lanelets = autoware::radar_object_tracker::utils::getClosestValidLanelets(
       object, lanelet_map_ptr_, max_distance_from_lane_, max_angle_diff_from_lane_);
 
     // 1. If the object is not close to any lanelet, delete the tracker
     const bool no_closest_lanelet = closest_lanelets.empty();
     // 2. If the object velocity direction is not close to the lanelet direction, delete the tracker
     const bool is_velocity_direction_close_to_lanelet =
-      radar_object_tracker_utils::hasValidVelocityDirectionToLanelet(
+      autoware::radar_object_tracker::utils::hasValidVelocityDirectionToLanelet(
         object, closest_lanelets, max_lateral_velocity_);
     if (no_closest_lanelet || !is_velocity_direction_close_to_lanelet) {
       // std::cout << "object removed due to map based noise filter" << " no close lanelet: " <<
@@ -421,5 +423,8 @@ void RadarObjectTrackerNode::publish(const rclcpp::Time & time) const
   // Publish
   tracked_objects_pub_->publish(output_msg);
 }
+}  // namespace autoware::radar_object_tracker
+
+#include <rclcpp_components/register_node_macro.hpp>
 
-RCLCPP_COMPONENTS_REGISTER_NODE(RadarObjectTrackerNode)
+RCLCPP_COMPONENTS_REGISTER_NODE(autoware::radar_object_tracker::RadarObjectTrackerNode)
diff --git a/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp b/perception/radar_object_tracker/src/radar_object_tracker_node.hpp
similarity index 90%
rename from perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp
rename to perception/radar_object_tracker/src/radar_object_tracker_node.hpp
index fe6d472eddd08..dee2cd549c986 100644
--- a/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp
+++ b/perception/radar_object_tracker/src/radar_object_tracker_node.hpp
@@ -12,17 +12,26 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RADAR_OBJECT_TRACKER__RADAR_OBJECT_TRACKER_NODE__RADAR_OBJECT_TRACKER_NODE_HPP_
-#define RADAR_OBJECT_TRACKER__RADAR_OBJECT_TRACKER_NODE__RADAR_OBJECT_TRACKER_NODE_HPP_
+#ifndef RADAR_OBJECT_TRACKER_NODE_HPP_
+#define RADAR_OBJECT_TRACKER_NODE_HPP_
 
-#include "radar_object_tracker/data_association/data_association.hpp"
+#include "autoware_radar_object_tracker/association/data_association.hpp"
 
+#include <lanelet2_extension/utility/message_conversion.hpp>
+#include <lanelet2_extension/utility/query.hpp>
+#include <lanelet2_extension/utility/utilities.hpp>
 #include <rclcpp/rclcpp.hpp>
 
-#include <autoware_perception_msgs/msg/detected_objects.hpp>
-#include <autoware_perception_msgs/msg/tracked_objects.hpp>
+#include "autoware_perception_msgs/msg/detected_objects.hpp"
+#include "autoware_perception_msgs/msg/tracked_objects.hpp"
 #include <geometry_msgs/msg/pose_stamped.hpp>
 
+#include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_core/geometry/BoundingBox.h>
+#include <lanelet2_core/geometry/Lanelet.h>
+#include <lanelet2_core/geometry/Point.h>
+#include <lanelet2_routing/RoutingGraph.h>
+#include <lanelet2_traffic_rules/TrafficRulesFactory.h>
 #include <tf2/LinearMath/Transform.h>
 #include <tf2/convert.h>
 #include <tf2/transform_datatypes.h>
@@ -32,17 +41,6 @@
 #else
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #endif
-
-#include <lanelet2_extension/utility/message_conversion.hpp>
-#include <lanelet2_extension/utility/query.hpp>
-#include <lanelet2_extension/utility/utilities.hpp>
-
-#include <lanelet2_core/LaneletMap.h>
-#include <lanelet2_core/geometry/BoundingBox.h>
-#include <lanelet2_core/geometry/Lanelet.h>
-#include <lanelet2_core/geometry/Point.h>
-#include <lanelet2_routing/RoutingGraph.h>
-#include <lanelet2_traffic_rules/TrafficRulesFactory.h>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 
@@ -51,6 +49,8 @@
 #include <memory>
 #include <string>
 
+namespace autoware::radar_object_tracker
+{
 using autoware_map_msgs::msg::LaneletMapBin;
 using autoware_perception_msgs::msg::DetectedObject;
 using autoware_perception_msgs::msg::DetectedObjects;
@@ -132,4 +132,6 @@ class RadarObjectTrackerNode : public rclcpp::Node
   inline bool shouldTrackerPublish(const std::shared_ptr<const Tracker> tracker) const;
 };
 
-#endif  // RADAR_OBJECT_TRACKER__RADAR_OBJECT_TRACKER_NODE__RADAR_OBJECT_TRACKER_NODE_HPP_
+}  // namespace autoware::radar_object_tracker
+
+#endif  // RADAR_OBJECT_TRACKER_NODE_HPP_
diff --git a/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp b/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp
index e1603fbfab702..057a8653faea0 100644
--- a/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp
+++ b/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp
@@ -16,12 +16,14 @@
 // Author: v1.0 Yukihiro Saito
 //
 
-#include "radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp"
+#define EIGEN_MPL2_ONLY
 
-#include "radar_object_tracker/utils/utils.hpp"
+#include "autoware_radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp"
 
-#include <autoware/universe_utils/geometry/boost_polygon_utils.hpp>
-#include <autoware/universe_utils/math/unit_conversion.hpp>
+#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp"
+#include "autoware/universe_utils/math/unit_conversion.hpp"
+#include "autoware/universe_utils/ros/msg_covariance.hpp"
+#include "autoware_radar_object_tracker/utils/utils.hpp"
 
 #include <bits/stdc++.h>
 #include <tf2/LinearMath/Matrix3x3.h>
@@ -34,7 +36,6 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #endif
 
-#define EIGEN_MPL2_ONLY
 #include <Eigen/Core>
 #include <Eigen/Dense>
 #include <Eigen/Geometry>
@@ -42,7 +43,10 @@
 
 #include <yaml-cpp/yaml.h>
 
+namespace autoware::radar_object_tracker
+{
 using Label = autoware_perception_msgs::msg::ObjectClassification;
+using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
 
 // init static member variables
 bool ConstantTurnRateMotionTracker::is_initialized_ = false;
@@ -114,10 +118,10 @@ ConstantTurnRateMotionTracker::ConstantTurnRateMotionTracker(
   // Rotate the covariance matrix according to the vehicle yaw
   // because p0_cov_x and y are in the vehicle coordinate system.
   if (object.kinematics.has_position_covariance) {
-    P_xy_local << object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X],
-      object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y],
-      object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X],
-      object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y];
+    P_xy_local << object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_X],
+      object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_Y],
+      object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Y_X],
+      object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Y_Y];
     P_xy = R * P_xy_local * R.transpose();
   } else {
     // rotate
@@ -128,9 +132,9 @@ ConstantTurnRateMotionTracker::ConstantTurnRateMotionTracker(
 
   // covariance often written in object frame
   if (object.kinematics.has_twist_covariance) {
-    const auto vx_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X];
+    const auto vx_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X];
     // const auto vy_cov =
-    // object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y];
+    // object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::Y_Y];
     P(IDX::VX, IDX::VX) = vx_cov;
   } else {
     P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx;
@@ -371,11 +375,11 @@ bool ConstantTurnRateMotionTracker::measureWithPose(
       Rxy_local << ekf_params_.r_cov_x, 0, 0, r_cov_y;  // xy in base_link coordinate
       Rxy = RotationBaseLink * Rxy_local * RotationBaseLink.transpose();
     } else {
-      Rxy_local << object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X],
-        object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y],
-        object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X],
+      Rxy_local << object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_X],
+        object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_Y],
+        object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Y_X],
         object.kinematics.pose_with_covariance
-          .covariance[utils::MSG_COV_IDX::Y_Y];  // xy in vehicle coordinate
+          .covariance[XYZRPY_COV_IDX::Y_Y];  // xy in vehicle coordinate
       Rxy = RotationYaw * Rxy_local * RotationYaw.transpose();
     }
     R_block_list.push_back(Rxy);
@@ -427,7 +431,7 @@ bool ConstantTurnRateMotionTracker::measureWithPose(
     if (!object.kinematics.has_twist_covariance) {
       R_vx << ekf_params_.r_cov_vx;
     } else {
-      R_vx << object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X];
+      R_vx << object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X];
     }
     R_block_list.push_back(R_vx);
   }
@@ -442,9 +446,9 @@ bool ConstantTurnRateMotionTracker::measureWithPose(
   }
 
   // stacking matrices vertically or diagonally
-  const auto C = utils::stackMatricesVertically(C_list);
-  const auto Y = utils::stackMatricesVertically(Y_list);
-  const auto R = utils::stackMatricesDiagonally(R_block_list);
+  const auto C = autoware::radar_object_tracker::utils::stackMatricesVertically(C_list);
+  const auto Y = autoware::radar_object_tracker::utils::stackMatricesVertically(Y_list);
+  const auto R = autoware::radar_object_tracker::utils::stackMatricesDiagonally(R_block_list);
 
   // 4. EKF update
   if (!ekf_.update(Y, C, R)) {
@@ -582,27 +586,27 @@ bool ConstantTurnRateMotionTracker::getTrackedObject(
   constexpr double z_cov = 1. * 1.;      // TODO(yukkysaito) Currently tentative
   constexpr double yaw_cov = 0.1 * 0.1;  // TODO(yukkysaito) Currently tentative
 
-  pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P_xy(IDX::X, IDX::X);
-  pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P_xy(IDX::X, IDX::Y);
-  pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P_xy(IDX::Y, IDX::X);
-  pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P_xy(IDX::Y, IDX::Y);
-  pose_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = z_cov;
-  pose_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = no_info_cov;
-  pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov;
-  pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = yaw_cov;
+  pose_with_cov.covariance[XYZRPY_COV_IDX::X_X] = P_xy(IDX::X, IDX::X);
+  pose_with_cov.covariance[XYZRPY_COV_IDX::X_Y] = P_xy(IDX::X, IDX::Y);
+  pose_with_cov.covariance[XYZRPY_COV_IDX::Y_X] = P_xy(IDX::Y, IDX::X);
+  pose_with_cov.covariance[XYZRPY_COV_IDX::Y_Y] = P_xy(IDX::Y, IDX::Y);
+  pose_with_cov.covariance[XYZRPY_COV_IDX::Z_Z] = z_cov;
+  pose_with_cov.covariance[XYZRPY_COV_IDX::ROLL_ROLL] = no_info_cov;
+  pose_with_cov.covariance[XYZRPY_COV_IDX::PITCH_PITCH] = no_info_cov;
+  pose_with_cov.covariance[XYZRPY_COV_IDX::YAW_YAW] = yaw_cov;
 
   // twist covariance
   constexpr double vz_cov = 0.2 * 0.2;  // TODO(Yoshi Ri) Currently tentative
   constexpr double wz_cov = 0.2 * 0.2;  // TODO(yukkysaito) Currently tentative
 
-  twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX);
-  twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = 0.0;
-  twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = 0.0;
-  twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = no_info_cov;
-  twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov;
-  twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = no_info_cov;
-  twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov;
-  twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = wz_cov;
+  twist_with_cov.covariance[XYZRPY_COV_IDX::X_X] = P(IDX::VX, IDX::VX);
+  twist_with_cov.covariance[XYZRPY_COV_IDX::X_Y] = 0.0;
+  twist_with_cov.covariance[XYZRPY_COV_IDX::Y_X] = 0.0;
+  twist_with_cov.covariance[XYZRPY_COV_IDX::Y_Y] = no_info_cov;
+  twist_with_cov.covariance[XYZRPY_COV_IDX::Z_Z] = vz_cov;
+  twist_with_cov.covariance[XYZRPY_COV_IDX::ROLL_ROLL] = no_info_cov;
+  twist_with_cov.covariance[XYZRPY_COV_IDX::PITCH_PITCH] = no_info_cov;
+  twist_with_cov.covariance[XYZRPY_COV_IDX::YAW_YAW] = wz_cov;
 
   // set shape
   if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) {
@@ -622,3 +626,5 @@ bool ConstantTurnRateMotionTracker::getTrackedObject(
 
   return true;
 }
+
+}  // namespace autoware::radar_object_tracker
diff --git a/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp b/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp
index 1bc548fa7a951..a9f7fffff1156 100644
--- a/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp
+++ b/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp
@@ -16,12 +16,14 @@
 // Author: v1.0 Yukihiro Saito
 //
 
-#include "radar_object_tracker/tracker/model/linear_motion_tracker.hpp"
+#define EIGEN_MPL2_ONLY
 
-#include "radar_object_tracker/utils/utils.hpp"
+#include "autoware_radar_object_tracker/tracker/model/linear_motion_tracker.hpp"
 
-#include <autoware/universe_utils/geometry/boost_polygon_utils.hpp>
-#include <autoware/universe_utils/math/unit_conversion.hpp>
+#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp"
+#include "autoware/universe_utils/math/unit_conversion.hpp"
+#include "autoware/universe_utils/ros/msg_covariance.hpp"
+#include "autoware_radar_object_tracker/utils/utils.hpp"
 
 #include <bits/stdc++.h>
 #include <tf2/LinearMath/Matrix3x3.h>
@@ -34,7 +36,6 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #endif
 
-#define EIGEN_MPL2_ONLY
 #include <Eigen/Core>
 #include <Eigen/Dense>
 #include <Eigen/Geometry>
@@ -42,7 +43,10 @@
 
 #include <yaml-cpp/yaml.h>
 
+namespace autoware::radar_object_tracker
+{
 using Label = autoware_perception_msgs::msg::ObjectClassification;
+using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
 
 // initialize static parameter
 bool LinearMotionTracker::is_initialized_ = false;
@@ -119,10 +123,10 @@ LinearMotionTracker::LinearMotionTracker(
   // Rotate the covariance matrix according to the vehicle yaw
   // because p0_cov_x and y are in the vehicle coordinate system.
   if (object.kinematics.has_position_covariance) {
-    P_xy_local << object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X],
-      object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y],
-      object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X],
-      object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y];
+    P_xy_local << object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_X],
+      object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_Y],
+      object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Y_X],
+      object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Y_Y];
     P_xy = R * P_xy_local * R.transpose();
   } else {
     // rotate
@@ -130,29 +134,15 @@ LinearMotionTracker::LinearMotionTracker(
   }
   // covariance often written in object frame
   if (object.kinematics.has_twist_covariance) {
-    const auto vx_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X];
-    const auto vy_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y];
+    const auto vx_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X];
+    const auto vy_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::Y_Y];
     P_v_xy_local << vx_cov, 0.0, 0.0, vy_cov;
     P_v_xy = R * P_v_xy_local * R.transpose();
   } else {
     P_v_xy = R * P_v_xy_local * R.transpose();
   }
-  // acceleration covariance often written in object frame
-  const bool has_acceleration_covariance =
-    false;  // currently message does not have acceleration covariance
-  if (has_acceleration_covariance) {
-    // const auto ax_cov =
-    //   object.kinematics.acceleration_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; // This
-    //   is future update
-    // const auto ay_cov =
-    //   object.kinematics.acceleration_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; // This
-    //   is future update
-    //  Eigen::Matrix2d P_a_xy_local;
-    //  P_a_xy_local << ax_cov, 0.0, 0.0, ay_cov;
-    P_a_xy = R * P_a_xy_local * R.transpose();
-  } else {
-    P_a_xy = R * P_a_xy_local * R.transpose();
-  }
+
+  P_a_xy = R * P_a_xy_local * R.transpose();
 
   // put value in P matrix
   // use block description. This assume x,y,vx,vy,ax,ay in this order
@@ -415,11 +405,11 @@ bool LinearMotionTracker::measureWithPose(
       Rxy_local << ekf_params_.r_cov_x, 0, 0, r_cov_y;  // xy in base_link coordinate
       Rxy = RotationBaseLink * Rxy_local * RotationBaseLink.transpose();
     } else {
-      Rxy_local << object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X],
-        object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y],
-        object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X],
+      Rxy_local << object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_X],
+        object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_Y],
+        object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Y_X],
         object.kinematics.pose_with_covariance
-          .covariance[utils::MSG_COV_IDX::Y_Y];  // xy in vehicle coordinate
+          .covariance[XYZRPY_COV_IDX::Y_Y];  // xy in vehicle coordinate
       Rxy = RotationYaw * Rxy_local * RotationYaw.transpose();
     }
     R_block_list.push_back(Rxy);
@@ -447,8 +437,8 @@ bool LinearMotionTracker::measureWithPose(
       R_v_xy_local << ekf_params_.r_cov_vx, 0, 0, ekf_params_.r_cov_vy;
       R_v_xy = RotationBaseLink * R_v_xy_local * RotationBaseLink.transpose();
     } else {
-      R_v_xy_local << object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X],
-        0, 0, object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y];
+      R_v_xy_local << object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X], 0, 0,
+        object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::Y_Y];
       R_v_xy = RotationYaw * R_v_xy_local * RotationYaw.transpose();
     }
     R_block_list.push_back(R_v_xy);
@@ -467,9 +457,9 @@ bool LinearMotionTracker::measureWithPose(
   // Eigen::MatrixXd R = Eigen::MatrixXd::Zero(row_number, row_number);
 
   // stacking matrices vertically or diagonally
-  const auto C = utils::stackMatricesVertically(C_list);
-  const auto Y = utils::stackMatricesVertically(Y_list);
-  const auto R = utils::stackMatricesDiagonally(R_block_list);
+  const auto C = autoware::radar_object_tracker::utils::stackMatricesVertically(C_list);
+  const auto Y = autoware::radar_object_tracker::utils::stackMatricesVertically(Y_list);
+  const auto R = autoware::radar_object_tracker::utils::stackMatricesDiagonally(R_block_list);
 
   // 4. EKF update
   if (!ekf_.update(Y, C, R)) {
@@ -637,37 +627,37 @@ bool LinearMotionTracker::getTrackedObject(
   constexpr double z_cov = 1. * 1.;      // TODO(yukkysaito) Currently tentative
   constexpr double yaw_cov = 0.1 * 0.1;  // TODO(yukkysaito) Currently tentative
 
-  pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P_xy(IDX::X, IDX::X);
-  pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P_xy(IDX::X, IDX::Y);
-  pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P_xy(IDX::Y, IDX::X);
-  pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P_xy(IDX::Y, IDX::Y);
-  pose_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = z_cov;
-  pose_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = no_info_cov;
-  pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov;
-  pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = yaw_cov;
+  pose_with_cov.covariance[XYZRPY_COV_IDX::X_X] = P_xy(IDX::X, IDX::X);
+  pose_with_cov.covariance[XYZRPY_COV_IDX::X_Y] = P_xy(IDX::X, IDX::Y);
+  pose_with_cov.covariance[XYZRPY_COV_IDX::Y_X] = P_xy(IDX::Y, IDX::X);
+  pose_with_cov.covariance[XYZRPY_COV_IDX::Y_Y] = P_xy(IDX::Y, IDX::Y);
+  pose_with_cov.covariance[XYZRPY_COV_IDX::Z_Z] = z_cov;
+  pose_with_cov.covariance[XYZRPY_COV_IDX::ROLL_ROLL] = no_info_cov;
+  pose_with_cov.covariance[XYZRPY_COV_IDX::PITCH_PITCH] = no_info_cov;
+  pose_with_cov.covariance[XYZRPY_COV_IDX::YAW_YAW] = yaw_cov;
 
   // twist covariance
   constexpr double vz_cov = 0.2 * 0.2;  // TODO(Yoshi Ri) Currently tentative
   constexpr double wz_cov = 0.2 * 0.2;  // TODO(yukkysaito) Currently tentative
 
-  twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P_v_xy(IDX::X, IDX::X);
-  twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P_v_xy(IDX::X, IDX::Y);
-  twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P_v_xy(IDX::Y, IDX::X);
-  twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P_v_xy(IDX::Y, IDX::Y);
-  twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov;
-  twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = no_info_cov;
-  twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov;
-  twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = wz_cov;
+  twist_with_cov.covariance[XYZRPY_COV_IDX::X_X] = P_v_xy(IDX::X, IDX::X);
+  twist_with_cov.covariance[XYZRPY_COV_IDX::X_Y] = P_v_xy(IDX::X, IDX::Y);
+  twist_with_cov.covariance[XYZRPY_COV_IDX::Y_X] = P_v_xy(IDX::Y, IDX::X);
+  twist_with_cov.covariance[XYZRPY_COV_IDX::Y_Y] = P_v_xy(IDX::Y, IDX::Y);
+  twist_with_cov.covariance[XYZRPY_COV_IDX::Z_Z] = vz_cov;
+  twist_with_cov.covariance[XYZRPY_COV_IDX::ROLL_ROLL] = no_info_cov;
+  twist_with_cov.covariance[XYZRPY_COV_IDX::PITCH_PITCH] = no_info_cov;
+  twist_with_cov.covariance[XYZRPY_COV_IDX::YAW_YAW] = wz_cov;
 
   // acceleration covariance
-  acceleration_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P_a_xy(IDX::X, IDX::X);
-  acceleration_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P_a_xy(IDX::X, IDX::Y);
-  acceleration_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P_a_xy(IDX::Y, IDX::X);
-  acceleration_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P_a_xy(IDX::Y, IDX::Y);
-  acceleration_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = no_info_cov;
-  acceleration_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = no_info_cov;
-  acceleration_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov;
-  acceleration_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = no_info_cov;
+  acceleration_with_cov.covariance[XYZRPY_COV_IDX::X_X] = P_a_xy(IDX::X, IDX::X);
+  acceleration_with_cov.covariance[XYZRPY_COV_IDX::X_Y] = P_a_xy(IDX::X, IDX::Y);
+  acceleration_with_cov.covariance[XYZRPY_COV_IDX::Y_X] = P_a_xy(IDX::Y, IDX::X);
+  acceleration_with_cov.covariance[XYZRPY_COV_IDX::Y_Y] = P_a_xy(IDX::Y, IDX::Y);
+  acceleration_with_cov.covariance[XYZRPY_COV_IDX::Z_Z] = no_info_cov;
+  acceleration_with_cov.covariance[XYZRPY_COV_IDX::ROLL_ROLL] = no_info_cov;
+  acceleration_with_cov.covariance[XYZRPY_COV_IDX::PITCH_PITCH] = no_info_cov;
+  acceleration_with_cov.covariance[XYZRPY_COV_IDX::YAW_YAW] = no_info_cov;
 
   // set shape
   if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) {
@@ -687,3 +677,4 @@ bool LinearMotionTracker::getTrackedObject(
 
   return true;
 }
+}  // namespace autoware::radar_object_tracker
diff --git a/perception/radar_object_tracker/src/tracker/model/tracker_base.cpp b/perception/radar_object_tracker/src/tracker/model/tracker_base.cpp
index d38b80067cf5a..ce63d96dfb83a 100644
--- a/perception/radar_object_tracker/src/tracker/model/tracker_base.cpp
+++ b/perception/radar_object_tracker/src/tracker/model/tracker_base.cpp
@@ -14,13 +14,15 @@
 //
 //
 
-#include "radar_object_tracker/tracker/model/tracker_base.hpp"
+#include "autoware_radar_object_tracker/tracker/model/tracker_base.hpp"
 
-#include "radar_object_tracker/utils/utils.hpp"
+#include "autoware_radar_object_tracker/utils/utils.hpp"
 
 #include <algorithm>
 #include <random>
 
+namespace autoware::radar_object_tracker
+{
 Tracker::Tracker(
   const rclcpp::Time & time,
   const std::vector<autoware_perception_msgs::msg::ObjectClassification> & classification)
@@ -61,3 +63,4 @@ geometry_msgs::msg::PoseWithCovariance Tracker::getPoseWithCovariance(
   getTrackedObject(time, object);
   return object.kinematics.pose_with_covariance;
 }
+}  // namespace autoware::radar_object_tracker
diff --git a/perception/radar_object_tracker/src/utils/radar_object_tracker_utils.cpp b/perception/radar_object_tracker/src/utils/radar_object_tracker_utils.cpp
index ba2500a60beff..f0a6a5dfd384d 100644
--- a/perception/radar_object_tracker/src/utils/radar_object_tracker_utils.cpp
+++ b/perception/radar_object_tracker/src/utils/radar_object_tracker_utils.cpp
@@ -12,9 +12,9 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "radar_object_tracker/utils/radar_object_tracker_utils.hpp"
+#include "autoware_radar_object_tracker/utils/radar_object_tracker_utils.hpp"
 
-namespace radar_object_tracker_utils
+namespace autoware::radar_object_tracker::utils
 {
 
 boost::optional<geometry_msgs::msg::Transform> getTransformAnonymous(
@@ -151,4 +151,4 @@ bool hasValidVelocityDirectionToLanelet(
   return false;
 }
 
-}  // namespace radar_object_tracker_utils
+}  // namespace autoware::radar_object_tracker::utils
diff --git a/perception/radar_object_tracker/src/utils/utils.cpp b/perception/radar_object_tracker/src/utils/utils.cpp
index cb9d3813e0ecc..351c4cfd21159 100644
--- a/perception/radar_object_tracker/src/utils/utils.cpp
+++ b/perception/radar_object_tracker/src/utils/utils.cpp
@@ -12,9 +12,9 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "radar_object_tracker/utils/utils.hpp"
+#include "autoware_radar_object_tracker/utils/utils.hpp"
 
-namespace utils
+namespace autoware::radar_object_tracker::utils
 {
 // concatenate matrices vertically
 Eigen::MatrixXd stackMatricesVertically(const std::vector<Eigen::MatrixXd> & matrices)
@@ -68,4 +68,4 @@ Eigen::MatrixXd stackMatricesDiagonally(const std::vector<Eigen::MatrixXd> & mat
   return result;
 }
 
-}  // namespace utils
+}  // namespace autoware::radar_object_tracker::utils
diff --git a/perception/radar_object_tracker/test/test_radar_object_tracker_utils.cpp b/perception/radar_object_tracker/test/test_radar_object_tracker_utils.cpp
index 681ae9f1854f7..e9b2a9800d628 100644
--- a/perception/radar_object_tracker/test/test_radar_object_tracker_utils.cpp
+++ b/perception/radar_object_tracker/test/test_radar_object_tracker_utils.cpp
@@ -12,12 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "radar_object_tracker/utils/radar_object_tracker_utils.hpp"
+#include "autoware_radar_object_tracker/utils/radar_object_tracker_utils.hpp"
 
 #include <gtest/gtest.h>
 
+using autoware::radar_object_tracker::utils::checkCloseLaneletCondition;
 using autoware_perception_msgs::msg::TrackedObject;
-using radar_object_tracker_utils::checkCloseLaneletCondition;
 
 // helper function to create a dummy straight lanelet
 lanelet::Lanelet createDummyStraightLanelet(double length, double width)
diff --git a/perception/radar_tracks_msgs_converter/CMakeLists.txt b/perception/radar_tracks_msgs_converter/CMakeLists.txt
index 766fb4b939904..4ff3be1fa45a4 100644
--- a/perception/radar_tracks_msgs_converter/CMakeLists.txt
+++ b/perception/radar_tracks_msgs_converter/CMakeLists.txt
@@ -5,12 +5,12 @@ find_package(autoware_cmake REQUIRED)
 autoware_package()
 
 ## Targets
-ament_auto_add_library(radar_tracks_msgs_converter_node_component SHARED
-  src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/radar_tracks_msgs_converter_node.cpp
 )
 
-rclcpp_components_register_node(radar_tracks_msgs_converter_node_component
-  PLUGIN "radar_tracks_msgs_converter::RadarTracksMsgsConverterNode"
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "autoware::radar_tracks_msgs_converter::RadarTracksMsgsConverterNode"
   EXECUTABLE radar_tracks_msgs_converter_node
 )
 
diff --git a/perception/radar_tracks_msgs_converter/package.xml b/perception/radar_tracks_msgs_converter/package.xml
index ad0257dbbd726..534c9a20fa0df 100644
--- a/perception/radar_tracks_msgs_converter/package.xml
+++ b/perception/radar_tracks_msgs_converter/package.xml
@@ -17,14 +17,12 @@
 
   <depend>autoware_perception_msgs</depend>
   <depend>autoware_universe_utils</depend>
-  <depend>geometry_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>radar_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>tf2</depend>
   <depend>tf2_geometry_msgs</depend>
-  <depend>tf2_ros</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
diff --git a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.cpp
similarity index 98%
rename from perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp
rename to perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.cpp
index 36f4d3cd3da90..5d45595733c4a 100644
--- a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp
+++ b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp"
+#include "radar_tracks_msgs_converter_node.hpp"
 
 #include "autoware/universe_utils/geometry/geometry.hpp"
 #include "autoware/universe_utils/math/unit_conversion.hpp"
@@ -56,7 +56,7 @@ bool update_param(
 }
 }  // namespace
 
-namespace radar_tracks_msgs_converter
+namespace autoware::radar_tracks_msgs_converter
 {
 
 enum class RadarTrackObjectID {
@@ -407,7 +407,7 @@ uint8_t RadarTracksMsgsConverterNode::convertClassification(const uint16_t class
   }
 }
 
-}  // namespace radar_tracks_msgs_converter
+}  // namespace autoware::radar_tracks_msgs_converter
 
 #include "rclcpp_components/register_node_macro.hpp"
-RCLCPP_COMPONENTS_REGISTER_NODE(radar_tracks_msgs_converter::RadarTracksMsgsConverterNode)
+RCLCPP_COMPONENTS_REGISTER_NODE(autoware::radar_tracks_msgs_converter::RadarTracksMsgsConverterNode)
diff --git a/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.hpp
similarity index 93%
rename from perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp
rename to perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.hpp
index 30d02c90b0594..36528711cc403 100644
--- a/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp
+++ b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RADAR_TRACKS_MSGS_CONVERTER__RADAR_TRACKS_MSGS_CONVERTER_NODE_HPP_
-#define RADAR_TRACKS_MSGS_CONVERTER__RADAR_TRACKS_MSGS_CONVERTER_NODE_HPP_
+#ifndef RADAR_TRACKS_MSGS_CONVERTER_NODE_HPP_
+#define RADAR_TRACKS_MSGS_CONVERTER_NODE_HPP_
 
 #include "autoware/universe_utils/ros/transform_listener.hpp"
 #include "rclcpp/rclcpp.hpp"
@@ -32,7 +32,7 @@
 #include <string>
 #include <vector>
 
-namespace radar_tracks_msgs_converter
+namespace autoware::radar_tracks_msgs_converter
 {
 using autoware_perception_msgs::msg::DetectedObject;
 using autoware_perception_msgs::msg::DetectedObjectKinematics;
@@ -114,6 +114,6 @@ class RadarTracksMsgsConverterNode : public rclcpp::Node
   uint8_t convertClassification(const uint16_t classification);
 };
 
-}  // namespace radar_tracks_msgs_converter
+}  // namespace autoware::radar_tracks_msgs_converter
 
-#endif  // RADAR_TRACKS_MSGS_CONVERTER__RADAR_TRACKS_MSGS_CONVERTER_NODE_HPP_
+#endif  // RADAR_TRACKS_MSGS_CONVERTER_NODE_HPP_
diff --git a/perception/simple_object_merger/CMakeLists.txt b/perception/simple_object_merger/CMakeLists.txt
index 10bd0efa7b8fc..1a65bdf96050e 100644
--- a/perception/simple_object_merger/CMakeLists.txt
+++ b/perception/simple_object_merger/CMakeLists.txt
@@ -6,12 +6,12 @@ find_package(autoware_cmake REQUIRED)
 autoware_package()
 
 # Targets
-ament_auto_add_library(simple_object_merger_node_component SHARED
-  src/simple_object_merger_node/simple_object_merger_node.cpp
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/simple_object_merger_node.cpp
 )
 
-rclcpp_components_register_node(simple_object_merger_node_component
-  PLUGIN "simple_object_merger::SimpleObjectMergerNode"
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "autoware::simple_object_merger::SimpleObjectMergerNode"
   EXECUTABLE simple_object_merger_node
 )
 
diff --git a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp b/perception/simple_object_merger/src/simple_object_merger_node.cpp
similarity index 96%
rename from perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp
rename to perception/simple_object_merger/src/simple_object_merger_node.cpp
index faa531560ba09..98c7fd03c1e99 100644
--- a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp
+++ b/perception/simple_object_merger/src/simple_object_merger_node.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "simple_object_merger/simple_object_merger_node.hpp"
+#include "simple_object_merger_node.hpp"
 
 #include <geometry_msgs/msg/pose_stamped.hpp>
 
@@ -65,7 +65,7 @@ autoware_perception_msgs::msg::DetectedObjects::SharedPtr getTransformedObjects(
 
 }  // namespace
 
-namespace simple_object_merger
+namespace autoware::simple_object_merger
 {
 using namespace std::literals;
 using std::chrono::duration;
@@ -193,7 +193,7 @@ void SimpleObjectMergerNode::onTimer()
   pub_objects_->publish(output_objects);
 }
 
-}  // namespace simple_object_merger
+}  // namespace autoware::simple_object_merger
 
 #include "rclcpp_components/register_node_macro.hpp"
-RCLCPP_COMPONENTS_REGISTER_NODE(simple_object_merger::SimpleObjectMergerNode)
+RCLCPP_COMPONENTS_REGISTER_NODE(autoware::simple_object_merger::SimpleObjectMergerNode)
diff --git a/perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp b/perception/simple_object_merger/src/simple_object_merger_node.hpp
similarity index 89%
rename from perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp
rename to perception/simple_object_merger/src/simple_object_merger_node.hpp
index d25eef20cc676..d5f907ff09ef2 100644
--- a/perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp
+++ b/perception/simple_object_merger/src/simple_object_merger_node.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef SIMPLE_OBJECT_MERGER__SIMPLE_OBJECT_MERGER_NODE_HPP_
-#define SIMPLE_OBJECT_MERGER__SIMPLE_OBJECT_MERGER_NODE_HPP_
+#ifndef SIMPLE_OBJECT_MERGER_NODE_HPP_
+#define SIMPLE_OBJECT_MERGER_NODE_HPP_
 
 #include "autoware/universe_utils/ros/transform_listener.hpp"
 #include "rclcpp/rclcpp.hpp"
@@ -25,7 +25,7 @@
 #include <string>
 #include <vector>
 
-namespace simple_object_merger
+namespace autoware::simple_object_merger
 {
 using autoware_perception_msgs::msg::DetectedObject;
 using autoware_perception_msgs::msg::DetectedObjects;
@@ -76,6 +76,6 @@ class SimpleObjectMergerNode : public rclcpp::Node
   size_t input_topic_size;
 };
 
-}  // namespace simple_object_merger
+}  // namespace autoware::simple_object_merger
 
-#endif  // SIMPLE_OBJECT_MERGER__SIMPLE_OBJECT_MERGER_NODE_HPP_
+#endif  // SIMPLE_OBJECT_MERGER_NODE_HPP_
diff --git a/perception/tensorrt_yolox/src/tensorrt_yolox.cpp b/perception/tensorrt_yolox/src/tensorrt_yolox.cpp
index ad8297052719a..34155c3477003 100644
--- a/perception/tensorrt_yolox/src/tensorrt_yolox.cpp
+++ b/perception/tensorrt_yolox/src/tensorrt_yolox.cpp
@@ -1279,7 +1279,6 @@ void TrtYoloX::getColorizedMask(
   int height = mask.rows;
   if ((cmask.cols != mask.cols) || (cmask.rows != mask.rows)) {
     throw std::runtime_error("input and output image have difference size.");
-    return;
   }
   for (int y = 0; y < height; y++) {
     for (int x = 0; x < width; x++) {
diff --git a/perception/tracking_object_merger/CMakeLists.txt b/perception/tracking_object_merger/CMakeLists.txt
index 9c8e5a321d9be..7e7c698365922 100644
--- a/perception/tracking_object_merger/CMakeLists.txt
+++ b/perception/tracking_object_merger/CMakeLists.txt
@@ -22,22 +22,22 @@ include_directories(
     ${EIGEN3_INCLUDE_DIR}
 )
 
-ament_auto_add_library(decorative_tracker_merger_node SHARED
-  src/data_association/data_association.cpp
-  src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp
-  src/decorative_tracker_merger.cpp
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/association/data_association.cpp
+  src/association/solver/mu_successive_shortest_path_wrapper.cpp
+  src/decorative_tracker_merger_node.cpp
   src/utils/utils.cpp
   src/utils/tracker_state.cpp
 )
 
-target_link_libraries(decorative_tracker_merger_node
+target_link_libraries(${PROJECT_NAME}
   Eigen3::Eigen
   glog::glog
 )
 
-rclcpp_components_register_node(decorative_tracker_merger_node
-  PLUGIN "tracking_object_merger::DecorativeTrackerMergerNode"
-  EXECUTABLE decorative_tracker_merger
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "autoware::tracking_object_merger::DecorativeTrackerMergerNode"
+  EXECUTABLE decorative_tracker_merger_node
 )
 
 ament_auto_package(INSTALL_TO_SHARE
diff --git a/perception/tracking_object_merger/include/tracking_object_merger/data_association/data_association.hpp b/perception/tracking_object_merger/include/autoware_tracking_object_merger/association/data_association.hpp
similarity index 77%
rename from perception/tracking_object_merger/include/tracking_object_merger/data_association/data_association.hpp
rename to perception/tracking_object_merger/include/autoware_tracking_object_merger/association/data_association.hpp
index 96a474f9dac95..a272e42a74e80 100644
--- a/perception/tracking_object_merger/include/tracking_object_merger/data_association/data_association.hpp
+++ b/perception/tracking_object_merger/include/autoware_tracking_object_merger/association/data_association.hpp
@@ -16,26 +16,28 @@
 // Author: v1.0 Yukihiro Saito
 //
 
-#ifndef TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_
-#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_
-
-#include <list>
-#include <memory>
-#include <unordered_map>
-#include <vector>
+#ifndef AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_
+#define AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_
 
 #define EIGEN_MPL2_ONLY
-#include "tracking_object_merger/data_association/solver/gnn_solver.hpp"
-#include "tracking_object_merger/utils/tracker_state.hpp"
+
+#include "autoware_tracking_object_merger/association/solver/gnn_solver.hpp"
+#include "autoware_tracking_object_merger/utils/tracker_state.hpp"
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 
-#include <autoware_perception_msgs/msg/detected_objects.hpp>
-#include <autoware_perception_msgs/msg/tracked_objects.hpp>
+#include "autoware_perception_msgs/msg/detected_objects.hpp"
+#include "autoware_perception_msgs/msg/tracked_objects.hpp"
 
+#include <list>
+#include <memory>
 #include <string>
+#include <unordered_map>
+#include <vector>
 
+namespace autoware::tracking_object_merger
+{
 class DataAssociation
 {
 private:
@@ -68,4 +70,6 @@ class DataAssociation
   virtual ~DataAssociation() {}
 };
 
-#endif  // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_
+}  // namespace autoware::tracking_object_merger
+
+#endif  // AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_
diff --git a/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver.hpp b/perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/gnn_solver.hpp
similarity index 55%
rename from perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver.hpp
rename to perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/gnn_solver.hpp
index 31240848f1977..4cea0e3cbe96f 100644
--- a/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver.hpp
+++ b/perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/gnn_solver.hpp
@@ -12,11 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_
-#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_
+#ifndef AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_
+#define AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_
 
-#include "tracking_object_merger/data_association/solver/gnn_solver_interface.hpp"
-#include "tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp"
-#include "tracking_object_merger/data_association/solver/successive_shortest_path.hpp"
+#include "autoware_tracking_object_merger/association/solver/gnn_solver_interface.hpp"
+#include "autoware_tracking_object_merger/association/solver/mu_ssp.hpp"
+#include "autoware_tracking_object_merger/association/solver/ssp.hpp"
 
-#endif  // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_
+#endif  // AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_
diff --git a/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver_interface.hpp b/perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/gnn_solver_interface.hpp
similarity index 71%
rename from perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver_interface.hpp
rename to perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/gnn_solver_interface.hpp
index e915b5d2a9e7b..d751075773a09 100644
--- a/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver_interface.hpp
+++ b/perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/gnn_solver_interface.hpp
@@ -12,13 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
-#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
+#ifndef AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
+#define AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
 
 #include <unordered_map>
 #include <vector>
 
-namespace gnn_solver
+namespace autoware::tracking_object_merger::gnn_solver
 {
 class GnnSolverInterface
 {
@@ -30,6 +30,6 @@ class GnnSolverInterface
     const std::vector<std::vector<double>> & cost, std::unordered_map<int, int> * direct_assignment,
     std::unordered_map<int, int> * reverse_assignment) = 0;
 };
-}  // namespace gnn_solver
+}  // namespace autoware::tracking_object_merger::gnn_solver
 
-#endif  // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
+#endif  // AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
diff --git a/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp b/perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/mu_ssp.hpp
similarity index 68%
rename from perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp
rename to perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/mu_ssp.hpp
index 5c0ebc04fdde3..47c23bc8eb1b1 100644
--- a/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp
+++ b/perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/mu_ssp.hpp
@@ -12,15 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_
-#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_
+#ifndef AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_
+#define AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_
 
-#include "tracking_object_merger/data_association/solver/gnn_solver_interface.hpp"
+#include "autoware_tracking_object_merger/association/solver/gnn_solver_interface.hpp"
 
 #include <unordered_map>
 #include <vector>
 
-namespace gnn_solver
+namespace autoware::tracking_object_merger::gnn_solver
 {
 class MuSSP : public GnnSolverInterface
 {
@@ -32,6 +32,6 @@ class MuSSP : public GnnSolverInterface
     const std::vector<std::vector<double>> & cost, std::unordered_map<int, int> * direct_assignment,
     std::unordered_map<int, int> * reverse_assignment) override;
 };
-}  // namespace gnn_solver
+}  // namespace autoware::tracking_object_merger::gnn_solver
 
-#endif  // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_
+#endif  // AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_
diff --git a/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/successive_shortest_path.hpp b/perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/ssp.hpp
similarity index 68%
rename from perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/successive_shortest_path.hpp
rename to perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/ssp.hpp
index 47a737cf58298..bcfac73f3e43b 100644
--- a/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/successive_shortest_path.hpp
+++ b/perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/ssp.hpp
@@ -12,15 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_
-#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_
+#ifndef AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_
+#define AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_
 
-#include "tracking_object_merger/data_association/solver/gnn_solver_interface.hpp"
+#include "autoware_tracking_object_merger/association/solver/gnn_solver_interface.hpp"
 
 #include <unordered_map>
 #include <vector>
 
-namespace gnn_solver
+namespace autoware::tracking_object_merger::gnn_solver
 {
 class SSP : public GnnSolverInterface
 {
@@ -32,6 +32,6 @@ class SSP : public GnnSolverInterface
     const std::vector<std::vector<double>> & cost, std::unordered_map<int, int> * direct_assignment,
     std::unordered_map<int, int> * reverse_assignment) override;
 };
-}  // namespace gnn_solver
+}  // namespace autoware::tracking_object_merger::gnn_solver
 
-#endif  // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_
+#endif  // AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_
diff --git a/perception/tracking_object_merger/include/tracking_object_merger/utils/tracker_state.hpp b/perception/tracking_object_merger/include/autoware_tracking_object_merger/utils/tracker_state.hpp
similarity index 90%
rename from perception/tracking_object_merger/include/tracking_object_merger/utils/tracker_state.hpp
rename to perception/tracking_object_merger/include/autoware_tracking_object_merger/utils/tracker_state.hpp
index a92cd9194b26a..ed1f69191b652 100644
--- a/perception/tracking_object_merger/include/tracking_object_merger/utils/tracker_state.hpp
+++ b/perception/tracking_object_merger/include/autoware_tracking_object_merger/utils/tracker_state.hpp
@@ -12,17 +12,17 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_
-#define TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_
+#ifndef AUTOWARE_TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_
+#define AUTOWARE_TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 #include <rclcpp/rclcpp.hpp>
 
-#include <autoware_perception_msgs/msg/object_classification.hpp>
-#include <autoware_perception_msgs/msg/tracked_object.hpp>
-#include <autoware_perception_msgs/msg/tracked_object_kinematics.hpp>
-#include <autoware_perception_msgs/msg/tracked_objects.hpp>
+#include "autoware_perception_msgs/msg/object_classification.hpp"
+#include "autoware_perception_msgs/msg/tracked_object.hpp"
+#include "autoware_perception_msgs/msg/tracked_object_kinematics.hpp"
+#include "autoware_perception_msgs/msg/tracked_objects.hpp"
 #include <std_msgs/msg/header.hpp>
 #include <unique_identifier_msgs/msg/uuid.hpp>
 
@@ -32,6 +32,9 @@
 #include <string>
 #include <unordered_map>
 #include <vector>
+
+namespace autoware::tracking_object_merger
+{
 using autoware_perception_msgs::msg::TrackedObject;
 using autoware_perception_msgs::msg::TrackedObjects;
 
@@ -144,5 +147,6 @@ class TrackerState
 
 TrackedObjects getTrackedObjectsFromTrackerStates(
   std::vector<TrackerState> & tracker_states, const rclcpp::Time & time);
+}  // namespace autoware::tracking_object_merger
 
-#endif  // TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_
+#endif  // AUTOWARE_TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_
diff --git a/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp b/perception/tracking_object_merger/include/autoware_tracking_object_merger/utils/utils.hpp
similarity index 73%
rename from perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp
rename to perception/tracking_object_merger/include/autoware_tracking_object_merger/utils/utils.hpp
index 03352512d5545..eb288c4e853ad 100644
--- a/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp
+++ b/perception/tracking_object_merger/include/autoware_tracking_object_merger/utils/utils.hpp
@@ -14,19 +14,18 @@
 //
 //
 
-#ifndef TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_
-#define TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_
+#ifndef AUTOWARE_TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_
+#define AUTOWARE_TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_
 
-// #include <autoware/universe_utils/autoware_universe_utils.hpp>
 #include "autoware/universe_utils/geometry/geometry.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
-#include <autoware_perception_msgs/msg/object_classification.hpp>
-#include <autoware_perception_msgs/msg/shape.hpp>
-#include <autoware_perception_msgs/msg/tracked_object.hpp>
-#include <autoware_perception_msgs/msg/tracked_object_kinematics.hpp>
-#include <autoware_perception_msgs/msg/tracked_objects.hpp>
+#include "autoware_perception_msgs/msg/object_classification.hpp"
+#include "autoware_perception_msgs/msg/shape.hpp"
+#include "autoware_perception_msgs/msg/tracked_object.hpp"
+#include "autoware_perception_msgs/msg/tracked_object_kinematics.hpp"
+#include "autoware_perception_msgs/msg/tracked_objects.hpp"
 #include <geometry_msgs/msg/polygon.hpp>
 #include <geometry_msgs/msg/vector3.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
@@ -42,49 +41,13 @@
 #include <tuple>
 #include <vector>
 
+namespace autoware::tracking_object_merger
+{
 using autoware_perception_msgs::msg::TrackedObject;
 using autoware_perception_msgs::msg::TrackedObjects;
+
 namespace utils
 {
-enum MSG_COV_IDX {
-  X_X = 0,
-  X_Y = 1,
-  X_Z = 2,
-  X_ROLL = 3,
-  X_PITCH = 4,
-  X_YAW = 5,
-  Y_X = 6,
-  Y_Y = 7,
-  Y_Z = 8,
-  Y_ROLL = 9,
-  Y_PITCH = 10,
-  Y_YAW = 11,
-  Z_X = 12,
-  Z_Y = 13,
-  Z_Z = 14,
-  Z_ROLL = 15,
-  Z_PITCH = 16,
-  Z_YAW = 17,
-  ROLL_X = 18,
-  ROLL_Y = 19,
-  ROLL_Z = 20,
-  ROLL_ROLL = 21,
-  ROLL_PITCH = 22,
-  ROLL_YAW = 23,
-  PITCH_X = 24,
-  PITCH_Y = 25,
-  PITCH_Z = 26,
-  PITCH_ROLL = 27,
-  PITCH_PITCH = 28,
-  PITCH_YAW = 29,
-  YAW_X = 30,
-  YAW_Y = 31,
-  YAW_Z = 32,
-  YAW_ROLL = 33,
-  YAW_PITCH = 34,
-  YAW_YAW = 35
-};
-
 // linear interpolation for tracked objects
 TrackedObject linearInterpolationForTrackedObject(
   const TrackedObject & obj1, const TrackedObject & obj2);
@@ -103,6 +66,9 @@ TrackedObjects interpolateTrackedObjects(
 
 namespace merger_utils
 {
+using autoware_perception_msgs::msg::TrackedObject;
+using autoware_perception_msgs::msg::TrackedObjects;
+
 // merge policy
 enum MergePolicy : int { SKIP = 0, OVERWRITE = 1, FUSION = 2 };
 
@@ -132,4 +98,6 @@ void updateWholeTrackedObject(TrackedObject & main_obj, const TrackedObject & su
 
 }  // namespace merger_utils
 
-#endif  // TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_
+}  // namespace autoware::tracking_object_merger
+
+#endif  // AUTOWARE_TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_
diff --git a/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml b/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml
index 52b0841f97e97..2cc2a69e295e6 100644
--- a/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml
+++ b/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml
@@ -7,7 +7,7 @@
   <arg name="merge_options_path" default="$(find-pkg-share tracking_object_merger)/config/decorative_tracker_merger_policy.param.yaml"/>
   <arg name="node_param_file_path" default="$(find-pkg-share tracking_object_merger)/config/decorative_tracker_merger.param.yaml"/>
 
-  <node pkg="tracking_object_merger" exec="decorative_tracker_merger" name="$(anon decorative_tracker_merger)" output="screen">
+  <node pkg="tracking_object_merger" exec="decorative_tracker_merger_node" name="$(anon decorative_tracker_merger)" output="screen">
     <remap from="input/main_object" to="$(var input/main_object)"/>
     <remap from="input/sub_object" to="$(var input/sub_object)"/>
     <remap from="output/object" to="$(var output)"/>
diff --git a/perception/tracking_object_merger/package.xml b/perception/tracking_object_merger/package.xml
index 370f5beb3e3b3..cc23578773fe2 100644
--- a/perception/tracking_object_merger/package.xml
+++ b/perception/tracking_object_merger/package.xml
@@ -23,7 +23,6 @@
   <depend>rclcpp_components</depend>
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
-  <depend>tier4_perception_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
diff --git a/perception/tracking_object_merger/src/data_association/data_association.cpp b/perception/tracking_object_merger/src/association/data_association.cpp
similarity index 96%
rename from perception/tracking_object_merger/src/data_association/data_association.cpp
rename to perception/tracking_object_merger/src/association/data_association.cpp
index 4d10a415df7da..e77a71744fe9b 100644
--- a/perception/tracking_object_merger/src/data_association/data_association.cpp
+++ b/perception/tracking_object_merger/src/association/data_association.cpp
@@ -12,12 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "tracking_object_merger/data_association/data_association.hpp"
+#include "autoware_tracking_object_merger/association/data_association.hpp"
 
 #include "autoware/universe_utils/geometry/geometry.hpp"
+#include "autoware_tracking_object_merger/association/solver/gnn_solver.hpp"
+#include "autoware_tracking_object_merger/utils/utils.hpp"
 #include "object_recognition_utils/object_recognition_utils.hpp"
-#include "tracking_object_merger/data_association/solver/gnn_solver.hpp"
-#include "tracking_object_merger/utils/utils.hpp"
 
 #include <algorithm>
 #include <fstream>
@@ -47,6 +47,8 @@ double getFormedYawAngle(
 }
 }  // namespace
 
+namespace autoware::tracking_object_merger
+{
 DataAssociation::DataAssociation(
   std::vector<int> can_assign_vector, std::vector<double> max_dist_vector,
   std::vector<double> max_rad_vector, std::vector<double> min_iou_vector,
@@ -229,3 +231,5 @@ double DataAssociation::calcScoreBetweenObjects(
   }
   return score;
 }
+
+}  // namespace autoware::tracking_object_merger
diff --git a/perception/tracking_object_merger/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp b/perception/tracking_object_merger/src/association/solver/mu_successive_shortest_path_wrapper.cpp
similarity index 85%
rename from perception/tracking_object_merger/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp
rename to perception/tracking_object_merger/src/association/solver/mu_successive_shortest_path_wrapper.cpp
index fcc79c0a3cbd7..d2db6eba068ec 100644
--- a/perception/tracking_object_merger/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp
+++ b/perception/tracking_object_merger/src/association/solver/mu_successive_shortest_path_wrapper.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp"
+#include "autoware_tracking_object_merger/association/solver/mu_ssp.hpp"
 
 #include <mussp/mussp.h>
 
@@ -24,7 +24,7 @@
 #include <unordered_map>
 #include <vector>
 
-namespace gnn_solver
+namespace autoware::tracking_object_merger::gnn_solver
 {
 void MuSSP::maximizeLinearAssignment(
   const std::vector<std::vector<double>> & cost, std::unordered_map<int, int> * direct_assignment,
@@ -38,4 +38,4 @@ void MuSSP::maximizeLinearAssignment(
   // Solve DA by muSSP
   solve_muSSP(cost, direct_assignment, reverse_assignment);
 }
-}  // namespace gnn_solver
+}  // namespace autoware::tracking_object_merger::gnn_solver
diff --git a/perception/tracking_object_merger/src/data_association/successive_shortest_path/successive_shortest_path.cpp b/perception/tracking_object_merger/src/association/solver/successive_shortest_path.cpp
similarity index 98%
rename from perception/tracking_object_merger/src/data_association/successive_shortest_path/successive_shortest_path.cpp
rename to perception/tracking_object_merger/src/association/solver/successive_shortest_path.cpp
index 133f0d377373f..7be715b24bd5f 100644
--- a/perception/tracking_object_merger/src/data_association/successive_shortest_path/successive_shortest_path.cpp
+++ b/perception/tracking_object_merger/src/association/solver/successive_shortest_path.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "tracking_object_merger/data_association/solver/successive_shortest_path.hpp"
+#include "autoware_tracking_object_merger/association/solver/ssp.hpp"
 
 #include <algorithm>
 #include <cassert>
@@ -22,7 +22,7 @@
 #include <utility>
 #include <vector>
 
-namespace gnn_solver
+namespace autoware::tracking_object_merger::gnn_solver
 {
 struct ResidualEdge
 {
@@ -367,4 +367,4 @@ void SSP::maximizeLinearAssignment(
   }
 #endif
 }
-}  // namespace gnn_solver
+}  // namespace autoware::tracking_object_merger::gnn_solver
diff --git a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp b/perception/tracking_object_merger/src/decorative_tracker_merger_node.cpp
similarity index 97%
rename from perception/tracking_object_merger/src/decorative_tracker_merger.cpp
rename to perception/tracking_object_merger/src/decorative_tracker_merger_node.cpp
index 99dccab3ff72c..6f65941c0a747 100644
--- a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp
+++ b/perception/tracking_object_merger/src/decorative_tracker_merger_node.cpp
@@ -12,11 +12,16 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "tracking_object_merger/decorative_tracker_merger.hpp"
+#define EIGEN_MPL2_ONLY
+
+#include "decorative_tracker_merger_node.hpp"
 
+#include "autoware_tracking_object_merger/association/solver/ssp.hpp"
+#include "autoware_tracking_object_merger/utils/utils.hpp"
 #include "object_recognition_utils/object_recognition_utils.hpp"
-#include "tracking_object_merger/data_association/solver/successive_shortest_path.hpp"
-#include "tracking_object_merger/utils/utils.hpp"
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
 
 #include <boost/optional.hpp>
 
@@ -25,13 +30,9 @@
 #include <chrono>
 #include <unordered_map>
 
-#define EIGEN_MPL2_ONLY
-#include <Eigen/Core>
-#include <Eigen/Geometry>
-
 using Label = autoware_perception_msgs::msg::ObjectClassification;
 
-namespace tracking_object_merger
+namespace autoware::tracking_object_merger
 {
 
 using autoware_perception_msgs::msg::TrackedObject;
@@ -419,7 +420,7 @@ TrackerState DecorativeTrackerMergerNode::createNewTracker(
   return new_tracker;
 }
 
-}  // namespace tracking_object_merger
+}  // namespace autoware::tracking_object_merger
 
 #include <rclcpp_components/register_node_macro.hpp>
-RCLCPP_COMPONENTS_REGISTER_NODE(tracking_object_merger::DecorativeTrackerMergerNode)
+RCLCPP_COMPONENTS_REGISTER_NODE(autoware::tracking_object_merger::DecorativeTrackerMergerNode)
diff --git a/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp b/perception/tracking_object_merger/src/decorative_tracker_merger_node.hpp
similarity index 87%
rename from perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp
rename to perception/tracking_object_merger/src/decorative_tracker_merger_node.hpp
index 3683ba02b38cd..ae19f51876490 100644
--- a/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp
+++ b/perception/tracking_object_merger/src/decorative_tracker_merger_node.hpp
@@ -12,19 +12,19 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_HPP_
-#define TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_HPP_
+#ifndef DECORATIVE_TRACKER_MERGER_NODE_HPP_
+#define DECORATIVE_TRACKER_MERGER_NODE_HPP_
 
-#include "tracking_object_merger/data_association/data_association.hpp"
-#include "tracking_object_merger/utils/tracker_state.hpp"
-#include "tracking_object_merger/utils/utils.hpp"
+#include "autoware/universe_utils/ros/debug_publisher.hpp"
+#include "autoware/universe_utils/ros/published_time_publisher.hpp"
+#include "autoware/universe_utils/system/stop_watch.hpp"
+#include "autoware_tracking_object_merger/association/data_association.hpp"
+#include "autoware_tracking_object_merger/utils/tracker_state.hpp"
+#include "autoware_tracking_object_merger/utils/utils.hpp"
 
-#include <autoware/universe_utils/ros/debug_publisher.hpp>
-#include <autoware/universe_utils/ros/published_time_publisher.hpp>
-#include <autoware/universe_utils/system/stop_watch.hpp>
 #include <rclcpp/rclcpp.hpp>
 
-#include <autoware_perception_msgs/msg/tracked_objects.hpp>
+#include "autoware_perception_msgs/msg/tracked_objects.hpp"
 #include <std_msgs/msg/header.hpp>
 
 #ifdef ROS_DISTRO_GALACTIC
@@ -43,7 +43,7 @@
 #include <unordered_map>
 #include <vector>
 
-namespace tracking_object_merger
+namespace autoware::tracking_object_merger
 {
 
 class DecorativeTrackerMergerNode : public rclcpp::Node
@@ -131,6 +131,6 @@ class DecorativeTrackerMergerNode : public rclcpp::Node
   } logging_;
 };
 
-}  // namespace tracking_object_merger
+}  // namespace autoware::tracking_object_merger
 
-#endif  // TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_HPP_
+#endif  // DECORATIVE_TRACKER_MERGER_NODE_HPP_
diff --git a/perception/tracking_object_merger/src/utils/tracker_state.cpp b/perception/tracking_object_merger/src/utils/tracker_state.cpp
index 36da060a962c3..54b1d73c375a5 100644
--- a/perception/tracking_object_merger/src/utils/tracker_state.cpp
+++ b/perception/tracking_object_merger/src/utils/tracker_state.cpp
@@ -12,9 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "tracking_object_merger/utils/tracker_state.hpp"
+#include "autoware_tracking_object_merger/utils/tracker_state.hpp"
 
-#include "tracking_object_merger/utils/utils.hpp"
+#include "autoware_tracking_object_merger/utils/utils.hpp"
+
+namespace autoware::tracking_object_merger
+{
 
 using autoware_perception_msgs::msg::TrackedObject;
 using autoware_perception_msgs::msg::TrackedObjects;
@@ -325,3 +328,5 @@ TrackedObjects getTrackedObjectsFromTrackerStates(
   tracked_objects.header.frame_id = "map";  // TODO(yoshiri): get frame_id from input
   return tracked_objects;
 }
+
+}  // namespace autoware::tracking_object_merger
diff --git a/perception/tracking_object_merger/src/utils/utils.cpp b/perception/tracking_object_merger/src/utils/utils.cpp
index 90437d3ebd7aa..1ae705aa38f15 100644
--- a/perception/tracking_object_merger/src/utils/utils.cpp
+++ b/perception/tracking_object_merger/src/utils/utils.cpp
@@ -12,11 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "tracking_object_merger/utils/utils.hpp"
+#include "autoware_tracking_object_merger/utils/utils.hpp"
 
-#include <autoware_perception_msgs/msg/shape.hpp>
-#include <autoware_perception_msgs/msg/tracked_object.hpp>
-#include <autoware_perception_msgs/msg/tracked_objects.hpp>
+#include "autoware_perception_msgs/msg/shape.hpp"
+#include "autoware_perception_msgs/msg/tracked_object.hpp"
+#include "autoware_perception_msgs/msg/tracked_objects.hpp"
 
 #include <algorithm>
 #include <cmath>
@@ -25,7 +25,7 @@
 
 using autoware_perception_msgs::msg::TrackedObject;
 using autoware_perception_msgs::msg::TrackedObjects;
-namespace utils
+namespace autoware::tracking_object_merger::utils
 {
 
 /**
@@ -243,9 +243,9 @@ TrackedObjects interpolateTrackedObjects(
   return output_objects;
 }
 
-}  // namespace utils
+}  // namespace autoware::tracking_object_merger::utils
 
-namespace merger_utils
+namespace autoware::tracking_object_merger::merger_utils
 {
 
 double mean(const double a, const double b)
@@ -513,4 +513,4 @@ void updateWholeTrackedObject(TrackedObject & main_obj, const TrackedObject & su
   main_obj = sub_obj;
 }
 
-}  // namespace merger_utils
+}  // namespace autoware::tracking_object_merger::merger_utils
diff --git a/planning/.pages b/planning/.pages
index 0ebebc3c6ac7d..2e3dd92011d19 100644
--- a/planning/.pages
+++ b/planning/.pages
@@ -79,7 +79,6 @@ nav:
     - 'RTC Interface': planning/autoware_rtc_interface
   - 'Additional Tools':
     - 'Remaining Distance Time Calculator': planning/autoware_remaining_distance_time_calculator
-    - 'RTC Replayer': planning/rtc_replayer
     - 'Planning Debug Tools':
       - 'About Planning Debug Tools': https://github.com/autowarefoundation/autoware_tools/tree/main/planning/planning_debug_tools
       - 'Stop Reason Visualizer': https://github.com/autowarefoundation/autoware_tools/blob/main/planning/planning_debug_tools/doc-stop-reason-visualizer.md
diff --git a/planning/autoware_rtc_interface/README.md b/planning/autoware_rtc_interface/README.md
index 19d200592bcf3..635f692266f7e 100644
--- a/planning/autoware_rtc_interface/README.md
+++ b/planning/autoware_rtc_interface/README.md
@@ -185,6 +185,10 @@ Return `true` if `uuid` is registered.
 If `uuid` is registered, return `true`.
 If not, return `false`.
 
+## Debugging Tools
+
+There is a debugging tool called [RTC replayer](https://github.com/autowarefoundation/autoware_tools/tree/main/planning/autoware_rtc_replayer) for the RTC interface.
+
 ## Assumptions / Known limits
 
 ## Future extensions / Unimplemented parts
diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp
index aefbd9ef20f84..5c66f5b906bba 100644
--- a/planning/autoware_velocity_smoother/src/node.cpp
+++ b/planning/autoware_velocity_smoother/src/node.cpp
@@ -1013,7 +1013,6 @@ VelocitySmootherNode::AlgorithmType VelocitySmootherNode::getAlgorithmType(
   }
 
   throw std::domain_error("[VelocitySmootherNode] undesired algorithm is selected.");
-  return AlgorithmType::INVALID;
 }
 
 double VelocitySmootherNode::calcTravelDistance() const
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md
index 1be3e04914cda..3e155aba0af2e 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md
@@ -382,13 +382,14 @@ In addition, the safety check has a time hysteresis, and if the path is judged "
 
 ### Parameters for safety check
 
-| Name                     | Unit | Type   | Description                                                                                              | Default value                |
-| :----------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------- | :--------------------------- |
-| enable_safety_check      | [-]  | bool   | flag whether to use safety check                                                                         | true                         |
-| method                   | [-]  | string | method for safety check. `RSS` or `integral_predicted_polygon`                                           | `integral_predicted_polygon` |
-| keep_unsafe_time         | [s]  | double | safety check Hysteresis time. if the path is judged "safe" for the time it is finally treated as "safe". | 3.0                          |
-| check_all_predicted_path | -    | bool   | Flag to check all predicted paths                                                                        | true                         |
-| publish_debug_marker     | -    | bool   | Flag to publish debug markers                                                                            | false                        |
+| Name                                 | Unit  | Type   | Description                                                                                              | Default value                |
+| :----------------------------------- | :---- | :----- | :------------------------------------------------------------------------------------------------------- | :--------------------------- |
+| enable_safety_check                  | [-]   | bool   | flag whether to use safety check                                                                         | true                         |
+| method                               | [-]   | string | method for safety check. `RSS` or `integral_predicted_polygon`                                           | `integral_predicted_polygon` |
+| keep_unsafe_time                     | [s]   | double | safety check Hysteresis time. if the path is judged "safe" for the time it is finally treated as "safe". | 3.0                          |
+| check_all_predicted_path             | -     | bool   | Flag to check all predicted paths                                                                        | true                         |
+| publish_debug_marker                 | -     | bool   | Flag to publish debug markers                                                                            | false                        |
+| `collision_check_yaw_diff_threshold` | [rad] | double | Maximum yaw difference between ego and object when executing rss-based collision checking                | 3.1416                       |
 
 #### Parameters for RSS safety check
 
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml
index 915d256aba1db..53e06631a81d5 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml
@@ -180,6 +180,7 @@
             time_horizon: 10.0
           # hysteresis factor to expand/shrink polygon with the value
           hysteresis_factor_expand_rate: 1.0
+          collision_check_yaw_diff_threshold: 3.1416
           # temporary
           backward_path_length: 100.0
           forward_path_length: 100.0
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp
index 2ac0b9493e67a..6caeb411f6680 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp
@@ -2319,8 +2319,10 @@ std::pair<bool, bool> GoalPlannerModule::isSafePath(
       return autoware::behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS(
         pull_over_path, ego_predicted_path, filtered_objects, collision_check,
         planner_data->parameters, safety_check_params->rss_params,
-        objects_filtering_params->use_all_predicted_path, hysteresis_factor);
-    } else if (parameters.safety_check_params.method == "integral_predicted_polygon") {
+        objects_filtering_params->use_all_predicted_path, hysteresis_factor,
+        safety_check_params->collision_check_yaw_diff_threshold);
+    }
+    if (parameters.safety_check_params.method == "integral_predicted_polygon") {
       return utils::path_safety_checker::checkSafetyWithIntegralPredictedPolygon(
         ego_predicted_path, vehicle_info_, filtered_objects,
         objects_filtering_params->check_all_predicted_path,
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp
index 901fe351fc68b..b079db9babf31 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp
@@ -357,6 +357,8 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node)
     p.safety_check_params.method = node->declare_parameter<std::string>(safety_check_ns + "method");
     p.safety_check_params.hysteresis_factor_expand_rate =
       node->declare_parameter<double>(safety_check_ns + "hysteresis_factor_expand_rate");
+    p.safety_check_params.collision_check_yaw_diff_threshold =
+      node->declare_parameter<double>(safety_check_ns + "collision_check_yaw_diff_threshold");
     p.safety_check_params.backward_path_length =
       node->declare_parameter<double>(safety_check_ns + "backward_path_length");
     p.safety_check_params.forward_path_length =
@@ -778,6 +780,9 @@ void GoalPlannerModuleManager::updateModuleParams(
     updateParam<double>(
       parameters, safety_check_ns + "hysteresis_factor_expand_rate",
       p->safety_check_params.hysteresis_factor_expand_rate);
+    updateParam<double>(
+      parameters, safety_check_ns + "collision_check_yaw_diff_threshold",
+      p->safety_check_params.collision_check_yaw_diff_threshold);
     updateParam<double>(
       parameters, safety_check_ns + "backward_path_length",
       p->safety_check_params.backward_path_length);
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md
index a0870fe428c8a..5790be377b7aa 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md
+++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md
@@ -725,6 +725,7 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi
 | `check_objects_on_current_lanes`                         | [-]   | boolean | If true, the lane change module check objects on current lanes when performing collision assessment.                                                                                                       | false         |
 | `check_objects_on_other_lanes`                           | [-]   | boolean | If true, the lane change module include objects on other lanes. when performing collision assessment                                                                                                       | false         |
 | `use_all_predicted_path`                                 | [-]   | boolean | If false, use only the predicted path that has the maximum confidence.                                                                                                                                     | true          |
+| `safety_check.collision_check_yaw_diff_threshold`        | [rad] | double  | Maximum yaw difference between ego and object when executing rss-based collision checking                                                                                                                  | 3.1416        |
 
 #### safety constraints during lane change path is computed
 
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml
index 1ab33514c5f24..d2f695071649a 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml
+++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml
@@ -30,6 +30,7 @@
       # safety check
       safety_check:
         allow_loose_check_for_cancel: true
+        collision_check_yaw_diff_threshold: 3.1416
         execution:
           expected_front_deceleration: -1.0
           expected_rear_deceleration: -1.0
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp
index e2afb6abeec03..ffd2754acc38f 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp
@@ -137,6 +137,7 @@ struct LaneChangeParameters
 
   // safety check
   bool allow_loose_check_for_cancel{true};
+  double collision_check_yaw_diff_threshold{3.1416};
   utils::path_safety_checker::RSSparams rss_params{};
   utils::path_safety_checker::RSSparams rss_params_for_abort{};
   utils::path_safety_checker::RSSparams rss_params_for_stuck{};
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp
index 61ff54e8f99f3..22c93f7d4bfd5 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp
@@ -102,6 +102,8 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node)
   // safety check
   p.allow_loose_check_for_cancel =
     getOrDeclareParameter<bool>(*node, parameter("safety_check.allow_loose_check_for_cancel"));
+  p.collision_check_yaw_diff_threshold = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.collision_check_yaw_diff_threshold"));
 
   p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter<double>(
     *node, parameter("safety_check.execution.longitudinal_distance_min_threshold"));
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp
index 505bb3ef93340..ebfa1e7ac260c 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp
@@ -30,6 +30,7 @@
 #include <lanelet2_core/geometry/Polygon.h>
 
 #include <algorithm>
+#include <cmath>
 #include <limits>
 #include <memory>
 #include <utility>
@@ -467,8 +468,16 @@ void NormalLaneChange::insertStopPoint(
 
 PathWithLaneId NormalLaneChange::getReferencePath() const
 {
-  return utils::getCenterLinePathFromLanelet(
-    status_.lane_change_path.info.target_lanes.front(), planner_data_);
+  lanelet::ConstLanelet closest_lanelet;
+  if (!lanelet::utils::query::getClosestLanelet(
+        status_.lane_change_path.info.target_lanes, getEgoPose(), &closest_lanelet)) {
+    return prev_module_output_.reference_path;
+  }
+  const auto reference_path = utils::getCenterLinePathFromLanelet(closest_lanelet, planner_data_);
+  if (reference_path.points.empty()) {
+    return prev_module_output_.reference_path;
+  }
+  return reference_path;
 }
 
 std::optional<PathWithLaneId> NormalLaneChange::extendPath()
@@ -2048,6 +2057,8 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
     lane_change_parameters_->lane_expansion_left_offset,
     lane_change_parameters_->lane_expansion_right_offset);
 
+  constexpr double collision_check_yaw_diff_threshold{M_PI};
+
   for (const auto & obj : collision_check_objects) {
     auto current_debug_data = utils::path_safety_checker::createObjectDebug(obj);
     const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj(
@@ -2056,7 +2067,8 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
     for (const auto & obj_path : obj_predicted_paths) {
       const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons(
         path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0,
-        get_max_velocity_for_safety_check(), current_debug_data.second);
+        get_max_velocity_for_safety_check(), collision_check_yaw_diff_threshold,
+        current_debug_data.second);
 
       if (collided_polygons.empty()) {
         utils::path_safety_checker::updateCollisionCheckDebugMap(
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md b/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md
index 4733fe7832da6..5511202c390b1 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md
@@ -48,7 +48,7 @@ $$
 rss_{dist} = v_{rear} (t_{reaction} + t_{margin}) + \frac{v_{rear}^2}{2|a_{rear, decel}|} - \frac{v_{front}^2}{2|a_{front, decel|}}
 $$
 
-where $V_{front}$, $v_{rear}$ are front and rear vehicle velocity respectively and $a_{rear, front}$, $a_{rear, decel}$ are front and rear vehicle deceleration.
+where $V_{front}$, $v_{rear}$ are front and rear vehicle velocity respectively and $a_{rear, front}$, $a_{rear, decel}$ are front and rear vehicle deceleration. Note that RSS distance is normally used for objects traveling in the same direction, if the yaw difference between a given ego pose and object pose is more than a user-defined yaw difference threshold, the rss collision check will be skipped for that specific pair of poses.
 
 #### 5. Create extended ego and target object polygons
 
@@ -56,7 +56,7 @@ In this step, we compute extended ego and target object polygons. The extended p
 
 ![extended_polygons](../images/path_safety_checker/extended_polygons.drawio.svg)
 
-As the picture shows, we expand the rear object polygon. For the longitudinal side, we extend it with the RSS distance, and for the lateral side, we extend it by the lateral margin
+As the picture shows, we expand the rear object polygon. For the longitudinal side, we extend it with the RSS distance, and for the lateral side, we extend it by the lateral margin.
 
 #### 6. Check overlap
 
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp
index 313f9df471938..0e7d1cee65f02 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp
@@ -23,6 +23,7 @@
 
 #include <boost/uuid/uuid_hash.hpp>
 
+#include <cmath>
 #include <string>
 #include <unordered_map>
 #include <utility>
@@ -203,7 +204,9 @@ struct SafetyCheckParams
   /// possible values: ["RSS", "integral_predicted_polygon"]
   double keep_unsafe_time{0.0};  ///< Time to keep unsafe before changing to safe.
   double hysteresis_factor_expand_rate{
-    0.0};                            ///< Hysteresis factor to expand/shrink polygon with the value.
+    0.0};  ///< Hysteresis factor to expand/shrink polygon with the value.
+  double collision_check_yaw_diff_threshold{
+    3.1416};                         ///< threshold yaw difference between ego and object.
   double backward_path_length{0.0};  ///< Length of the backward lane for path generation.
   double forward_path_length{0.0};   ///< Length of the forward path lane for path generation.
   RSSparams rss_params{};            ///< Parameters related to the RSS model.
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp
index 3f1094dfd70b6..d0d19b6b8fed2 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp
@@ -26,6 +26,7 @@
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/twist.hpp>
 
+#include <cmath>
 #include <vector>
 
 namespace autoware::behavior_path_planner::utils::path_safety_checker
@@ -98,7 +99,8 @@ bool checkSafetyWithRSS(
   const std::vector<PoseWithVelocityStamped> & ego_predicted_path,
   const std::vector<ExtendedPredictedObject> & objects, CollisionCheckDebugMap & debug_map,
   const BehaviorPathPlannerParameters & parameters, const RSSparams & rss_params,
-  const bool check_all_predicted_path, const double hysteresis_factor);
+  const bool check_all_predicted_path, const double hysteresis_factor,
+  const double yaw_difference_th);
 
 /**
  * @brief Iterate the points in the ego and target's predicted path and
@@ -111,6 +113,8 @@ bool checkSafetyWithRSS(
  * @param common_parameters The common parameters used in behavior path planner.
  * @param front_object_deceleration The deceleration of the object in the front.(used in RSS)
  * @param rear_object_deceleration The deceleration of the object in the rear.(used in RSS)
+ * @param yaw_difference_th maximum yaw difference between any given ego path pose and object
+ * predicted path pose.
  * @param debug The debug information for collision checking.
  * @return true if distance is safe.
  */
@@ -120,7 +124,7 @@ bool checkCollision(
   const ExtendedPredictedObject & target_object,
   const PredictedPathWithPolygon & target_object_path,
   const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
-  const double hysteresis_factor, CollisionCheckDebug & debug);
+  const double hysteresis_factor, const double yaw_difference_th, CollisionCheckDebug & debug);
 
 /**
  * @brief Iterate the points in the ego and target's predicted path and
@@ -142,7 +146,8 @@ std::vector<Polygon2d> getCollidedPolygons(
   const ExtendedPredictedObject & target_object,
   const PredictedPathWithPolygon & target_object_path,
   const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
-  const double hysteresis_factor, const double max_velocity_limit, CollisionCheckDebug & debug);
+  const double hysteresis_factor, const double max_velocity_limit, const double yaw_difference_th,
+  CollisionCheckDebug & debug);
 
 bool checkPolygonsIntersects(
   const std::vector<Polygon2d> & polys_1, const std::vector<Polygon2d> & polys_2);
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp
index 7bb8c13aa65fb..b05d869cca6b5 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp
@@ -26,6 +26,9 @@
 #include <boost/geometry/algorithms/union.hpp>
 #include <boost/geometry/strategies/strategies.hpp>
 
+#include <tf2/utils.h>
+
+#include <cmath>
 #include <limits>
 
 namespace autoware::behavior_path_planner::utils::path_safety_checker
@@ -482,7 +485,8 @@ bool checkSafetyWithRSS(
   const std::vector<PoseWithVelocityStamped> & ego_predicted_path,
   const std::vector<ExtendedPredictedObject> & objects, CollisionCheckDebugMap & debug_map,
   const BehaviorPathPlannerParameters & parameters, const RSSparams & rss_params,
-  const bool check_all_predicted_path, const double hysteresis_factor)
+  const bool check_all_predicted_path, const double hysteresis_factor,
+  const double yaw_difference_th)
 {
   // Check for collisions with each predicted path of the object
   const bool is_safe = !std::any_of(objects.begin(), objects.end(), [&](const auto & object) {
@@ -495,7 +499,7 @@ bool checkSafetyWithRSS(
       obj_predicted_paths.begin(), obj_predicted_paths.end(), [&](const auto & obj_path) {
         const bool has_collision = !utils::path_safety_checker::checkCollision(
           planned_path, ego_predicted_path, object, obj_path, parameters, rss_params,
-          hysteresis_factor, current_debug_data.second);
+          hysteresis_factor, yaw_difference_th, current_debug_data.second);
 
         utils::path_safety_checker::updateCollisionCheckDebugMap(
           debug_map, current_debug_data, !has_collision);
@@ -559,11 +563,12 @@ bool checkCollision(
   const ExtendedPredictedObject & target_object,
   const PredictedPathWithPolygon & target_object_path,
   const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
-  const double hysteresis_factor, CollisionCheckDebug & debug)
+  const double hysteresis_factor, const double yaw_difference_th, CollisionCheckDebug & debug)
 {
   const auto collided_polygons = getCollidedPolygons(
     planned_path, predicted_ego_path, target_object, target_object_path, common_parameters,
-    rss_parameters, hysteresis_factor, std::numeric_limits<double>::max(), debug);
+    rss_parameters, hysteresis_factor, std::numeric_limits<double>::max(), yaw_difference_th,
+    debug);
   return collided_polygons.empty();
 }
 
@@ -573,7 +578,8 @@ std::vector<Polygon2d> getCollidedPolygons(
   const ExtendedPredictedObject & target_object,
   const PredictedPathWithPolygon & target_object_path,
   const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
-  double hysteresis_factor, const double max_velocity_limit, CollisionCheckDebug & debug)
+  double hysteresis_factor, const double max_velocity_limit, const double yaw_difference_th,
+  CollisionCheckDebug & debug)
 {
   {
     debug.ego_predicted_path = predicted_ego_path;
@@ -604,6 +610,11 @@ std::vector<Polygon2d> getCollidedPolygons(
     const auto & ego_polygon = interpolated_data->poly;
     const auto ego_velocity = std::min(interpolated_data->velocity, max_velocity_limit);
 
+    const double ego_yaw = tf2::getYaw(ego_pose.orientation);
+    const double object_yaw = tf2::getYaw(obj_pose.orientation);
+    const double yaw_difference = autoware::universe_utils::normalizeRadian(ego_yaw - object_yaw);
+    if (std::abs(yaw_difference) > yaw_difference_th) continue;
+
     // check overlap
     if (boost::geometry::overlaps(ego_polygon, obj_polygon)) {
       debug.unsafe_reason = "overlap_polygon";
diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md
index 066708891652a..dba4a0cfce5e7 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md
+++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md
@@ -436,17 +436,18 @@ Parameters under `target_filtering` are related to filtering target objects for
 
 Parameters under `safety_check_params` define the configuration for safety check.
 
-| Name                                           | Unit | Type   | Description                                 | Default value |
-| :--------------------------------------------- | :--- | :----- | :------------------------------------------ | :------------ |
-| enable_safety_check                            | -    | bool   | Flag to enable safety check                 | true          |
-| check_all_predicted_path                       | -    | bool   | Flag to check all predicted paths           | true          |
-| publish_debug_marker                           | -    | bool   | Flag to publish debug markers               | false         |
-| rss_params.rear_vehicle_reaction_time          | [s]  | double | Reaction time for rear vehicles             | 2.0           |
-| rss_params.rear_vehicle_safety_time_margin     | [s]  | double | Safety time margin for rear vehicles        | 1.0           |
-| rss_params.lateral_distance_max_threshold      | [m]  | double | Maximum lateral distance threshold          | 2.0           |
-| rss_params.longitudinal_distance_min_threshold | [m]  | double | Minimum longitudinal distance threshold     | 3.0           |
-| rss_params.longitudinal_velocity_delta_time    | [s]  | double | Delta time for longitudinal velocity        | 0.8           |
-| hysteresis_factor_expand_rate                  | -    | double | Rate to expand/shrink the hysteresis factor | 1.0           |
+| Name                                           | Unit | Type   | Description                                                                               | Default value |
+| :--------------------------------------------- | :--- | :----- | :---------------------------------------------------------------------------------------- | :------------ |
+| enable_safety_check                            | -    | bool   | Flag to enable safety check                                                               | true          |
+| check_all_predicted_path                       | -    | bool   | Flag to check all predicted paths                                                         | true          |
+| publish_debug_marker                           | -    | bool   | Flag to publish debug markers                                                             | false         |
+| rss_params.rear_vehicle_reaction_time          | [s]  | double | Reaction time for rear vehicles                                                           | 2.0           |
+| rss_params.rear_vehicle_safety_time_margin     | [s]  | double | Safety time margin for rear vehicles                                                      | 1.0           |
+| rss_params.lateral_distance_max_threshold      | [m]  | double | Maximum lateral distance threshold                                                        | 2.0           |
+| rss_params.longitudinal_distance_min_threshold | [m]  | double | Minimum longitudinal distance threshold                                                   | 3.0           |
+| rss_params.longitudinal_velocity_delta_time    | [s]  | double | Delta time for longitudinal velocity                                                      | 0.8           |
+| hysteresis_factor_expand_rate                  | -    | double | Rate to expand/shrink the hysteresis factor                                               | 1.0           |
+| collision_check_yaw_diff_threshold             | -    | double | Maximum yaw difference between ego and object when executing rss-based collision checking | 1.578         |
 
 ## **Path Generation**
 
diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml
index ce7ead87b54c7..d39a320a19e14 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml
+++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml
@@ -143,8 +143,10 @@
             lateral_distance_max_threshold: 2.0
             longitudinal_distance_min_threshold: 3.0
             longitudinal_velocity_delta_time: 0.8
+            extended_polygon_policy: "along_path" # [-] select "rectangle" or "along_path"
           # hysteresis factor to expand/shrink polygon
           hysteresis_factor_expand_rate: 1.0
+          collision_check_yaw_diff_threshold: 1.578
           # temporary
           backward_path_length: 30.0
           forward_path_length: 100.0
diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp
index 7554098414442..b07d6497463ef 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp
@@ -77,7 +77,7 @@ struct PullOutStatus
   // record if the ego has departed from the start point
   bool has_departed{false};
 
-  PullOutStatus() {}
+  PullOutStatus() = default;
 };
 
 class StartPlannerModule : public SceneModuleInterface
@@ -90,7 +90,7 @@ class StartPlannerModule : public SceneModuleInterface
     std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
       objects_of_interest_marker_interface_ptr_map);
 
-  ~StartPlannerModule()
+  ~StartPlannerModule() override
   {
     if (freespace_planner_timer_) {
       freespace_planner_timer_->cancel();
@@ -296,16 +296,16 @@ class StartPlannerModule : public SceneModuleInterface
   PathWithLaneId getCurrentPath() const;
   void planWithPriority(
     const std::vector<Pose> & start_pose_candidates, const Pose & refined_start_pose,
-    const Pose & goal_pose, const std::string search_priority);
+    const Pose & goal_pose, const std::string & search_priority);
   PathWithLaneId generateStopPath() const;
   lanelet::ConstLanelets getPathRoadLanes(const PathWithLaneId & path) const;
   std::vector<DrivableLanes> generateDrivableLanes(const PathWithLaneId & path) const;
   void updatePullOutStatus();
   void updateStatusAfterBackwardDriving();
   PredictedObjects filterStopObjectsInPullOutLanes(
-    const lanelet::ConstLanelets & pull_out_lanes, const geometry_msgs::msg::Point & current_pose,
-    const double velocity_threshold, const double object_check_backward_distance,
-    const double object_check_forward_distance) const;
+    const lanelet::ConstLanelets & pull_out_lanes, const geometry_msgs::msg::Point & current_point,
+    const double velocity_threshold, const double object_check_forward_distance,
+    const double object_check_backward_distance) const;
   bool needToPrepareBlinkerBeforeStartDrivingForward() const;
   bool hasReachedFreespaceEnd() const;
   bool hasReachedPullOutEnd() const;
diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp
index 9451bb3870648..873bab1043485 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp
@@ -283,6 +283,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
       node->declare_parameter<double>(safety_check_ns + "forward_path_length");
     p.safety_check_params.publish_debug_marker =
       node->declare_parameter<bool>(safety_check_ns + "publish_debug_marker");
+    p.safety_check_params.collision_check_yaw_diff_threshold =
+      node->declare_parameter<double>(safety_check_ns + "collision_check_yaw_diff_threshold");
   }
 
   // RSSparams
@@ -298,6 +300,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
       node->declare_parameter<double>(rss_ns + "longitudinal_distance_min_threshold");
     p.safety_check_params.rss_params.longitudinal_velocity_delta_time =
       node->declare_parameter<double>(rss_ns + "longitudinal_velocity_delta_time");
+    p.safety_check_params.rss_params.extended_polygon_policy =
+      node->declare_parameter<std::string>(rss_ns + "extended_polygon_policy");
   }
 
   // surround moving obstacle check
@@ -367,7 +371,6 @@ void StartPlannerModuleManager::updateModuleParams(
     updateParam<double>(
       parameters, ns + "extra_width_margin_for_rear_obstacle",
       p->extra_width_margin_for_rear_obstacle);
-
     updateParam<std::vector<double>>(
       parameters, ns + "collision_check_margins", p->collision_check_margins);
     updateParam<double>(
@@ -658,6 +661,9 @@ void StartPlannerModuleManager::updateModuleParams(
     updateParam<bool>(
       parameters, safety_check_ns + "publish_debug_marker",
       p->safety_check_params.publish_debug_marker);
+    updateParam<double>(
+      parameters, safety_check_ns + "collision_check_yaw_diff_threshold",
+      p->safety_check_params.collision_check_yaw_diff_threshold);
   }
   const std::string rss_ns = safety_check_ns + "rss_params.";
   {
@@ -676,6 +682,9 @@ void StartPlannerModuleManager::updateModuleParams(
     updateParam<double>(
       parameters, rss_ns + "longitudinal_velocity_delta_time",
       p->safety_check_params.rss_params.longitudinal_velocity_delta_time);
+    updateParam<std::string>(
+      parameters, rss_ns + "extended_polygon_policy",
+      p->safety_check_params.rss_params.extended_polygon_policy);
   }
   std::string surround_moving_obstacle_check_ns = ns + "surround_moving_obstacle_check.";
   {
diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp
index 9400f3ff40286..08d5ea4365575 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp
@@ -34,7 +34,9 @@
 #include <lanelet2_core/geometry/Lanelet.h>
 
 #include <algorithm>
+#include <cmath>
 #include <memory>
+#include <optional>
 #include <string>
 #include <utility>
 #include <vector>
@@ -65,7 +67,7 @@ StartPlannerModule::StartPlannerModule(
 {
   lane_departure_checker_ = std::make_shared<LaneDepartureChecker>();
   lane_departure_checker_->setVehicleInfo(vehicle_info_);
-  autoware::lane_departure_checker::Param lane_departure_checker_params;
+  autoware::lane_departure_checker::Param lane_departure_checker_params{};
   lane_departure_checker_params.footprint_extra_margin =
     parameters->lane_departure_check_expansion_margin;
 
@@ -104,7 +106,7 @@ void StartPlannerModule::onFreespacePlannerTimer()
   std::optional<ModuleStatus> current_status_opt{std::nullopt};
   std::optional<StartPlannerParameters> parameters_opt{std::nullopt};
   std::optional<PullOutStatus> pull_out_status_opt{std::nullopt};
-  bool is_stopped;
+  bool is_stopped{false};
 
   // making a local copy of thread sensitive data
   {
@@ -501,7 +503,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const
   // Get the closest target obj width in the relevant lanes
   const auto closest_object_width = std::invoke([&]() -> std::optional<double> {
     double arc_length_to_closet_object = std::numeric_limits<double>::max();
-    double closest_object_width = -1.0;
+    std::optional<double> closest_object_width = std::nullopt;
     std::for_each(
       target_objects_on_lane.on_current_lane.begin(), target_objects_on_lane.on_current_lane.end(),
       [&](const auto & o) {
@@ -512,7 +514,6 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const
         arc_length_to_closet_object = arc_length;
         closest_object_width = o.shape.dimensions.y;
       });
-    if (closest_object_width < 0.0) return std::nullopt;
     return closest_object_width;
   });
   if (!closest_object_width) return false;
@@ -837,7 +838,7 @@ PathWithLaneId StartPlannerModule::getCurrentPath() const
 
 void StartPlannerModule::planWithPriority(
   const std::vector<Pose> & start_pose_candidates, const Pose & refined_start_pose,
-  const Pose & goal_pose, const std::string search_priority)
+  const Pose & goal_pose, const std::string & search_priority)
 {
   if (start_pose_candidates.empty()) return;
 
@@ -1112,8 +1113,8 @@ void StartPlannerModule::updateStatusAfterBackwardDriving()
   waitApproval();
   // To enable approval of the forward path, the RTC status is removed.
   removeRTCStatus();
-  for (auto itr = uuid_map_.begin(); itr != uuid_map_.end(); ++itr) {
-    itr->second = generateUUID();
+  for (auto & itr : uuid_map_) {
+    itr.second = generateUUID();
   }
 }
 
@@ -1303,7 +1304,7 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo()
     return ignore_signal_.value() == id;
   };
 
-  const auto update_ignore_signal = [this](const lanelet::Id & id, const bool is_ignore) {
+  const auto update_ignore_signal = [](const lanelet::Id & id, const bool is_ignore) {
     return is_ignore ? std::make_optional(id) : std::nullopt;
   };
 
@@ -1422,10 +1423,12 @@ bool StartPlannerModule::isSafePath() const
   merged_target_object.insert(
     merged_target_object.end(), target_objects_on_lane.on_shoulder_lane.begin(),
     target_objects_on_lane.on_shoulder_lane.end());
+
   return autoware::behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS(
     pull_out_path, ego_predicted_path, merged_target_object, debug_data_.collision_check,
     planner_data_->parameters, safety_check_params_->rss_params,
-    objects_filtering_params_->use_all_predicted_path, hysteresis_factor);
+    objects_filtering_params_->use_all_predicted_path, hysteresis_factor,
+    safety_check_params_->collision_check_yaw_diff_threshold);
 }
 
 bool StartPlannerModule::isGoalBehindOfEgoInSameRouteSegment() const
@@ -1693,7 +1696,7 @@ void StartPlannerModule::setDebugData()
 
   // safety check
   if (parameters_->safety_check_params.enable_safety_check) {
-    if (debug_data_.ego_predicted_path.size() > 0) {
+    if (!debug_data_.ego_predicted_path.empty()) {
       const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath(
         debug_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution);
       add(
@@ -1702,7 +1705,7 @@ void StartPlannerModule::setDebugData()
         debug_marker_);
     }
 
-    if (debug_data_.filtered_objects.objects.size() > 0) {
+    if (!debug_data_.filtered_objects.objects.empty()) {
       add(
         createObjectsMarkerArray(
           debug_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9),
diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md
index 33abc8eaf40ed..ec139b9a8b099 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md
+++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md
@@ -485,11 +485,6 @@ title Filtering flow for vehicle type objects
 start
 
 partition isSatisfiedWithVehicleCodition() {
-if(object is force avoidance target ?) then (yes)
-#FF006C :return true;
-stop
-else (\n no)
-
 if(isNeverAvoidanceTarget()) then (yes)
 #00FFB1 :return false;
 stop
@@ -537,7 +532,6 @@ endif
 endif
 endif
 endif
-endif
 #00FFB1 :return false;
 stop
 }
@@ -766,7 +760,7 @@ Basically, this module tries to generate avoidance path in order to keep lateral
 
 But if there isn't enough space to keep `soft_margin` distance, this module shortens soft constraint lateral margin. The parameter `soft_margin` is a maximum value of soft constraint, and actual soft margin can be a value between 0.0 and `soft_margin`. On the other hand, this module definitely keeps `hard_margin` or `hard_margin_for_parked_vehicle` depending on the situation. Thus, the minimum value of total lateral margin is `hard_margin`/`hard_margin_for_parked_vehicle`, and the maximum value is the sum of `hard_margin`/`hard_margin_for_parked_vehicle` and `soft_margin`.
 
-Following figure shows the situation where this module shortens lateral soft constraint in order not to drive opposite direction lane when user set a parameter `use_opposite_lane` to `false`.
+Following figure shows the situation where this module shortens lateral soft constraint in order not to drive opposite direction lane when user set a parameter `use_lane_type` to `same_direction_lane`.
 
 ![fig](./images/path_generation/adjust_margin.png)
 
@@ -790,7 +784,7 @@ On the other hand, if the lateral distance is larger than `hard_margin`/`hard_ma
 
 ### When there is not enough space
 
-This module inserts stop point only when the ego can potentially avoid the object. So, if it is not able to keep distance more than `hard_margin`/`hard_margin_for_parked_vehicle`, this module does nothing. Following figure shows the situation where this module is not able to keep enough lateral distance when user set a parameter `use_opposite_lane` to `false`.
+This module inserts stop point only when the ego can potentially avoid the object. So, if it is not able to keep distance more than `hard_margin`/`hard_margin_for_parked_vehicle`, this module does nothing. Following figure shows the situation where this module is not able to keep enough lateral distance when user set a parameter `use_lane_type` to `same_direction_lane`.
 
 ![fig](./images/path_generation/do_nothing.png)
 
@@ -810,15 +804,19 @@ Usable lane for avoidance module can be selected by config file.
 
 ```yaml
       ...
-      use_adjacent_lane: true
-      use_opposite_lane: true
+      # drivable lane setting. this module is able to use not only current lane but also right/left lane
+      # if the current lane(=lanelt::Lanelet) and the rignt/left lane share the boundary(=lanelet::Linestring) in HDMap.
+      # "current_lane"           : use only current lane. this module doesn't use adjacent lane to avoid object.
+      # "same_direction_lane"    : this module uses same direction lane to avoid object if need.
+      # "opposite_direction_lane": this module uses both same direction and opposite direction lane.
+      use_lane_type: "opposite_direction_lane"
 ```
 
-When user set parameter both `use_adjacent_lane` and `use_opposite_lane` to `true`, it is possible to use opposite lane.
+When user set parameter `use_lane_type` to `opposite_direction_lane`, it is possible to use opposite lane.
 
 ![fig](./images/path_generation/opposite_direction.png)
 
-When user only set parameter `use_adjacent_lane` to `true`, the module doesn't create path that overlaps opposite lane.
+When user set parameter `use_lane_type` to `same_direction_lane`, the module doesn't create path that overlaps opposite lane.
 
 ![fig](./images/path_generation/same_direction.png)
 
@@ -978,21 +976,25 @@ This module supports drivable area expansion for following polygons defined in H
 Please set the flags to `true` when user wants to make it possible to use those areas in avoidance maneuver.
 
 ```yaml
+# drivable lane setting. this module is able to use not only current lane but also right/left lane
+# if the current lane(=lanelt::Lanelet) and the rignt/left lane share the boundary(=lanelet::Linestring) in HDMap.
+# "current_lane"           : use only current lane. this module doesn't use adjacent lane to avoid object.
+# "same_direction_lane"    : this module uses same direction lane to avoid object if need.
+# "opposite_direction_lane": this module uses both same direction and opposite direction lane.
+use_lane_type: "opposite_direction_lane"
 # drivable area setting
-use_adjacent_lane: true
-use_opposite_lane: true
 use_intersection_areas: true
 use_hatched_road_markings: true
 use_freespace_areas: true
 ```
 
-|                       |                                                            |                                                                                                                                                                                                                     |
-| --------------------- | ---------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
-| adjacent lane         | ![fig](./images/advanced/avoidance_same_direction.png)     |                                                                                                                                                                                                                     |
-| opposite lane         | ![fig](./images/advanced/avoidance_opposite_direction.png) |                                                                                                                                                                                                                     |
-| intersection area     | ![fig](./images/advanced/avoidance_intersection.png)       | The intersection area is defined on Lanelet map. See [here](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md)                               |
-| hatched road markings | ![fig](./images/advanced/avoidance_zebra.png)              | The hatched road marking is defined on Lanelet map. See [here](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md#hatched-road-markings-area) |
-| freespace area        | ![fig](./images/advanced/avoidance_freespace.png)          | The freespace area is defined on Lanelet map. (unstable)                                                                                                                                                            |
+|                                        |                                                            |                                                                                                                                                                                                                     |
+| -------------------------------------- | ---------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| use_lane_type: same_direction_lane     | ![fig](./images/advanced/avoidance_same_direction.png)     |                                                                                                                                                                                                                     |
+| use_lane_type: opposite_direction_lane | ![fig](./images/advanced/avoidance_opposite_direction.png) |                                                                                                                                                                                                                     |
+| intersection area                      | ![fig](./images/advanced/avoidance_intersection.png)       | The intersection area is defined on Lanelet map. See [here](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md)                               |
+| hatched road markings                  | ![fig](./images/advanced/avoidance_zebra.png)              | The hatched road marking is defined on Lanelet map. See [here](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md#hatched-road-markings-area) |
+| freespace area                         | ![fig](./images/advanced/avoidance_freespace.png)          | The freespace area is defined on Lanelet map. (unstable)                                                                                                                                                            |
 
 ## Future extensions / Unimplemented parts
 
diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml
index 324be9a73439e..3087ccc93934b 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml
+++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml
@@ -5,9 +5,13 @@
       resample_interval_for_planning: 0.3               # [m] FOR DEVELOPER
       resample_interval_for_output: 4.0                 # [m] FOR DEVELOPER
 
+      # drivable lane setting. this module is able to use not only current lane but also right/left lane
+      # if the current lane(=lanelet::Lanelet) and the right/left lane share the boundary(=lanelet::Linestring) in HDMap.
+      # "current_lane"           : use only current lane. this module doesn't use adjacent lane to avoid object.
+      # "same_direction_lane"    : this module uses same direction lane to avoid object if need.
+      # "opposite_direction_lane": this module uses both same direction and opposite direction lane.
+      use_lane_type: "opposite_direction_lane"
       # drivable area setting
-      use_adjacent_lane: true
-      use_opposite_lane: true
       use_intersection_areas: true
       use_hatched_road_markings: true
       use_freespace_areas: true
@@ -174,6 +178,7 @@
         safety_check_backward_distance: 100.0           # [m]
         hysteresis_factor_expand_rate: 1.5              # [-]
         hysteresis_factor_safe_count: 3                 # [-]
+        collision_check_yaw_diff_threshold: 3.1416      # [rad]
         # predicted path parameters
         min_velocity: 1.38                              # [m/s]
         max_velocity: 50.0                              # [m/s]
diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp
index f05230f4ec19d..d02b39047e71c 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp
@@ -101,12 +101,8 @@ struct AvoidanceParameters
   // computational cost for latter modules.
   double resample_interval_for_output = 3.0;
 
-  // enable avoidance to be perform only in lane with same direction
-  bool use_adjacent_lane{true};
-
-  // enable avoidance to be perform in opposite lane direction
-  // to use this, enable_avoidance_over_same_direction need to be set to true.
-  bool use_opposite_lane{true};
+  // drivable lane config
+  std::string use_lane_type{"current_lane"};
 
   // if this param is true, it reverts avoidance path when the path is no longer needed.
   bool enable_cancel_maneuver{false};
@@ -223,6 +219,8 @@ struct AvoidanceParameters
   size_t hysteresis_factor_safe_count;
   double hysteresis_factor_expand_rate{0.0};
 
+  double collision_check_yaw_diff_threshold{3.1416};
+
   bool consider_front_overhang{true};
   bool consider_rear_overhang{true};
 
diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp
index b761939589968..84cf7c4e33d26 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp
@@ -46,8 +46,7 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
   // drivable area
   {
     const std::string ns = "avoidance.";
-    p.use_adjacent_lane = getOrDeclareParameter<bool>(*node, ns + "use_adjacent_lane");
-    p.use_opposite_lane = getOrDeclareParameter<bool>(*node, ns + "use_opposite_lane");
+    p.use_lane_type = getOrDeclareParameter<std::string>(*node, ns + "use_lane_type");
     p.use_intersection_areas = getOrDeclareParameter<bool>(*node, ns + "use_intersection_areas");
     p.use_hatched_road_markings =
       getOrDeclareParameter<bool>(*node, ns + "use_hatched_road_markings");
@@ -199,6 +198,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
       getOrDeclareParameter<double>(*node, ns + "hysteresis_factor_expand_rate");
     p.hysteresis_factor_safe_count =
       getOrDeclareParameter<int>(*node, ns + "hysteresis_factor_safe_count");
+    p.collision_check_yaw_diff_threshold =
+      getOrDeclareParameter<double>(*node, ns + "collision_check_yaw_diff_threshold");
   }
 
   // safety check predicted path params
diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json
index 266a4cf8a23ab..db3215fa8d238 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json
+++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json
@@ -22,15 +22,11 @@
           "description": "Path generation method.",
           "default": "shift_line_base"
         },
-        "use_adjacent_lane": {
-          "type": "boolean",
-          "description": "Extend avoidance trajectory to adjacent lanes that has same direction. If false, avoidance only happen in current lane.",
-          "default": "true"
-        },
-        "use_opposite_lane": {
-          "type": "boolean",
-          "description": "Extend avoidance trajectory to opposite direction lane. `use_adjacent_lane` must be `true` to take effects.",
-          "default": "true"
+        "use_lane_type": {
+          "type": "string",
+          "enum": ["current_lane", "same_direction_lane", "opposite_direction_lane"],
+          "description": "Drivable lane configuration.",
+          "default": "opposite_direction_lane"
         },
         "use_hatched_road_markings": {
           "type": "boolean",
@@ -927,6 +923,11 @@
               "description": "Hysteresis count that be used for chattering prevention.",
               "default": 10
             },
+            "collision_check_yaw_diff_threshold": {
+              "type": "number",
+              "description": "Max yaw difference between ego and object when doing collision check",
+              "default": 3.1416
+            },
             "min_velocity": {
               "type": "number",
               "description": "Minimum velocity of the ego vehicle's predicted path.",
@@ -1442,8 +1443,7 @@
         "resample_interval_for_planning",
         "resample_interval_for_output",
         "path_generation_method",
-        "use_adjacent_lane",
-        "use_opposite_lane",
+        "use_lane_type",
         "use_hatched_road_markings",
         "use_intersection_areas",
         "use_freespace_areas",
diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp
index b0fd1b5c1b8ab..824466a1ad3ad 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp
@@ -781,7 +781,8 @@ bool StaticObstacleAvoidanceModule::isSafePath(
     for (const auto & obj_path : obj_predicted_paths) {
       if (!utils::path_safety_checker::checkCollision(
             shifted_path.path, ego_predicted_path, object, obj_path, p, parameters_->rss_params,
-            hysteresis_factor, current_debug_data.second)) {
+            hysteresis_factor, parameters_->collision_check_yaw_diff_threshold,
+            current_debug_data.second)) {
         utils::path_safety_checker::updateCollisionCheckDebugMap(
           debug.collision_check, current_debug_data, false);
 
diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp
index 70de9e9499917..8d0f9904b5bad 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp
@@ -2238,14 +2238,16 @@ DrivableLanes generateExpandedDrivableLanes(
   current_drivable_lanes.left_lane = lanelet;
   current_drivable_lanes.right_lane = lanelet;
 
-  if (!parameters->use_adjacent_lane) {
+  if (parameters->use_lane_type == "current_lane") {
     return current_drivable_lanes;
   }
 
+  const auto use_opposite_lane = parameters->use_lane_type == "opposite_direction_lane";
+
   // 1. get left/right side lanes
   const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) {
-    const auto all_left_lanelets = route_handler->getAllLeftSharedLinestringLanelets(
-      target_lane, parameters->use_opposite_lane, true);
+    const auto all_left_lanelets =
+      route_handler->getAllLeftSharedLinestringLanelets(target_lane, use_opposite_lane, true);
     if (!all_left_lanelets.empty()) {
       current_drivable_lanes.left_lane = all_left_lanelets.back();  // leftmost lanelet
       pushUniqueVector(
@@ -2254,8 +2256,8 @@ DrivableLanes generateExpandedDrivableLanes(
     }
   };
   const auto update_right_lanelets = [&](const lanelet::ConstLanelet & target_lane) {
-    const auto all_right_lanelets = route_handler->getAllRightSharedLinestringLanelets(
-      target_lane, parameters->use_opposite_lane, true);
+    const auto all_right_lanelets =
+      route_handler->getAllRightSharedLinestringLanelets(target_lane, use_opposite_lane, true);
     if (!all_right_lanelets.empty()) {
       current_drivable_lanes.right_lane = all_right_lanelets.back();  // rightmost lanelet
       pushUniqueVector(
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp
index 7abd88d59f398..9cbe962e88cae 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp
@@ -83,7 +83,7 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
 
   // Get stop point
   const auto stop_point = arc_lane_utils::createTargetPoint(
-    original_path, stop_line, lane_id_, planner_param_.stop_margin,
+    original_path, stop_line, planner_param_.stop_margin,
     planner_data_->vehicle_info_.max_longitudinal_offset_m);
   if (!stop_point) {
     return true;
@@ -128,7 +128,7 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
   if (planner_param_.use_dead_line) {
     // Use '-' for margin because it's the backward distance from stop line
     const auto dead_line_point = arc_lane_utils::createTargetPoint(
-      original_path, stop_line, lane_id_, -planner_param_.dead_line_margin,
+      original_path, stop_line, -planner_param_.dead_line_margin,
       planner_data_->vehicle_info_.max_longitudinal_offset_m);
 
     if (dead_line_point) {
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp
index d007416dccb47..0478aea34a44f 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp
@@ -125,7 +125,7 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason
     return true;
   }
   const auto stop_point = arc_lane_utils::createTargetPoint(
-    original_path, stop_line.value(), lane_id_, planner_param_.stop_margin,
+    original_path, stop_line.value(), planner_param_.stop_margin,
     planner_data_->vehicle_info_.max_longitudinal_offset_m);
   if (!stop_point) {
     setSafe(true);
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp
index 77b7d25f9f46e..769932259e3e6 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp
@@ -61,20 +61,9 @@ std::optional<geometry_msgs::msg::Point> checkCollision(
 template <class T>
 std::optional<PathIndexWithPoint> findCollisionSegment(
   const T & path, const geometry_msgs::msg::Point & stop_line_p1,
-  const geometry_msgs::msg::Point & stop_line_p2, const size_t target_lane_id)
+  const geometry_msgs::msg::Point & stop_line_p2)
 {
   for (size_t i = 0; i < path.points.size() - 1; ++i) {
-    const auto & prev_lane_ids = path.points.at(i).lane_ids;
-    const auto & next_lane_ids = path.points.at(i + 1).lane_ids;
-
-    const bool is_target_lane_in_prev_lane =
-      std::find(prev_lane_ids.begin(), prev_lane_ids.end(), target_lane_id) != prev_lane_ids.end();
-    const bool is_target_lane_in_next_lane =
-      std::find(next_lane_ids.begin(), next_lane_ids.end(), target_lane_id) != next_lane_ids.end();
-    if (!is_target_lane_in_prev_lane && !is_target_lane_in_next_lane) {
-      continue;
-    }
-
     const auto & p1 =
       autoware::universe_utils::getPoint(path.points.at(i));  // Point before collision point
     const auto & p2 =
@@ -92,12 +81,12 @@ std::optional<PathIndexWithPoint> findCollisionSegment(
 
 template <class T>
 std::optional<PathIndexWithPoint> findCollisionSegment(
-  const T & path, const LineString2d & stop_line, const size_t target_lane_id)
+  const T & path, const LineString2d & stop_line)
 {
   const auto stop_line_p1 = convertToGeomPoint(stop_line.at(0));
   const auto stop_line_p2 = convertToGeomPoint(stop_line.at(1));
 
-  return findCollisionSegment(path, stop_line_p1, stop_line_p2, target_lane_id);
+  return findCollisionSegment(path, stop_line_p1, stop_line_p2);
 }
 
 template <class T>
@@ -194,7 +183,7 @@ geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffse
 
 std::optional<PathIndexWithPose> createTargetPoint(
   const tier4_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line,
-  const size_t lane_id, const double margin, const double vehicle_offset);
+  const double margin, const double vehicle_offset);
 
 }  // namespace arc_lane_utils
 }  // namespace autoware::behavior_velocity_planner
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp
index 3e029174b288d..da6ef7262de74 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp
@@ -109,10 +109,10 @@ std::optional<PathIndexWithOffset> findOffsetSegment(
 
 std::optional<PathIndexWithPose> createTargetPoint(
   const tier4_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line,
-  const size_t lane_id, const double margin, const double vehicle_offset)
+  const double margin, const double vehicle_offset)
 {
   // Find collision segment
-  const auto collision_segment = findCollisionSegment(path, stop_line, lane_id);
+  const auto collision_segment = findCollisionSegment(path, stop_line);
   if (!collision_segment) {
     // No collision
     return {};
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp
index ddedec80dc0f3..db9a63169e565 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp
@@ -49,14 +49,11 @@ TEST(findCollisionSegment, nominal)
    *
    **/
   auto path = test::generatePath(0.0, 0.0, 5.0, 0.0, 6);
-  for (auto & point : path.points) {
-    point.lane_ids.push_back(100);
-  }
 
   LineString2d stop_line;
   stop_line.emplace_back(Point2d(3.5, 3.0));
   stop_line.emplace_back(Point2d(3.5, -3.0));
-  auto segment = arc_lane_utils::findCollisionSegment(path, stop_line, 100);
+  auto segment = arc_lane_utils::findCollisionSegment(path, stop_line);
   EXPECT_EQ(segment->first, static_cast<size_t>(3));
   EXPECT_DOUBLE_EQ(segment->second.x, 3.5);
   EXPECT_DOUBLE_EQ(segment->second.y, 0.0);
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp
index 6075706a672d0..04ea5ca872098 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp
@@ -52,7 +52,7 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop
 
   // Calculate stop pose and insert index
   const auto stop_point = arc_lane_utils::createTargetPoint(
-    *path, stop_line, lane_id_, planner_param_.stop_margin,
+    *path, stop_line, planner_param_.stop_margin,
     planner_data_->vehicle_info_.max_longitudinal_offset_m);
 
   // If no collision found, do nothing
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp
index d27cc23223630..1bf26d9f95c6a 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp
@@ -379,7 +379,7 @@ std::optional<size_t> VirtualTrafficLightModule::getPathIndexOfFirstEndLine()
     end_line_p2.y = end_line.back().y();
 
     const auto collision =
-      arc_lane_utils::findCollisionSegment(module_data_.path, end_line_p1, end_line_p2, lane_id_);
+      arc_lane_utils::findCollisionSegment(module_data_.path, end_line_p1, end_line_p2);
     if (!collision) {
       continue;
     }
diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp
index aeeaabeb9b510..af63b50bceb39 100644
--- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp
+++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp
@@ -50,7 +50,7 @@ size_t calculateStartIndex(
 /// @param[in] start_idx starting index of the input trajectory
 /// @param[in] factor factor used for downsampling
 /// @return downsampled trajectory
-TrajectoryPoints downsampleTrajectory(
+TrajectoryPoints downsample_trajectory(
   const TrajectoryPoints & trajectory, const size_t start_idx, const int factor);
 
 /// @brief recalculate the steering angle of the trajectory
@@ -123,16 +123,6 @@ std::vector<autoware::motion_velocity_planner::SlowdownInterval> calculate_slowd
   const std::vector<multi_linestring_t> & projections, const std::vector<polygon_t> & footprints,
   ProjectionParameters & projection_params, const VelocityParameters & velocity_params,
   autoware::motion_utils::VirtualWalls & virtual_walls);
-
-/// @brief copy the velocity profile of a downsampled trajectory to the original trajectory
-/// @param[in] downsampled_trajectory downsampled trajectory
-/// @param[in] trajectory input trajectory
-/// @param[in] start_idx starting index of the downsampled trajectory relative to the input
-/// @param[in] factor downsampling factor
-/// @return input trajectory with the velocity profile of the downsampled trajectory
-TrajectoryPoints copyDownsampledVelocity(
-  const TrajectoryPoints & downsampled_traj, TrajectoryPoints trajectory, const size_t start_idx,
-  const int factor);
 }  // namespace autoware::motion_velocity_planner::obstacle_velocity_limiter
 
 #endif  // OBSTACLE_VELOCITY_LIMITER_HPP_
diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp
index eb8a33ede2c72..efbdcdaf464e2 100644
--- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp
+++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp
@@ -23,6 +23,7 @@
 
 #include <autoware/motion_utils/marker/virtual_wall_marker_creator.hpp>
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
+#include <autoware/motion_velocity_planner_common/trajectory_preprocessing.hpp>
 #include <autoware/universe_utils/ros/update_param.hpp>
 #include <autoware/universe_utils/system/stop_watch.hpp>
 #include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
@@ -159,7 +160,7 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan(
   const auto end_idx = obstacle_velocity_limiter::calculateEndIndex(
     original_traj_points, start_idx, preprocessing_params_.max_length,
     preprocessing_params_.max_duration);
-  auto downsampled_traj_points = obstacle_velocity_limiter::downsampleTrajectory(
+  auto downsampled_traj_points = downsample_trajectory(
     original_traj_points, start_idx, end_idx, preprocessing_params_.downsample_factor);
   obstacle_velocity_limiter::ObstacleMasks obstacle_masks;
   const auto preprocessing_us = stopwatch.toc("preprocessing");
diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp
index f4b1304d67e5e..73e05bed88c37 100644
--- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp
+++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp
@@ -55,17 +55,6 @@ size_t calculateEndIndex(
   return idx;
 }
 
-TrajectoryPoints downsampleTrajectory(
-  const TrajectoryPoints & trajectory, const size_t start_idx, const size_t end_idx,
-  const int factor)
-{
-  if (factor < 1) return trajectory;
-  TrajectoryPoints downsampled_traj;
-  downsampled_traj.reserve((end_idx - start_idx) / factor);
-  for (size_t i = start_idx; i <= end_idx; i += factor) downsampled_traj.push_back(trajectory[i]);
-  return downsampled_traj;
-}
-
 void calculateSteeringAngles(TrajectoryPoints & trajectory, const double wheel_base)
 {
   auto prev_point = trajectory.front();
@@ -82,16 +71,4 @@ void calculateSteeringAngles(TrajectoryPoints & trajectory, const double wheel_b
       std::atan2(wheel_base * d_heading, point.longitudinal_velocity_mps * dt);
   }
 }
-
-TrajectoryPoints copyDownsampledVelocity(
-  const TrajectoryPoints & downsampled_traj, TrajectoryPoints trajectory, const size_t start_idx,
-  const int factor)
-{
-  const auto size = std::min(downsampled_traj.size(), trajectory.size());
-  for (size_t i = 0; i < size; ++i) {
-    trajectory[start_idx + i * factor].longitudinal_velocity_mps =
-      downsampled_traj[i].longitudinal_velocity_mps;
-  }
-  return trajectory;
-}
 }  // namespace autoware::motion_velocity_planner::obstacle_velocity_limiter
diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.hpp
index cb9fe40e64907..e098ee6abe822 100644
--- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.hpp
+++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.hpp
@@ -37,31 +37,11 @@ size_t calculateEndIndex(
   const TrajectoryPoints & trajectory, const size_t start_idx, const double max_length,
   const double max_duration);
 
-/// @brief downsample a trajectory, reducing its number of points by the given factor
-/// @param[in] trajectory input trajectory
-/// @param[in] start_idx starting index of the input trajectory
-/// @param[in] end_idx ending index of the input trajectory
-/// @param[in] factor factor used for downsampling
-/// @return downsampled trajectory
-TrajectoryPoints downsampleTrajectory(
-  const TrajectoryPoints & trajectory, const size_t start_idx, const size_t end_idx,
-  const int factor);
-
 /// @brief recalculate the steering angle of the trajectory
 /// @details uses the change in headings for calculation
 /// @param[inout] trajectory input trajectory
 /// @param[in] wheel_base wheel base of the vehicle
 void calculateSteeringAngles(TrajectoryPoints & trajectory, const double wheel_base);
-
-/// @brief copy the velocity profile of a downsampled trajectory to the original trajectory
-/// @param[in] downsampled_trajectory downsampled trajectory
-/// @param[in] trajectory input trajectory
-/// @param[in] start_idx starting index of the downsampled trajectory relative to the input
-/// @param[in] factor downsampling factor
-/// @return input trajectory with the velocity profile of the downsampled trajectory
-TrajectoryPoints copyDownsampledVelocity(
-  const TrajectoryPoints & downsampled_traj, TrajectoryPoints trajectory, const size_t start_idx,
-  const int factor);
 }  // namespace autoware::motion_velocity_planner::obstacle_velocity_limiter
 
 #endif  // TRAJECTORY_PREPROCESSING_HPP_
diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp
index 8397d0c116090..8facb77932aa5 100644
--- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp
+++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp
@@ -25,6 +25,7 @@
 
 #include <autoware/motion_utils/trajectory/interpolation.hpp>
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
+#include <autoware/motion_velocity_planner_common/trajectory_preprocessing.hpp>
 #include <autoware/universe_utils/ros/parameter.hpp>
 #include <autoware/universe_utils/ros/update_param.hpp>
 #include <autoware/universe_utils/system/stop_watch.hpp>
@@ -160,9 +161,11 @@ VelocityPlanningResult OutOfLaneModule::plan(
   stopwatch.tic();
   out_of_lane::EgoData ego_data;
   ego_data.pose = planner_data->current_odometry.pose.pose;
-  ego_data.trajectory_points = ego_trajectory_points;
-  ego_data.first_trajectory_idx =
+  const auto start_idx =
     autoware::motion_utils::findNearestSegmentIndex(ego_trajectory_points, ego_data.pose.position);
+  ego_data.trajectory_points =
+    downsample_trajectory(ego_trajectory_points, start_idx, ego_trajectory_points.size(), 10);
+  ego_data.first_trajectory_idx = 0;
   ego_data.velocity = planner_data->current_odometry.twist.twist.linear.x;
   ego_data.max_decel = planner_data->velocity_smoother_->getMinDecel();
   stopwatch.tic("calculate_trajectory_footprints");
diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/trajectory_preprocessing.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/trajectory_preprocessing.hpp
new file mode 100644
index 0000000000000..95728c4738c5b
--- /dev/null
+++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/trajectory_preprocessing.hpp
@@ -0,0 +1,46 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__TRAJECTORY_PREPROCESSING_HPP_
+#define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__TRAJECTORY_PREPROCESSING_HPP_
+
+#include <autoware_planning_msgs/msg/trajectory_point.hpp>
+
+#include <vector>
+
+namespace autoware::motion_velocity_planner
+{
+/// @brief downsample a trajectory, reducing its number of points by the given factor
+/// @param[in] trajectory input trajectory
+/// @param[in] start_idx starting index of the input trajectory
+/// @param[in] end_idx ending index of the input trajectory
+/// @param[in] factor factor used for downsampling
+/// @return downsampled trajectory
+std::vector<autoware_planning_msgs::msg::TrajectoryPoint> downsample_trajectory(
+  const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
+  const size_t start_idx, const size_t end_idx, const int factor)
+{
+  if (factor < 1) {
+    return trajectory;
+  }
+  std::vector<autoware_planning_msgs::msg::TrajectoryPoint> downsampled_traj;
+  downsampled_traj.reserve((end_idx - start_idx) / factor + 1);
+  for (size_t i = start_idx; i <= end_idx; i += factor) {
+    downsampled_traj.push_back(trajectory[i]);
+  }
+  return downsampled_traj;
+}
+}  // namespace autoware::motion_velocity_planner
+
+#endif  // AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__TRAJECTORY_PREPROCESSING_HPP_
diff --git a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp
index e9058f234e98f..1ab368b6f2a8d 100644
--- a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp
+++ b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp
@@ -639,14 +639,11 @@ double AdaptiveCruiseController::calcTargetVelocity_D(
     return 0.0;
   }
 
-  double add_vel_d = 0;
-
-  add_vel_d = diff_vel;
+  double add_vel_d = diff_vel;
   if (add_vel_d >= 0) {
-    diff_vel *= param_.d_coeff_pos;
-  }
-  if (add_vel_d < 0) {
-    diff_vel *= param_.d_coeff_neg;
+    add_vel_d *= param_.d_coeff_pos;
+  } else {
+    add_vel_d *= param_.d_coeff_neg;
   }
   add_vel_d = boost::algorithm::clamp(add_vel_d, -param_.d_max_vel_norm, param_.d_max_vel_norm);
 
diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt
index 331d5e819df77..376553aa62917 100644
--- a/simulator/simple_planning_simulator/CMakeLists.txt
+++ b/simulator/simple_planning_simulator/CMakeLists.txt
@@ -19,6 +19,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp
   src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp
   src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp
+  src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp
   src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp
   src/simple_planning_simulator/utils/csv_loader.cpp
 )
diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md
index 4902832f836a9..c3d02da64b882 100644
--- a/simulator/simple_planning_simulator/README.md
+++ b/simulator/simple_planning_simulator/README.md
@@ -62,6 +62,7 @@ The purpose of this simulator is for the integration test of planning and contro
 - `DELAY_STEER_VEL`
 - `DELAY_STEER_ACC`
 - `DELAY_STEER_ACC_GEARED`
+- `DELAY_STEER_ACC_GEARED_WO_FALL_GUARD`
 - `DELAY_STEER_MAP_ACC_GEARED`: applies 1D dynamics and time delay to the steering and acceleration commands. The simulated acceleration is determined by a value converted through the provided acceleration map. This model is valuable for an accurate simulation with acceleration deviations in a real vehicle.
 - `LEARNED_STEER_VEL`: launches learned python models. More about this [here](../learning_based_vehicle_model).
 
@@ -69,26 +70,26 @@ The `IDEAL` model moves ideally as commanded, while the `DELAY` model moves base
 
 The table below shows which models correspond to what parameters. The model names are written in abbreviated form (e.g. IDEAL_STEER_VEL = I_ST_V).
 
-| Name                       | Type   | Description                                                                                                 | I_ST_V | I_ST_A | I_ST_A_G | D_ST_V | D_ST_A | D_ST_A_G | D_ST_M_ACC_G | L_S_V | Default value | unit    |
-| :------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :----- | :----- | :------- | :----- | :----- | :------- | :----------- | :---- | :------------ | :------ |
-| acc_time_delay             | double | dead time for the acceleration input                                                                        | x      | x      | x        | x      | o      | o        | o            | x     | 0.1           | [s]     |
-| steer_time_delay           | double | dead time for the steering input                                                                            | x      | x      | x        | o      | o      | o        | o            | x     | 0.24          | [s]     |
-| vel_time_delay             | double | dead time for the velocity input                                                                            | x      | x      | x        | o      | x      | x        | x            | x     | 0.25          | [s]     |
-| acc_time_constant          | double | time constant of the 1st-order acceleration dynamics                                                        | x      | x      | x        | x      | o      | o        | o            | x     | 0.1           | [s]     |
-| steer_time_constant        | double | time constant of the 1st-order steering dynamics                                                            | x      | x      | x        | o      | o      | o        | o            | x     | 0.27          | [s]     |
-| steer_dead_band            | double | dead band for steering angle                                                                                | x      | x      | x        | o      | o      | o        | x            | x     | 0.0           | [rad]   |
-| vel_time_constant          | double | time constant of the 1st-order velocity dynamics                                                            | x      | x      | x        | o      | x      | x        | x            | x     | 0.5           | [s]     |
-| vel_lim                    | double | limit of velocity                                                                                           | x      | x      | x        | o      | o      | o        | o            | x     | 50.0          | [m/s]   |
-| vel_rate_lim               | double | limit of acceleration                                                                                       | x      | x      | x        | o      | o      | o        | o            | x     | 7.0           | [m/ss]  |
-| steer_lim                  | double | limit of steering angle                                                                                     | x      | x      | x        | o      | o      | o        | o            | x     | 1.0           | [rad]   |
-| steer_rate_lim             | double | limit of steering angle change rate                                                                         | x      | x      | x        | o      | o      | o        | o            | x     | 5.0           | [rad/s] |
-| steer_bias                 | double | bias for steering angle                                                                                     | x      | x      | x        | o      | o      | o        | o            | x     | 0.0           | [rad]   |
-| debug_acc_scaling_factor   | double | scaling factor for accel command                                                                            | x      | x      | x        | x      | o      | o        | x            | x     | 1.0           | [-]     |
-| debug_steer_scaling_factor | double | scaling factor for steer command                                                                            | x      | x      | x        | x      | o      | o        | x            | x     | 1.0           | [-]     |
-| acceleration_map_path      | string | path to csv file for acceleration map which converts velocity and ideal acceleration to actual acceleration | x      | x      | x        | x      | x      | x        | o            | x     | -             | [-]     |
-| model_module_paths         | string | path to a python module where the model is implemented                                                      | x      | x      | x        | x      | x      | x        | x            | o     | -             | [-]     |
-| model_param_paths          | string | path to the file where model parameters are stored (can be empty string if no parameter file is required)   | x      | x      | x        | x      | x      | x        | x            | o     | -             | [-]     |
-| model_class_names          | string | name of the class that implements the model                                                                 | x      | x      | x        | x      | x      | x        | x            | o     | -             | [-]     |
+| Name                       | Type   | Description                                                                                                 | I_ST_V | I_ST_A | I_ST_A_G | D_ST_V | D_ST_A | D_ST_A_G | D_ST_A_G_WO_FG | D_ST_M_ACC_G | L_S_V | Default value | unit    |
+| :------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :----- | :----- | :------- | :----- | :----- | :------- | :------------- | :----------- | :---- | :------------ | :------ |
+| acc_time_delay             | double | dead time for the acceleration input                                                                        | x      | x      | x        | x      | o      | o        | o              | o            | x     | 0.1           | [s]     |
+| steer_time_delay           | double | dead time for the steering input                                                                            | x      | x      | x        | o      | o      | o        | o              | o            | x     | 0.24          | [s]     |
+| vel_time_delay             | double | dead time for the velocity input                                                                            | x      | x      | x        | o      | x      | x        | x              | x            | x     | 0.25          | [s]     |
+| acc_time_constant          | double | time constant of the 1st-order acceleration dynamics                                                        | x      | x      | x        | x      | o      | o        | o              | o            | x     | 0.1           | [s]     |
+| steer_time_constant        | double | time constant of the 1st-order steering dynamics                                                            | x      | x      | x        | o      | o      | o        | o              | o            | x     | 0.27          | [s]     |
+| steer_dead_band            | double | dead band for steering angle                                                                                | x      | x      | x        | o      | o      | o        | o              | x            | x     | 0.0           | [rad]   |
+| vel_time_constant          | double | time constant of the 1st-order velocity dynamics                                                            | x      | x      | x        | o      | x      | x        | x              | x            | x     | 0.5           | [s]     |
+| vel_lim                    | double | limit of velocity                                                                                           | x      | x      | x        | o      | o      | o        | o              | o            | x     | 50.0          | [m/s]   |
+| vel_rate_lim               | double | limit of acceleration                                                                                       | x      | x      | x        | o      | o      | o        | o              | o            | x     | 7.0           | [m/ss]  |
+| steer_lim                  | double | limit of steering angle                                                                                     | x      | x      | x        | o      | o      | o        | o              | o            | x     | 1.0           | [rad]   |
+| steer_rate_lim             | double | limit of steering angle change rate                                                                         | x      | x      | x        | o      | o      | o        | o              | o            | x     | 5.0           | [rad/s] |
+| steer_bias                 | double | bias for steering angle                                                                                     | x      | x      | x        | o      | o      | o        | o              | o            | x     | 0.0           | [rad]   |
+| debug_acc_scaling_factor   | double | scaling factor for accel command                                                                            | x      | x      | x        | x      | o      | o        | o              | x            | x     | 1.0           | [-]     |
+| debug_steer_scaling_factor | double | scaling factor for steer command                                                                            | x      | x      | x        | x      | o      | o        | o              | x            | x     | 1.0           | [-]     |
+| acceleration_map_path      | string | path to csv file for acceleration map which converts velocity and ideal acceleration to actual acceleration | x      | x      | x        | x      | x      | x        | x              | o            | x     | -             | [-]     |
+| model_module_paths         | string | path to a python module where the model is implemented                                                      | x      | x      | x        | x      | x      | x        | x              | x            | o     | -             | [-]     |
+| model_param_paths          | string | path to the file where model parameters are stored (can be empty string if no parameter file is required)   | x      | x      | x        | x      | x      | x        | x              | x            | o     | -             | [-]     |
+| model_class_names          | string | name of the class that implements the model                                                                 | x      | x      | x        | x      | x      | x        | x              | x            | o     | -             | [-]     |
 
 _Note:_ Parameters `model_module_paths`, `model_param_paths`, and `model_class_names` need to have the same length.
 
diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp
index 18a3a3bae501a..86e4346770f0c 100644
--- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp
+++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp
@@ -205,7 +205,8 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
     IDEAL_STEER_VEL = 4,
     DELAY_STEER_VEL = 5,
     DELAY_STEER_MAP_ACC_GEARED = 6,
-    LEARNED_STEER_VEL = 7
+    LEARNED_STEER_VEL = 7,
+    DELAY_STEER_ACC_GEARED_WO_FALL_GUARD = 8
   } vehicle_model_type_;  //!< @brief vehicle model type to decide the model dynamics
   std::shared_ptr<SimModelInterface> vehicle_model_ptr_;  //!< @brief vehicle model pointer
 
diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp
index ced7349f28914..2d38ef31498b6 100644
--- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp
+++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp
@@ -17,6 +17,7 @@
 
 #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp"
 #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp"
+#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp"
 #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp"
 #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp"
 #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp"
diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp
new file mode 100644
index 0000000000000..d7a6a8284d704
--- /dev/null
+++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp
@@ -0,0 +1,149 @@
+// Copyright 2024 The Autoware Foundation.
+//
+// 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 SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_WO_FALL_GUARD_HPP_  // NOLINT
+#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_WO_FALL_GUARD_HPP_  // NOLINT
+
+#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp"
+
+#include <Eigen/Core>
+#include <Eigen/LU>
+
+#include <deque>
+#include <iostream>
+#include <queue>
+
+class SimModelDelaySteerAccGearedWoFallGuard : public SimModelInterface
+{
+public:
+  /**
+   * @param [in] vx_lim velocity limit [m/s]
+   * @param [in] steer_lim steering limit [rad]
+   * @param [in] vx_rate_lim acceleration limit [m/ss]
+   * @param [in] steer_rate_lim steering angular velocity limit [rad/ss]
+   * @param [in] wheelbase vehicle wheelbase length [m]
+   * @param [in] dt delta time information to set input buffer for delay
+   * @param [in] acc_delay time delay for accel command [s]
+   * @param [in] acc_time_constant time constant for 1D model of accel dynamics
+   * @param [in] steer_delay time delay for steering command [s]
+   * @param [in] steer_time_constant time constant for 1D model of steering dynamics
+   * @param [in] steer_dead_band dead band for steering angle [rad]
+   * @param [in] steer_bias steering bias [rad]
+   * @param [in] debug_acc_scaling_factor scaling factor for accel command
+   * @param [in] debug_steer_scaling_factor scaling factor for steering command
+   */
+  SimModelDelaySteerAccGearedWoFallGuard(
+    double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase,
+    double dt, double acc_delay, double acc_time_constant, double steer_delay,
+    double steer_time_constant, double steer_dead_band, double steer_bias,
+    double debug_acc_scaling_factor, double debug_steer_scaling_factor);
+
+  /**
+   * @brief default destructor
+   */
+  ~SimModelDelaySteerAccGearedWoFallGuard() = default;
+
+private:
+  const double MIN_TIME_CONSTANT;  //!< @brief minimum time constant
+
+  enum IDX {
+    X = 0,
+    Y,
+    YAW,
+    VX,
+    STEER,
+    PEDAL_ACCX,
+  };
+  enum IDX_U { PEDAL_ACCX_DES = 0, GEAR, SLOPE_ACCX, STEER_DES };
+
+  const double vx_lim_;          //!< @brief velocity limit [m/s]
+  const double vx_rate_lim_;     //!< @brief acceleration limit [m/ss]
+  const double steer_lim_;       //!< @brief steering limit [rad]
+  const double steer_rate_lim_;  //!< @brief steering angular velocity limit [rad/s]
+  const double wheelbase_;       //!< @brief vehicle wheelbase length [m]
+
+  std::deque<double> acc_input_queue_;       //!< @brief buffer for accel command
+  std::deque<double> steer_input_queue_;     //!< @brief buffer for steering command
+  const double acc_delay_;                   //!< @brief time delay for accel command [s]
+  const double acc_time_constant_;           //!< @brief time constant for accel dynamics
+  const double steer_delay_;                 //!< @brief time delay for steering command [s]
+  const double steer_time_constant_;         //!< @brief time constant for steering dynamics
+  const double steer_dead_band_;             //!< @brief dead band for steering angle [rad]
+  const double steer_bias_;                  //!< @brief steering angle bias [rad]
+  const double debug_acc_scaling_factor_;    //!< @brief scaling factor for accel command
+  const double debug_steer_scaling_factor_;  //!< @brief scaling factor for steering command
+
+  /**
+   * @brief set queue buffer for input command
+   * @param [in] dt delta time
+   */
+  void initializeInputQueue(const double & dt);
+
+  /**
+   * @brief get vehicle position x
+   */
+  double getX() override;
+
+  /**
+   * @brief get vehicle position y
+   */
+  double getY() override;
+
+  /**
+   * @brief get vehicle angle yaw
+   */
+  double getYaw() override;
+
+  /**
+   * @brief get vehicle velocity vx
+   */
+  double getVx() override;
+
+  /**
+   * @brief get vehicle lateral velocity
+   */
+  double getVy() override;
+
+  /**
+   * @brief get vehicle longitudinal acceleration
+   */
+  double getAx() override;
+
+  /**
+   * @brief get vehicle angular-velocity wz
+   */
+  double getWz() override;
+
+  /**
+   * @brief get vehicle steering angle
+   */
+  double getSteer() override;
+
+  /**
+   * @brief update vehicle states
+   * @param [in] dt delta time [s]
+   */
+  void update(const double & dt) override;
+
+  /**
+   * @brief calculate derivative of states with time delay steering model
+   * @param [in] state current model state
+   * @param [in] input input vector to model
+   */
+  Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override;
+};
+
+// clang-format off
+#endif  // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_WO_FALL_GUARD_HPP_  // NOLINT
+// clang-format on
diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp
index 71c89fcf4df42..7d09aabdc9abd 100644
--- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp
+++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp
@@ -269,6 +269,12 @@ void SimplePlanningSimulator::initialize_vehicle_model()
       vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0,
       acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, steer_dead_band,
       steer_bias, debug_acc_scaling_factor, debug_steer_scaling_factor);
+  } else if (vehicle_model_type_str == "DELAY_STEER_ACC_GEARED_WO_FALL_GUARD") {
+    vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC_GEARED_WO_FALL_GUARD;
+    vehicle_model_ptr_ = std::make_shared<SimModelDelaySteerAccGearedWoFallGuard>(
+      vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0,
+      acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, steer_dead_band,
+      steer_bias, debug_acc_scaling_factor, debug_steer_scaling_factor);
   } else if (vehicle_model_type_str == "DELAY_STEER_MAP_ACC_GEARED") {
     vehicle_model_type_ = VehicleModelType::DELAY_STEER_MAP_ACC_GEARED;
     const std::string acceleration_map_path =
@@ -471,7 +477,7 @@ void SimplePlanningSimulator::set_input(const Control & cmd, const double acc_by
 {
   const auto steer = cmd.lateral.steering_tire_angle;
   const auto vel = cmd.longitudinal.velocity;
-  const auto accel = cmd.longitudinal.acceleration;
+  const auto acc_by_cmd = cmd.longitudinal.acceleration;
 
   using autoware_vehicle_msgs::msg::GearCommand;
   Eigen::VectorXd input(vehicle_model_ptr_->getDimU());
@@ -479,12 +485,15 @@ void SimplePlanningSimulator::set_input(const Control & cmd, const double acc_by
 
   // TODO(Watanabe): The definition of the sign of acceleration in REVERSE mode is different
   // between .auto and proposal.iv, and will be discussed later.
-  float acc = accel + acc_by_slope;
-  if (gear == GearCommand::NONE) {
-    acc = 0.0;
-  } else if (gear == GearCommand::REVERSE || gear == GearCommand::REVERSE_2) {
-    acc = -accel - acc_by_slope;
-  }
+  const float combined_acc = [&] {
+    if (gear == GearCommand::NONE) {
+      return 0.0;
+    } else if (gear == GearCommand::REVERSE || gear == GearCommand::REVERSE_2) {
+      return -acc_by_cmd + acc_by_slope;
+    } else {
+      return acc_by_cmd + acc_by_slope;
+    }
+  }();
 
   if (
     vehicle_model_type_ == VehicleModelType::IDEAL_STEER_VEL ||
@@ -494,12 +503,15 @@ void SimplePlanningSimulator::set_input(const Control & cmd, const double acc_by
   } else if (  // NOLINT
     vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC ||
     vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC) {
-    input << acc, steer;
+    input << combined_acc, steer;
   } else if (  // NOLINT
     vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED ||
     vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED ||
     vehicle_model_type_ == VehicleModelType::DELAY_STEER_MAP_ACC_GEARED) {
-    input << acc, steer;
+    input << combined_acc, steer;
+  } else if (  // NOLINT
+    vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED_WO_FALL_GUARD) {
+    input << acc_by_cmd, gear, acc_by_slope, steer;
   }
   vehicle_model_ptr_->setInput(input);
 }
@@ -599,6 +611,7 @@ void SimplePlanningSimulator::set_initial_state(const Pose & pose, const Twist &
   } else if (  // NOLINT
     vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC ||
     vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED ||
+    vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED_WO_FALL_GUARD ||
     vehicle_model_type_ == VehicleModelType::DELAY_STEER_MAP_ACC_GEARED) {
     state << x, y, yaw, vx, steer, accx;
   }
diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp
new file mode 100644
index 0000000000000..c699d704724f5
--- /dev/null
+++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp
@@ -0,0 +1,183 @@
+// Copyright 2024 The Autoware Foundation.
+//
+// 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 "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp"
+
+#include "autoware_vehicle_msgs/msg/gear_command.hpp"
+
+#include <algorithm>
+
+SimModelDelaySteerAccGearedWoFallGuard::SimModelDelaySteerAccGearedWoFallGuard(
+  double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase,
+  double dt, double acc_delay, double acc_time_constant, double steer_delay,
+  double steer_time_constant, double steer_dead_band, double steer_bias,
+  double debug_acc_scaling_factor, double debug_steer_scaling_factor)
+: SimModelInterface(6 /* dim x */, 4 /* dim u */),
+  MIN_TIME_CONSTANT(0.03),
+  vx_lim_(vx_lim),
+  vx_rate_lim_(vx_rate_lim),
+  steer_lim_(steer_lim),
+  steer_rate_lim_(steer_rate_lim),
+  wheelbase_(wheelbase),
+  acc_delay_(acc_delay),
+  acc_time_constant_(std::max(acc_time_constant, MIN_TIME_CONSTANT)),
+  steer_delay_(steer_delay),
+  steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)),
+  steer_dead_band_(steer_dead_band),
+  steer_bias_(steer_bias),
+  debug_acc_scaling_factor_(std::max(debug_acc_scaling_factor, 0.0)),
+  debug_steer_scaling_factor_(std::max(debug_steer_scaling_factor, 0.0))
+{
+  initializeInputQueue(dt);
+}
+
+double SimModelDelaySteerAccGearedWoFallGuard::getX()
+{
+  return state_(IDX::X);
+}
+double SimModelDelaySteerAccGearedWoFallGuard::getY()
+{
+  return state_(IDX::Y);
+}
+double SimModelDelaySteerAccGearedWoFallGuard::getYaw()
+{
+  return state_(IDX::YAW);
+}
+double SimModelDelaySteerAccGearedWoFallGuard::getVx()
+{
+  return state_(IDX::VX);
+}
+double SimModelDelaySteerAccGearedWoFallGuard::getVy()
+{
+  return 0.0;
+}
+double SimModelDelaySteerAccGearedWoFallGuard::getAx()
+{
+  return state_(IDX::PEDAL_ACCX);
+}
+double SimModelDelaySteerAccGearedWoFallGuard::getWz()
+{
+  return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_;
+  ;
+}
+double SimModelDelaySteerAccGearedWoFallGuard::getSteer()
+{
+  // return measured values with bias added to actual values
+  return state_(IDX::STEER) + steer_bias_;
+}
+void SimModelDelaySteerAccGearedWoFallGuard::update(const double & dt)
+{
+  Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_);
+
+  acc_input_queue_.push_back(input_(IDX_U::PEDAL_ACCX_DES));
+  delayed_input(IDX_U::PEDAL_ACCX_DES) = acc_input_queue_.front();
+  acc_input_queue_.pop_front();
+  steer_input_queue_.push_back(input_(IDX_U::STEER_DES));
+  delayed_input(IDX_U::STEER_DES) = steer_input_queue_.front();
+  steer_input_queue_.pop_front();
+  delayed_input(IDX_U::GEAR) = input_(IDX_U::GEAR);
+  delayed_input(IDX_U::SLOPE_ACCX) = input_(IDX_U::SLOPE_ACCX);
+
+  const auto prev_state = state_;
+  updateEuler(dt, delayed_input);
+  // we cannot use updateRungeKutta() because the differentiability or the continuity condition is
+  // not satisfied, but we can use Runge-Kutta method with code reconstruction.
+
+  // take velocity limit explicitly
+  state_(IDX::VX) = std::max(-vx_lim_, std::min(state_(IDX::VX), vx_lim_));
+
+  if (
+    prev_state(IDX::VX) * state_(IDX::VX) <= 0.0 &&
+    -state_(IDX::PEDAL_ACCX) >= std::abs(delayed_input(IDX_U::SLOPE_ACCX))) {
+    // stop condition is satisfied
+    state_(IDX::VX) = 0.0;
+  }
+}
+
+void SimModelDelaySteerAccGearedWoFallGuard::initializeInputQueue(const double & dt)
+{
+  size_t acc_input_queue_size = static_cast<size_t>(round(acc_delay_ / dt));
+  acc_input_queue_.resize(acc_input_queue_size);
+  std::fill(acc_input_queue_.begin(), acc_input_queue_.end(), 0.0);
+
+  size_t steer_input_queue_size = static_cast<size_t>(round(steer_delay_ / dt));
+  steer_input_queue_.resize(steer_input_queue_size);
+  std::fill(steer_input_queue_.begin(), steer_input_queue_.end(), 0.0);
+}
+
+Eigen::VectorXd SimModelDelaySteerAccGearedWoFallGuard::calcModel(
+  const Eigen::VectorXd & state, const Eigen::VectorXd & input)
+{
+  auto sat = [](double val, double u, double l) { return std::max(std::min(val, u), l); };
+
+  const double vel = sat(state(IDX::VX), vx_lim_, -vx_lim_);
+  const double pedal_acc = sat(state(IDX::PEDAL_ACCX), vx_rate_lim_, -vx_rate_lim_);
+  const double yaw = state(IDX::YAW);
+  const double steer = state(IDX::STEER);
+  const double pedal_acc_des =
+    sat(input(IDX_U::PEDAL_ACCX_DES), vx_rate_lim_, -vx_rate_lim_) * debug_acc_scaling_factor_;
+  const double steer_des =
+    sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_) * debug_steer_scaling_factor_;
+  // NOTE: `steer_des` is calculated by control from measured values. getSteer() also gets the
+  // measured value. The steer_rate used in the motion calculation is obtained from these
+  // differences.
+  const double steer_diff = getSteer() - steer_des;
+  const double steer_diff_with_dead_band = std::invoke([&]() {
+    if (steer_diff > steer_dead_band_) {
+      return steer_diff - steer_dead_band_;
+    } else if (steer_diff < -steer_dead_band_) {
+      return steer_diff + steer_dead_band_;
+    } else {
+      return 0.0;
+    }
+  });
+  const double steer_rate =
+    sat(-steer_diff_with_dead_band / steer_time_constant_, steer_rate_lim_, -steer_rate_lim_);
+
+  Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_);
+
+  d_state(IDX::X) = vel * cos(yaw);
+  d_state(IDX::Y) = vel * sin(yaw);
+  d_state(IDX::YAW) = vel * std::tan(steer) / wheelbase_;
+  d_state(IDX::VX) = [&] {
+    if (pedal_acc >= 0.0) {
+      using autoware_vehicle_msgs::msg::GearCommand;
+      const auto gear = input(IDX_U::GEAR);
+      if (gear == GearCommand::NONE || gear == GearCommand::PARK) {
+        return 0.0;
+      } else if (gear == GearCommand::NEUTRAL) {
+        return input(IDX_U::SLOPE_ACCX);
+      } else if (gear == GearCommand::REVERSE || gear == GearCommand::REVERSE_2) {
+        return -pedal_acc + input(IDX_U::SLOPE_ACCX);
+      } else {
+        return pedal_acc + input(IDX_U::SLOPE_ACCX);
+      }
+    } else {
+      constexpr double brake_dead_band = 1e-3;
+      if (vel > brake_dead_band) {
+        return pedal_acc + input(IDX_U::SLOPE_ACCX);
+      } else if (vel < -brake_dead_band) {
+        return -pedal_acc + input(IDX_U::SLOPE_ACCX);
+      } else if (-pedal_acc >= std::abs(input(IDX_U::SLOPE_ACCX))) {
+        return 0.0;
+      } else {
+        return input(IDX_U::SLOPE_ACCX);
+      }
+    }
+  }();
+  d_state(IDX::STEER) = steer_rate;
+  d_state(IDX::PEDAL_ACCX) = -(pedal_acc - pedal_acc_des) / acc_time_constant_;
+
+  return d_state;
+}
diff --git a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp
index dc49bf17a11fc..c61237f83e5bd 100644
--- a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp
+++ b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp
@@ -271,6 +271,7 @@ TEST_P(TestSimplePlanningSimulator, TestIdealSteerVel)
 const std::string VEHICLE_MODEL_LIST[] = {   // NOLINT
   "IDEAL_STEER_VEL", "IDEAL_STEER_ACC", "IDEAL_STEER_ACC_GEARED",
   "DELAY_STEER_VEL", "DELAY_STEER_ACC", "DELAY_STEER_ACC_GEARED",
+  "DELAY_STEER_ACC_GEARED_WO_FALL_GUARD"
 };
 // clang-format on
 
diff --git a/system/default_ad_api/src/interface.cpp b/system/default_ad_api/src/interface.cpp
index d53d12afcc499..e0009d5bdc382 100644
--- a/system/default_ad_api/src/interface.cpp
+++ b/system/default_ad_api/src/interface.cpp
@@ -21,7 +21,7 @@ InterfaceNode::InterfaceNode(const rclcpp::NodeOptions & options) : Node("interf
 {
   const auto on_interface_version = [](auto, auto res) {
     res->major = 1;
-    res->minor = 3;
+    res->minor = 4;
     res->patch = 0;
   };
 
diff --git a/system/default_ad_api/test/node/interface_version.py b/system/default_ad_api/test/node/interface_version.py
index 63a79e8722be3..10d00d1dbad30 100644
--- a/system/default_ad_api/test/node/interface_version.py
+++ b/system/default_ad_api/test/node/interface_version.py
@@ -30,7 +30,7 @@
 
 if response.major != 1:
     exit(1)
-if response.minor != 3:
+if response.minor != 4:
     exit(1)
 if response.patch != 0:
     exit(1)