diff --git a/.cppcheck_suppressions b/.cppcheck_suppressions
index f52eeb4e3ab2c..79cd117930541 100644
--- a/.cppcheck_suppressions
+++ b/.cppcheck_suppressions
@@ -9,7 +9,6 @@ missingInclude
 missingIncludeSystem
 noConstructor
 passedByValue
-redundantInitialization
 // cspell: ignore uninit
 uninitMemberVar
 unknownMacro
diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS
index 2c1698dc81bce..e338a395482e9 100644
--- a/.github/CODEOWNERS
+++ b/.github/CODEOWNERS
@@ -5,7 +5,8 @@ common/autoware_grid_map_utils/** maxime.clement@tier4.jp
 common/autoware_motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp
 common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/** ahmed.ebrahim@leodrive.ai
 common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/** khalil@leodrive.ai
-common/autoware_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp yoshi.ri@tier4.jp
+common/autoware_path_distance_calculator/** isamu.takagi@tier4.jp
+common/autoware_perception_rviz_plugin/** opensource@apex.ai shunsuke.miura@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp yoshi.ri@tier4.jp
 common/autoware_point_types/** david.wong@tier4.jp max.schmeller@tier4.jp
 common/autoware_test_utils/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp zulfaqar.azmi@tier4.jp
 common/autoware_testing/** adam.dabrowski@robotec.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp
@@ -22,9 +23,8 @@ common/glog_component/** takamasa.horibe@tier4.jp
 common/goal_distance_calculator/** taiki.tanaka@tier4.jp
 common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp
 common/kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp
-common/object_recognition_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yoshi.ri@tier4.jp
+common/object_recognition_utils/** shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yoshi.ri@tier4.jp
 common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp
-common/path_distance_calculator/** isamu.takagi@tier4.jp
 common/perception_utils/** shunsuke.miura@tier4.jp yoshi.ri@tier4.jp
 common/polar_grid/** yukihiro.saito@tier4.jp
 common/qp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp
@@ -34,7 +34,7 @@ common/tier4_adapi_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp kosu
 common/tier4_api_utils/** isamu.takagi@tier4.jp
 common/tier4_camera_view_rviz_plugin/** makoto.ybauta@tier4.jp uken.ryu@tier4.jp
 common/tier4_datetime_rviz_plugin/** isamu.takagi@tier4.jp
-common/tier4_localization_rviz_plugin/** isamu.takagi@tier4.jp takamasa.horibe@tier4.jp
+common/tier4_localization_rviz_plugin/** isamu.takagi@tier4.jp takamasa.horibe@tier4.jp yamato.ando@tier4.jp
 common/tier4_perception_rviz_plugin/** yukihiro.saito@tier4.jp
 common/tier4_planning_rviz_plugin/** takayuki.murooka@tier4.jp yukihiro.saito@tier4.jp
 common/tier4_state_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp khalil@leodrive.ai
@@ -83,17 +83,17 @@ localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/*
 localization/autoware_landmark_based_localizer/autoware_landmark_manager/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 localization/autoware_pose_covariance_modifier/** melike@leodrive.ai
 localization/autoware_stop_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
+localization/autoware_twist2accel/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 localization/ekf_localizer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp
 localization/geo_pose_projector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 localization/gyro_odometer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 localization/localization_error_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 localization/localization_util/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 localization/ndt_scan_matcher/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
-localization/pose2twist/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
+localization/autoware_pose2twist/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 localization/pose_estimator_arbiter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 localization/pose_initializer/** anh.nguyen.2@tier4.jp isamu.takagi@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 localization/pose_instability_detector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
-localization/twist2accel/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 localization/yabloc/yabloc_common/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 localization/yabloc/yabloc_image_processing/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 localization/yabloc/yabloc_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
@@ -104,21 +104,10 @@ map/map_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakam
 map/map_projection_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 map/map_tf_generator/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 map/util/lanelet2_map_preprocessor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
-perception/autoware_crosswalk_traffic_light_estimator/** satoshi.ota@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp
-perception/autoware_lidar_transfusion/** amadeusz.szymko.2@tier4.jp kenzo.lobos@tier4.jp satoshi.tanaka@tier4.jp
-perception/autoware_map_based_prediction/** kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yoshi.ri@tier4.jp
-perception/autoware_multi_object_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
-perception/autoware_object_merger/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
-perception/autoware_object_range_splitter/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
-perception/autoware_object_velocity_splitter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp
-perception/autoware_radar_crossing_objects_noise_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp
-perception/autoware_radar_fusion_to_detected_object/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp
-perception/autoware_radar_object_clustering/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp
-perception/autoware_radar_object_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
-perception/autoware_tracking_object_merger/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
 perception/autoware_bytetrack/** manato.hirabayashi@tier4.jp yoshi.ri@tier4.jp
 perception/autoware_cluster_merger/** dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp
 perception/autoware_compare_map_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp yukihiro.saito@tier4.jp
+perception/autoware_crosswalk_traffic_light_estimator/** satoshi.ota@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp
 perception/autoware_detected_object_feature_remover/** tomoya.kimura@tier4.jp
 perception/autoware_detected_object_validation/** dai.nguyen@tier4.jp shintaro.tomie@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
 perception/autoware_detection_by_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
@@ -128,14 +117,25 @@ perception/autoware_ground_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@t
 perception/autoware_image_projection_based_fusion/** dai.nguyen@tier4.jp koji.minoda@tier4.jp kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
 perception/autoware_lidar_apollo_instance_segmentation/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
 perception/autoware_lidar_centerpoint/** kenzo.lobos@tier4.jp koji.minoda@tier4.jp
+perception/autoware_lidar_transfusion/** amadeusz.szymko.2@tier4.jp kenzo.lobos@tier4.jp satoshi.tanaka@tier4.jp
+perception/autoware_map_based_prediction/** kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yoshi.ri@tier4.jp
+perception/autoware_multi_object_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
+perception/autoware_object_merger/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
+perception/autoware_object_range_splitter/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
+perception/autoware_object_velocity_splitter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp
 perception/autoware_occupancy_grid_map_outlier_filter/** abrahammonrroy@yahoo.com yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
-perception/autoware_radar_tracks_msgs_converter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp
 perception/autoware_probabilistic_occupancy_grid_map/** mamoru.sobue@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
+perception/autoware_radar_crossing_objects_noise_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp
+perception/autoware_radar_fusion_to_detected_object/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp
+perception/autoware_radar_object_clustering/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp
+perception/autoware_radar_object_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
+perception/autoware_radar_tracks_msgs_converter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp
 perception/autoware_raindrop_cluster_filter/** dai.nguyen@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
-perception/autoware_shape_estimation/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
+perception/autoware_shape_estimation/** kcolak@leodrive.ai yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
 perception/autoware_simple_object_merger/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp
 perception/autoware_tensorrt_classifier/** kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp
 perception/autoware_tensorrt_yolox/** dan.umeda@tier4.jp manato.hirabayashi@tier4.jp
+perception/autoware_tracking_object_merger/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
 perception/autoware_traffic_light_arbiter/** kenzo.lobos@tier4.jp shunsuke.miura@tier4.jp
 perception/autoware_traffic_light_classifier/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp yukihiro.saito@tier4.jp
 perception/autoware_traffic_light_fine_detector/** shintaro.tomie@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp
@@ -162,7 +162,7 @@ planning/autoware_rtc_interface/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tie
 planning/autoware_scenario_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp
 planning/autoware_static_centerline_generator/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp
 planning/autoware_surround_obstacle_checker/** go.sakayori@tier4.jp satoshi.ota@tier4.jp
-planning/autoware_velocity_smoother/** fumiya.watanabe@tier4.jp makoto.kurihara@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp
+planning/autoware_velocity_smoother/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp makoto.kurihara@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp
 planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp
 planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp
 planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp
@@ -199,23 +199,26 @@ planning/sampling_based_planner/autoware_bezier_sampler/** maxime.clement@tier4.
 planning/sampling_based_planner/autoware_frenet_planner/** maxime.clement@tier4.jp
 planning/sampling_based_planner/autoware_path_sampler/** maxime.clement@tier4.jp
 planning/sampling_based_planner/autoware_sampler_common/** maxime.clement@tier4.jp
-sensing/gnss_poser/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 sensing/autoware_image_diagnostics/** dai.nguyen@tier4.jp yoshi.ri@tier4.jp
 sensing/autoware_image_transport_decompressor/** kenzo.lobos@tier4.jp yukihiro.saito@tier4.jp
-sensing/imu_corrector/** taiki.yamada@tier4.jp yamato.ando@tier4.jp
-sensing/livox/autoware_livox_tag_filter/** kenzo.lobos@tier4.jp ryohsuke.mitsudome@tier4.jp
 sensing/autoware_pointcloud_preprocessor/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp david.wong@tier4.jp kenzo.lobos@tier4.jp kyoichi.sugahara@tier4.jp melike@leodrive.ai shunsuke.miura@tier4.jp yihsiang.fang@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
 sensing/autoware_radar_scan_to_pointcloud2/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp
 sensing/autoware_radar_static_pointcloud_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp
 sensing/autoware_radar_threshold_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp
 sensing/autoware_radar_tracks_noise_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp
+sensing/gnss_poser/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
+sensing/imu_corrector/** taiki.yamada@tier4.jp yamato.ando@tier4.jp
+sensing/livox/autoware_livox_tag_filter/** kenzo.lobos@tier4.jp ryohsuke.mitsudome@tier4.jp
 sensing/tier4_pcl_extensions/** david.wong@tier4.jp kenzo.lobos@tier4.jp ryu.yamamoto@tier4.jp
 sensing/vehicle_velocity_converter/** ryu.yamamoto@tier4.jp
+simulator/autoware_carla_interface/** maxime.clement@tier4.jp mradityagio@gmail.com
 simulator/dummy_perception_publisher/** yukihiro.saito@tier4.jp
 simulator/fault_injection/** keisuke.shima@tier4.jp
 simulator/learning_based_vehicle_model/** maxime.clement@tier4.jp nagy.tomas@tier4.jp
 simulator/simple_planning_simulator/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp temkei.kem@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp
 simulator/vehicle_door_simulator/** isamu.takagi@tier4.jp
+system/autoware_component_monitor/** memin@leodrive.ai
+system/autoware_processing_time_checker/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp
 system/bluetooth_monitor/** fumihito.ito@tier4.jp
 system/component_state_monitor/** isamu.takagi@tier4.jp
 system/default_ad_api/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp
diff --git a/.github/workflows/build-and-test-daily.yaml b/.github/workflows/build-and-test-daily.yaml
index 0b21ba56640a7..66275444482b5 100644
--- a/.github/workflows/build-and-test-daily.yaml
+++ b/.github/workflows/build-and-test-daily.yaml
@@ -7,7 +7,7 @@ on:
 
 jobs:
   build-and-test-daily:
-    runs-on: [self-hosted, linux, X64]
+    runs-on: [self-hosted, linux, X64, gpu]
     container: ${{ matrix.container }}${{ matrix.container-suffix }}
     strategy:
       fail-fast: false
diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml
index 8d2158f3b911c..d41e6cf481b76 100644
--- a/.github/workflows/build-and-test-differential.yaml
+++ b/.github/workflows/build-and-test-differential.yaml
@@ -21,7 +21,7 @@ jobs:
   build-and-test-differential:
     needs: make-sure-label-is-present
     if: ${{ needs.make-sure-label-is-present.outputs.result == 'true' }}
-    runs-on: ubuntu-latest
+    runs-on: ${{ matrix.runner }}
     container: ${{ matrix.container }}${{ matrix.container-suffix }}
     strategy:
       fail-fast: false
@@ -35,6 +35,10 @@ jobs:
           - rosdistro: humble
             container: ghcr.io/autowarefoundation/autoware:latest-autoware-universe
             build-depends-repos: build_depends.repos
+          - container-suffix: -cuda
+            runner: [self-hosted, linux, X64, cpu]
+          - container-suffix: ""
+            runner: ubuntu-latest
     steps:
       - name: Set PR fetch depth
         run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}"
diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml
index 85dc6c2fb89bf..716d6b8df5fbc 100644
--- a/.github/workflows/build-and-test.yaml
+++ b/.github/workflows/build-and-test.yaml
@@ -12,7 +12,7 @@ env:
 
 jobs:
   build-and-test:
-    runs-on: [self-hosted, linux, X64]
+    runs-on: [self-hosted, linux, X64, gpu]
     container: ${{ matrix.container }}${{ matrix.container-suffix }}
     strategy:
       fail-fast: false
diff --git a/README.md b/README.md
index 23d0b172554fd..35818990cee95 100644
--- a/README.md
+++ b/README.md
@@ -1,5 +1,7 @@
 # Autoware Universe
 
+[![codecov](https://codecov.io/github/autowarefoundation/autoware.universe/graph/badge.svg?token=KQP68YQ65D)](https://codecov.io/github/autowarefoundation/autoware.universe)
+
 ## Welcome to Autoware Universe
 
 Autoware Universe serves as a foundational pillar within the Autoware ecosystem, playing a critical role in enhancing the core functionalities of autonomous driving technologies.
diff --git a/codecov.yaml b/codecov.yaml
index ecc77815e97e6..255312a29ccf7 100644
--- a/codecov.yaml
+++ b/codecov.yaml
@@ -26,3 +26,7 @@ flag_management:
         type: patch
         target: 0% # Make CI always succeed
         threshold: 100% # Make CI always succeed
+
+ignore:
+  - "**/test/*"
+  - "**/test/**/*"
diff --git a/common/.pages b/common/.pages
index 7a87ae2711e10..3b1326b5b6dd7 100644
--- a/common/.pages
+++ b/common/.pages
@@ -50,7 +50,7 @@ nav:
     - 'traffic_light_recognition_marker_publisher': common/traffic_light_recognition_marker_publisher/Readme
   - 'Node':
     - 'Goal Distance Calculator': common/goal_distance_calculator/Readme
-    - 'Path Distance Calculator': common/path_distance_calculator/Readme
+    - 'Path Distance Calculator': common/autoware_path_distance_calculator/Readme
   - 'Others':
     - 'autoware_ad_api_specs': common/autoware_ad_api_specs
     - 'component_interface_specs': common/component_interface_specs
diff --git a/common/autoware_auto_common/test/test_template_utils.cpp b/common/autoware_auto_common/test/test_template_utils.cpp
index a670aaab83cfa..05e468dc4787e 100644
--- a/common/autoware_auto_common/test/test_template_utils.cpp
+++ b/common/autoware_auto_common/test/test_template_utils.cpp
@@ -27,7 +27,7 @@ struct FalseType
 
 struct Foo
 {
-  CorrectType bar(CorrectType, const CorrectType &, CorrectType *) { return CorrectType{}; }
+  static CorrectType bar(CorrectType, const CorrectType &, CorrectType *) { return CorrectType{}; }
 };
 
 template <template <typename> class Expression, typename... Ts>
diff --git a/common/path_distance_calculator/CMakeLists.txt b/common/autoware_path_distance_calculator/CMakeLists.txt
similarity index 73%
rename from common/path_distance_calculator/CMakeLists.txt
rename to common/autoware_path_distance_calculator/CMakeLists.txt
index 0768371398def..6064164f2eff7 100644
--- a/common/path_distance_calculator/CMakeLists.txt
+++ b/common/autoware_path_distance_calculator/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.14)
-project(path_distance_calculator)
+project(autoware_path_distance_calculator)
 
 find_package(autoware_cmake REQUIRED)
 autoware_package()
@@ -9,7 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
 )
 
 rclcpp_components_register_node(${PROJECT_NAME}
-  PLUGIN "PathDistanceCalculator"
+  PLUGIN "autoware::path_distance_calculator::PathDistanceCalculator"
   EXECUTABLE ${PROJECT_NAME}_node
 )
 
diff --git a/common/path_distance_calculator/Readme.md b/common/autoware_path_distance_calculator/Readme.md
similarity index 97%
rename from common/path_distance_calculator/Readme.md
rename to common/autoware_path_distance_calculator/Readme.md
index fed0476047ba9..c6389c930f62a 100644
--- a/common/path_distance_calculator/Readme.md
+++ b/common/autoware_path_distance_calculator/Readme.md
@@ -1,4 +1,4 @@
-# path_distance_calculator
+# autoware_path_distance_calculator
 
 ## Purpose
 
diff --git a/common/path_distance_calculator/launch/path_distance_calculator.launch.xml b/common/autoware_path_distance_calculator/launch/path_distance_calculator.launch.xml
similarity index 68%
rename from common/path_distance_calculator/launch/path_distance_calculator.launch.xml
rename to common/autoware_path_distance_calculator/launch/path_distance_calculator.launch.xml
index 92805fb10c49a..15ab838c7da7a 100644
--- a/common/path_distance_calculator/launch/path_distance_calculator.launch.xml
+++ b/common/autoware_path_distance_calculator/launch/path_distance_calculator.launch.xml
@@ -1,7 +1,7 @@
 <launch>
   <arg name="path" default="/planning/scenario_planning/lane_driving/behavior_planning/path"/>
   <arg name="distance" default="~/distance"/>
-  <node pkg="path_distance_calculator" exec="path_distance_calculator_node" name="path_distance_calculator">
+  <node pkg="autoware_path_distance_calculator" exec="autoware_path_distance_calculator_node" name="path_distance_calculator">
     <remap from="~/input/path" to="$(var path)"/>
     <remap from="~/output/distance" to="$(var distance)"/>
   </node>
diff --git a/common/path_distance_calculator/package.xml b/common/autoware_path_distance_calculator/package.xml
similarity index 86%
rename from common/path_distance_calculator/package.xml
rename to common/autoware_path_distance_calculator/package.xml
index 1921cbb1c4a20..f6410ffb24dc5 100644
--- a/common/path_distance_calculator/package.xml
+++ b/common/autoware_path_distance_calculator/package.xml
@@ -1,9 +1,9 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>path_distance_calculator</name>
+  <name>autoware_path_distance_calculator</name>
   <version>0.0.0</version>
-  <description>The path_distance_calculator package</description>
+  <description>The autoware_path_distance_calculator package</description>
   <maintainer email="isamu.takagi@tier4.jp">Takagi, Isamu</maintainer>
   <license>Apache License 2.0</license>
 
diff --git a/common/path_distance_calculator/src/path_distance_calculator.cpp b/common/autoware_path_distance_calculator/src/path_distance_calculator.cpp
similarity index 91%
rename from common/path_distance_calculator/src/path_distance_calculator.cpp
rename to common/autoware_path_distance_calculator/src/path_distance_calculator.cpp
index 833c806cdd6db..b91752e547196 100644
--- a/common/path_distance_calculator/src/path_distance_calculator.cpp
+++ b/common/autoware_path_distance_calculator/src/path_distance_calculator.cpp
@@ -22,6 +22,8 @@
 #include <utility>
 #include <vector>
 
+namespace autoware::path_distance_calculator
+{
 PathDistanceCalculator::PathDistanceCalculator(const rclcpp::NodeOptions & options)
 : Node("path_distance_calculator", options), self_pose_listener_(this)
 {
@@ -54,6 +56,6 @@ PathDistanceCalculator::PathDistanceCalculator(const rclcpp::NodeOptions & optio
     pub_dist_->publish(msg);
   });
 }
-
+}  // namespace autoware::path_distance_calculator
 #include <rclcpp_components/register_node_macro.hpp>
-RCLCPP_COMPONENTS_REGISTER_NODE(PathDistanceCalculator)
+RCLCPP_COMPONENTS_REGISTER_NODE(autoware::path_distance_calculator::PathDistanceCalculator)
diff --git a/common/path_distance_calculator/src/path_distance_calculator.hpp b/common/autoware_path_distance_calculator/src/path_distance_calculator.hpp
similarity index 93%
rename from common/path_distance_calculator/src/path_distance_calculator.hpp
rename to common/autoware_path_distance_calculator/src/path_distance_calculator.hpp
index 6624f316401df..ff83483676467 100644
--- a/common/path_distance_calculator/src/path_distance_calculator.hpp
+++ b/common/autoware_path_distance_calculator/src/path_distance_calculator.hpp
@@ -22,6 +22,9 @@
 #include <autoware_planning_msgs/msg/path.hpp>
 #include <tier4_debug_msgs/msg/float64_stamped.hpp>
 
+namespace autoware::path_distance_calculator
+{
+
 class PathDistanceCalculator : public rclcpp::Node
 {
 public:
@@ -35,4 +38,6 @@ class PathDistanceCalculator : public rclcpp::Node
   autoware::universe_utils::SelfPoseListener self_pose_listener_;
 };
 
+}  // namespace autoware::path_distance_calculator
+
 #endif  // PATH_DISTANCE_CALCULATOR_HPP_
diff --git a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp
index 8d29900e1da26..fed20e24f9787 100644
--- a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp
+++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp
@@ -104,7 +104,7 @@ get_2d_shape_marker_ptr(
 AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr
 get_label_marker_ptr(
   const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
-  const std::string label, const std_msgs::msg::ColorRGBA & color_rgba);
+  const std::string & label, const std_msgs::msg::ColorRGBA & color_rgba);
 
 AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr
 get_existence_probability_marker_ptr(
diff --git a/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp
index 910bd6ed815e6..9c25c2dc3781e 100644
--- a/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp
+++ b/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp
@@ -459,7 +459,7 @@ visualization_msgs::msg::Marker::SharedPtr get_uuid_marker_ptr(
 
 visualization_msgs::msg::Marker::SharedPtr get_label_marker_ptr(
   const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
-  const std::string label, const std_msgs::msg::ColorRGBA & color_rgba)
+  const std::string & label, const std_msgs::msg::ColorRGBA & color_rgba)
 {
   auto marker_ptr = std::make_shared<Marker>();
   marker_ptr->type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/ros/static_transform_buffer.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/static_transform_buffer.hpp
new file mode 100644
index 0000000000000..175d1934e973d
--- /dev/null
+++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/static_transform_buffer.hpp
@@ -0,0 +1,165 @@
+// 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__UNIVERSE_UTILS__ROS__STATIC_TRANSFORM_BUFFER_HPP_
+#define AUTOWARE__UNIVERSE_UTILS__ROS__STATIC_TRANSFORM_BUFFER_HPP_
+
+#include "autoware/universe_utils/ros/transform_listener.hpp"
+
+#include <eigen3/Eigen/Core>
+#include <pcl_ros/transforms.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <sensor_msgs/point_cloud2_iterator.hpp>
+
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <utility>
+
+namespace std
+{
+template <>
+struct hash<std::pair<std::string, std::string>>
+{
+  size_t operator()(const std::pair<std::string, std::string> & p) const
+  {
+    size_t h1 = std::hash<std::string>{}(p.first);
+    size_t h2 = std::hash<std::string>{}(p.second);
+    return h1 ^ (h2 << 1u);
+  }
+};
+}  // namespace std
+
+namespace autoware::universe_utils
+{
+using std::chrono_literals::operator""ms;
+using Key = std::pair<std::string, std::string>;
+struct PairEqual
+{
+  bool operator()(const Key & p1, const Key & p2) const
+  {
+    return p1.first == p2.first && p1.second == p2.second;
+  }
+};
+using TFMap = std::unordered_map<Key, Eigen::Matrix4f, std::hash<Key>, PairEqual>;
+
+class StaticTransformBuffer
+{
+public:
+  StaticTransformBuffer() = default;
+
+  /**
+   * @brief Retrieves a transform between two static frames.
+   *
+   * This function attempts to retrieve a transform between the target frame and the source frame.
+   * If success, the transform matrix is set to the output parameter and the function returns true.
+   * Otherwise, transform matrix is set to identity and the function returns false. Transform
+   * Listener is destroyed after function call.
+   *
+   * @param node A pointer to the ROS node.
+   * @param target_frame The target frame.
+   * @param source_frame The source frame.
+   * @param eigen_transform The output Eigen transform matrix. Is set to identity if the transform
+   * is not found.
+   * @return True if the transform was successfully retrieved, false otherwise.
+   */
+  bool getTransform(
+    rclcpp::Node * node, const std::string & target_frame, const std::string & source_frame,
+    Eigen::Matrix4f & eigen_transform)
+  {
+    auto key = std::make_pair(target_frame, source_frame);
+    auto key_inv = std::make_pair(source_frame, target_frame);
+
+    // Check if the transform is already in the buffer
+    auto it = buffer_.find(key);
+    if (it != buffer_.end()) {
+      eigen_transform = it->second;
+      return true;
+    }
+
+    // Check if the inverse transform is already in the buffer
+    auto it_inv = buffer_.find(key_inv);
+    if (it_inv != buffer_.end()) {
+      eigen_transform = it_inv->second.inverse();
+      buffer_[key] = eigen_transform;
+      return true;
+    }
+
+    // Check if transform is needed
+    if (target_frame == source_frame) {
+      eigen_transform = Eigen::Matrix4f::Identity();
+      buffer_[key] = eigen_transform;
+      return true;
+    }
+
+    // Get the transform from the TF tree
+    auto tf_listener = std::make_unique<autoware::universe_utils::TransformListener>(node);
+    auto tf = tf_listener->getTransform(
+      target_frame, source_frame, rclcpp::Time(0), rclcpp::Duration(1000ms));
+    RCLCPP_DEBUG(
+      node->get_logger(), "Trying to enqueue %s -> %s transform to static TFs buffer...",
+      target_frame.c_str(), source_frame.c_str());
+    if (tf == nullptr) {
+      eigen_transform = Eigen::Matrix4f::Identity();
+      return false;
+    }
+    pcl_ros::transformAsMatrix(*tf, eigen_transform);
+    buffer_[key] = eigen_transform;
+    return true;
+  }
+
+  /**
+   * Transforms a point cloud from one frame to another.
+   *
+   * @param node A pointer to the ROS node.
+   * @param target_frame The target frame to transform the point cloud to.
+   * @param cloud_in The input point cloud to be transformed.
+   * @param cloud_out The transformed point cloud.
+   * @return True if the transformation is successful, false otherwise.
+   */
+  bool transformPointcloud(
+    rclcpp::Node * node, const std::string & target_frame,
+    const sensor_msgs::msg::PointCloud2 & cloud_in, sensor_msgs::msg::PointCloud2 & cloud_out)
+  {
+    if (
+      pcl::getFieldIndex(cloud_in, "x") == -1 || pcl::getFieldIndex(cloud_in, "y") == -1 ||
+      pcl::getFieldIndex(cloud_in, "z") == -1) {
+      RCLCPP_ERROR(node->get_logger(), "Input pointcloud does not have xyz fields");
+      return false;
+    }
+    if (target_frame == cloud_in.header.frame_id) {
+      cloud_out = cloud_in;
+      return true;
+    }
+    Eigen::Matrix4f eigen_transform;
+    if (!getTransform(node, target_frame, cloud_in.header.frame_id, eigen_transform)) {
+      return false;
+    }
+    pcl_ros::transformPointCloud(eigen_transform, cloud_in, cloud_out);
+    cloud_out.header.frame_id = target_frame;
+    return true;
+  }
+
+private:
+  TFMap buffer_;
+};
+
+}  // namespace autoware::universe_utils
+
+#endif  // AUTOWARE__UNIVERSE_UTILS__ROS__STATIC_TRANSFORM_BUFFER_HPP_
diff --git a/common/autoware_universe_utils/package.xml b/common/autoware_universe_utils/package.xml
index de0461b5a841d..b4809e720b8d4 100644
--- a/common/autoware_universe_utils/package.xml
+++ b/common/autoware_universe_utils/package.xml
@@ -24,6 +24,7 @@
   <depend>pcl_ros</depend>
   <depend>rclcpp</depend>
   <depend>tf2</depend>
+  <depend>tf2_eigen</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tier4_debug_msgs</depend>
   <depend>tier4_planning_msgs</depend>
diff --git a/common/autoware_universe_utils/test/src/ros/test_static_transform_buffer.cpp b/common/autoware_universe_utils/test/src/ros/test_static_transform_buffer.cpp
new file mode 100644
index 0000000000000..5200378e3bf52
--- /dev/null
+++ b/common/autoware_universe_utils/test/src/ros/test_static_transform_buffer.cpp
@@ -0,0 +1,210 @@
+// 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.
+
+#include "autoware/universe_utils/ros/static_transform_buffer.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+#include <tf2_eigen/tf2_eigen.hpp>
+
+#include <gtest/gtest.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+
+#include <memory>
+
+class TestStaticTransformBuffer : public ::testing::Test
+{
+protected:
+  std::shared_ptr<rclcpp::Node> node_{nullptr};
+  std::shared_ptr<autoware::universe_utils::StaticTransformBuffer> static_tf_buffer_{nullptr};
+  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_broadcaster_;
+  geometry_msgs::msg::TransformStamped tf_base_to_lidar_;
+  Eigen::Matrix4f eigen_base_to_lidar_;
+  std::unique_ptr<sensor_msgs::msg::PointCloud2> cloud_in_;
+
+  geometry_msgs::msg::TransformStamped generateTransformMsg(
+    const int32_t seconds, const uint32_t nanoseconds, const std::string & parent_frame,
+    const std::string & child_frame, double x, double y, double z, double qx, double qy, double qz,
+    double qw)
+  {
+    rclcpp::Time timestamp(seconds, nanoseconds, RCL_ROS_TIME);
+    geometry_msgs::msg::TransformStamped tf_msg;
+    tf_msg.header.stamp = timestamp;
+    tf_msg.header.frame_id = parent_frame;
+    tf_msg.child_frame_id = child_frame;
+    tf_msg.transform.translation.x = x;
+    tf_msg.transform.translation.y = y;
+    tf_msg.transform.translation.z = z;
+    tf_msg.transform.rotation.x = qx;
+    tf_msg.transform.rotation.y = qy;
+    tf_msg.transform.rotation.z = qz;
+    tf_msg.transform.rotation.w = qw;
+    return tf_msg;
+  }
+
+  void SetUp() override
+  {
+    node_ = std::make_shared<rclcpp::Node>("test_static_transform_buffer");
+    static_tf_buffer_ = std::make_shared<autoware::universe_utils::StaticTransformBuffer>();
+    tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(node_);
+
+    tf_base_to_lidar_ = generateTransformMsg(
+      10, 100'000'000, "base_link", "lidar_top", 0.690, 0.000, 2.100, -0.007, -0.007, 0.692, 0.722);
+    eigen_base_to_lidar_ = tf2::transformToEigen(tf_base_to_lidar_).matrix().cast<float>();
+    tf_broadcaster_->sendTransform(tf_base_to_lidar_);
+    cloud_in_ = std::make_unique<sensor_msgs::msg::PointCloud2>();
+
+    // Set up the fields for x, y, and z coordinates
+    cloud_in_->fields.resize(3);
+    sensor_msgs::PointCloud2Modifier modifier(*cloud_in_);
+    modifier.setPointCloud2FieldsByString(1, "xyz");
+
+    // Resize the cloud to hold points_per_pointcloud_ points
+    modifier.resize(10);
+
+    // Create an iterator for the x, y, z fields
+    sensor_msgs::PointCloud2Iterator<float> iter_x(*cloud_in_, "x");
+    sensor_msgs::PointCloud2Iterator<float> iter_y(*cloud_in_, "y");
+    sensor_msgs::PointCloud2Iterator<float> iter_z(*cloud_in_, "z");
+
+    // Populate the point cloud
+    for (size_t i = 0; i < modifier.size(); ++i, ++iter_x, ++iter_y, ++iter_z) {
+      *iter_x = static_cast<float>(i);
+      *iter_y = static_cast<float>(i);
+      *iter_z = static_cast<float>(i);
+    }
+
+    // Set up cloud header
+    cloud_in_->header.frame_id = "lidar_top";
+    cloud_in_->header.stamp = rclcpp::Time(10, 100'000'000);
+
+    ASSERT_TRUE(rclcpp::ok());
+  }
+
+  void TearDown() override { static_tf_buffer_.reset(); }
+};
+
+TEST_F(TestStaticTransformBuffer, TestTransformNoExist)
+{
+  Eigen::Matrix4f transform;
+  auto success = static_tf_buffer_->getTransform(node_.get(), "base_link", "fake_link", transform);
+  EXPECT_TRUE(transform.isIdentity());
+  EXPECT_FALSE(success);
+}
+
+TEST_F(TestStaticTransformBuffer, TestTransformBase)
+{
+  Eigen::Matrix4f eigen_base_to_lidar;
+  auto success =
+    static_tf_buffer_->getTransform(node_.get(), "base_link", "lidar_top", eigen_base_to_lidar);
+  EXPECT_TRUE(eigen_base_to_lidar.isApprox(eigen_base_to_lidar_, 0.001));
+  EXPECT_TRUE(success);
+}
+
+TEST_F(TestStaticTransformBuffer, TestTransformSameFrame)
+{
+  Eigen::Matrix4f eigen_base_to_base;
+  auto success =
+    static_tf_buffer_->getTransform(node_.get(), "base_link", "base_link", eigen_base_to_base);
+  EXPECT_TRUE(eigen_base_to_base.isApprox(Eigen::Matrix4f::Identity(), 0.001));
+  EXPECT_TRUE(success);
+}
+
+TEST_F(TestStaticTransformBuffer, TestTransformInverse)
+{
+  Eigen::Matrix4f eigen_lidar_to_base;
+  auto success =
+    static_tf_buffer_->getTransform(node_.get(), "lidar_top", "base_link", eigen_lidar_to_base);
+  EXPECT_TRUE(eigen_lidar_to_base.isApprox(eigen_base_to_lidar_.inverse(), 0.001));
+  EXPECT_TRUE(success);
+}
+
+TEST_F(TestStaticTransformBuffer, TestTransformMultipleCall)
+{
+  Eigen::Matrix4f eigen_transform;
+  EXPECT_FALSE(
+    static_tf_buffer_->getTransform(node_.get(), "base_link", "fake_link", eigen_transform));
+  EXPECT_TRUE(eigen_transform.isApprox(Eigen::Matrix4f::Identity(), 0.001));
+  EXPECT_TRUE(
+    static_tf_buffer_->getTransform(node_.get(), "lidar_top", "base_link", eigen_transform));
+  EXPECT_TRUE(eigen_transform.isApprox(eigen_base_to_lidar_.inverse(), 0.001));
+  EXPECT_TRUE(
+    static_tf_buffer_->getTransform(node_.get(), "fake_link", "fake_link", eigen_transform));
+  EXPECT_TRUE(eigen_transform.isApprox(Eigen::Matrix4f::Identity(), 0.001));
+  EXPECT_TRUE(
+    static_tf_buffer_->getTransform(node_.get(), "base_link", "lidar_top", eigen_transform));
+  EXPECT_TRUE(eigen_transform.isApprox(eigen_base_to_lidar_, 0.001));
+  EXPECT_FALSE(
+    static_tf_buffer_->getTransform(node_.get(), "fake_link", "lidar_top", eigen_transform));
+  EXPECT_TRUE(eigen_transform.isApprox(Eigen::Matrix4f::Identity(), 0.001));
+  EXPECT_TRUE(
+    static_tf_buffer_->getTransform(node_.get(), "base_link", "lidar_top", eigen_transform));
+  EXPECT_TRUE(eigen_transform.isApprox(eigen_base_to_lidar_, 0.001));
+}
+
+TEST_F(TestStaticTransformBuffer, TestTransformEmptyPointCloud)
+{
+  auto cloud_in = std::make_unique<sensor_msgs::msg::PointCloud2>();
+  cloud_in->header.frame_id = "lidar_top";
+  cloud_in->header.stamp = rclcpp::Time(10, 100'000'000);
+  auto cloud_out = std::make_unique<sensor_msgs::msg::PointCloud2>();
+
+  EXPECT_FALSE(
+    static_tf_buffer_->transformPointcloud(node_.get(), "lidar_top", *cloud_in, *cloud_out));
+  EXPECT_FALSE(
+    static_tf_buffer_->transformPointcloud(node_.get(), "base_link", *cloud_in, *cloud_out));
+  EXPECT_FALSE(
+    static_tf_buffer_->transformPointcloud(node_.get(), "fake_link", *cloud_in, *cloud_out));
+}
+
+TEST_F(TestStaticTransformBuffer, TestTransformEmptyPointCloudNoHeader)
+{
+  auto cloud_in = std::make_unique<sensor_msgs::msg::PointCloud2>();
+  auto cloud_out = std::make_unique<sensor_msgs::msg::PointCloud2>();
+
+  EXPECT_FALSE(
+    static_tf_buffer_->transformPointcloud(node_.get(), "lidar_top", *cloud_in, *cloud_out));
+  EXPECT_FALSE(
+    static_tf_buffer_->transformPointcloud(node_.get(), "base_link", *cloud_in, *cloud_out));
+  EXPECT_FALSE(
+    static_tf_buffer_->transformPointcloud(node_.get(), "fake_link", *cloud_in, *cloud_out));
+}
+
+TEST_F(TestStaticTransformBuffer, TestTransformPointCloud)
+{
+  auto cloud_out = std::make_unique<sensor_msgs::msg::PointCloud2>();
+
+  // Transform cloud with header
+  EXPECT_TRUE(
+    static_tf_buffer_->transformPointcloud(node_.get(), "lidar_top", *cloud_in_, *cloud_out));
+  EXPECT_TRUE(
+    static_tf_buffer_->transformPointcloud(node_.get(), "base_link", *cloud_in_, *cloud_out));
+  EXPECT_FALSE(
+    static_tf_buffer_->transformPointcloud(node_.get(), "fake_link", *cloud_in_, *cloud_out));
+}
+
+TEST_F(TestStaticTransformBuffer, TestTransformPointCloudNoHeader)
+{
+  auto cloud_out = std::make_unique<sensor_msgs::msg::PointCloud2>();
+
+  // Transform cloud without header
+  auto cloud_in = std::make_unique<sensor_msgs::msg::PointCloud2>(*cloud_in_);
+  cloud_in->header.frame_id = "";
+  cloud_in->header.stamp = rclcpp::Time(0, 0);
+  EXPECT_FALSE(
+    static_tf_buffer_->transformPointcloud(node_.get(), "lidar_top", *cloud_in, *cloud_out));
+  EXPECT_FALSE(
+    static_tf_buffer_->transformPointcloud(node_.get(), "base_link", *cloud_in, *cloud_out));
+  EXPECT_FALSE(
+    static_tf_buffer_->transformPointcloud(node_.get(), "fake_link", *cloud_in, *cloud_out));
+}
diff --git a/common/qp_interface/include/qp_interface/osqp_interface.hpp b/common/qp_interface/include/qp_interface/osqp_interface.hpp
index 14c03a91d8fa1..55792beaaff11 100644
--- a/common/qp_interface/include/qp_interface/osqp_interface.hpp
+++ b/common/qp_interface/include/qp_interface/osqp_interface.hpp
@@ -33,7 +33,7 @@ class OSQPInterface : public QPInterface
 public:
   /// \brief Constructor without problem formulation
   OSQPInterface(
-    const bool enable_warm_start = false,
+    const bool enable_warm_start = false, const int max_iteration = 20000,
     const c_float eps_abs = std::numeric_limits<c_float>::epsilon(),
     const c_float eps_rel = std::numeric_limits<c_float>::epsilon(), const bool polish = true,
     const bool verbose = false);
diff --git a/common/qp_interface/include/qp_interface/proxqp_interface.hpp b/common/qp_interface/include/qp_interface/proxqp_interface.hpp
index f75fb3da5954c..775ba0fcc42b8 100644
--- a/common/qp_interface/include/qp_interface/proxqp_interface.hpp
+++ b/common/qp_interface/include/qp_interface/proxqp_interface.hpp
@@ -31,8 +31,8 @@ class ProxQPInterface : public QPInterface
 {
 public:
   explicit ProxQPInterface(
-    const bool enable_warm_start, const double eps_abs, const double eps_rel,
-    const bool verbose = false);
+    const bool enable_warm_start, const int max_iteration, const double eps_abs,
+    const double eps_rel, const bool verbose = false);
 
   int getIterationNumber() const override;
   bool isSolved() const override;
diff --git a/common/qp_interface/src/osqp_interface.cpp b/common/qp_interface/src/osqp_interface.cpp
index 0e8cb7e13988a..3ada25e8fa023 100644
--- a/common/qp_interface/src/osqp_interface.cpp
+++ b/common/qp_interface/src/osqp_interface.cpp
@@ -21,8 +21,8 @@
 namespace autoware::common
 {
 OSQPInterface::OSQPInterface(
-  const bool enable_warm_start, const c_float eps_abs, const c_float eps_rel, const bool polish,
-  const bool verbose)
+  const bool enable_warm_start, const int max_iteration, const c_float eps_abs,
+  const c_float eps_rel, const bool polish, const bool verbose)
 : QPInterface(enable_warm_start), work_{nullptr, OSQPWorkspaceDeleter}
 {
   settings_ = std::make_unique<OSQPSettings>();
@@ -35,8 +35,8 @@ OSQPInterface::OSQPInterface(
     settings_->eps_abs = eps_abs;
     settings_->eps_prim_inf = 1.0E-4;
     settings_->eps_dual_inf = 1.0E-4;
-    settings_->warm_start = true;
-    settings_->max_iter = 4000;
+    settings_->warm_start = enable_warm_start;
+    settings_->max_iter = max_iteration;
     settings_->verbose = verbose;
     settings_->polish = polish;
   }
diff --git a/common/qp_interface/src/proxqp_interface.cpp b/common/qp_interface/src/proxqp_interface.cpp
index 507f3858cbf1b..d26543f907e62 100644
--- a/common/qp_interface/src/proxqp_interface.cpp
+++ b/common/qp_interface/src/proxqp_interface.cpp
@@ -19,9 +19,11 @@ namespace autoware::common
 using proxsuite::proxqp::QPSolverOutput;
 
 ProxQPInterface::ProxQPInterface(
-  const bool enable_warm_start, const double eps_abs, const double eps_rel, const bool verbose)
+  const bool enable_warm_start, const int max_iteration, const double eps_abs, const double eps_rel,
+  const bool verbose)
 : QPInterface(enable_warm_start)
 {
+  settings_.max_iter = max_iteration;
   settings_.eps_abs = eps_abs;
   settings_.eps_rel = eps_rel;
   settings_.verbose = verbose;
diff --git a/common/qp_interface/test/test_osqp_interface.cpp b/common/qp_interface/test/test_osqp_interface.cpp
index 9a02dd4b00934..4d0a88c50475c 100644
--- a/common/qp_interface/test/test_osqp_interface.cpp
+++ b/common/qp_interface/test/test_osqp_interface.cpp
@@ -63,7 +63,7 @@ TEST(TestOsqpInterface, BasicQp)
 
   {
     // Define problem during optimization
-    autoware::common::OSQPInterface osqp(false, 1e-6);
+    autoware::common::OSQPInterface osqp(false, 4000, 1e-6);
     const auto solution = osqp.QPInterface::optimize(P, A, q, l, u);
     const auto status = osqp.getStatus();
     const auto polish_status = osqp.getPolishStatus();
@@ -72,7 +72,7 @@ TEST(TestOsqpInterface, BasicQp)
 
   {
     // Define problem during initialization
-    autoware::common::OSQPInterface osqp(false, 1e-6);
+    autoware::common::OSQPInterface osqp(false, 4000, 1e-6);
     const auto solution = osqp.QPInterface::optimize(P, A, q, l, u);
     const auto status = osqp.getStatus();
     const auto polish_status = osqp.getPolishStatus();
@@ -87,7 +87,7 @@ TEST(TestOsqpInterface, BasicQp)
     std::vector<double> q_ini(2, 0.0);
     std::vector<double> l_ini(4, 0.0);
     std::vector<double> u_ini(4, 0.0);
-    autoware::common::OSQPInterface osqp(false, 1e-6);
+    autoware::common::OSQPInterface osqp(false, 4000, 1e-6);
     osqp.QPInterface::optimize(P_ini, A_ini, q_ini, l_ini, u_ini);
   }
 
@@ -95,7 +95,7 @@ TEST(TestOsqpInterface, BasicQp)
     // Define problem during initialization with csc matrix
     CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P);
     CSC_Matrix A_csc = calCSCMatrix(A);
-    autoware::common::OSQPInterface osqp(false, 1e-6);
+    autoware::common::OSQPInterface osqp(false, 4000, 1e-6);
 
     const auto solution = osqp.optimize(P_csc, A_csc, q, l, u);
     const auto status = osqp.getStatus();
@@ -111,7 +111,7 @@ TEST(TestOsqpInterface, BasicQp)
     std::vector<double> q_ini(2, 0.0);
     std::vector<double> l_ini(4, 0.0);
     std::vector<double> u_ini(4, 0.0);
-    autoware::common::OSQPInterface osqp(false, 1e-6);
+    autoware::common::OSQPInterface osqp(false, 4000, 1e-6);
     osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini);
 
     // Redefine problem before optimization
@@ -132,7 +132,7 @@ TEST(TestOsqpInterface, BasicQp)
     std::vector<double> q_ini(2, 0.0);
     std::vector<double> l_ini(4, 0.0);
     std::vector<double> u_ini(4, 0.0);
-    autoware::common::OSQPInterface osqp(true, 1e-6);  // enable warm start
+    autoware::common::OSQPInterface osqp(true, 4000, 1e-6);  // enable warm start
     osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini);
 
     // Redefine problem before optimization
diff --git a/common/qp_interface/test/test_proxqp_interface.cpp b/common/qp_interface/test/test_proxqp_interface.cpp
index a8b1bb29dafa0..e9cb90ef5d442 100644
--- a/common/qp_interface/test/test_proxqp_interface.cpp
+++ b/common/qp_interface/test/test_proxqp_interface.cpp
@@ -57,7 +57,7 @@ TEST(TestProxqpInterface, BasicQp)
 
   {
     // Define problem during optimization
-    autoware::common::ProxQPInterface proxqp(false, 1e-9, 1e-9, false);
+    autoware::common::ProxQPInterface proxqp(false, 4000, 1e-9, 1e-9, false);
     const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u);
     const auto status = proxqp.getStatus();
     check_result(solution, status);
@@ -65,7 +65,7 @@ TEST(TestProxqpInterface, BasicQp)
 
   {
     // Define problem during optimization with warm start
-    autoware::common::ProxQPInterface proxqp(true, 1e-9, 1e-9, false);
+    autoware::common::ProxQPInterface proxqp(true, 4000, 1e-9, 1e-9, false);
     {
       const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u);
       const auto status = proxqp.getStatus();
diff --git a/control/autoware_autonomous_emergency_braking/CMakeLists.txt b/control/autoware_autonomous_emergency_braking/CMakeLists.txt
index 85290c6c66730..9f7a64c18da9a 100644
--- a/control/autoware_autonomous_emergency_braking/CMakeLists.txt
+++ b/control/autoware_autonomous_emergency_braking/CMakeLists.txt
@@ -30,8 +30,11 @@ rclcpp_components_register_node(${AEB_NODE}
 )
 
 if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  ament_lint_auto_find_test_dependencies()
+  ament_add_ros_isolated_gtest(test_aeb
+  test/test.cpp)
+
+target_link_libraries(test_aeb ${AEB_NODE})
+
 endif()
 
 ament_auto_package(
diff --git a/control/autoware_autonomous_emergency_braking/README.md b/control/autoware_autonomous_emergency_braking/README.md
index db27ca1b4f699..1382a24d98b0a 100644
--- a/control/autoware_autonomous_emergency_braking/README.md
+++ b/control/autoware_autonomous_emergency_braking/README.md
@@ -197,6 +197,7 @@ When vehicle odometry information is faulty, it is possible that the MPC fails t
 | use_predicted_trajectory          | [-]    | bool   | flag to use the predicted path from the control module                                                                                                                                          | true          |
 | use_imu_path                      | [-]    | bool   | flag to use the predicted path generated by sensor data                                                                                                                                         | true          |
 | use_object_velocity_calculation   | [-]    | bool   | flag to use the object velocity calculation. If set to false, object velocity is set to 0 [m/s]                                                                                                 | true          |
+| check_autoware_state              | [-]    | bool   | flag to enable or disable autoware state check. If set to false, the AEB module will run even when the ego vehicle is not in AUTONOMOUS state.                                                  | true          |
 | detection_range_min_height        | [m]    | double | minimum hight of detection range used for avoiding the ghost brake by false positive point clouds                                                                                               | 0.0           |
 | detection_range_max_height_margin | [m]    | double | margin for maximum hight of detection range used for avoiding the ghost brake by false positive point clouds. `detection_range_max_height = vehicle_height + detection_range_max_height_margin` | 0.0           |
 | voxel_grid_x                      | [m]    | double | down sampling parameters of x-axis for voxel grid filter                                                                                                                                        | 0.05          |
diff --git a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml
index 25db9448f758b..f13e95c6db8e5 100644
--- a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml
+++ b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml
@@ -6,6 +6,7 @@
     use_pointcloud_data: true
     use_predicted_object_data: true
     use_object_velocity_calculation: true
+    check_autoware_state: true
     min_generated_path_length: 0.5
     imu_prediction_time_horizon: 1.5
     imu_prediction_time_interval: 0.1
diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp
index 9fd6d0192439c..24eaa8516a732 100644
--- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp
+++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp
@@ -71,6 +71,9 @@ using Vector3 = geometry_msgs::msg::Vector3;
 using autoware_perception_msgs::msg::PredictedObject;
 using autoware_perception_msgs::msg::PredictedObjects;
 
+/**
+ * @brief Struct to store object data
+ */
 struct ObjectData
 {
   rclcpp::Time stamp;
@@ -80,22 +83,44 @@ struct ObjectData
   double distance_to_object{0.0};
 };
 
+/**
+ * @brief Class to manage collision data
+ */
 class CollisionDataKeeper
 {
 public:
+  /**
+   * @brief Constructor for CollisionDataKeeper
+   * @param clock Shared pointer to the clock
+   */
   explicit CollisionDataKeeper(rclcpp::Clock::SharedPtr clock) { clock_ = clock; }
 
+  /**
+   * @brief Set timeout values for collision and obstacle data
+   * @param collision_keep_time Time to keep collision data
+   * @param previous_obstacle_keep_time Time to keep previous obstacle data
+   */
   void setTimeout(const double collision_keep_time, const double previous_obstacle_keep_time)
   {
     collision_keep_time_ = collision_keep_time;
     previous_obstacle_keep_time_ = previous_obstacle_keep_time;
   }
 
+  /**
+   * @brief Get timeout values for collision and obstacle data
+   * @return Pair of collision and obstacle data timeout values
+   */
   std::pair<double, double> getTimeout()
   {
     return {collision_keep_time_, previous_obstacle_keep_time_};
   }
 
+  /**
+   * @brief Check if object data has expired
+   * @param data Optional reference to the object data
+   * @param timeout Timeout value to check against
+   * @return True if object data has expired, false otherwise
+   */
   bool checkObjectDataExpired(std::optional<ObjectData> & data, const double timeout)
   {
     if (!data.has_value()) return true;
@@ -109,38 +134,70 @@ class CollisionDataKeeper
     return false;
   }
 
+  /**
+   * @brief Check if collision data has expired
+   * @return True if collision data has expired, false otherwise
+   */
   bool checkCollisionExpired()
   {
     return this->checkObjectDataExpired(closest_object_, collision_keep_time_);
   }
 
+  /**
+   * @brief Check if previous object data has expired
+   * @return True if previous object data has expired, false otherwise
+   */
   bool checkPreviousObjectDataExpired()
   {
     return this->checkObjectDataExpired(prev_closest_object_, previous_obstacle_keep_time_);
   }
 
+  /**
+   * @brief Get the closest object data
+   * @return Object data of the closest object
+   */
   [[nodiscard]] ObjectData get() const
   {
     return (closest_object_.has_value()) ? closest_object_.value() : ObjectData();
   }
 
+  /**
+   * @brief Get the previous closest object data
+   * @return Object data of the previous closest object
+   */
   [[nodiscard]] ObjectData getPreviousObjectData() const
   {
     return (prev_closest_object_.has_value()) ? prev_closest_object_.value() : ObjectData();
   }
 
+  /**
+   * @brief Set collision data
+   * @param data Object data to set
+   */
   void setCollisionData(const ObjectData & data)
   {
     closest_object_ = std::make_optional<ObjectData>(data);
   }
 
+  /**
+   * @brief Set previous object data
+   * @param data Object data to set
+   */
   void setPreviousObjectData(const ObjectData & data)
   {
     prev_closest_object_ = std::make_optional<ObjectData>(data);
   }
 
+  /**
+   * @brief Reset the obstacle velocity history
+   */
   void resetVelocityHistory() { obstacle_velocity_history_.clear(); }
 
+  /**
+   * @brief Update the velocity history with current object velocity
+   * @param current_object_velocity Current object velocity
+   * @param current_object_velocity_time_stamp Timestamp of the current object velocity
+   */
   void updateVelocityHistory(
     const double current_object_velocity, const rclcpp::Time & current_object_velocity_time_stamp)
   {
@@ -158,6 +215,10 @@ class CollisionDataKeeper
       current_object_velocity, current_object_velocity_time_stamp);
   }
 
+  /**
+   * @brief Get the median obstacle velocity from history
+   * @return Optional median obstacle velocity
+   */
   [[nodiscard]] std::optional<double> getMedianObstacleVelocity() const
   {
     if (obstacle_velocity_history_.empty()) return std::nullopt;
@@ -177,6 +238,13 @@ class CollisionDataKeeper
     return (vel1 + vel2) / 2.0;
   }
 
+  /**
+   * @brief Calculate object speed from velocity history
+   * @param closest_object Closest object data
+   * @param path Ego vehicle path
+   * @param current_ego_speed Current ego vehicle speed
+   * @return Optional calculated object speed
+   */
   std::optional<double> calcObjectSpeedFromHistory(
     const ObjectData & closest_object, const Path & path, const double current_ego_speed)
   {
@@ -241,9 +309,16 @@ class CollisionDataKeeper
   rclcpp::Clock::SharedPtr clock_;
 };
 
+/**
+ * @brief Autonomous Emergency Braking (AEB) node
+ */
 class AEB : public rclcpp::Node
 {
 public:
+  /**
+   * @brief Constructor for AEB
+   * @param node_options Options for the node
+   */
   explicit AEB(const rclcpp::NodeOptions & node_options);
 
   // subscriber
@@ -260,57 +335,163 @@ class AEB : public rclcpp::Node
     this, "/autoware/state"};
   // publisher
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_obstacle_pointcloud_;
-  rclcpp::Publisher<MarkerArray>::SharedPtr debug_ego_path_publisher_;  // debug
+  rclcpp::Publisher<MarkerArray>::SharedPtr debug_marker_publisher_;
+  rclcpp::Publisher<MarkerArray>::SharedPtr info_marker_publisher_;
 
   // timer
   rclcpp::TimerBase::SharedPtr timer_;
 
   // callback
+  /**
+   * @brief Callback for point cloud messages
+   * @param input_msg Shared pointer to the point cloud message
+   */
   void onPointCloud(const PointCloud2::ConstSharedPtr input_msg);
+
+  /**
+   * @brief Callback for IMU messages
+   * @param input_msg Shared pointer to the IMU message
+   */
   void onImu(const Imu::ConstSharedPtr input_msg);
+
+  /**
+   * @brief Timer callback function
+   */
   void onTimer();
+
+  /**
+   * @brief Callback for parameter updates
+   * @param parameters Vector of updated parameters
+   * @return Set parameters result
+   */
   rcl_interfaces::msg::SetParametersResult onParameter(
     const std::vector<rclcpp::Parameter> & parameters);
 
+  /**
+   * @brief Fetch the latest data from subscribers
+   * @return True if data fetch was successful, false otherwise
+   */
   bool fetchLatestData();
 
-  // main function
+  /**
+   * @brief Diagnostic check for collisions
+   * @param stat Diagnostic status wrapper
+   */
   void onCheckCollision(DiagnosticStatusWrapper & stat);
+
+  /**
+   * @brief Check for collisions
+   * @param debug_markers Marker array for debugging
+   * @return True if a collision is detected, false otherwise
+   */
   bool checkCollision(MarkerArray & debug_markers);
+
+  /**
+   * @brief Check if there is a collision with the closest object
+   * @param current_v Current velocity of the ego vehicle
+   * @param closest_object Data of the closest object
+   * @return True if a collision is detected, false otherwise
+   */
   bool hasCollision(const double current_v, const ObjectData & closest_object);
 
+  /**
+   * @brief Generate the ego vehicle path
+   * @param curr_v Current velocity of the ego vehicle
+   * @param curr_w Current angular velocity of the ego vehicle
+   * @return Generated ego path
+   */
   Path generateEgoPath(const double curr_v, const double curr_w);
+
+  /**
+   * @brief Generate the ego vehicle path from the predicted trajectory
+   * @param predicted_traj Predicted trajectory of the ego vehicle
+   * @return Optional generated ego path
+   */
   std::optional<Path> generateEgoPath(const Trajectory & predicted_traj);
+
+  /**
+   * @brief Generate the footprint of the path with extra width margin
+   * @param path Ego vehicle path
+   * @param extra_width_margin Extra width margin for the footprint
+   * @return Vector of polygons representing the path footprint
+   */
   std::vector<Polygon2d> generatePathFootprint(const Path & path, const double extra_width_margin);
 
+  /**
+   * @brief Create object data using point cloud clusters
+   * @param ego_path Ego vehicle path
+   * @param ego_polys Polygons representing the ego vehicle footprint
+   * @param stamp Timestamp of the data
+   * @param objects Vector to store the created object data
+   * @param obstacle_points_ptr Pointer to the point cloud of obstacles
+   */
   void createObjectDataUsingPointCloudClusters(
     const Path & ego_path, const std::vector<Polygon2d> & ego_polys, const rclcpp::Time & stamp,
     std::vector<ObjectData> & objects,
     const pcl::PointCloud<pcl::PointXYZ>::Ptr obstacle_points_ptr);
 
+  /**
+   * @brief Create object data using predicted objects
+   * @param ego_path Ego vehicle path
+   * @param ego_polys Polygons representing the ego vehicle footprint
+   * @param objects Vector to store the created object data
+   */
   void createObjectDataUsingPredictedObjects(
     const Path & ego_path, const std::vector<Polygon2d> & ego_polys,
     std::vector<ObjectData> & objects);
 
+  /**
+   * @brief Crop the point cloud with the ego vehicle footprint path
+   * @param ego_polys Polygons representing the ego vehicle footprint
+   * @param filtered_objects Pointer to the filtered point cloud of obstacles
+   */
   void cropPointCloudWithEgoFootprintPath(
     const std::vector<Polygon2d> & ego_polys, pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_objects);
 
-  void createObjectDataUsingPointCloudClusters(
-    const Path & ego_path, const std::vector<Polygon2d> & ego_polys, const rclcpp::Time & stamp,
-    std::vector<ObjectData> & objects);
-  void cropPointCloudWithEgoFootprintPath(const std::vector<Polygon2d> & ego_polys);
-
+  /**
+   * @brief Add a marker for debugging
+   * @param current_time Current time
+   * @param path Ego vehicle path
+   * @param polygons Polygons representing the ego vehicle footprint
+   * @param objects Vector of object data
+   * @param closest_object Optional data of the closest object
+   * @param color_r Red color component
+   * @param color_g Green color component
+   * @param color_b Blue color component
+   * @param color_a Alpha (transparency) component
+   * @param ns Namespace for the marker
+   * @param debug_markers Marker array for debugging
+   */
   void addMarker(
     const rclcpp::Time & current_time, const Path & path, const std::vector<Polygon2d> & polygons,
     const std::vector<ObjectData> & objects, const std::optional<ObjectData> & closest_object,
     const double color_r, const double color_g, const double color_b, const double color_a,
     const std::string & ns, MarkerArray & debug_markers);
 
+  /**
+   * @brief Add a collision marker for debugging
+   * @param data Data of the collision object
+   * @param debug_markers Marker array for debugging
+   */
   void addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers);
 
+  /**
+   * @brief Add an info marker stop wall in front of the ego vehicle
+   * @param markers Data of the closest object
+   */
+  void addVirtualStopWallMarker(MarkerArray & markers);
+
+  /**
+   * @brief Calculate object speed from history
+   * @param closest_object Data of the closest object
+   * @param path Ego vehicle path
+   * @param current_ego_speed Current speed of the ego vehicle
+   * @return Optional calculated object speed
+   */
   std::optional<double> calcObjectSpeedFromHistory(
     const ObjectData & closest_object, const Path & path, const double current_ego_speed);
 
+  // Member variables
   PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr};
   VelocityReport::ConstSharedPtr current_velocity_ptr_{nullptr};
   Vector3::SharedPtr angular_velocity_ptr_{nullptr};
@@ -327,7 +508,7 @@ class AEB : public rclcpp::Node
   // diag
   Updater updater_{this};
 
-  // member variables
+  // Member variables
   bool publish_debug_pointcloud_;
   bool publish_debug_markers_;
   bool use_predicted_trajectory_;
@@ -335,6 +516,7 @@ class AEB : public rclcpp::Node
   bool use_pointcloud_data_;
   bool use_predicted_object_data_;
   bool use_object_velocity_calculation_;
+  bool check_autoware_state_;
   double path_footprint_extra_margin_;
   double detection_range_min_height_;
   double detection_range_max_height_margin_;
diff --git a/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml b/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml
index fe0df41ca89c7..34d0492799577 100644
--- a/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml
+++ b/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml
@@ -3,7 +3,6 @@
   <arg name="input_pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
   <arg name="input_velocity" default="/vehicle/status/velocity_status"/>
   <arg name="input_imu" default="/sensing/imu/imu_data"/>
-  <arg name="input_odometry" default="/localization/kinematic_state"/>
   <arg name="input_predicted_trajectory" default="/control/trajectory_follower/lateral/predicted_trajectory"/>
   <arg name="input_objects" default="/perception/object_recognition/objects"/>
 
diff --git a/control/autoware_autonomous_emergency_braking/package.xml b/control/autoware_autonomous_emergency_braking/package.xml
index d0c75f893cd2d..173d419bf6d9a 100644
--- a/control/autoware_autonomous_emergency_braking/package.xml
+++ b/control/autoware_autonomous_emergency_braking/package.xml
@@ -14,12 +14,17 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+  <test_depend>autoware_test_utils</test_depend>
 
   <depend>autoware_control_msgs</depend>
   <depend>autoware_motion_utils</depend>
   <depend>autoware_planning_msgs</depend>
   <depend>autoware_pointcloud_preprocessor</depend>
   <depend>autoware_system_msgs</depend>
+  <depend>autoware_test_utils</depend>
   <depend>autoware_universe_utils</depend>
   <depend>autoware_vehicle_info_utils</depend>
   <depend>autoware_vehicle_msgs</depend>
@@ -38,9 +43,6 @@
   <depend>tf2_ros</depend>
   <depend>visualization_msgs</depend>
 
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>autoware_lint_common</test_depend>
-
   <export>
     <build_type>ament_cmake</build_type>
   </export>
diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp
index 69bedd02d201b..cd97a8301ad9d 100644
--- a/control/autoware_autonomous_emergency_braking/src/node.cpp
+++ b/control/autoware_autonomous_emergency_braking/src/node.cpp
@@ -125,7 +125,8 @@ AEB::AEB(const rclcpp::NodeOptions & node_options)
   {
     pub_obstacle_pointcloud_ =
       this->create_publisher<sensor_msgs::msg::PointCloud2>("~/debug/obstacle_pointcloud", 1);
-    debug_ego_path_publisher_ = this->create_publisher<MarkerArray>("~/debug/markers", 1);
+    debug_marker_publisher_ = this->create_publisher<MarkerArray>("~/debug/markers", 1);
+    info_marker_publisher_ = this->create_publisher<MarkerArray>("~/info/markers", 1);
   }
   // Diagnostics
   {
@@ -140,6 +141,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options)
   use_pointcloud_data_ = declare_parameter<bool>("use_pointcloud_data");
   use_predicted_object_data_ = declare_parameter<bool>("use_predicted_object_data");
   use_object_velocity_calculation_ = declare_parameter<bool>("use_object_velocity_calculation");
+  check_autoware_state_ = declare_parameter<bool>("check_autoware_state");
   path_footprint_extra_margin_ = declare_parameter<double>("path_footprint_extra_margin");
   detection_range_min_height_ = declare_parameter<double>("detection_range_min_height");
   detection_range_max_height_margin_ =
@@ -193,6 +195,7 @@ rcl_interfaces::msg::SetParametersResult AEB::onParameter(
   updateParam<bool>(parameters, "use_predicted_object_data", use_predicted_object_data_);
   updateParam<bool>(
     parameters, "use_object_velocity_calculation", use_object_velocity_calculation_);
+  updateParam<bool>(parameters, "check_autoware_state", check_autoware_state_);
   updateParam<double>(parameters, "path_footprint_extra_margin", path_footprint_extra_margin_);
   updateParam<double>(parameters, "detection_range_min_height", detection_range_min_height_);
   updateParam<double>(
@@ -363,7 +366,7 @@ bool AEB::fetchLatestData()
   }
 
   autoware_state_ = sub_autoware_state_.takeData();
-  if (!autoware_state_) {
+  if (check_autoware_state_ && !autoware_state_) {
     return missing("autoware_state");
   }
 
@@ -373,6 +376,7 @@ bool AEB::fetchLatestData()
 void AEB::onCheckCollision(DiagnosticStatusWrapper & stat)
 {
   MarkerArray debug_markers;
+  MarkerArray info_markers;
   checkCollision(debug_markers);
 
   if (!collision_data_keeper_.checkCollisionExpired()) {
@@ -386,6 +390,7 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat)
     if (publish_debug_markers_) {
       addCollisionMarker(data, debug_markers);
     }
+    addVirtualStopWallMarker(info_markers);
   } else {
     const std::string error_msg = "[AEB]: No Collision";
     const auto diag_level = DiagnosticStatus::OK;
@@ -393,7 +398,8 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat)
   }
 
   // publish debug markers
-  debug_ego_path_publisher_->publish(debug_markers);
+  debug_marker_publisher_->publish(debug_markers);
+  info_marker_publisher_->publish(info_markers);
 }
 
 bool AEB::checkCollision(MarkerArray & debug_markers)
@@ -406,7 +412,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers)
   }
 
   // if not driving, disable aeb
-  if (autoware_state_->state != AutowareState::DRIVING) {
+  if (check_autoware_state_ && autoware_state_->state != AutowareState::DRIVING) {
     return false;
   }
 
@@ -501,7 +507,8 @@ bool AEB::checkCollision(MarkerArray & debug_markers)
   };
 
   // Data of filtered point cloud
-  pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_objects(new PointCloud);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_objects =
+    pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
   // evaluate if there is a collision for both paths
   const bool has_collision =
     has_collision_ego(filtered_objects) || has_collision_predicted(filtered_objects);
@@ -890,20 +897,17 @@ void AEB::addMarker(
       "Object velocity: " + std::to_string(obj.velocity) + " [m/s]\n";
     closest_object_velocity_marker_array.text +=
       "Object relative velocity to ego: " + std::to_string(obj.velocity - std::abs(ego_velocity)) +
-      " [m/s]";
+      " [m/s]\n";
+    closest_object_velocity_marker_array.text +=
+      "Object distance to ego: " + std::to_string(obj.distance_to_object) + " [m]\n";
+    closest_object_velocity_marker_array.text +=
+      "RSS distance: " + std::to_string(obj.rss) + " [m]";
     debug_markers.markers.push_back(closest_object_velocity_marker_array);
   }
 }
 
-void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers)
+void AEB::addVirtualStopWallMarker(MarkerArray & markers)
 {
-  auto point_marker = autoware::universe_utils::createDefaultMarker(
-    "base_link", data.stamp, "collision_point", 0, Marker::SPHERE,
-    autoware::universe_utils::createMarkerScale(0.3, 0.3, 0.3),
-    autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.3));
-  point_marker.pose.position = data.position;
-  debug_markers.markers.push_back(point_marker);
-
   const auto ego_map_pose = std::invoke([this]() -> std::optional<geometry_msgs::msg::Pose> {
     geometry_msgs::msg::TransformStamped tf_current_pose;
     geometry_msgs::msg::Pose p;
@@ -928,10 +932,20 @@ void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_marker
       ego_map_pose.value(), base_to_front_offset, 0.0, 0.0, 0.0);
     const auto virtual_stop_wall = autoware::motion_utils::createStopVirtualWallMarker(
       ego_front_pose, "autonomous_emergency_braking", this->now(), 0);
-    autoware::universe_utils::appendMarkerArray(virtual_stop_wall, &debug_markers);
+    autoware::universe_utils::appendMarkerArray(virtual_stop_wall, &markers);
   }
 }
 
+void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers)
+{
+  auto point_marker = autoware::universe_utils::createDefaultMarker(
+    "base_link", data.stamp, "collision_point", 0, Marker::SPHERE,
+    autoware::universe_utils::createMarkerScale(0.3, 0.3, 0.3),
+    autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.3));
+  point_marker.pose.position = data.position;
+  debug_markers.markers.push_back(point_marker);
+}
+
 }  // namespace autoware::motion::control::autonomous_emergency_braking
 
 #include <rclcpp_components/register_node_macro.hpp>
diff --git a/control/autoware_autonomous_emergency_braking/src/utils.cpp b/control/autoware_autonomous_emergency_braking/src/utils.cpp
index f3b5192855d84..1b9fad6af9860 100644
--- a/control/autoware_autonomous_emergency_braking/src/utils.cpp
+++ b/control/autoware_autonomous_emergency_braking/src/utils.cpp
@@ -49,6 +49,9 @@ PredictedObject transformObjectFrame(
 Polygon2d convertPolygonObjectToGeometryPolygon(
   const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape)
 {
+  if (obj_shape.footprint.points.empty()) {
+    return {};
+  }
   Polygon2d object_polygon;
   tf2::Transform tf_map2obj;
   fromMsg(current_pose, tf_map2obj);
diff --git a/control/autoware_autonomous_emergency_braking/test/test.cpp b/control/autoware_autonomous_emergency_braking/test/test.cpp
new file mode 100644
index 0000000000000..42054e718d09c
--- /dev/null
+++ b/control/autoware_autonomous_emergency_braking/test/test.cpp
@@ -0,0 +1,326 @@
+// Copyright 2024 TIER IV
+//
+// 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 "test.hpp"
+
+#include "autoware/autonomous_emergency_braking/node.hpp"
+#include "autoware/universe_utils/geometry/geometry.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/time.hpp>
+
+#include <autoware_perception_msgs/msg/detail/shape__struct.hpp>
+#include <geometry_msgs/msg/detail/pose__struct.hpp>
+
+#include <gtest/gtest.h>
+#include <pcl/memory.h>
+#include <tf2/LinearMath/Transform.h>
+
+#include <memory>
+#include <thread>
+
+namespace autoware::motion::control::autonomous_emergency_braking::test
+{
+using autoware::universe_utils::Polygon2d;
+using autoware_perception_msgs::msg::PredictedObject;
+using autoware_perception_msgs::msg::PredictedObjects;
+using geometry_msgs::msg::Point;
+using geometry_msgs::msg::Pose;
+using geometry_msgs::msg::TransformStamped;
+using geometry_msgs::msg::Vector3;
+using std_msgs::msg::Header;
+
+Header get_header(const char * const frame_id, rclcpp::Time t)
+{
+  std_msgs::msg::Header header;
+  header.stamp = t;
+  header.frame_id = frame_id;
+  return header;
+};
+
+Imu make_imu_message(
+  const Header & header, const double ax, const double ay, const double yaw,
+  const double angular_velocity_z)
+{
+  Imu imu_msg;
+  imu_msg.header = header;
+  imu_msg.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw);
+  imu_msg.angular_velocity.z = angular_velocity_z;
+  imu_msg.linear_acceleration.x = ax;
+  imu_msg.linear_acceleration.y = ay;
+  return imu_msg;
+};
+
+VelocityReport make_velocity_report_msg(
+  const Header & header, const double lat_velocity, const double long_velocity,
+  const double heading_rate)
+{
+  VelocityReport velocity_msg;
+  velocity_msg.header = header;
+  velocity_msg.lateral_velocity = lat_velocity;
+  velocity_msg.longitudinal_velocity = long_velocity;
+  velocity_msg.heading_rate = heading_rate;
+  return velocity_msg;
+}
+
+std::shared_ptr<AEB> generateNode()
+{
+  auto node_options = rclcpp::NodeOptions{};
+
+  const auto aeb_dir =
+    ament_index_cpp::get_package_share_directory("autoware_autonomous_emergency_braking");
+  const auto vehicle_info_util_dir =
+    ament_index_cpp::get_package_share_directory("autoware_vehicle_info_utils");
+
+  node_options.arguments(
+    {"--ros-args", "--params-file", aeb_dir + "/config/autonomous_emergency_braking.param.yaml",
+     "--ros-args", "--params-file", vehicle_info_util_dir + "/config/vehicle_info.param.yaml"});
+  return std::make_shared<AEB>(node_options);
+};
+
+std::shared_ptr<PubSubNode> generatePubSubNode()
+{
+  auto node_options = rclcpp::NodeOptions{};
+  node_options.arguments({"--ros-args"});
+  return std::make_shared<PubSubNode>(node_options);
+};
+
+PubSubNode::PubSubNode(const rclcpp::NodeOptions & node_options)
+: Node("test_aeb_pubsub", node_options)
+{
+  rclcpp::QoS qos{1};
+  qos.transient_local();
+
+  pub_imu_ = create_publisher<Imu>("~/input/imu", qos);
+  pub_point_cloud_ = create_publisher<PointCloud2>("~/input/pointcloud", qos);
+  pub_velocity_ = create_publisher<VelocityReport>("~/input/velocity", qos);
+  pub_predicted_traj_ = create_publisher<Trajectory>("~/input/predicted_trajectory", qos);
+  pub_predicted_objects_ = create_publisher<PredictedObjects>("~/input/objects", qos);
+  pub_autoware_state_ = create_publisher<AutowareState>("autoware/state", qos);
+}
+
+TEST_F(TestAEB, checkCollision)
+{
+  constexpr double longitudinal_velocity = 3.0;
+  ObjectData object_collision;
+  object_collision.distance_to_object = 0.5;
+  object_collision.velocity = 0.1;
+  ASSERT_TRUE(aeb_node_->hasCollision(longitudinal_velocity, object_collision));
+
+  ObjectData object_no_collision;
+  object_no_collision.distance_to_object = 10.0;
+  object_no_collision.velocity = 0.1;
+  ASSERT_FALSE(aeb_node_->hasCollision(longitudinal_velocity, object_no_collision));
+}
+
+TEST_F(TestAEB, checkImuPathGeneration)
+{
+  constexpr double longitudinal_velocity = 3.0;
+  constexpr double yaw_rate = 0.05;
+  const auto imu_path = aeb_node_->generateEgoPath(longitudinal_velocity, yaw_rate);
+  ASSERT_FALSE(imu_path.empty());
+
+  const double dt = aeb_node_->imu_prediction_time_interval_;
+  const double horizon = aeb_node_->imu_prediction_time_horizon_;
+  ASSERT_TRUE(imu_path.size() >= static_cast<size_t>(horizon / dt));
+
+  const auto footprint = aeb_node_->generatePathFootprint(imu_path, 0.0);
+  ASSERT_FALSE(footprint.empty());
+  ASSERT_TRUE(footprint.size() == imu_path.size() - 1);
+
+  const auto stamp = rclcpp::Time();
+  pcl::PointCloud<pcl::PointXYZ>::Ptr obstacle_points_ptr =
+    pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+  {
+    const double x_start{0.0};
+    const double y_start{0.0};
+
+    for (size_t i = 0; i < 15; ++i) {
+      pcl::PointXYZ p1(
+        x_start + static_cast<double>(i / 100.0), y_start - static_cast<double>(i / 100.0), 0.5);
+      pcl::PointXYZ p2(
+        x_start + static_cast<double>((i + 10) / 100.0), y_start - static_cast<double>(i / 100.0),
+        0.5);
+      obstacle_points_ptr->push_back(p1);
+      obstacle_points_ptr->push_back(p2);
+    }
+  }
+  std::vector<ObjectData> objects;
+  aeb_node_->createObjectDataUsingPointCloudClusters(
+    imu_path, footprint, stamp, objects, obstacle_points_ptr);
+  ASSERT_FALSE(objects.empty());
+}
+
+TEST_F(TestAEB, checkIncompleteImuPathGeneration)
+{
+  const double dt = aeb_node_->imu_prediction_time_interval_;
+  const double horizon = aeb_node_->imu_prediction_time_horizon_;
+  const double min_generated_path_length = aeb_node_->min_generated_path_length_;
+  const double slow_velocity = min_generated_path_length / (2.0 * horizon);
+  constexpr double yaw_rate = 0.05;
+  const auto imu_path = aeb_node_->generateEgoPath(slow_velocity, yaw_rate);
+
+  ASSERT_FALSE(imu_path.empty());
+  ASSERT_TRUE(imu_path.size() >= static_cast<size_t>(horizon / dt));
+  ASSERT_TRUE(autoware::motion_utils::calcArcLength(imu_path) >= min_generated_path_length);
+
+  const auto footprint = aeb_node_->generatePathFootprint(imu_path, 0.0);
+  ASSERT_FALSE(footprint.empty());
+  ASSERT_TRUE(footprint.size() == imu_path.size() - 1);
+}
+
+TEST_F(TestAEB, checkEmptyPathAtZeroSpeed)
+{
+  const double velocity = 0.0;
+  constexpr double yaw_rate = 0.0;
+  const auto imu_path = aeb_node_->generateEgoPath(velocity, yaw_rate);
+  ASSERT_EQ(imu_path.size(), 1);
+}
+
+TEST_F(TestAEB, checkParamUpdate)
+{
+  std::vector<rclcpp::Parameter> parameters{rclcpp::Parameter("param")};
+  const auto result = aeb_node_->onParameter(parameters);
+  ASSERT_TRUE(result.successful);
+}
+
+TEST_F(TestAEB, checkEmptyFetchData)
+{
+  ASSERT_FALSE(aeb_node_->fetchLatestData());
+}
+
+TEST_F(TestAEB, checkConvertObjectToPolygon)
+{
+  using autoware_perception_msgs::msg::Shape;
+  PredictedObject obj_cylinder;
+  obj_cylinder.shape.type = Shape::CYLINDER;
+  obj_cylinder.shape.dimensions.x = 1.0;
+  Pose obj_cylinder_pose;
+  obj_cylinder_pose.position.x = 1.0;
+  obj_cylinder_pose.position.y = 1.0;
+  obj_cylinder.kinematics.initial_pose_with_covariance.pose = obj_cylinder_pose;
+  const auto cylinder_polygon = utils::convertObjToPolygon(obj_cylinder);
+  ASSERT_FALSE(cylinder_polygon.outer().empty());
+
+  PredictedObject obj_box;
+  obj_box.shape.type = Shape::BOUNDING_BOX;
+  obj_box.shape.dimensions.x = 1.0;
+  obj_box.shape.dimensions.y = 2.0;
+  Pose obj_box_pose;
+  obj_box_pose.position.x = 1.0;
+  obj_box_pose.position.y = 1.0;
+  obj_box.kinematics.initial_pose_with_covariance.pose = obj_box_pose;
+  const auto box_polygon = utils::convertObjToPolygon(obj_box);
+  ASSERT_FALSE(box_polygon.outer().empty());
+
+  geometry_msgs::msg::TransformStamped tf_stamped;
+  geometry_msgs::msg::Transform transform;
+
+  constexpr double yaw{0.0};
+  transform.rotation = autoware::universe_utils::createQuaternionFromYaw(yaw);
+  geometry_msgs::msg::Vector3 translation;
+  translation.x = 1.0;
+  translation.y = 0.0;
+  translation.z = 0.0;
+  transform.translation = translation;
+  tf_stamped.set__transform(transform);
+  const auto t_obj_box = utils::transformObjectFrame(obj_box, tf_stamped);
+  const auto t_pose = t_obj_box.kinematics.initial_pose_with_covariance.pose;
+  Pose expected_pose;
+  expected_pose.position.x = obj_box_pose.position.x + translation.x;
+  expected_pose.position.y = obj_box_pose.position.y + translation.y;
+  expected_pose.position.z = obj_box_pose.position.z + translation.z;
+
+  ASSERT_DOUBLE_EQ(expected_pose.position.x, t_pose.position.x);
+  ASSERT_DOUBLE_EQ(expected_pose.position.y, t_pose.position.y);
+  ASSERT_DOUBLE_EQ(expected_pose.position.z, t_pose.position.z);
+}
+
+TEST_F(TestAEB, CollisionDataKeeper)
+{
+  using namespace std::literals::chrono_literals;
+  constexpr double collision_keeping_sec{1.0}, previous_obstacle_keep_time{1.0};
+  CollisionDataKeeper collision_data_keeper_(aeb_node_->get_clock());
+  collision_data_keeper_.setTimeout(collision_keeping_sec, previous_obstacle_keep_time);
+  ASSERT_TRUE(collision_data_keeper_.checkCollisionExpired());
+  ASSERT_TRUE(collision_data_keeper_.checkPreviousObjectDataExpired());
+
+  ObjectData obj;
+  obj.stamp = aeb_node_->now();
+  obj.velocity = 0.0;
+  obj.position.x = 0.0;
+  rclcpp::sleep_for(100ms);
+
+  ObjectData obj2;
+  obj2.stamp = aeb_node_->now();
+  obj2.velocity = 0.0;
+  obj2.position.x = 0.1;
+  rclcpp::sleep_for(100ms);
+
+  constexpr double ego_longitudinal_velocity = 3.0;
+  constexpr double yaw_rate = 0.0;
+  const auto imu_path = aeb_node_->generateEgoPath(ego_longitudinal_velocity, yaw_rate);
+
+  const auto speed_null =
+    collision_data_keeper_.calcObjectSpeedFromHistory(obj, imu_path, ego_longitudinal_velocity);
+  ASSERT_FALSE(speed_null.has_value());
+
+  const auto median_velocity =
+    collision_data_keeper_.calcObjectSpeedFromHistory(obj2, imu_path, ego_longitudinal_velocity);
+  ASSERT_TRUE(median_velocity.has_value());
+
+  // object speed is 1.0 m/s greater than ego's = 0.1 [m] / 0.1 [s] + longitudinal_velocity
+  ASSERT_TRUE(std::abs(median_velocity.value() - 4.0) < 1e-2);
+  rclcpp::sleep_for(1100ms);
+  ASSERT_TRUE(collision_data_keeper_.checkCollisionExpired());
+}
+
+TEST_F(TestAEB, TestCropPointCloud)
+{
+  constexpr double longitudinal_velocity = 3.0;
+  constexpr double yaw_rate = 0.05;
+  const auto imu_path = aeb_node_->generateEgoPath(longitudinal_velocity, yaw_rate);
+  ASSERT_FALSE(imu_path.empty());
+
+  constexpr size_t n_points{15};
+  // Create n_points inside the path and 1 point outside.
+  pcl::PointCloud<pcl::PointXYZ>::Ptr obstacle_points_ptr =
+    pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+  {
+    constexpr double x_start{0.0};
+    constexpr double y_start{0.0};
+
+    for (size_t i = 0; i < n_points; ++i) {
+      const double offset_1 = static_cast<double>(i / 100.0);
+      const double offset_2 = static_cast<double>((i + 10) / 100.0);
+      pcl::PointXYZ p1(x_start + offset_1, y_start - offset_1, 0.5);
+      pcl::PointXYZ p2(x_start + offset_2, y_start - offset_1, 0.5);
+      obstacle_points_ptr->push_back(p1);
+      obstacle_points_ptr->push_back(p2);
+    }
+    pcl::PointXYZ p_out(x_start + 100.0, y_start + 100, 0.5);
+    obstacle_points_ptr->push_back(p_out);
+  }
+  aeb_node_->obstacle_ros_pointcloud_ptr_ = std::make_shared<PointCloud2>();
+  pcl::toROSMsg(*obstacle_points_ptr, *aeb_node_->obstacle_ros_pointcloud_ptr_);
+  const auto footprint = aeb_node_->generatePathFootprint(imu_path, 0.0);
+
+  pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_objects =
+    pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+  aeb_node_->cropPointCloudWithEgoFootprintPath(footprint, filtered_objects);
+  // Check if the point outside the path was excluded
+  ASSERT_TRUE(filtered_objects->points.size() == 2 * n_points);
+}
+
+}  // namespace autoware::motion::control::autonomous_emergency_braking::test
diff --git a/control/autoware_autonomous_emergency_braking/test/test.hpp b/control/autoware_autonomous_emergency_braking/test/test.hpp
new file mode 100644
index 0000000000000..86edbf73a2a12
--- /dev/null
+++ b/control/autoware_autonomous_emergency_braking/test/test.hpp
@@ -0,0 +1,116 @@
+// Copyright 2024 TIER IV
+//
+// 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 TEST_HPP_
+#define TEST_HPP_
+
+#include "ament_index_cpp/get_package_share_directory.hpp"
+#include "autoware_test_utils/autoware_test_utils.hpp"
+#include "autoware_test_utils/mock_data_parser.hpp"
+#include "gtest/gtest.h"
+
+#include <autoware/autonomous_emergency_braking/node.hpp>
+#include <autoware/autonomous_emergency_braking/utils.hpp>
+#include <rclcpp/clock.hpp>
+#include <rclcpp/logging.hpp>
+#include <rclcpp/time.hpp>
+
+#include <memory>
+#include <string>
+#include <vector>
+namespace autoware::motion::control::autonomous_emergency_braking::test
+{
+using autoware_planning_msgs::msg::Trajectory;
+using autoware_system_msgs::msg::AutowareState;
+using autoware_vehicle_msgs::msg::VelocityReport;
+using nav_msgs::msg::Odometry;
+using sensor_msgs::msg::Imu;
+using sensor_msgs::msg::PointCloud2;
+using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
+using autoware::universe_utils::Polygon2d;
+using autoware::vehicle_info_utils::VehicleInfo;
+using diagnostic_updater::DiagnosticStatusWrapper;
+using diagnostic_updater::Updater;
+using visualization_msgs::msg::Marker;
+using visualization_msgs::msg::MarkerArray;
+using Path = std::vector<geometry_msgs::msg::Pose>;
+using Vector3 = geometry_msgs::msg::Vector3;
+using autoware_perception_msgs::msg::PredictedObject;
+using autoware_perception_msgs::msg::PredictedObjects;
+using std_msgs::msg::Header;
+
+std::shared_ptr<AEB> generateNode();
+Header get_header(const char * const frame_id, rclcpp::Time t);
+Imu make_imu_message(
+  const Header & header, const double ax, const double ay, const double yaw,
+  const double angular_velocity_z);
+VelocityReport make_velocity_report_msg(
+  const Header & header, const double lat_velocity, const double long_velocity,
+  const double heading_rate);
+class PubSubNode : public rclcpp::Node
+{
+public:
+  explicit PubSubNode(const rclcpp::NodeOptions & node_options);
+  // publisher
+  rclcpp::Publisher<Imu>::SharedPtr pub_imu_;
+  rclcpp::Publisher<PointCloud2>::SharedPtr pub_point_cloud_;
+  rclcpp::Publisher<VelocityReport>::SharedPtr pub_velocity_;
+  rclcpp::Publisher<Trajectory>::SharedPtr pub_predicted_traj_;
+  rclcpp::Publisher<PredictedObjects>::SharedPtr pub_predicted_objects_;
+  rclcpp::Publisher<AutowareState>::SharedPtr pub_autoware_state_;
+  // timer
+  // rclcpp::TimerBase::SharedPtr timer_;
+  void publishDefaultTopicsNoSpin()
+  {
+    const auto header = get_header("base_link", now());
+    const auto imu_msg = make_imu_message(header, 0.0, 0.0, 0.0, 0.05);
+    const auto velocity_msg = make_velocity_report_msg(header, 0.0, 3.0, 0.0);
+
+    pub_imu_->publish(imu_msg);
+    pub_velocity_->publish(velocity_msg);
+  };
+};
+
+std::shared_ptr<PubSubNode> generatePubSubNode();
+
+class TestAEB : public ::testing::Test
+{
+public:
+  TestAEB() {}
+  TestAEB(const TestAEB &) = delete;
+  TestAEB(TestAEB &&) = delete;
+  TestAEB & operator=(const TestAEB &) = delete;
+  TestAEB & operator=(TestAEB &&) = delete;
+  ~TestAEB() override = default;
+
+  void SetUp() override
+  {
+    rclcpp::init(0, nullptr);
+    pub_sub_node_ = generatePubSubNode();
+    aeb_node_ = generateNode();
+  }
+
+  void TearDown() override
+  {
+    aeb_node_.reset();
+    pub_sub_node_.reset();
+    rclcpp::shutdown();
+  }
+
+  std::shared_ptr<PubSubNode> pub_sub_node_;
+  std::shared_ptr<AEB> aeb_node_;
+};
+}  // namespace autoware::motion::control::autonomous_emergency_braking::test
+
+#endif  // TEST_HPP_
diff --git a/control/autoware_control_validator/CMakeLists.txt b/control/autoware_control_validator/CMakeLists.txt
index 2b25b5b984c12..785fb8472ca5a 100644
--- a/control/autoware_control_validator/CMakeLists.txt
+++ b/control/autoware_control_validator/CMakeLists.txt
@@ -36,8 +36,13 @@ else()
   target_link_libraries(autoware_control_validator_component "${cpp_typesupport_target}")
 endif()
 
-# if(BUILD_TESTING)
-# endif()
+if(BUILD_TESTING)
+  file(GLOB_RECURSE TEST_SOURCES test/*.cpp)
+  ament_add_gtest(test_autoware_control_validator
+    ${TEST_SOURCES}
+  )
+  target_link_libraries(test_autoware_control_validator autoware_control_validator_component)
+endif()
 
 ament_auto_package(
   INSTALL_TO_SHARE
diff --git a/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp b/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp
index 515f1c4cec339..56a1e39391b10 100644
--- a/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp
+++ b/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp
@@ -17,18 +17,21 @@
 
 #include "autoware/control_validator/debug_marker.hpp"
 #include "autoware/universe_utils/ros/polling_subscriber.hpp"
-#include "autoware_control_validator/msg/control_validator_status.hpp"
-#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp"
+#include "autoware_vehicle_info_utils/vehicle_info.hpp"
+#include "diagnostic_updater/diagnostic_updater.hpp"
 
-#include <diagnostic_updater/diagnostic_updater.hpp>
+#include <autoware_control_validator/msg/control_validator_status.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_planning_msgs/msg/trajectory.hpp>
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 
+#include <cstdint>
 #include <memory>
 #include <string>
+#include <tuple>
+#include <utility>
 
 namespace autoware::control_validator
 {
@@ -46,44 +49,103 @@ struct ValidationParams
   double max_over_velocity_ratio_threshold;
 };
 
+/**
+ * @class ControlValidator
+ * @brief Validates control commands by comparing predicted trajectories against reference
+ * trajectories.
+ */
 class ControlValidator : public rclcpp::Node
 {
 public:
+  /**
+   * @brief Constructor
+   * @param options Node options
+   */
   explicit ControlValidator(const rclcpp::NodeOptions & options);
 
-  void onPredictedTrajectory(const Trajectory::ConstSharedPtr msg);
-
-  bool checkValidMaxDistanceDeviation(const Trajectory & predicted_trajectory);
-  bool checkValidVelocityDeviation(
-    const Trajectory & reference_trajectory, const Odometry & kinematics);
+  /**
+   * @brief Callback function for the predicted trajectory.
+   * @param msg Predicted trajectory message
+   */
+  void on_predicted_trajectory(const Trajectory::ConstSharedPtr msg);
+
+  /**
+   * @brief Calculate the maximum lateral distance between the reference trajectory and predicted
+   * trajectory.
+   * @param predicted_trajectory Predicted trajectory
+   * @param reference_trajectory Reference trajectory
+   * @return A pair consisting of the maximum lateral deviation and a boolean indicating validity
+   */
+  std::pair<double, bool> calc_lateral_deviation_status(
+    const Trajectory & predicted_trajectory, const Trajectory & reference_trajectory) const;
+
+  /**
+   * @brief Calculate the velocity deviation between the reference trajectory and the current
+   * vehicle kinematics.
+   * @param reference_trajectory Reference trajectory
+   * @param kinematics Current vehicle kinematics
+   * @return A tuple containing the current velocity, desired velocity, and a boolean indicating
+   * validity
+   */
+  std::tuple<double, double, bool> calc_velocity_deviation_status(
+    const Trajectory & reference_trajectory, const Odometry & kinematics) const;
 
 private:
-  void setupDiag();
-
-  void setupParameters();
-
-  bool isDataReady();
-
+  /**
+   * @brief Setup diagnostic updater
+   */
+  void setup_diag();
+
+  /**
+   * @brief Setup parameters from the parameter server
+   */
+  void setup_parameters();
+
+  /**
+   * @brief Check if all required data is ready for validation
+   * @return Boolean indicating readiness of data
+   */
+  bool is_data_ready();
+
+  /**
+   * @brief Validate the predicted trajectory against the reference trajectory and current
+   * kinematics
+   * @param predicted_trajectory Predicted trajectory
+   * @param reference_trajectory Reference trajectory
+   * @param kinematics Current vehicle kinematics
+   */
   void validate(
     const Trajectory & predicted_trajectory, const Trajectory & reference_trajectory,
     const Odometry & kinematics);
 
-  void publishPredictedTrajectory();
-  void publishDebugInfo();
-  void displayStatus();
-
-  void setStatus(DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg);
-
+  /**
+   * @brief Publish debug information
+   */
+  void publish_debug_info();
+
+  /**
+   * @brief Display validation status on terminal
+   */
+  void display_status();
+
+  /**
+   * @brief Set the diagnostic status
+   * @param stat Diagnostic status wrapper
+   * @param is_ok Boolean indicating if the status is okay
+   * @param msg Status message
+   */
+  void set_status(
+    DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg) const;
+
+  // Subscribers and publishers
   rclcpp::Subscription<Trajectory>::SharedPtr sub_predicted_traj_;
-  autoware::universe_utils::InterProcessPollingSubscriber<Odometry> sub_kinematics_{
-    this, "~/input/kinematics"};
-  autoware::universe_utils::InterProcessPollingSubscriber<Trajectory> sub_reference_traj_{
-    this, "~/input/reference_trajectory"};
+  universe_utils::InterProcessPollingSubscriber<Odometry>::SharedPtr sub_kinematics_;
+  universe_utils::InterProcessPollingSubscriber<Trajectory>::SharedPtr sub_reference_traj_;
   rclcpp::Publisher<ControlValidatorStatus>::SharedPtr pub_status_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_markers_;
 
   // system parameters
-  int diag_error_count_threshold_ = 0;
+  int64_t diag_error_count_threshold_ = 0;
   bool display_on_terminal_ = true;
 
   Updater diag_updater_{this};
@@ -91,13 +153,14 @@ class ControlValidator : public rclcpp::Node
   ControlValidatorStatus validation_status_;
   ValidationParams validation_params_;  // for thresholds
 
-  // ego nearest index search
-  double ego_nearest_dist_threshold_;
-  double ego_nearest_yaw_threshold_;
-
-  autoware::vehicle_info_utils::VehicleInfo vehicle_info_;
+  vehicle_info_utils::VehicleInfo vehicle_info_;
 
-  bool isAllValid(const ControlValidatorStatus & status);
+  /**
+   * @brief Check if all validation criteria are met
+   * @param status Validation status
+   * @return Boolean indicating if all criteria are met
+   */
+  static bool is_all_valid(const ControlValidatorStatus & status);
 
   Trajectory::ConstSharedPtr current_reference_trajectory_;
   Trajectory::ConstSharedPtr current_predicted_trajectory_;
diff --git a/control/autoware_control_validator/include/autoware/control_validator/debug_marker.hpp b/control/autoware_control_validator/include/autoware/control_validator/debug_marker.hpp
index c03ecb46b8117..4c084ecbcc3e8 100644
--- a/control/autoware_control_validator/include/autoware/control_validator/debug_marker.hpp
+++ b/control/autoware_control_validator/include/autoware/control_validator/debug_marker.hpp
@@ -22,23 +22,41 @@
 #include <visualization_msgs/msg/marker_array.hpp>
 
 #include <map>
-#include <memory>
 #include <string>
-#include <vector>
 
+/**
+ * @brief Class for publishing debug markers
+ */
 class ControlValidatorDebugMarkerPublisher
 {
 public:
+  /**
+   * @brief Constructor
+   */
   explicit ControlValidatorDebugMarkerPublisher(rclcpp::Node * node);
 
-  void pushPoseMarker(
-    const autoware_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id = 0);
-  void pushPoseMarker(const geometry_msgs::msg::Pose & pose, const std::string & ns, int id = 0);
-  void pushVirtualWall(const geometry_msgs::msg::Pose & pose);
-  void pushWarningMsg(const geometry_msgs::msg::Pose & pose, const std::string & msg);
+  /**
+   * @brief Push a virtual wall
+   * @param pose pose of the virtual wall
+   */
+  void push_virtual_wall(const geometry_msgs::msg::Pose & pose);
+
+  /**
+   * @brief Push a warning message
+   * @param pose pose of the warning message
+   * @param msg warning message
+   */
+  void push_warning_msg(const geometry_msgs::msg::Pose & pose, const std::string & msg);
+
+  /**
+   * @brief Publish markers
+   */
   void publish();
 
-  void clearMarkers();
+  /**
+   * @brief Clear markers
+   */
+  void clear_markers();
 
 private:
   rclcpp::Node * node_;
@@ -47,14 +65,6 @@ class ControlValidatorDebugMarkerPublisher
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_viz_pub_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr virtual_wall_pub_;
   std::map<std::string, int> marker_id_;
-
-  int getMarkerId(const std::string & ns)
-  {
-    if (marker_id_.count(ns) == 0) {
-      marker_id_[ns] = 0.0;
-    }
-    return marker_id_[ns]++;
-  }
 };
 
 #endif  // AUTOWARE__CONTROL_VALIDATOR__DEBUG_MARKER_HPP_
diff --git a/control/autoware_control_validator/include/autoware/control_validator/utils.hpp b/control/autoware_control_validator/include/autoware/control_validator/utils.hpp
index 375ad557d02df..85ee1f44bd457 100644
--- a/control/autoware_control_validator/include/autoware/control_validator/utils.hpp
+++ b/control/autoware_control_validator/include/autoware/control_validator/utils.hpp
@@ -15,41 +15,26 @@
 #ifndef AUTOWARE__CONTROL_VALIDATOR__UTILS_HPP_
 #define AUTOWARE__CONTROL_VALIDATOR__UTILS_HPP_
 
-#include <autoware/motion_utils/trajectory/conversion.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_planning_msgs/msg/trajectory.hpp>
 
-#include <string>
-#include <utility>
-#include <vector>
-
 namespace autoware::control_validator
 {
-using autoware::motion_utils::convertToTrajectory;
-using autoware::motion_utils::convertToTrajectoryPointArray;
-using autoware_planning_msgs::msg::Trajectory;
-using autoware_planning_msgs::msg::TrajectoryPoint;
-using geometry_msgs::msg::Pose;
-using TrajectoryPoints = std::vector<TrajectoryPoint>;
-
-void shiftPose(Pose & pose, double longitudinal);
-
-void insertPointInPredictedTrajectory(
-  TrajectoryPoints & modified_trajectory, const geometry_msgs::msg::Pose & reference_pose,
-  const TrajectoryPoints & predicted_trajectory);
-
-TrajectoryPoints reverseTrajectoryPoints(const TrajectoryPoints & trajectory);
-
-bool removeFrontTrajectoryPoint(
-  const TrajectoryPoints & trajectory_points, TrajectoryPoints & modified_trajectory_points,
-  const TrajectoryPoints & predicted_trajectory_points);
-
-Trajectory alignTrajectoryWithReferenceTrajectory(
-  const Trajectory & trajectory, const Trajectory & predicted_trajectory);
-
-double calcMaxLateralDistance(
-  const Trajectory & trajectory, const Trajectory & predicted_trajectory);
+/**
+ * @brief Shift pose along the yaw direction
+ */
+void shift_pose(geometry_msgs::msg::Pose & pose, double longitudinal);
+
+/**
+ * @brief Calculate the maximum lateral distance between the reference trajectory and the predicted
+ * trajectory
+ * @param reference_trajectory reference trajectory
+ * @param predicted_trajectory predicted trajectory
+ */
+double calc_max_lateral_distance(
+  const autoware_planning_msgs::msg::Trajectory & reference_trajectory,
+  const autoware_planning_msgs::msg::Trajectory & predicted_trajectory);
 }  // namespace autoware::control_validator
 
 #endif  // AUTOWARE__CONTROL_VALIDATOR__UTILS_HPP_
diff --git a/control/autoware_control_validator/package.xml b/control/autoware_control_validator/package.xml
index 71fefafbaffb1..b48ebef70bc58 100644
--- a/control/autoware_control_validator/package.xml
+++ b/control/autoware_control_validator/package.xml
@@ -34,6 +34,7 @@
   <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
+  <test_depend>autoware_test_utils</test_depend>
 
   <exec_depend>rosidl_default_runtime</exec_depend>
   <member_of_group>rosidl_interface_packages</member_of_group>
diff --git a/control/autoware_control_validator/src/control_validator.cpp b/control/autoware_control_validator/src/control_validator.cpp
index e5696e3c87afb..2370d274fecc8 100644
--- a/control/autoware_control_validator/src/control_validator.cpp
+++ b/control/autoware_control_validator/src/control_validator.cpp
@@ -16,37 +16,47 @@
 
 #include "autoware/control_validator/utils.hpp"
 #include "autoware/motion_utils/trajectory/interpolation.hpp"
-#include "autoware/motion_utils/trajectory/trajectory.hpp"
+#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp"
 
+#include <nav_msgs/msg/odometry.hpp>
+
+#include <cstdint>
 #include <memory>
 #include <string>
-#include <utility>
 
 namespace autoware::control_validator
 {
 using diagnostic_msgs::msg::DiagnosticStatus;
 
 ControlValidator::ControlValidator(const rclcpp::NodeOptions & options)
-: Node("control_validator", options)
+: Node("control_validator", options), validation_params_(), vehicle_info_()
 {
   using std::placeholders::_1;
+
   sub_predicted_traj_ = create_subscription<Trajectory>(
     "~/input/predicted_trajectory", 1,
-    std::bind(&ControlValidator::onPredictedTrajectory, this, _1));
+    std::bind(&ControlValidator::on_predicted_trajectory, this, _1));
+  sub_kinematics_ =
+    universe_utils::InterProcessPollingSubscriber<nav_msgs::msg::Odometry>::create_subscription(
+      this, "~/input/kinematics", 1);
+  sub_reference_traj_ =
+    autoware::universe_utils::InterProcessPollingSubscriber<Trajectory>::create_subscription(
+      this, "~/input/reference_trajectory", 1);
 
   pub_status_ = create_publisher<ControlValidatorStatus>("~/output/validation_status", 1);
+
   pub_markers_ = create_publisher<visualization_msgs::msg::MarkerArray>("~/output/markers", 1);
 
   debug_pose_publisher_ = std::make_shared<ControlValidatorDebugMarkerPublisher>(this);
 
-  setupParameters();
+  setup_parameters();
 
-  setupDiag();
+  setup_diag();
 }
 
-void ControlValidator::setupParameters()
+void ControlValidator::setup_parameters()
 {
-  diag_error_count_threshold_ = declare_parameter<int>("diag_error_count_threshold");
+  diag_error_count_threshold_ = declare_parameter<int64_t>("diag_error_count_threshold");
   display_on_terminal_ = declare_parameter<bool>("display_on_terminal");
 
   {
@@ -60,14 +70,18 @@ void ControlValidator::setupParameters()
   try {
     vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo();
   } catch (...) {
-    RCLCPP_ERROR(get_logger(), "failed to get vehicle info. use default value.");
     vehicle_info_.front_overhang_m = 0.5;
     vehicle_info_.wheel_base_m = 4.0;
+    RCLCPP_ERROR(
+      get_logger(),
+      "failed to get vehicle info. use default value. vehicle_info_.front_overhang_m: %.2f, "
+      "vehicle_info_.wheel_base_m: %.2f",
+      vehicle_info_.front_overhang_m, vehicle_info_.wheel_base_m);
   }
 }
 
-void ControlValidator::setStatus(
-  DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg)
+void ControlValidator::set_status(
+  DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg) const
 {
   if (is_ok) {
     stat.summary(DiagnosticStatus::OK, "validated.");
@@ -81,72 +95,71 @@ void ControlValidator::setStatus(
   }
 }
 
-void ControlValidator::setupDiag()
+void ControlValidator::setup_diag()
 {
   auto & d = diag_updater_;
   d.setHardwareID("control_validator");
 
   std::string ns = "control_validation_";
   d.add(ns + "max_distance_deviation", [&](auto & stat) {
-    setStatus(
+    set_status(
       stat, validation_status_.is_valid_max_distance_deviation,
       "control output is deviated from trajectory");
   });
   d.add(ns + "velocity_deviation", [&](auto & stat) {
-    setStatus(
+    set_status(
       stat, validation_status_.is_valid_velocity_deviation,
       "current velocity is deviated from the desired velocity");
   });
 }
 
-bool ControlValidator::isDataReady()
+bool ControlValidator::is_data_ready()
 {
-  const auto waiting = [this](const auto s) {
-    RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for %s", s);
+  const auto waiting = [this](const auto topic_name) {
+    RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for %s", topic_name);
     return false;
   };
 
   if (!current_kinematics_) {
-    return waiting("current_kinematics_");
+    return waiting(sub_kinematics_->subscriber()->get_topic_name());
   }
   if (!current_reference_trajectory_) {
-    return waiting("current_reference_trajectory_");
+    return waiting(sub_reference_traj_->subscriber()->get_topic_name());
   }
   if (!current_predicted_trajectory_) {
-    return waiting("current_predicted_trajectory_");
+    return waiting(sub_predicted_traj_->get_topic_name());
   }
   return true;
 }
 
-void ControlValidator::onPredictedTrajectory(const Trajectory::ConstSharedPtr msg)
+void ControlValidator::on_predicted_trajectory(const Trajectory::ConstSharedPtr msg)
 {
   current_predicted_trajectory_ = msg;
-  current_reference_trajectory_ = sub_reference_traj_.takeData();
-  current_kinematics_ = sub_kinematics_.takeData();
+  current_reference_trajectory_ = sub_reference_traj_->takeData();
+  current_kinematics_ = sub_kinematics_->takeData();
 
-  if (!isDataReady()) return;
+  if (!is_data_ready()) return;
 
-  debug_pose_publisher_->clearMarkers();
+  debug_pose_publisher_->clear_markers();
 
   validate(*current_predicted_trajectory_, *current_reference_trajectory_, *current_kinematics_);
 
   diag_updater_.force_update();
 
   // for debug
-  publishDebugInfo();
-  displayStatus();
+  publish_debug_info();
+  display_status();
 }
 
-void ControlValidator::publishDebugInfo()
+void ControlValidator::publish_debug_info()
 {
-  validation_status_.stamp = get_clock()->now();
   pub_status_->publish(validation_status_);
 
-  if (!isAllValid(validation_status_)) {
+  if (!is_all_valid(validation_status_)) {
     geometry_msgs::msg::Pose front_pose = current_kinematics_->pose.pose;
-    shiftPose(front_pose, vehicle_info_.front_overhang_m + vehicle_info_.wheel_base_m);
-    debug_pose_publisher_->pushVirtualWall(front_pose);
-    debug_pose_publisher_->pushWarningMsg(front_pose, "INVALID CONTROL");
+    shift_pose(front_pose, vehicle_info_.front_overhang_m + vehicle_info_.wheel_base_m);
+    debug_pose_publisher_->push_virtual_wall(front_pose);
+    debug_pose_publisher_->push_warning_msg(front_pose, "INVALID CONTROL");
   }
   debug_pose_publisher_->publish();
 }
@@ -161,39 +174,46 @@ void ControlValidator::validate(
       "predicted_trajectory size is less than 2. Cannot validate.");
     return;
   }
+  if (reference_trajectory.points.size() < 2) {
+    RCLCPP_ERROR_THROTTLE(
+      get_logger(), *get_clock(), 1000,
+      "reference_trajectory size is less than 2. Cannot validate.");
+    return;
+  }
+
+  validation_status_.stamp = get_clock()->now();
 
-  auto & s = validation_status_;
+  std::tie(
+    validation_status_.max_distance_deviation, validation_status_.is_valid_max_distance_deviation) =
+    calc_lateral_deviation_status(predicted_trajectory, *current_reference_trajectory_);
 
-  s.is_valid_max_distance_deviation = checkValidMaxDistanceDeviation(predicted_trajectory);
-  s.is_valid_velocity_deviation = checkValidVelocityDeviation(reference_trajectory, kinematics);
+  std::tie(
+    validation_status_.current_velocity, validation_status_.desired_velocity,
+    validation_status_.is_valid_velocity_deviation) =
+    calc_velocity_deviation_status(*current_reference_trajectory_, kinematics);
 
-  s.invalid_count = isAllValid(s) ? 0 : s.invalid_count + 1;
+  validation_status_.invalid_count =
+    is_all_valid(validation_status_) ? 0 : validation_status_.invalid_count + 1;
 }
 
-bool ControlValidator::checkValidMaxDistanceDeviation(const Trajectory & predicted_trajectory)
+std::pair<double, bool> ControlValidator::calc_lateral_deviation_status(
+  const Trajectory & predicted_trajectory, const Trajectory & reference_trajectory) const
 {
-  validation_status_.max_distance_deviation =
-    calcMaxLateralDistance(*current_reference_trajectory_, predicted_trajectory);
-  if (
-    validation_status_.max_distance_deviation >
-    validation_params_.max_distance_deviation_threshold) {
-    return false;
-  }
-  return true;
+  auto max_distance_deviation =
+    calc_max_lateral_distance(reference_trajectory, predicted_trajectory);
+  return {
+    max_distance_deviation,
+    max_distance_deviation <= validation_params_.max_distance_deviation_threshold};
 }
 
-bool ControlValidator::checkValidVelocityDeviation(
-  const Trajectory & reference_trajectory, const Odometry & kinematics)
+std::tuple<double, double, bool> ControlValidator::calc_velocity_deviation_status(
+  const Trajectory & reference_trajectory, const Odometry & kinematics) const
 {
   const double current_vel = kinematics.twist.twist.linear.x;
-  if (reference_trajectory.points.size() < 2) return true;
   const double desired_vel =
     autoware::motion_utils::calcInterpolatedPoint(reference_trajectory, kinematics.pose.pose)
       .longitudinal_velocity_mps;
 
-  validation_status_.current_velocity = current_vel;
-  validation_status_.desired_velocity = desired_vel;
-
   const bool is_over_velocity =
     std::abs(current_vel) >
     std::abs(desired_vel) * (1.0 + validation_params_.max_over_velocity_ratio_threshold) +
@@ -202,15 +222,15 @@ bool ControlValidator::checkValidVelocityDeviation(
     std::signbit(current_vel * desired_vel) &&
     std::abs(current_vel) > validation_params_.max_reverse_velocity_threshold;
 
-  return !(is_over_velocity || is_reverse_velocity);
+  return {current_vel, desired_vel, !(is_over_velocity || is_reverse_velocity)};
 }
 
-bool ControlValidator::isAllValid(const ControlValidatorStatus & s)
+bool ControlValidator::is_all_valid(const ControlValidatorStatus & s)
 {
   return s.is_valid_max_distance_deviation && s.is_valid_velocity_deviation;
 }
 
-void ControlValidator::displayStatus()
+void ControlValidator::display_status()
 {
   if (!display_on_terminal_) return;
   rclcpp::Clock clock{RCL_ROS_TIME};
diff --git a/control/autoware_control_validator/src/debug_marker.cpp b/control/autoware_control_validator/src/debug_marker.cpp
index d5d8644515ab1..dfd073884ce89 100644
--- a/control/autoware_control_validator/src/debug_marker.cpp
+++ b/control/autoware_control_validator/src/debug_marker.cpp
@@ -17,9 +17,7 @@
 #include <autoware/motion_utils/marker/marker_helper.hpp>
 #include <autoware/universe_utils/ros/marker_helper.hpp>
 
-#include <memory>
 #include <string>
-#include <vector>
 
 using visualization_msgs::msg::Marker;
 
@@ -33,47 +31,13 @@ ControlValidatorDebugMarkerPublisher::ControlValidatorDebugMarkerPublisher(rclcp
     node_->create_publisher<visualization_msgs::msg::MarkerArray>("~/virtual_wall", 1);
 }
 
-void ControlValidatorDebugMarkerPublisher::clearMarkers()
+void ControlValidatorDebugMarkerPublisher::clear_markers()
 {
   marker_array_.markers.clear();
   marker_array_virtual_wall_.markers.clear();
 }
 
-void ControlValidatorDebugMarkerPublisher::pushPoseMarker(
-  const autoware_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id)
-{
-  pushPoseMarker(p.pose, ns, id);
-}
-
-void ControlValidatorDebugMarkerPublisher::pushPoseMarker(
-  const geometry_msgs::msg::Pose & pose, const std::string & ns, int id)
-{
-  using autoware::universe_utils::createMarkerColor;
-
-  // append arrow marker
-  std_msgs::msg::ColorRGBA color;
-  if (id == 0)  // Red
-  {
-    color = createMarkerColor(1.0, 0.0, 0.0, 0.999);
-  }
-  if (id == 1)  // Green
-  {
-    color = createMarkerColor(0.0, 1.0, 0.0, 0.999);
-  }
-  if (id == 2)  // Blue
-  {
-    color = createMarkerColor(0.0, 0.0, 1.0, 0.999);
-  }
-  Marker marker = autoware::universe_utils::createDefaultMarker(
-    "map", node_->get_clock()->now(), ns, getMarkerId(ns), Marker::ARROW,
-    autoware::universe_utils::createMarkerScale(0.2, 0.1, 0.3), color);
-  marker.lifetime = rclcpp::Duration::from_seconds(0.2);
-  marker.pose = pose;
-
-  marker_array_.markers.push_back(marker);
-}
-
-void ControlValidatorDebugMarkerPublisher::pushWarningMsg(
+void ControlValidatorDebugMarkerPublisher::push_warning_msg(
   const geometry_msgs::msg::Pose & pose, const std::string & msg)
 {
   visualization_msgs::msg::Marker marker = autoware::universe_utils::createDefaultMarker(
@@ -86,7 +50,7 @@ void ControlValidatorDebugMarkerPublisher::pushWarningMsg(
   marker_array_virtual_wall_.markers.push_back(marker);
 }
 
-void ControlValidatorDebugMarkerPublisher::pushVirtualWall(const geometry_msgs::msg::Pose & pose)
+void ControlValidatorDebugMarkerPublisher::push_virtual_wall(const geometry_msgs::msg::Pose & pose)
 {
   const auto now = node_->get_clock()->now();
   const auto stop_wall_marker =
diff --git a/control/autoware_control_validator/src/utils.cpp b/control/autoware_control_validator/src/utils.cpp
index 821d64c3af6e6..12f4ea70f2ca1 100644
--- a/control/autoware_control_validator/src/utils.cpp
+++ b/control/autoware_control_validator/src/utils.cpp
@@ -14,26 +14,37 @@
 
 #include "autoware/control_validator/utils.hpp"
 
-#include <autoware/motion_utils/trajectory/interpolation.hpp>
-#include <autoware/motion_utils/trajectory/trajectory.hpp>
-#include <autoware/universe_utils/geometry/geometry.hpp>
+#include "autoware/motion_utils/trajectory/conversion.hpp"
+#include "autoware/motion_utils/trajectory/interpolation.hpp"
+#include "autoware/motion_utils/trajectory/trajectory.hpp"
+#include "autoware/universe_utils/geometry/geometry.hpp"
 
-#include <memory>
-#include <string>
-#include <utility>
 #include <vector>
 
 namespace autoware::control_validator
 {
 
-void shiftPose(Pose & pose, double longitudinal)
+using autoware::motion_utils::convertToTrajectory;
+using autoware::motion_utils::convertToTrajectoryPointArray;
+using autoware_planning_msgs::msg::Trajectory;
+using autoware_planning_msgs::msg::TrajectoryPoint;
+using geometry_msgs::msg::Pose;
+using TrajectoryPoints = std::vector<TrajectoryPoint>;
+
+void shift_pose(Pose & pose, double longitudinal)
 {
   const auto yaw = tf2::getYaw(pose.orientation);
   pose.position.x += std::cos(yaw) * longitudinal;
   pose.position.y += std::sin(yaw) * longitudinal;
 }
 
-void insertPointInPredictedTrajectory(
+/**
+ * @brief Insert interpolated point along the predicted_trajectory to the modified_trajectory
+ * @param[inout] modified_trajectory modified trajectory
+ * @param[in] reference_pose reference pose
+ * @param[in] predicted_trajectory predicted trajectory
+ */
+void insert_point_in_predicted_trajectory(
   TrajectoryPoints & modified_trajectory, const Pose & reference_pose,
   const TrajectoryPoints & predicted_trajectory)
 {
@@ -42,7 +53,7 @@ void insertPointInPredictedTrajectory(
   modified_trajectory.insert(modified_trajectory.begin(), point_to_interpolate);
 }
 
-TrajectoryPoints reverseTrajectoryPoints(const TrajectoryPoints & trajectory_points)
+TrajectoryPoints reverse_trajectory_points(const TrajectoryPoints & trajectory_points)
 {
   TrajectoryPoints reversed_trajectory_points;
   reversed_trajectory_points.reserve(trajectory_points.size());
@@ -52,7 +63,7 @@ TrajectoryPoints reverseTrajectoryPoints(const TrajectoryPoints & trajectory_poi
   return reversed_trajectory_points;
 }
 
-bool removeFrontTrajectoryPoint(
+bool remove_front_trajectory_point(
   const TrajectoryPoints & trajectory_points, TrajectoryPoints & modified_trajectory_points,
   const TrajectoryPoints & predicted_trajectory_points)
 {
@@ -72,7 +83,7 @@ bool removeFrontTrajectoryPoint(
   return predicted_trajectory_point_removed;
 }
 
-Trajectory alignTrajectoryWithReferenceTrajectory(
+Trajectory align_trajectory_with_reference_trajectory(
   const Trajectory & trajectory, const Trajectory & predicted_trajectory)
 {
   const auto last_seg_length = autoware::motion_utils::calcSignedArcLength(
@@ -109,11 +120,11 @@ Trajectory alignTrajectoryWithReferenceTrajectory(
   // ↓
   // predicted_trajectory:                   pNew--p3----//------pN
   // trajectory:                               t1--------//------tN
-  auto predicted_trajectory_point_removed = removeFrontTrajectoryPoint(
+  auto predicted_trajectory_point_removed = remove_front_trajectory_point(
     trajectory_points, modified_trajectory_points, predicted_trajectory_points);
 
   if (predicted_trajectory_point_removed) {
-    insertPointInPredictedTrajectory(
+    insert_point_in_predicted_trajectory(
       modified_trajectory_points, trajectory_points.front().pose, predicted_trajectory_points);
   }
 
@@ -125,28 +136,29 @@ Trajectory alignTrajectoryWithReferenceTrajectory(
   // predicted_trajectory:           p1-----//------pN-2-pNew
   // trajectory:                     t1-----//-----tN-1--tN
 
-  auto reversed_predicted_trajectory_points = reverseTrajectoryPoints(predicted_trajectory_points);
-  auto reversed_trajectory_points = reverseTrajectoryPoints(trajectory_points);
-  auto reversed_modified_trajectory_points = reverseTrajectoryPoints(modified_trajectory_points);
+  auto reversed_predicted_trajectory_points =
+    reverse_trajectory_points(predicted_trajectory_points);
+  auto reversed_trajectory_points = reverse_trajectory_points(trajectory_points);
+  auto reversed_modified_trajectory_points = reverse_trajectory_points(modified_trajectory_points);
 
-  auto reversed_predicted_trajectory_point_removed = removeFrontTrajectoryPoint(
+  auto reversed_predicted_trajectory_point_removed = remove_front_trajectory_point(
     reversed_trajectory_points, reversed_modified_trajectory_points,
     reversed_predicted_trajectory_points);
 
   if (reversed_predicted_trajectory_point_removed) {
-    insertPointInPredictedTrajectory(
+    insert_point_in_predicted_trajectory(
       reversed_modified_trajectory_points, reversed_trajectory_points.front().pose,
       reversed_predicted_trajectory_points);
   }
 
-  return convertToTrajectory(reverseTrajectoryPoints(reversed_modified_trajectory_points));
+  return convertToTrajectory(reverse_trajectory_points(reversed_modified_trajectory_points));
 }
 
-double calcMaxLateralDistance(
+double calc_max_lateral_distance(
   const Trajectory & reference_trajectory, const Trajectory & predicted_trajectory)
 {
   const auto alined_predicted_trajectory =
-    alignTrajectoryWithReferenceTrajectory(reference_trajectory, predicted_trajectory);
+    align_trajectory_with_reference_trajectory(reference_trajectory, predicted_trajectory);
   double max_dist = 0;
   for (const auto & point : alined_predicted_trajectory.points) {
     const auto p0 = autoware::universe_utils::getPoint(point);
diff --git a/control/autoware_control_validator/test/test_control_validator.cpp b/control/autoware_control_validator/test/test_control_validator.cpp
new file mode 100644
index 0000000000000..c4d5b04fa920c
--- /dev/null
+++ b/control/autoware_control_validator/test/test_control_validator.cpp
@@ -0,0 +1,170 @@
+// 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.
+
+#include "autoware/control_validator/control_validator.hpp"
+
+#include <ament_index_cpp/get_package_share_directory.hpp>
+#include <rclcpp/node_options.hpp>
+
+#include <autoware_planning_msgs/msg/trajectory.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+#include <gtest/gtest-param-test.h>
+#include <gtest/gtest.h>
+#include <tf2/LinearMath/Quaternion.h>
+
+#include <tuple>
+
+using autoware_planning_msgs::msg::Trajectory;
+using autoware_planning_msgs::msg::TrajectoryPoint;
+
+Trajectory make_linear_trajectory(
+  const TrajectoryPoint & start, const TrajectoryPoint & end, size_t num_points, double velocity)
+{
+  auto create_quaternion = [](double yaw) {
+    tf2::Quaternion q;
+    q.setRPY(0, 0, yaw);
+    return tf2::toMsg(q);
+  };
+
+  double yaw = std::atan2(
+    end.pose.position.y - start.pose.position.y, end.pose.position.x - start.pose.position.x);
+  yaw += (velocity < 0) ? M_PI : 0;
+
+  Trajectory trajectory;
+  trajectory.points.reserve(num_points);
+
+  for (size_t i = 0; i < num_points; ++i) {
+    double ratio = static_cast<double>(i) / static_cast<double>(num_points - 1);
+
+    TrajectoryPoint point;
+    point.pose.position.x =
+      start.pose.position.x + ratio * (end.pose.position.x - start.pose.position.x);
+    point.pose.position.y =
+      start.pose.position.y + ratio * (end.pose.position.y - start.pose.position.y);
+    point.pose.orientation = create_quaternion(yaw);
+    point.longitudinal_velocity_mps = static_cast<float>(velocity);
+    point.lateral_velocity_mps = 0.0;
+
+    trajectory.points.emplace_back(point);
+  }
+
+  return trajectory;
+}
+
+TrajectoryPoint make_trajectory_point(double x, double y)
+{
+  TrajectoryPoint point;
+  point.pose.position.x = x;
+  point.pose.position.y = y;
+  return point;
+}
+
+class ControlValidatorTest
+: public ::testing::TestWithParam<std::tuple<Trajectory, Trajectory, double, bool>>
+{
+protected:
+  void SetUp() override
+  {
+    rclcpp::init(0, nullptr);
+    rclcpp::NodeOptions options;
+    options.arguments(
+      {"--ros-args", "--params-file",
+       ament_index_cpp::get_package_share_directory("autoware_control_validator") +
+         "/config/control_validator.param.yaml",
+       "--params-file",
+       ament_index_cpp::get_package_share_directory("autoware_test_utils") +
+         "/config/test_vehicle_info.param.yaml"});
+
+    node = std::make_shared<autoware::control_validator::ControlValidator>(options);
+  }
+
+  void TearDown() override { rclcpp::shutdown(); }
+
+  std::shared_ptr<autoware::control_validator::ControlValidator> node;
+};
+
+TEST_P(ControlValidatorTest, test_calc_lateral_deviation_status)
+{
+  auto [reference_trajectory, predicted_trajectory, expected_deviation, expected_condition] =
+    GetParam();
+  auto [deviation, is_valid] =
+    node->calc_lateral_deviation_status(predicted_trajectory, reference_trajectory);
+
+  EXPECT_EQ(is_valid, expected_condition);
+  EXPECT_NEAR(deviation, expected_deviation, 1e-5);
+}
+
+INSTANTIATE_TEST_SUITE_P(
+  ControlValidatorTests, ControlValidatorTest,
+  ::testing::Values(
+
+    std::make_tuple(
+      make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, 1.0),
+      make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0.99), 11, 1.0),
+      0.99, true),
+
+    std::make_tuple(
+      make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, 1.0),
+      make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 1.0), 11, 1.0),
+      1.0, true),
+
+    std::make_tuple(
+      make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, 1.0),
+      make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 1.01), 11, 1.0),
+      1.01, false),
+
+    std::make_tuple(
+      make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, -1.0),
+      make_linear_trajectory(
+        make_trajectory_point(0, 0), make_trajectory_point(10, 0.99), 11, -1.0),
+      0.99, true),
+
+    std::make_tuple(
+      make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, -1.0),
+      make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 1.0), 11, -1.0),
+      1.0, true),
+
+    std::make_tuple(
+      make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, -1.0),
+      make_linear_trajectory(
+        make_trajectory_point(0, 0), make_trajectory_point(10, 1.01), 11, -1.0),
+      1.01, false),
+
+    std::make_tuple(
+      make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, 1.0),
+      make_linear_trajectory(make_trajectory_point(11, 0), make_trajectory_point(20, 0.0), 11, 1.0),
+      0.0, true),
+
+    std::make_tuple(
+      make_linear_trajectory(make_trajectory_point(11, 0), make_trajectory_point(20, 0.0), 11, 1.0),
+      make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, 1.0),
+      0.0, true),
+
+    std::make_tuple(
+      make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, 1.0),
+      make_linear_trajectory(make_trajectory_point(1, 0), make_trajectory_point(10, 1.0), 11, 1.0),
+      1.0, true),
+
+    std::make_tuple(
+      make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, 1.0),
+      make_linear_trajectory(make_trajectory_point(-1, 0), make_trajectory_point(10, 1.0), 11, 1.0),
+      1.0, true),
+
+    std::make_tuple(
+      make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, 1.0),
+      make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(20, 2.0), 21, 1.0),
+      1.0, true))
+
+);
diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp
index b43a7d0d0369a..7e39cb6cba8c4 100644
--- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp
+++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp
@@ -17,6 +17,7 @@
 
 #include <autoware/universe_utils/geometry/boost_geometry.hpp>
 #include <autoware/universe_utils/geometry/pose_deviation.hpp>
+#include <autoware/universe_utils/system/time_keeper.hpp>
 #include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
 #include <rosidl_runtime_cpp/message_initialization.hpp>
 
@@ -101,6 +102,12 @@ struct Output
 class LaneDepartureChecker
 {
 public:
+  LaneDepartureChecker(
+    std::shared_ptr<universe_utils::TimeKeeper> time_keeper =
+      std::make_shared<universe_utils::TimeKeeper>())
+  : time_keeper_(time_keeper)
+  {
+  }
   Output update(const Input & input);
 
   void setParam(const Param & param, const autoware::vehicle_info_utils::VehicleInfo vehicle_info)
@@ -156,9 +163,9 @@ class LaneDepartureChecker
   static std::vector<LinearRing2d> createVehiclePassingAreas(
     const std::vector<LinearRing2d> & vehicle_footprints);
 
-  static bool willLeaveLane(
+  bool willLeaveLane(
     const lanelet::ConstLanelets & candidate_lanelets,
-    const std::vector<LinearRing2d> & vehicle_footprints);
+    const std::vector<LinearRing2d> & vehicle_footprints) const;
 
   double calcMaxSearchLengthForBoundaries(const Trajectory & trajectory) const;
 
@@ -166,9 +173,11 @@ class LaneDepartureChecker
     const lanelet::LaneletMap & lanelet_map, const geometry_msgs::msg::Point & ego_point,
     const double max_search_length, const std::vector<std::string> & boundary_types_to_detect);
 
-  static bool willCrossBoundary(
+  bool willCrossBoundary(
     const std::vector<LinearRing2d> & vehicle_footprints,
-    const SegmentRtree & uncrossable_segments);
+    const SegmentRtree & uncrossable_segments) const;
+
+  mutable std::shared_ptr<universe_utils::TimeKeeper> time_keeper_;
 };
 }  // namespace autoware::lane_departure_checker
 
diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp
index 7104d20f122fb..fcb64607f6d46 100644
--- a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp
+++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp
@@ -160,6 +160,8 @@ Output LaneDepartureChecker::update(const Input & input)
 bool LaneDepartureChecker::checkPathWillLeaveLane(
   const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   std::vector<LinearRing2d> vehicle_footprints = createVehicleFootprints(path);
   lanelet::ConstLanelets candidate_lanelets = getCandidateLanelets(lanelets, vehicle_footprints);
   return willLeaveLane(candidate_lanelets, vehicle_footprints);
@@ -290,8 +292,10 @@ std::vector<LinearRing2d> LaneDepartureChecker::createVehiclePassingAreas(
 
 bool LaneDepartureChecker::willLeaveLane(
   const lanelet::ConstLanelets & candidate_lanelets,
-  const std::vector<LinearRing2d> & vehicle_footprints)
+  const std::vector<LinearRing2d> & vehicle_footprints) const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   for (const auto & vehicle_footprint : vehicle_footprints) {
     if (isOutOfLane(candidate_lanelets, vehicle_footprint)) {
       return true;
@@ -304,6 +308,8 @@ bool LaneDepartureChecker::willLeaveLane(
 std::vector<std::pair<double, lanelet::Lanelet>> LaneDepartureChecker::getLaneletsFromPath(
   const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   // Get Footprint Hull basic polygon
   std::vector<LinearRing2d> vehicle_footprints = createVehicleFootprints(path);
   LinearRing2d footprint_hull = createHullFromFootprints(vehicle_footprints);
@@ -326,6 +332,8 @@ std::optional<autoware::universe_utils::Polygon2d>
 LaneDepartureChecker::getFusedLaneletPolygonForPath(
   const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   const auto lanelets_distance_pair = getLaneletsFromPath(lanelet_map_ptr, path);
   auto to_polygon2d =
     [](const lanelet::BasicPolygon2d & poly) -> autoware::universe_utils::Polygon2d {
@@ -365,6 +373,8 @@ LaneDepartureChecker::getFusedLaneletPolygonForPath(
 bool LaneDepartureChecker::checkPathWillLeaveLane(
   const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   // check if the footprint is not fully contained within the fused lanelets polygon
   const std::vector<LinearRing2d> vehicle_footprints = createVehicleFootprints(path);
   const auto fused_lanelets_polygon = getFusedLaneletPolygonForPath(lanelet_map_ptr, path);
@@ -379,17 +389,26 @@ bool LaneDepartureChecker::checkPathWillLeaveLane(
 PathWithLaneId LaneDepartureChecker::cropPointsOutsideOfLanes(
   const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path, const size_t end_index)
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   PathWithLaneId temp_path;
   const auto fused_lanelets_polygon = getFusedLaneletPolygonForPath(lanelet_map_ptr, path);
   if (path.points.empty() || !fused_lanelets_polygon) return temp_path;
   const auto vehicle_footprints = createVehicleFootprints(path);
-  size_t idx = 0;
-  std::for_each(vehicle_footprints.begin(), vehicle_footprints.end(), [&](const auto & footprint) {
-    if (idx > end_index || boost::geometry::within(footprint, fused_lanelets_polygon.value())) {
-      temp_path.points.push_back(path.points.at(idx));
-    }
-    ++idx;
-  });
+
+  {
+    universe_utils::ScopedTimeTrack st2(
+      "check if footprint is within fused_lanlets_polygon", *time_keeper_);
+
+    size_t idx = 0;
+    std::for_each(
+      vehicle_footprints.begin(), vehicle_footprints.end(), [&](const auto & footprint) {
+        if (idx > end_index || boost::geometry::within(footprint, fused_lanelets_polygon.value())) {
+          temp_path.points.push_back(path.points.at(idx));
+        }
+        ++idx;
+      });
+  }
   PathWithLaneId cropped_path = path;
   cropped_path.points = temp_path.points;
   return cropped_path;
@@ -449,8 +468,11 @@ SegmentRtree LaneDepartureChecker::extractUncrossableBoundaries(
 }
 
 bool LaneDepartureChecker::willCrossBoundary(
-  const std::vector<LinearRing2d> & vehicle_footprints, const SegmentRtree & uncrossable_segments)
+  const std::vector<LinearRing2d> & vehicle_footprints,
+  const SegmentRtree & uncrossable_segments) const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   for (const auto & footprint : vehicle_footprints) {
     std::vector<Segment2d> intersection_result;
     uncrossable_segments.query(
diff --git a/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp b/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp
index 803a6ef1e8617..085addbe17fc9 100644
--- a/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp
+++ b/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp
@@ -450,15 +450,13 @@ boost::optional<PpOutput> PurePursuitLateralController::calcTargetCurvature(
   const bool is_reverse = (target_vel < 0);
   const double min_lookahead_distance =
     is_reverse ? param_.reverse_min_lookahead_distance : param_.min_lookahead_distance;
-  double lookahead_distance = min_lookahead_distance;
-  if (is_control_output) {
-    lookahead_distance = calcLookaheadDistance(
-      lateral_error, current_curvature, current_odometry_.twist.twist.linear.x,
-      min_lookahead_distance, is_control_output);
-  } else {
-    lookahead_distance = calcLookaheadDistance(
-      lateral_error, current_curvature, target_vel, min_lookahead_distance, is_control_output);
-  }
+  double lookahead_distance =
+    is_control_output
+      ? calcLookaheadDistance(
+          lateral_error, current_curvature, current_odometry_.twist.twist.linear.x,
+          min_lookahead_distance, is_control_output)
+      : calcLookaheadDistance(
+          lateral_error, current_curvature, target_vel, min_lookahead_distance, is_control_output);
 
   // Set PurePursuit data
   pure_pursuit_->setCurrentPose(pose);
diff --git a/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/interpolate.cpp b/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/interpolate.cpp
index 3a79b1f01a804..9b68594f167e8 100644
--- a/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/interpolate.cpp
+++ b/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/interpolate.cpp
@@ -113,9 +113,8 @@ void SplineInterpolate::generateSpline(const std::vector<double> & x)
   std::vector<double> w_;
   w_.push_back(0.0);
 
-  double tmp;
   for (int i = 1; i < N - 1; i++) {
-    tmp = 1.0 / (4.0 - w_[i - 1]);
+    const double tmp = 1.0 / (4.0 - w_[i - 1]);
     c_[i] = (c_[i] - c_[i - 1]) * tmp;
     w_.push_back(tmp);
   }
diff --git a/control/autoware_smart_mpc_trajectory_follower/CMakeLists.txt b/control/autoware_smart_mpc_trajectory_follower/CMakeLists.txt
index 57e7b596790c2..b4b21bded9c73 100644
--- a/control/autoware_smart_mpc_trajectory_follower/CMakeLists.txt
+++ b/control/autoware_smart_mpc_trajectory_follower/CMakeLists.txt
@@ -2,12 +2,31 @@ cmake_minimum_required(VERSION 3.14)
 project(autoware_smart_mpc_trajectory_follower)
 
 find_package(autoware_cmake REQUIRED)
+find_package(python_cmake_module REQUIRED)
+find_package(Python3 REQUIRED COMPONENTS Interpreter Development)
+find_package(pybind11_vendor REQUIRED)
+find_package(pybind11 REQUIRED)
+find_package(Eigen3 REQUIRED)
 autoware_package()
 
+execute_process(COMMAND bash -c "pip3 install numba==0.58.1 --force-reinstall")
+execute_process(COMMAND bash -c "pip3 install GPy")
+
+pybind11_add_module(proxima_calc SHARED
+  ${PROJECT_NAME}/src/proxima_calc.cpp
+)
+ament_target_dependencies(proxima_calc Eigen3)
+ament_get_python_install_dir(PYTHON_INSTALL_DIR)
+install(TARGETS proxima_calc
+  LIBRARY DESTINATION ${PYTHON_INSTALL_DIR}/${PROJECT_NAME}
+)
+
+ament_python_install_package(${PROJECT_NAME})
 install(PROGRAMS
-  autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py
+  scripts/pympc_trajectory_follower.py
   DESTINATION lib/${PROJECT_NAME}
 )
 ament_auto_package(
   INSTALL_TO_SHARE
+  autoware_smart_mpc_trajectory_follower/param
 )
diff --git a/control/autoware_smart_mpc_trajectory_follower/README.md b/control/autoware_smart_mpc_trajectory_follower/README.md
index bb31a1be3a8b5..31270f701fce6 100644
--- a/control/autoware_smart_mpc_trajectory_follower/README.md
+++ b/control/autoware_smart_mpc_trajectory_follower/README.md
@@ -17,20 +17,6 @@ This technology makes it relatively easy to operate model predictive control, wh
   </a>
 </p>
 
-## Setup
-
-After building autoware, move to `control/autoware_smart_mpc_trajectory_follower` and run the following command:
-
-```bash
-pip3 install .
-```
-
-If you have already installed and want to update the package, run the following command instead:
-
-```bash
-pip3 install -U .
-```
-
 ## Provided features
 
 This package provides smart MPC logic for path-following control as well as mechanisms for learning and evaluation. These features are described below.
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_NN.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_NN.py
index 84a0486b1e074..445897a4c580f 100644
--- a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_NN.py
+++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_NN.py
@@ -13,8 +13,8 @@
 # limitations under the License.
 
 
+from autoware_smart_mpc_trajectory_follower import proxima_calc
 from autoware_smart_mpc_trajectory_follower.scripts import drive_functions
-from autoware_smart_mpc_trajectory_follower.scripts import proxima_calc
 import numpy as np
 import torch
 from torch import nn
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py
index 81ad4f8b38790..24773ed2d3e20 100644
--- a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py
+++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py
@@ -17,23 +17,21 @@
 
 """Define primary parameters and functions to be used elsewhere."""
 from functools import partial
-import json
 import os
 from pathlib import Path
 import sys
 from typing import Callable
 
+from ament_index_python.packages import get_package_share_directory
 from numba import njit
 import numpy as np
 import yaml
 
-package_path_json = str(Path(__file__).parent.parent) + "/package_path.json"
-with open(package_path_json, "r") as file:
-    package_path = json.load(file)
+PACKAGE_NAME = "autoware_smart_mpc_trajectory_follower"
 
-mpc_param_path = (
-    package_path["path"] + "/autoware_smart_mpc_trajectory_follower/param/mpc_param.yaml"
-)
+param_path = Path(get_package_share_directory(PACKAGE_NAME)) / "param"
+
+mpc_param_path = param_path / "mpc_param.yaml"
 with open(mpc_param_path, "r") as yml:
     mpc_param = yaml.safe_load(yml)
 
@@ -102,9 +100,7 @@
 
 cap_pred_error = np.array(mpc_param["mpc_parameter"]["preprocessing"]["cap_pred_error"])
 
-nominal_param_path = (
-    package_path["path"] + "/autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml"
-)
+nominal_param_path = param_path / "nominal_param.yaml"
 with open(nominal_param_path, "r") as yml:
     nominal_param = yaml.safe_load(yml)
 # Vehicle body information given by default.
@@ -173,10 +169,7 @@
 )
 max_error_steer = float(mpc_param["mpc_parameter"]["compensation"]["max_error_steer"])
 
-
-trained_model_param_path = (
-    package_path["path"] + "/autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml"
-)
+trained_model_param_path = param_path / "trained_model_param.yaml"
 with open(trained_model_param_path, "r") as yml:
     trained_model_param = yaml.safe_load(yml)
 use_trained_model_diff = bool(
@@ -370,31 +363,12 @@
 )
 # upper limit of input
 
-
-package_path_split = str(package_path["path"]).split("/")
-control_dir_path = ""
-for i in range(len(package_path_split)):
-    control_dir_path += package_path_split[i]
-    if package_path_split[i] == "control":
-        break
-    control_dir_path += "/"
-
 read_limit_file = bool(mpc_param["mpc_parameter"]["limit"]["read_limit_file"])
 if read_limit_file:
-    limit_yaml_path = None
-    for curDir, dirs, files in os.walk(control_dir_path):
-        for name in files:
-            if name == "vehicle_cmd_gate.param.yaml":
-                if curDir.split("/")[-2] == "autoware_vehicle_cmd_gate":
-                    limit_yaml_path = curDir + "/" + name
-                    break
-
-    limit_params = None
-    if limit_yaml_path is not None:
-        with open(limit_yaml_path, "r") as yml:
-            limit_params = yaml.safe_load(yml)
-    else:
-        print("Error: limit_yaml_path is None")
+    limitter_package_path = Path(get_package_share_directory("autoware_vehicle_cmd_gate"))
+    limit_yaml_path = limitter_package_path / "config" / "vehicle_cmd_gate.param.yaml"
+    with open(limit_yaml_path, "r") as yml:
+        limit_params = yaml.safe_load(yml)
 
     if limit_params is not None:
         reference_speed_points = np.array(
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/proxima_calc.cpp b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/src/proxima_calc.cpp
similarity index 100%
rename from control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/proxima_calc.cpp
rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/src/proxima_calc.cpp
diff --git a/control/autoware_smart_mpc_trajectory_follower/package.xml b/control/autoware_smart_mpc_trajectory_follower/package.xml
index 46ce55058a000..3fbe1bedc106e 100644
--- a/control/autoware_smart_mpc_trajectory_follower/package.xml
+++ b/control/autoware_smart_mpc_trajectory_follower/package.xml
@@ -25,9 +25,11 @@
   <depend>autoware_planning_msgs</depend>
   <depend>autoware_system_msgs</depend>
   <depend>autoware_universe_utils</depend>
+  <depend>autoware_vehicle_cmd_gate</depend>
   <depend>autoware_vehicle_msgs</depend>
   <depend>pybind11_vendor</depend>
   <depend>python3-scipy</depend>
+  <depend>python3-torch</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
 
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py b/control/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py
similarity index 100%
rename from control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py
rename to control/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py
diff --git a/control/autoware_smart_mpc_trajectory_follower/setup.py b/control/autoware_smart_mpc_trajectory_follower/setup.py
deleted file mode 100644
index 22844b0f2d869..0000000000000
--- a/control/autoware_smart_mpc_trajectory_follower/setup.py
+++ /dev/null
@@ -1,42 +0,0 @@
-# cspell: ignore numba
-import glob
-import json
-from pathlib import Path
-import subprocess
-
-from setuptools import find_packages
-from setuptools import setup
-
-SKIP_PRE_INSTALL_FLAG = False
-
-if not SKIP_PRE_INSTALL_FLAG:
-    subprocess.run("pip3 install numba==0.58.1 --force-reinstall", shell=True)
-    subprocess.run("pip3 install pybind11", shell=True)
-    subprocess.run("pip3 install GPy", shell=True)
-    subprocess.run("pip3 install torch", shell=True)
-    package_path = {}
-    package_path["path"] = str(Path(__file__).parent)
-    with open("autoware_smart_mpc_trajectory_follower/package_path.json", "w") as f:
-        json.dump(package_path, f)
-    build_cpp_command = (
-        "g++ -Ofast -Wall -shared -std=c++11 -fPIC $(python3 -m pybind11 --includes) "
-    )
-    build_cpp_command += "autoware_smart_mpc_trajectory_follower/scripts/proxima_calc.cpp "
-    build_cpp_command += "-o autoware_smart_mpc_trajectory_follower/scripts/proxima_calc$(python3-config --extension-suffix) "
-    build_cpp_command += "-lrt -I/usr/include/eigen3"
-    subprocess.run(build_cpp_command, shell=True)
-
-so_path = (
-    "scripts/"
-    + glob.glob("autoware_smart_mpc_trajectory_follower/scripts/proxima_calc.*.so")[0].split("/")[
-        -1
-    ]
-)
-setup(
-    name="autoware_smart_mpc_trajectory_follower",
-    version="2.0.0",
-    packages=find_packages(),
-    package_data={
-        "autoware_smart_mpc_trajectory_follower": ["package_path.json", so_path],
-    },
-)
diff --git a/control/autoware_vehicle_cmd_gate/src/adapi_pause_interface.cpp b/control/autoware_vehicle_cmd_gate/src/adapi_pause_interface.cpp
index f14a62fba9661..a4bce3be35cb0 100644
--- a/control/autoware_vehicle_cmd_gate/src/adapi_pause_interface.cpp
+++ b/control/autoware_vehicle_cmd_gate/src/adapi_pause_interface.cpp
@@ -29,7 +29,7 @@ AdapiPauseInterface::AdapiPauseInterface(rclcpp::Node * node) : node_(node)
   publish();
 }
 
-bool AdapiPauseInterface::is_paused()
+bool AdapiPauseInterface::is_paused() const
 {
   return is_paused_;
 }
diff --git a/control/autoware_vehicle_cmd_gate/src/adapi_pause_interface.hpp b/control/autoware_vehicle_cmd_gate/src/adapi_pause_interface.hpp
index d75b544752e3f..6f3aa70bcea54 100644
--- a/control/autoware_vehicle_cmd_gate/src/adapi_pause_interface.hpp
+++ b/control/autoware_vehicle_cmd_gate/src/adapi_pause_interface.hpp
@@ -35,7 +35,7 @@ class AdapiPauseInterface
 
 public:
   explicit AdapiPauseInterface(rclcpp::Node * node);
-  bool is_paused();
+  bool is_paused() const;
   void publish();
   void update(const Control & control);
 
diff --git a/control/autoware_vehicle_cmd_gate/src/marker_helper.hpp b/control/autoware_vehicle_cmd_gate/src/marker_helper.hpp
index c9d7e86fa23dd..a2e357f6068a9 100644
--- a/control/autoware_vehicle_cmd_gate/src/marker_helper.hpp
+++ b/control/autoware_vehicle_cmd_gate/src/marker_helper.hpp
@@ -96,7 +96,7 @@ inline visualization_msgs::msg::Marker createMarker(
 inline visualization_msgs::msg::Marker createStringMarker(
   const std::string & frame_id, const std::string & ns, const int32_t id, const int32_t type,
   geometry_msgs::msg::Point point, geometry_msgs::msg::Vector3 scale,
-  const std_msgs::msg::ColorRGBA & color, const std::string text)
+  const std_msgs::msg::ColorRGBA & color, const std::string & text)
 {
   visualization_msgs::msg::Marker marker;
 
diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.cpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.cpp
index cec23b05b8191..60845788adcf5 100644
--- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.cpp
+++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.cpp
@@ -20,7 +20,7 @@
 namespace autoware::vehicle_cmd_gate
 {
 
-VehicleCmdFilter::VehicleCmdFilter()
+VehicleCmdFilter::VehicleCmdFilter() : param_()
 {
 }
 
@@ -40,43 +40,43 @@ bool VehicleCmdFilter::setParameterWithValidation(const VehicleCmdFilterParam &
   param_ = p;
   return true;
 }
-void VehicleCmdFilter::setSteerLim(LimitArray v)
+void VehicleCmdFilter::setSteerLim(const LimitArray & v)
 {
   auto tmp = param_;
   tmp.steer_lim = v;
   setParameterWithValidation(tmp);
 }
-void VehicleCmdFilter::setSteerRateLim(LimitArray v)
+void VehicleCmdFilter::setSteerRateLim(const LimitArray & v)
 {
   auto tmp = param_;
   tmp.steer_rate_lim = v;
   setParameterWithValidation(tmp);
 }
-void VehicleCmdFilter::setLonAccLim(LimitArray v)
+void VehicleCmdFilter::setLonAccLim(const LimitArray & v)
 {
   auto tmp = param_;
   tmp.lon_acc_lim = v;
   setParameterWithValidation(tmp);
 }
-void VehicleCmdFilter::setLonJerkLim(LimitArray v)
+void VehicleCmdFilter::setLonJerkLim(const LimitArray & v)
 {
   auto tmp = param_;
   tmp.lon_jerk_lim = v;
   setParameterWithValidation(tmp);
 }
-void VehicleCmdFilter::setLatAccLim(LimitArray v)
+void VehicleCmdFilter::setLatAccLim(const LimitArray & v)
 {
   auto tmp = param_;
   tmp.lat_acc_lim = v;
   setParameterWithValidation(tmp);
 }
-void VehicleCmdFilter::setLatJerkLim(LimitArray v)
+void VehicleCmdFilter::setLatJerkLim(const LimitArray & v)
 {
   auto tmp = param_;
   tmp.lat_jerk_lim = v;
   setParameterWithValidation(tmp);
 }
-void VehicleCmdFilter::setActualSteerDiffLim(LimitArray v)
+void VehicleCmdFilter::setActualSteerDiffLim(const LimitArray & v)
 {
   auto tmp = param_;
   tmp.actual_steer_diff_lim = v;
@@ -252,8 +252,7 @@ double VehicleCmdFilter::calcLatAcc(const Control & cmd, const double v) const
   return v * v * std::tan(cmd.lateral.steering_tire_angle) / param_.wheel_base;
 }
 
-double VehicleCmdFilter::limitDiff(
-  const double curr, const double prev, const double diff_lim) const
+double VehicleCmdFilter::limitDiff(const double curr, const double prev, const double diff_lim)
 {
   double diff = std::max(std::min(curr - prev, diff_lim), -diff_lim);
   return prev + diff;
diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.hpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.hpp
index 96663474f47a4..a4877692b6dac 100644
--- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.hpp
+++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.hpp
@@ -49,13 +49,13 @@ class VehicleCmdFilter
 
   void setWheelBase(double v) { param_.wheel_base = v; }
   void setVelLim(double v) { param_.vel_lim = v; }
-  void setSteerLim(LimitArray v);
-  void setSteerRateLim(LimitArray v);
-  void setLonAccLim(LimitArray v);
-  void setLonJerkLim(LimitArray v);
-  void setLatAccLim(LimitArray v);
-  void setLatJerkLim(LimitArray v);
-  void setActualSteerDiffLim(LimitArray v);
+  void setSteerLim(const LimitArray & v);
+  void setSteerRateLim(const LimitArray & v);
+  void setLonAccLim(const LimitArray & v);
+  void setLonJerkLim(const LimitArray & v);
+  void setLatAccLim(const LimitArray & v);
+  void setLatJerkLim(const LimitArray & v);
+  void setActualSteerDiffLim(const LimitArray & v);
   void setCurrentSpeed(double v) { current_speed_ = v; }
   void setParam(const VehicleCmdFilterParam & p);
   VehicleCmdFilterParam getParam() const;
@@ -75,7 +75,7 @@ class VehicleCmdFilter
   static IsFilterActivated checkIsActivated(
     const Control & c1, const Control & c2, const double tol = 1.0e-3);
 
-  Control getPrevCmd() { return prev_cmd_; }
+  Control getPrevCmd() const { return prev_cmd_; }
 
 private:
   VehicleCmdFilterParam param_;
@@ -87,7 +87,7 @@ class VehicleCmdFilter
   double calcLatAcc(const Control & cmd) const;
   double calcLatAcc(const Control & cmd, const double v) const;
   double calcSteerFromLatacc(const double v, const double latacc) const;
-  double limitDiff(const double curr, const double prev, const double diff_lim) const;
+  static double limitDiff(const double curr, const double prev, const double diff_lim);
 
   double interpolateFromSpeed(const LimitArray & limits) const;
   double getLonAccLim() const;
diff --git a/launch/tier4_autoware_api_launch/launch/deprecated_api.launch.xml b/launch/tier4_autoware_api_launch/launch/deprecated_api.launch.xml
index 0856b9ca073cd..cdd1702fec70b 100644
--- a/launch/tier4_autoware_api_launch/launch/deprecated_api.launch.xml
+++ b/launch/tier4_autoware_api_launch/launch/deprecated_api.launch.xml
@@ -15,6 +15,6 @@
   <!-- autoware api utils -->
   <group>
     <push-ros-namespace namespace="autoware_api/utils"/>
-    <include file="$(find-pkg-share path_distance_calculator)/launch/path_distance_calculator.launch.xml"/>
+    <include file="$(find-pkg-share autoware_path_distance_calculator)/launch/path_distance_calculator.launch.xml"/>
   </group>
 </launch>
diff --git a/launch/tier4_autoware_api_launch/package.xml b/launch/tier4_autoware_api_launch/package.xml
index c55f1c8038e83..d45d21b3ed949 100644
--- a/launch/tier4_autoware_api_launch/package.xml
+++ b/launch/tier4_autoware_api_launch/package.xml
@@ -14,9 +14,9 @@
   <exec_depend>ad_api_adaptors</exec_depend>
   <exec_depend>autoware_iv_external_api_adaptor</exec_depend>
   <exec_depend>autoware_iv_internal_api_adaptor</exec_depend>
+  <exec_depend>autoware_path_distance_calculator</exec_depend>
   <exec_depend>awapi_awiv_adapter</exec_depend>
   <exec_depend>default_ad_api</exec_depend>
-  <exec_depend>path_distance_calculator</exec_depend>
   <exec_depend>rosbridge_server</exec_depend>
   <exec_depend>topic_tools</exec_depend>
 
diff --git a/launch/tier4_control_launch/launch/control.launch.xml b/launch/tier4_control_launch/launch/control.launch.xml
index f5582b7aeb305..0eda6faaf4e05 100644
--- a/launch/tier4_control_launch/launch/control.launch.xml
+++ b/launch/tier4_control_launch/launch/control.launch.xml
@@ -192,7 +192,6 @@
         <remap from="~/input/pointcloud" to="/perception/obstacle_segmentation/pointcloud"/>
         <remap from="~/input/velocity" to="/vehicle/status/velocity_status"/>
         <remap from="~/input/imu" to="/sensing/imu/imu_data"/>
-        <remap from="~/input/odometry" to="/localization/kinematic_state"/>
         <remap from="~/input/objects" to="/perception/obstacle_segmentation/objects"/>
         <remap from="~/input/predicted_trajectory" to="/control/trajectory_follower/lateral/predicted_trajectory"/>
         <param from="$(var aeb_param_path)"/>
diff --git a/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml
index b39346de8f8da..fa479c71dcbac 100644
--- a/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml
+++ b/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml
@@ -26,7 +26,7 @@
   </group>
 
   <group>
-    <include file="$(find-pkg-share twist2accel)/launch/twist2accel.launch.xml">
+    <include file="$(find-pkg-share autoware_twist2accel)/launch/twist2accel.launch.xml">
       <arg name="in_odom" value="/localization/kinematic_state"/>
       <arg name="in_twist" value="/localization/twist_estimator/twist_with_covariance"/>
       <arg name="out_accel" value="/localization/acceleration"/>
diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py
index 79c99398966da..74b89e9298da4 100644
--- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py
+++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py
@@ -88,7 +88,7 @@ def create_parameter_dict(*args):
                 package="autoware_traffic_light_visualization",
                 plugin="autoware::traffic_light::TrafficLightRoiVisualizerNode",
                 name="traffic_light_roi_visualizer",
-                parameters=[create_parameter_dict("enable_fine_detection")],
+                parameters=[create_parameter_dict("enable_fine_detection", "use_image_transport")],
                 remappings=[
                     ("~/input/image", LaunchConfiguration("input/image")),
                     ("~/input/rois", LaunchConfiguration("output/rois")),
@@ -172,6 +172,7 @@ def add_launch_arg(name: str, default_value=None, description=None):
     classifier_share_dir = get_package_share_directory("autoware_traffic_light_classifier")
     add_launch_arg("enable_image_decompressor", "True")
     add_launch_arg("enable_fine_detection", "True")
+    add_launch_arg("use_image_transport", "True")
     add_launch_arg("input/image", "/sensing/camera/traffic_light/image_raw")
     add_launch_arg("output/rois", "/perception/traffic_light_recognition/rois")
     add_launch_arg(
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 14501c689dbe6..788f7bbc6b3a8 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
@@ -32,7 +32,7 @@ The positions and orientations of the AR-Tags are assumed to be written in the L
 
 ## Parameters
 
-{{ json_to_markdown("localization/landmark_based_localizer/ar_tag_based_localizer/schema/ar_tag_based_localizer.schema.json") }}
+{{ json_to_markdown("localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/schema/ar_tag_based_localizer.schema.json") }}
 
 ## How to launch
 
diff --git a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml
index 1b2469d108dfb..e8597a6fc9a4e 100644
--- a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml
+++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml
@@ -16,10 +16,9 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
-  <depend>ament_index_cpp</depend>
+  <test_depend>ament_index_cpp</test_depend>
   <depend>aruco</depend>
   <depend>autoware_landmark_manager</depend>
-  <depend>autoware_lanelet2_extension</depend>
   <depend>cv_bridge</depend>
   <depend>diagnostic_msgs</depend>
   <depend>localization_util</depend>
diff --git a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp
index cef3debf22a4f..80845402a59f3 100644
--- a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp
+++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp
@@ -116,7 +116,7 @@ ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options)
   */
   using std::placeholders::_1;
   map_bin_sub_ = this->create_subscription<LaneletMapBin>(
-    "~/input/lanelet2_map", rclcpp::QoS(10).durability(rclcpp::DurabilityPolicy::TransientLocal),
+    "~/input/lanelet2_map", rclcpp::QoS(1).durability(rclcpp::DurabilityPolicy::TransientLocal),
     std::bind(&ArTagBasedLocalizer::map_bin_callback, this, _1));
 
   rclcpp::QoS qos_sub(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
@@ -131,7 +131,7 @@ ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options)
   /*
     Publishers
   */
-  const rclcpp::QoS qos_pub_once = rclcpp::QoS(rclcpp::KeepLast(10)).transient_local().reliable();
+  const rclcpp::QoS qos_pub_once = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable();
   const rclcpp::QoS qos_pub_periodic(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
   pose_pub_ = this->create_publisher<PoseWithCovarianceStamped>(
     "~/output/pose_with_covariance", qos_pub_periodic);
diff --git a/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml
index 4880cf88bec5d..d69adc7904411 100644
--- a/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml
+++ b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml
@@ -20,6 +20,7 @@
 
   <depend>autoware_lanelet2_extension</depend>
   <depend>autoware_map_msgs</depend>
+  <depend>autoware_universe_utils</depend>
   <depend>geometry_msgs</depend>
   <depend>localization_util</depend>
   <depend>rclcpp</depend>
diff --git a/localization/pose2twist/CMakeLists.txt b/localization/autoware_pose2twist/CMakeLists.txt
similarity index 77%
rename from localization/pose2twist/CMakeLists.txt
rename to localization/autoware_pose2twist/CMakeLists.txt
index ee63d9f43559a..2783e1a69047a 100644
--- a/localization/pose2twist/CMakeLists.txt
+++ b/localization/autoware_pose2twist/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.14)
-project(pose2twist)
+project(autoware_pose2twist)
 
 find_package(autoware_cmake REQUIRED)
 autoware_package()
@@ -9,7 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
 )
 
 rclcpp_components_register_node(${PROJECT_NAME}
-  PLUGIN "Pose2Twist"
+  PLUGIN "autoware::pose2twist::Pose2Twist"
   EXECUTABLE ${PROJECT_NAME}_node
   EXECUTOR SingleThreadedExecutor
 )
@@ -19,6 +19,9 @@ if(BUILD_TESTING)
   ament_auto_add_gtest(test_angular_velocity
     test/test_angular_velocity.cpp
   )
+  target_include_directories(test_angular_velocity PRIVATE
+    src
+  )
 endif()
 
 ament_auto_package(
diff --git a/localization/pose2twist/README.md b/localization/autoware_pose2twist/README.md
similarity index 82%
rename from localization/pose2twist/README.md
rename to localization/autoware_pose2twist/README.md
index f1f7d6408fafb..55ca4667c423d 100644
--- a/localization/pose2twist/README.md
+++ b/localization/autoware_pose2twist/README.md
@@ -1,8 +1,8 @@
-# pose2twist
+# autoware_pose2twist
 
 ## Purpose
 
-This `pose2twist` calculates the velocity from the input pose history. In addition to the computed twist, this node outputs the linear-x and angular-z components as a float message to simplify debugging.
+This `autoware_pose2twist` calculates the velocity from the input pose history. In addition to the computed twist, this node outputs the linear-x and angular-z components as a float message to simplify debugging.
 
 The `twist.linear.x` is calculated as `sqrt(dx * dx + dy * dy + dz * dz) / dt`, and the values in the `y` and `z` fields are zero.
 The `twist.angular` is calculated as `relative_rotation_vector / dt` for each field.
diff --git a/localization/pose2twist/launch/pose2twist.launch.xml b/localization/autoware_pose2twist/launch/pose2twist.launch.xml
similarity index 79%
rename from localization/pose2twist/launch/pose2twist.launch.xml
rename to localization/autoware_pose2twist/launch/pose2twist.launch.xml
index 57a41dbfcf017..8ac802a5d0ce3 100644
--- a/localization/pose2twist/launch/pose2twist.launch.xml
+++ b/localization/autoware_pose2twist/launch/pose2twist.launch.xml
@@ -2,7 +2,7 @@
   <arg name="input_pose_topic" default="/localization/pose_estimator/pose" description=""/>
   <arg name="output_twist_topic" default="/estimate_twist" description=""/>
 
-  <node pkg="pose2twist" exec="pose2twist_node" output="both">
+  <node pkg="autoware_pose2twist" exec="autoware_pose2twist_node" output="both">
     <remap from="pose" to="$(var input_pose_topic)"/>
     <remap from="twist" to="$(var output_twist_topic)"/>
   </node>
diff --git a/localization/pose2twist/package.xml b/localization/autoware_pose2twist/package.xml
similarity index 92%
rename from localization/pose2twist/package.xml
rename to localization/autoware_pose2twist/package.xml
index 07e445c72978c..dc499f8eda5a4 100644
--- a/localization/pose2twist/package.xml
+++ b/localization/autoware_pose2twist/package.xml
@@ -1,9 +1,9 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>pose2twist</name>
+  <name>autoware_pose2twist</name>
   <version>0.1.0</version>
-  <description>The pose2twist package</description>
+  <description>The autoware_pose2twist package</description>
   <maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer>
   <maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
   <maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>
diff --git a/localization/pose2twist/src/pose2twist_core.cpp b/localization/autoware_pose2twist/src/pose2twist_core.cpp
similarity index 96%
rename from localization/pose2twist/src/pose2twist_core.cpp
rename to localization/autoware_pose2twist/src/pose2twist_core.cpp
index cdde78ed7e357..4dc7b5fb04209 100644
--- a/localization/pose2twist/src/pose2twist_core.cpp
+++ b/localization/autoware_pose2twist/src/pose2twist_core.cpp
@@ -12,12 +12,14 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "pose2twist/pose2twist_core.hpp"
+#include "pose2twist_core.hpp"
 
 #include <cmath>
 #include <cstddef>
 #include <functional>
 
+namespace autoware::pose2twist
+{
 Pose2Twist::Pose2Twist(const rclcpp::NodeOptions & options) : rclcpp::Node("pose2twist", options)
 {
   using std::placeholders::_1;
@@ -113,6 +115,7 @@ void Pose2Twist::callback_pose(geometry_msgs::msg::PoseStamped::SharedPtr pose_m
   angular_z_msg.data = static_cast<float>(twist_msg.twist.angular.z);
   angular_z_pub_->publish(angular_z_msg);
 }
+}  // namespace autoware::pose2twist
 
 #include <rclcpp_components/register_node_macro.hpp>
-RCLCPP_COMPONENTS_REGISTER_NODE(Pose2Twist)
+RCLCPP_COMPONENTS_REGISTER_NODE(autoware::pose2twist::Pose2Twist)
diff --git a/localization/pose2twist/include/pose2twist/pose2twist_core.hpp b/localization/autoware_pose2twist/src/pose2twist_core.hpp
similarity index 91%
rename from localization/pose2twist/include/pose2twist/pose2twist_core.hpp
rename to localization/autoware_pose2twist/src/pose2twist_core.hpp
index d1ff6ee5ff8b6..ed3e542beb857 100644
--- a/localization/pose2twist/include/pose2twist/pose2twist_core.hpp
+++ b/localization/autoware_pose2twist/src/pose2twist_core.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef POSE2TWIST__POSE2TWIST_CORE_HPP_
-#define POSE2TWIST__POSE2TWIST_CORE_HPP_
+#ifndef POSE2TWIST_CORE_HPP_
+#define POSE2TWIST_CORE_HPP_
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -27,6 +27,8 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #endif
 
+namespace autoware::pose2twist
+{
 // Compute the relative rotation of q2 from q1 as a rotation vector
 geometry_msgs::msg::Vector3 compute_relative_rotation_vector(
   const tf2::Quaternion & q1, const tf2::Quaternion & q2);
@@ -45,5 +47,6 @@ class Pose2Twist : public rclcpp::Node
   rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr linear_x_pub_;
   rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr angular_z_pub_;
 };
+}  // namespace autoware::pose2twist
 
-#endif  // POSE2TWIST__POSE2TWIST_CORE_HPP_
+#endif  // POSE2TWIST_CORE_HPP_
diff --git a/localization/pose2twist/test/test_angular_velocity.cpp b/localization/autoware_pose2twist/test/test_angular_velocity.cpp
similarity index 97%
rename from localization/pose2twist/test/test_angular_velocity.cpp
rename to localization/autoware_pose2twist/test/test_angular_velocity.cpp
index bf2ca0a3ba5c2..38eba6f329565 100644
--- a/localization/pose2twist/test/test_angular_velocity.cpp
+++ b/localization/autoware_pose2twist/test/test_angular_velocity.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "pose2twist/pose2twist_core.hpp"
+#include "pose2twist_core.hpp"
 
 #include <gtest/gtest.h>
 
@@ -102,7 +102,7 @@ TEST(AngularVelocityFromQuaternion, CheckNumericalValidity)
 
     // Calculate the relative rotation between the initial and final quaternion
     const geometry_msgs::msg::Vector3 rotation_vector =
-      compute_relative_rotation_vector(initial_q, final_q);
+      autoware::pose2twist::compute_relative_rotation_vector(initial_q, final_q);
 
     EXPECT_NEAR(rotation_vector.x, expected_axis.x() * expected_angle, acceptable_error);
     EXPECT_NEAR(rotation_vector.y, expected_axis.y() * expected_angle, acceptable_error);
diff --git a/localization/autoware_stop_filter/README.md b/localization/autoware_stop_filter/README.md
index 261b8c9da392c..9904707a59996 100644
--- a/localization/autoware_stop_filter/README.md
+++ b/localization/autoware_stop_filter/README.md
@@ -25,4 +25,4 @@ This node aims to:
 
 ## Parameters
 
-{{ json_to_markdown("localization/stop_filter/schema/stop_filter.schema.json") }}
+{{ json_to_markdown("localization/autoware_stop_filter/schema/stop_filter.schema.json") }}
diff --git a/localization/twist2accel/CMakeLists.txt b/localization/autoware_twist2accel/CMakeLists.txt
similarity index 81%
rename from localization/twist2accel/CMakeLists.txt
rename to localization/autoware_twist2accel/CMakeLists.txt
index 9178bf02288a8..c5e3aadfb7fe0 100644
--- a/localization/twist2accel/CMakeLists.txt
+++ b/localization/autoware_twist2accel/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.14)
-project(twist2accel)
+project(autoware_twist2accel)
 
 find_package(autoware_cmake REQUIRED)
 autoware_package()
@@ -9,7 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
 )
 
 rclcpp_components_register_node(${PROJECT_NAME}
-  PLUGIN "Twist2Accel"
+  PLUGIN "autoware::twist2accel::Twist2Accel"
   EXECUTABLE ${PROJECT_NAME}_node
   EXECUTOR SingleThreadedExecutor
 )
diff --git a/localization/twist2accel/README.md b/localization/autoware_twist2accel/README.md
similarity index 89%
rename from localization/twist2accel/README.md
rename to localization/autoware_twist2accel/README.md
index 5a540dca895d4..4bdf29947e0e7 100644
--- a/localization/twist2accel/README.md
+++ b/localization/autoware_twist2accel/README.md
@@ -1,4 +1,4 @@
-# twist2accel
+# autoware_twist2accel
 
 ## Purpose
 
@@ -21,7 +21,7 @@ This package is responsible for estimating acceleration using the output of `ekf
 
 ## Parameters
 
-{{ json_to_markdown("localization/twist2accel/schema/twist2accel.schema.json") }}
+{{ json_to_markdown("localization/autoware_twist2accel/schema/twist2accel.schema.json") }}
 
 ## Future work
 
diff --git a/localization/twist2accel/config/twist2accel.param.yaml b/localization/autoware_twist2accel/config/twist2accel.param.yaml
similarity index 100%
rename from localization/twist2accel/config/twist2accel.param.yaml
rename to localization/autoware_twist2accel/config/twist2accel.param.yaml
diff --git a/localization/twist2accel/launch/twist2accel.launch.xml b/localization/autoware_twist2accel/launch/twist2accel.launch.xml
similarity index 65%
rename from localization/twist2accel/launch/twist2accel.launch.xml
rename to localization/autoware_twist2accel/launch/twist2accel.launch.xml
index c534a288aac3e..6fe10a8c80c32 100644
--- a/localization/twist2accel/launch/twist2accel.launch.xml
+++ b/localization/autoware_twist2accel/launch/twist2accel.launch.xml
@@ -1,10 +1,10 @@
 <launch>
-  <arg name="param_file" default="$(find-pkg-share twist2accel)/config/twist2accel.param.yaml"/>
+  <arg name="param_file" default="$(find-pkg-share autoware_twist2accel)/config/twist2accel.param.yaml"/>
 
   <arg name="in_odom" default="in_odom"/>
   <arg name="in_twist" default="in_twist"/>
   <arg name="out_accel" default="out_accel"/>
-  <node pkg="twist2accel" exec="twist2accel_node" output="both">
+  <node pkg="autoware_twist2accel" exec="autoware_twist2accel_node" output="both">
     <remap from="input/odom" to="$(var in_odom)"/>
     <remap from="input/twist" to="$(var in_twist)"/>
     <remap from="output/accel" to="$(var out_accel)"/>
diff --git a/localization/twist2accel/package.xml b/localization/autoware_twist2accel/package.xml
similarity index 97%
rename from localization/twist2accel/package.xml
rename to localization/autoware_twist2accel/package.xml
index 158d8a8a8d283..af8938fd74a9f 100644
--- a/localization/twist2accel/package.xml
+++ b/localization/autoware_twist2accel/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>twist2accel</name>
+  <name>autoware_twist2accel</name>
   <version>0.0.0</version>
   <description>The acceleration estimation package</description>
   <maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer>
diff --git a/localization/twist2accel/schema/twist2accel.schema.json b/localization/autoware_twist2accel/schema/twist2accel.schema.json
similarity index 100%
rename from localization/twist2accel/schema/twist2accel.schema.json
rename to localization/autoware_twist2accel/schema/twist2accel.schema.json
diff --git a/localization/twist2accel/src/twist2accel.cpp b/localization/autoware_twist2accel/src/twist2accel.cpp
similarity index 96%
rename from localization/twist2accel/src/twist2accel.cpp
rename to localization/autoware_twist2accel/src/twist2accel.cpp
index 800d696b1dea8..2205a9d3a674e 100644
--- a/localization/twist2accel/src/twist2accel.cpp
+++ b/localization/autoware_twist2accel/src/twist2accel.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "twist2accel/twist2accel.hpp"
+#include "twist2accel.hpp"
 
 #include <rclcpp/logging.hpp>
 
@@ -22,6 +22,8 @@
 #include <string>
 #include <utility>
 
+namespace autoware::twist2accel
+{
 using std::placeholders::_1;
 
 Twist2Accel::Twist2Accel(const rclcpp::NodeOptions & node_options)
@@ -103,6 +105,7 @@ void Twist2Accel::estimate_accel(const geometry_msgs::msg::TwistStamped::SharedP
   pub_accel_->publish(accel_msg);
   prev_twist_ptr_ = msg;
 }
+}  // namespace autoware::twist2accel
 
 #include <rclcpp_components/register_node_macro.hpp>
-RCLCPP_COMPONENTS_REGISTER_NODE(Twist2Accel)
+RCLCPP_COMPONENTS_REGISTER_NODE(autoware::twist2accel::Twist2Accel)
diff --git a/localization/twist2accel/include/twist2accel/twist2accel.hpp b/localization/autoware_twist2accel/src/twist2accel.hpp
similarity index 91%
rename from localization/twist2accel/include/twist2accel/twist2accel.hpp
rename to localization/autoware_twist2accel/src/twist2accel.hpp
index e2fab219b11b4..97060c080179b 100644
--- a/localization/twist2accel/include/twist2accel/twist2accel.hpp
+++ b/localization/autoware_twist2accel/src/twist2accel.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef TWIST2ACCEL__TWIST2ACCEL_HPP_
-#define TWIST2ACCEL__TWIST2ACCEL_HPP_
+#ifndef TWIST2ACCEL_HPP_
+#define TWIST2ACCEL_HPP_
 
 #include "signal_processing/lowpass_filter_1d.hpp"
 
@@ -37,10 +37,12 @@
 #include <string>
 #include <vector>
 
+namespace autoware::twist2accel
+{
 class Twist2Accel : public rclcpp::Node
 {
 public:
-  explicit Twist2Accel(const rclcpp::NodeOptions & options);
+  explicit Twist2Accel(const rclcpp::NodeOptions & node_options);
 
 private:
   rclcpp::Publisher<geometry_msgs::msg::AccelWithCovarianceStamped>::SharedPtr
@@ -68,4 +70,5 @@ class Twist2Accel : public rclcpp::Node
   void callback_odometry(const nav_msgs::msg::Odometry::SharedPtr msg);
   void estimate_accel(const geometry_msgs::msg::TwistStamped::SharedPtr msg);
 };
-#endif  // TWIST2ACCEL__TWIST2ACCEL_HPP_
+}  // namespace autoware::twist2accel
+#endif  // TWIST2ACCEL_HPP_
diff --git a/localization/gyro_odometer/CMakeLists.txt b/localization/gyro_odometer/CMakeLists.txt
index 1b142b254d2e1..a832383ff4761 100644
--- a/localization/gyro_odometer/CMakeLists.txt
+++ b/localization/gyro_odometer/CMakeLists.txt
@@ -6,7 +6,6 @@ autoware_package()
 
 ament_auto_add_library(${PROJECT_NAME} SHARED
   src/gyro_odometer_core.cpp
-  src/diagnostics_module.cpp
 )
 
 target_link_libraries(${PROJECT_NAME} fmt)
diff --git a/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp b/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp
deleted file mode 100644
index c5b7b7a592013..0000000000000
--- a/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp
+++ /dev/null
@@ -1,65 +0,0 @@
-// Copyright 2023 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 GYRO_ODOMETER__DIAGNOSTICS_MODULE_HPP_
-#define GYRO_ODOMETER__DIAGNOSTICS_MODULE_HPP_
-
-#include <rclcpp/rclcpp.hpp>
-
-#include <diagnostic_msgs/msg/diagnostic_array.hpp>
-
-#include <string>
-#include <vector>
-
-namespace autoware::gyro_odometer
-{
-
-class DiagnosticsModule
-{
-public:
-  DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name);
-  void clear();
-  void add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg);
-  template <typename T>
-  void add_key_value(const std::string & key, const T & value);
-  void update_level_and_message(const int8_t level, const std::string & message);
-  void publish(const rclcpp::Time & publish_time_stamp);
-
-private:
-  [[nodiscard]] diagnostic_msgs::msg::DiagnosticArray create_diagnostics_array(
-    const rclcpp::Time & publish_time_stamp) const;
-
-  rclcpp::Clock::SharedPtr clock_;
-  rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diagnostics_pub_;
-
-  diagnostic_msgs::msg::DiagnosticStatus diagnostics_status_msg_;
-};
-
-template <typename T>
-void DiagnosticsModule::add_key_value(const std::string & key, const T & value)
-{
-  diagnostic_msgs::msg::KeyValue key_value;
-  key_value.key = key;
-  key_value.value = std::to_string(value);
-  add_key_value(key_value);
-}
-
-template <>
-void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value);
-template <>
-void DiagnosticsModule::add_key_value(const std::string & key, const bool & value);
-
-}  // namespace autoware::gyro_odometer
-
-#endif  // GYRO_ODOMETER__DIAGNOSTICS_MODULE_HPP_
diff --git a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp
index f1f7214c3b848..f569ed25a0c7f 100644
--- a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp
+++ b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp
@@ -18,7 +18,7 @@
 #include "autoware/universe_utils/ros/logger_level_configure.hpp"
 #include "autoware/universe_utils/ros/msg_covariance.hpp"
 #include "autoware/universe_utils/ros/transform_listener.hpp"
-#include "gyro_odometer/diagnostics_module.hpp"
+#include "localization_util/diagnostics_module.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
diff --git a/localization/gyro_odometer/package.xml b/localization/gyro_odometer/package.xml
index faa64a1313ebb..ba08a8852eca5 100644
--- a/localization/gyro_odometer/package.xml
+++ b/localization/gyro_odometer/package.xml
@@ -21,6 +21,7 @@
   <depend>diagnostic_msgs</depend>
   <depend>fmt</depend>
   <depend>geometry_msgs</depend>
+  <depend>localization_util</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>sensor_msgs</depend>
diff --git a/localization/gyro_odometer/src/diagnostics_module.cpp b/localization/gyro_odometer/src/diagnostics_module.cpp
deleted file mode 100644
index 5d687943cef53..0000000000000
--- a/localization/gyro_odometer/src/diagnostics_module.cpp
+++ /dev/null
@@ -1,110 +0,0 @@
-// Copyright 2023 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 "gyro_odometer/diagnostics_module.hpp"
-
-#include <rclcpp/rclcpp.hpp>
-
-#include <diagnostic_msgs/msg/diagnostic_array.hpp>
-
-#include <algorithm>
-#include <string>
-
-namespace autoware::gyro_odometer
-{
-
-DiagnosticsModule::DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name)
-: clock_(node->get_clock())
-{
-  diagnostics_pub_ =
-    node->create_publisher<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics", 10);
-
-  diagnostics_status_msg_.name =
-    std::string(node->get_name()) + std::string(": ") + diagnostic_name;
-  diagnostics_status_msg_.hardware_id = node->get_name();
-}
-
-void DiagnosticsModule::clear()
-{
-  diagnostics_status_msg_.values.clear();
-  diagnostics_status_msg_.values.shrink_to_fit();
-
-  diagnostics_status_msg_.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
-  diagnostics_status_msg_.message = "";
-}
-
-void DiagnosticsModule::add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg)
-{
-  auto it = std::find_if(
-    std::begin(diagnostics_status_msg_.values), std::end(diagnostics_status_msg_.values),
-    [key_value_msg](const auto & arg) { return arg.key == key_value_msg.key; });
-
-  if (it != std::cend(diagnostics_status_msg_.values)) {
-    it->value = key_value_msg.value;
-  } else {
-    diagnostics_status_msg_.values.push_back(key_value_msg);
-  }
-}
-
-template <>
-void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value)
-{
-  diagnostic_msgs::msg::KeyValue key_value;
-  key_value.key = key;
-  key_value.value = value;
-  add_key_value(key_value);
-}
-
-template <>
-void DiagnosticsModule::add_key_value(const std::string & key, const bool & value)
-{
-  diagnostic_msgs::msg::KeyValue key_value;
-  key_value.key = key;
-  key_value.value = value ? "True" : "False";
-  add_key_value(key_value);
-}
-
-void DiagnosticsModule::update_level_and_message(const int8_t level, const std::string & message)
-{
-  if ((level > diagnostic_msgs::msg::DiagnosticStatus::OK)) {
-    if (!diagnostics_status_msg_.message.empty()) {
-      diagnostics_status_msg_.message += "; ";
-    }
-    diagnostics_status_msg_.message += message;
-  }
-  if (level > diagnostics_status_msg_.level) {
-    diagnostics_status_msg_.level = level;
-  }
-}
-
-void DiagnosticsModule::publish(const rclcpp::Time & publish_time_stamp)
-{
-  diagnostics_pub_->publish(create_diagnostics_array(publish_time_stamp));
-}
-
-diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::create_diagnostics_array(
-  const rclcpp::Time & publish_time_stamp) const
-{
-  diagnostic_msgs::msg::DiagnosticArray diagnostics_msg;
-  diagnostics_msg.header.stamp = publish_time_stamp;
-  diagnostics_msg.status.push_back(diagnostics_status_msg_);
-
-  if (diagnostics_msg.status.at(0).level == diagnostic_msgs::msg::DiagnosticStatus::OK) {
-    diagnostics_msg.status.at(0).message = "OK";
-  }
-
-  return diagnostics_msg;
-}
-
-}  // namespace autoware::gyro_odometer
diff --git a/localization/localization_error_monitor/package.xml b/localization/localization_error_monitor/package.xml
index 02602cbebff37..c4fa77ce85a6b 100644
--- a/localization/localization_error_monitor/package.xml
+++ b/localization/localization_error_monitor/package.xml
@@ -21,7 +21,6 @@
 
   <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>
diff --git a/localization/localization_util/package.xml b/localization/localization_util/package.xml
index bb1ca3123ad27..86c74bb92f2f8 100644
--- a/localization/localization_util/package.xml
+++ b/localization/localization_util/package.xml
@@ -17,21 +17,13 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
-  <depend>autoware_universe_utils</depend>
-  <depend>fmt</depend>
+  <depend>diagnostic_msgs</depend>
   <depend>geometry_msgs</depend>
-  <depend>nav_msgs</depend>
   <depend>rclcpp</depend>
-  <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
-  <depend>std_srvs</depend>
   <depend>tf2</depend>
   <depend>tf2_eigen</depend>
   <depend>tf2_geometry_msgs</depend>
-  <depend>tf2_ros</depend>
-  <depend>tf2_sensor_msgs</depend>
-  <depend>tier4_debug_msgs</depend>
-  <depend>tier4_localization_msgs</depend>
   <depend>visualization_msgs</depend>
 
   <test_depend>ament_cmake_cppcheck</test_depend>
diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml
index 9e2cacab4d1fc..5d6b8d73440e4 100644
--- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml
+++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml
@@ -117,7 +117,7 @@
         initial_pose_offset_model_y: [0.5, -0.5, 0.0, 0.0, 0.0, 0.0]
 
         # In MULTI_NDT_SCORE, the parameter that adjusts the estimated 2D covariance
-        temperature: 0.1
+        temperature: 0.05
 
         # Scale value for adjusting the estimated covariance by a constant multiplication
         scale_factor: 1.0
diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml
index 932ee55874cea..c88760d3db92c 100644
--- a/localization/ndt_scan_matcher/package.xml
+++ b/localization/ndt_scan_matcher/package.xml
@@ -30,7 +30,6 @@
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>sensor_msgs</depend>
-  <depend>std_msgs</depend>
   <depend>std_srvs</depend>
   <depend>tf2</depend>
   <depend>tf2_eigen</depend>
diff --git a/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json b/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json
index 81350f3d2db37..a60277f90fa64 100644
--- a/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json
+++ b/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json
@@ -25,7 +25,7 @@
         "temperature": {
           "type": "number",
           "description": "In MULTI_NDT_SCORE, the parameter that adjusts the estimated 2D covariance",
-          "default": 0.1,
+          "default": 0.05,
           "exclusiveMinimum": 0
         },
         "scale_factor": {
diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
index d64107b03ebba..a90ec30aa91ff 100644
--- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
+++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
@@ -834,7 +834,7 @@ Eigen::Matrix2d NDTScanMatcher::estimate_covariance(
   if (
     param_.covariance.covariance_estimation.covariance_estimation_type ==
     CovarianceEstimationType::LAPLACE_APPROXIMATION) {
-    return pclomp::estimate_xy_covariance_by_Laplace_approximation(ndt_result.hessian);
+    return pclomp::estimate_xy_covariance_by_laplace_approximation(ndt_result.hessian);
   } else if (
     param_.covariance.covariance_estimation.covariance_estimation_type ==
     CovarianceEstimationType::MULTI_NDT) {
diff --git a/localization/pose_estimator_arbiter/package.xml b/localization/pose_estimator_arbiter/package.xml
index 1c1c926c75b7a..d97c9f15fc8ae 100644
--- a/localization/pose_estimator_arbiter/package.xml
+++ b/localization/pose_estimator_arbiter/package.xml
@@ -25,15 +25,11 @@
   <depend>geometry_msgs</depend>
   <depend>magic_enum</depend>
   <depend>pcl_conversions</depend>
-  <depend>pcl_ros</depend>
-  <depend>pluginlib</depend>
   <depend>rclcpp_components</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>
-  <depend>ublox_msgs</depend>
   <depend>visualization_msgs</depend>
-  <depend>yabloc_particle_filter</depend>
 
   <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>
diff --git a/localization/pose_instability_detector/package.xml b/localization/pose_instability_detector/package.xml
index 04e9747d3723b..7e0237cbaf780 100644
--- a/localization/pose_instability_detector/package.xml
+++ b/localization/pose_instability_detector/package.xml
@@ -17,7 +17,9 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
-  <depend>ament_index_cpp</depend>
+  <test_depend>ament_cmake_cppcheck</test_depend>
+  <test_depend>ament_index_cpp</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
   <depend>autoware_universe_utils</depend>
   <depend>diagnostic_msgs</depend>
   <depend>geometry_msgs</depend>
@@ -26,10 +28,6 @@
   <depend>rclcpp_components</depend>
   <depend>tf2</depend>
   <depend>tf2_geometry_msgs</depend>
-  <depend>tier4_debug_msgs</depend>
-
-  <test_depend>ament_cmake_cppcheck</test_depend>
-  <test_depend>ament_lint_auto</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>
diff --git a/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp b/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp
index 52752bff65770..d96285c0d5c00 100644
--- a/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp
+++ b/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp
@@ -40,7 +40,7 @@ GroundServer::GroundServer(const rclcpp::NodeOptions & options)
 {
   using std::placeholders::_1;
   using std::placeholders::_2;
-  const rclcpp::QoS map_qos = rclcpp::QoS(10).transient_local().reliable();
+  const rclcpp::QoS map_qos = rclcpp::QoS(1).transient_local().reliable();
 
   auto on_pose = std::bind(&GroundServer::on_pose_stamped, this, _1);
   auto on_map = std::bind(&GroundServer::on_map, this, _1);
diff --git a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp
index c95431d128000..6c3d43f33440f 100644
--- a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp
+++ b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp
@@ -27,8 +27,8 @@ namespace yabloc::ll2_decomposer
 Ll2Decomposer::Ll2Decomposer(const rclcpp::NodeOptions & options) : Node("ll2_to_image", options)
 {
   using std::placeholders::_1;
-  const rclcpp::QoS latch_qos = rclcpp::QoS(10).transient_local();
-  const rclcpp::QoS map_qos = rclcpp::QoS(10).transient_local().reliable();
+  const rclcpp::QoS latch_qos = rclcpp::QoS(1).transient_local();
+  const rclcpp::QoS map_qos = rclcpp::QoS(1).transient_local().reliable();
 
   // Publisher
   pub_road_marking_ = create_publisher<Cloud2>("~/output/ll2_road_marking", latch_qos);
diff --git a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp
index b011799e5b912..4e3728be53171 100644
--- a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp
+++ b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp
@@ -32,8 +32,7 @@ VoxelGridMapLoader::VoxelGridMapLoader(
 }
 
 bool VoxelGridMapLoader::is_close_points(
-  const pcl::PointXYZ point, const pcl::PointXYZ target_point,
-  const double distance_threshold) const
+  const pcl::PointXYZ point, const pcl::PointXYZ target_point, const double distance_threshold)
 {
   if (distance3D(point, target_point) < distance_threshold * distance_threshold) {
     return true;
@@ -52,7 +51,7 @@ void VoxelGridMapLoader::publish_downsampled_map(
 
 bool VoxelGridMapLoader::is_close_to_neighbor_voxels(
   const pcl::PointXYZ & point, const double distance_threshold, VoxelGridPointXYZ & voxel,
-  pcl::search::Search<pcl::PointXYZ>::Ptr tree) const
+  pcl::search::Search<pcl::PointXYZ>::Ptr tree)
 {
   const int index = voxel.getCentroidIndexAt(voxel.getGridCoordinates(point.x, point.y, point.z));
   if (index != -1) {
diff --git a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp
index b13f2e537ee55..6560ae82f8bce 100644
--- a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp
+++ b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp
@@ -97,7 +97,7 @@ class VoxelGridMapLoader
   rclcpp::Logger logger_;
   std::mutex * mutex_ptr_;
   double voxel_leaf_size_;
-  double voxel_leaf_size_z_;
+  double voxel_leaf_size_z_{};
   double downsize_ratio_z_axis_;
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr downsampled_map_pub_;
   bool debug_ = false;
@@ -111,9 +111,9 @@ class VoxelGridMapLoader
     std::string * tf_map_input_frame, std::mutex * mutex);
 
   virtual bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) = 0;
-  bool is_close_to_neighbor_voxels(
+  static bool is_close_to_neighbor_voxels(
     const pcl::PointXYZ & point, const double distance_threshold, VoxelGridPointXYZ & voxel,
-    pcl::search::Search<pcl::PointXYZ>::Ptr tree) const;
+    pcl::search::Search<pcl::PointXYZ>::Ptr tree);
   bool is_close_to_neighbor_voxels(
     const pcl::PointXYZ & point, const double distance_threshold, const PointCloudPtr & map,
     VoxelGridPointXYZ & voxel) const;
@@ -122,9 +122,8 @@ class VoxelGridMapLoader
     const double distance_threshold, const PointCloudPtr & map, VoxelGridPointXYZ & voxel) const;
 
   void publish_downsampled_map(const pcl::PointCloud<pcl::PointXYZ> & downsampled_pc);
-  bool is_close_points(
-    const pcl::PointXYZ point, const pcl::PointXYZ target_point,
-    const double distance_threshold) const;
+  static bool is_close_points(
+    const pcl::PointXYZ point, const pcl::PointXYZ target_point, const double distance_threshold);
   std::string * tf_map_input_frame_;
 };
 
@@ -224,7 +223,7 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
   }
   inline void updateDifferentialMapCells(
     const std::vector<autoware_map_msgs::msg::PointCloudMapCellWithID> & map_cells_to_add,
-    std::vector<std::string> map_cell_ids_to_remove)
+    const std::vector<std::string> & map_cell_ids_to_remove)
   {
     for (const auto & map_cell_to_add : map_cells_to_add) {
       addMapCellAndFilter(map_cell_to_add);
@@ -271,7 +270,7 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
     (*mutex_ptr_).unlock();
   }
 
-  inline void removeMapCell(const std::string map_cell_id_to_remove)
+  inline void removeMapCell(const std::string & map_cell_id_to_remove)
   {
     (*mutex_ptr_).lock();
     current_voxel_grid_dict_.erase(map_cell_id_to_remove);
diff --git a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp
index 6437134bc98a5..aed0ba5ab85ea 100644
--- a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp
+++ b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp
@@ -15,6 +15,7 @@
 #include "lanelet_filter.hpp"
 
 #include "autoware/universe_utils/geometry/geometry.hpp"
+#include "autoware/universe_utils/system/time_keeper.hpp"
 #include "autoware_lanelet2_extension/utility/message_conversion.hpp"
 #include "autoware_lanelet2_extension/utility/query.hpp"
 #include "object_recognition_utils/object_recognition_utils.hpp"
@@ -22,8 +23,10 @@
 #include <boost/geometry/algorithms/convex_hull.hpp>
 #include <boost/geometry/algorithms/disjoint.hpp>
 #include <boost/geometry/algorithms/intersects.hpp>
+#include <boost/geometry/index/rtree.hpp>
 
 #include <lanelet2_core/geometry/BoundingBox.h>
+#include <lanelet2_core/geometry/Lanelet.h>
 #include <lanelet2_core/geometry/LaneletMap.h>
 #include <lanelet2_core/geometry/Polygon.h>
 
@@ -100,18 +103,27 @@ void ObjectLaneletFilterNode::objectCallback(
     return;
   }
 
-  // calculate convex hull
-  const auto convex_hull = getConvexHull(transformed_objects);
+  if (!transformed_objects.objects.empty()) {
+    // calculate convex hull
+    const auto convex_hull = getConvexHull(transformed_objects);
 
-  // get intersected lanelets
-  lanelet::ConstLanelets intersected_lanelets = getIntersectedLanelets(convex_hull);
+    // get intersected lanelets
+    std::vector<BoxAndLanelet> intersected_lanelets_with_bbox = getIntersectedLanelets(convex_hull);
 
-  // filtering process
-  for (size_t index = 0; index < transformed_objects.objects.size(); ++index) {
-    const auto & transformed_object = transformed_objects.objects.at(index);
-    const auto & input_object = input_msg->objects.at(index);
-    filterObject(transformed_object, input_object, intersected_lanelets, output_object_msg);
+    // create R-Tree with intersected_lanelets for fast search
+    bgi::rtree<BoxAndLanelet, RtreeAlgo> local_rtree;
+    for (const auto & bbox_and_lanelet : intersected_lanelets_with_bbox) {
+      local_rtree.insert(bbox_and_lanelet);
+    }
+
+    // filtering process
+    for (size_t index = 0; index < transformed_objects.objects.size(); ++index) {
+      const auto & transformed_object = transformed_objects.objects.at(index);
+      const auto & input_object = input_msg->objects.at(index);
+      filterObject(transformed_object, input_object, local_rtree, output_object_msg);
+    }
   }
+
   object_pub_->publish(output_object_msg);
   published_time_publisher_->publish_if_subscribed(object_pub_, output_object_msg.header.stamp);
 
@@ -128,16 +140,20 @@ void ObjectLaneletFilterNode::objectCallback(
 bool ObjectLaneletFilterNode::filterObject(
   const autoware_perception_msgs::msg::DetectedObject & transformed_object,
   const autoware_perception_msgs::msg::DetectedObject & input_object,
-  const lanelet::ConstLanelets & intersected_lanelets,
+  const bgi::rtree<BoxAndLanelet, RtreeAlgo> & local_rtree,
   autoware_perception_msgs::msg::DetectedObjects & output_object_msg)
 {
   const auto & label = transformed_object.classification.front().label;
   if (filter_target_.isTarget(label)) {
+    // no tree, then no intersection
+    if (local_rtree.empty()) {
+      return false;
+    }
+
     bool filter_pass = true;
     // 1. is polygon overlap with road lanelets or shoulder lanelets
     if (filter_settings_.polygon_overlap_filter) {
-      const bool is_polygon_overlap =
-        isObjectOverlapLanelets(transformed_object, intersected_lanelets);
+      const bool is_polygon_overlap = isObjectOverlapLanelets(transformed_object, local_rtree);
       filter_pass = filter_pass && is_polygon_overlap;
     }
 
@@ -146,8 +162,7 @@ bool ObjectLaneletFilterNode::filterObject(
       transformed_object.kinematics.orientation_availability ==
       autoware_perception_msgs::msg::TrackedObjectKinematics::UNAVAILABLE;
     if (filter_settings_.lanelet_direction_filter && !orientation_not_available) {
-      const bool is_same_direction =
-        isSameDirectionWithLanelets(intersected_lanelets, transformed_object);
+      const bool is_same_direction = isSameDirectionWithLanelets(transformed_object, local_rtree);
       filter_pass = filter_pass && is_same_direction;
     }
 
@@ -199,55 +214,75 @@ LinearRing2d ObjectLaneletFilterNode::getConvexHull(
       candidate_points.emplace_back(p.x + pos.x, p.y + pos.y);
     }
   }
+  LinearRing2d convex_hull;
+  bg::convex_hull(candidate_points, convex_hull);
+
+  return convex_hull;
+}
+
+LinearRing2d ObjectLaneletFilterNode::getConvexHullFromObjectFootprint(
+  const autoware_perception_msgs::msg::DetectedObject & object)
+{
+  MultiPoint2d candidate_points;
+  const auto & pos = object.kinematics.pose_with_covariance.pose.position;
+  const auto footprint = setFootprint(object);
+
+  for (const auto & p : footprint.points) {
+    candidate_points.emplace_back(p.x + pos.x, p.y + pos.y);
+  }
 
   LinearRing2d convex_hull;
-  boost::geometry::convex_hull(candidate_points, convex_hull);
+  bg::convex_hull(candidate_points, convex_hull);
 
   return convex_hull;
 }
 
 // fetch the intersected candidate lanelets with bounding box and then
 // check the intersections among the lanelets and the convex hull
-lanelet::ConstLanelets ObjectLaneletFilterNode::getIntersectedLanelets(
+std::vector<BoxAndLanelet> ObjectLaneletFilterNode::getIntersectedLanelets(
   const LinearRing2d & convex_hull)
 {
-  namespace bg = boost::geometry;
-
-  lanelet::ConstLanelets intersected_lanelets;
+  std::vector<BoxAndLanelet> intersected_lanelets_with_bbox;
 
   // convert convex_hull to a 2D bounding box for searching in the LaneletMap
-  bg::model::box<bg::model::d2::point_xy<double>> bbox2d_convex_hull;
-  bg::envelope(convex_hull, bbox2d_convex_hull);
-  lanelet::BoundingBox2d bbox2d(
+  bg::model::box<bg::model::d2::point_xy<double>> bbox_of_convex_hull;
+  bg::envelope(convex_hull, bbox_of_convex_hull);
+  const lanelet::BoundingBox2d bbox2d(
     lanelet::BasicPoint2d(
-      bg::get<bg::min_corner, 0>(bbox2d_convex_hull),
-      bg::get<bg::min_corner, 1>(bbox2d_convex_hull)),
+      bg::get<bg::min_corner, 0>(bbox_of_convex_hull),
+      bg::get<bg::min_corner, 1>(bbox_of_convex_hull)),
     lanelet::BasicPoint2d(
-      bg::get<bg::max_corner, 0>(bbox2d_convex_hull),
-      bg::get<bg::max_corner, 1>(bbox2d_convex_hull)));
+      bg::get<bg::max_corner, 0>(bbox_of_convex_hull),
+      bg::get<bg::max_corner, 1>(bbox_of_convex_hull)));
 
-  lanelet::Lanelets candidates_lanelets = lanelet_map_ptr_->laneletLayer.search(bbox2d);
-  for (const auto & lanelet : candidates_lanelets) {
+  const lanelet::Lanelets candidate_lanelets = lanelet_map_ptr_->laneletLayer.search(bbox2d);
+  for (const auto & lanelet : candidate_lanelets) {
     // only check the road lanelets and road shoulder lanelets
     if (
       lanelet.hasAttribute(lanelet::AttributeName::Subtype) &&
       (lanelet.attribute(lanelet::AttributeName::Subtype).value() ==
          lanelet::AttributeValueString::Road ||
        lanelet.attribute(lanelet::AttributeName::Subtype).value() == "road_shoulder")) {
-      if (boost::geometry::intersects(convex_hull, lanelet.polygon2d().basicPolygon())) {
-        intersected_lanelets.emplace_back(lanelet);
+      if (bg::intersects(convex_hull, lanelet.polygon2d().basicPolygon())) {
+        // create bbox using boost for making the R-tree in later phase
+        lanelet::BoundingBox2d lanelet_bbox = lanelet::geometry::boundingBox2d(lanelet);
+        Point2d min_corner(lanelet_bbox.min().x(), lanelet_bbox.min().y());
+        Point2d max_corner(lanelet_bbox.max().x(), lanelet_bbox.max().y());
+        Box boost_bbox(min_corner, max_corner);
+
+        intersected_lanelets_with_bbox.emplace_back(std::make_pair(boost_bbox, lanelet));
       }
     }
   }
 
-  return intersected_lanelets;
+  return intersected_lanelets_with_bbox;
 }
 
 bool ObjectLaneletFilterNode::isObjectOverlapLanelets(
   const autoware_perception_msgs::msg::DetectedObject & object,
-  const lanelet::ConstLanelets & intersected_lanelets)
+  const bgi::rtree<BoxAndLanelet, RtreeAlgo> & local_rtree)
 {
-  // if has bounding box, use polygon overlap
+  // if object has bounding box, use polygon overlap
   if (utils::hasBoundingBox(object)) {
     Polygon2d polygon;
     const auto footprint = setFootprint(object);
@@ -258,8 +293,17 @@ bool ObjectLaneletFilterNode::isObjectOverlapLanelets(
       polygon.outer().emplace_back(point_transformed.x, point_transformed.y);
     }
     polygon.outer().push_back(polygon.outer().front());
-    return isPolygonOverlapLanelets(polygon, intersected_lanelets);
+
+    return isPolygonOverlapLanelets(polygon, local_rtree);
   } else {
+    const LinearRing2d object_convex_hull = getConvexHullFromObjectFootprint(object);
+
+    // create bounding box to search in the rtree
+    std::vector<BoxAndLanelet> candidates;
+    bg::model::box<bg::model::d2::point_xy<double>> bbox;
+    bg::envelope(object_convex_hull, bbox);
+    local_rtree.query(bgi::intersects(bbox), std::back_inserter(candidates));
+
     // if object do not have bounding box, check each footprint is inside polygon
     for (const auto & point : object.shape.footprint.points) {
       const geometry_msgs::msg::Point32 point_transformed =
@@ -268,30 +312,39 @@ bool ObjectLaneletFilterNode::isObjectOverlapLanelets(
       geometry_msgs::msg::Pose point2d;
       point2d.position.x = point_transformed.x;
       point2d.position.y = point_transformed.y;
-      for (const auto & lanelet : intersected_lanelets) {
-        if (lanelet::utils::isInLanelet(point2d, lanelet, 0.0)) {
+
+      for (const auto & candidate : candidates) {
+        if (lanelet::utils::isInLanelet(point2d, candidate.second, 0.0)) {
           return true;
         }
       }
     }
+
     return false;
   }
 }
 
 bool ObjectLaneletFilterNode::isPolygonOverlapLanelets(
-  const Polygon2d & polygon, const lanelet::ConstLanelets & intersected_lanelets)
+  const Polygon2d & polygon, const bgi::rtree<BoxAndLanelet, RtreeAlgo> & local_rtree)
 {
-  for (const auto & lanelet : intersected_lanelets) {
-    if (!boost::geometry::disjoint(polygon, lanelet.polygon2d().basicPolygon())) {
+  // create a bounding box from polygon for searching the local R-tree
+  std::vector<BoxAndLanelet> candidates;
+  bg::model::box<bg::model::d2::point_xy<double>> bbox_of_convex_hull;
+  bg::envelope(polygon, bbox_of_convex_hull);
+  local_rtree.query(bgi::intersects(bbox_of_convex_hull), std::back_inserter(candidates));
+
+  for (const auto & box_and_lanelet : candidates) {
+    if (!bg::disjoint(polygon, box_and_lanelet.second.polygon2d().basicPolygon())) {
       return true;
     }
   }
+
   return false;
 }
 
 bool ObjectLaneletFilterNode::isSameDirectionWithLanelets(
-  const lanelet::ConstLanelets & lanelets,
-  const autoware_perception_msgs::msg::DetectedObject & object)
+  const autoware_perception_msgs::msg::DetectedObject & object,
+  const bgi::rtree<BoxAndLanelet, RtreeAlgo> & local_rtree)
 {
   const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation);
   const double object_velocity_norm = std::hypot(
@@ -305,14 +358,30 @@ bool ObjectLaneletFilterNode::isSameDirectionWithLanelets(
   if (object_velocity_norm < filter_settings_.lanelet_direction_filter_object_speed_threshold) {
     return true;
   }
-  for (const auto & lanelet : lanelets) {
-    const bool is_in_lanelet =
-      lanelet::utils::isInLanelet(object.kinematics.pose_with_covariance.pose, lanelet, 0.0);
+
+  // we can not query by points, so create a small bounding box
+  // eps is not determined by any specific criteria; just a guess
+  constexpr double eps = 1.0e-6;
+  std::vector<BoxAndLanelet> candidates;
+  const Point2d min_corner(
+    object.kinematics.pose_with_covariance.pose.position.x - eps,
+    object.kinematics.pose_with_covariance.pose.position.y - eps);
+  const Point2d max_corner(
+    object.kinematics.pose_with_covariance.pose.position.x + eps,
+    object.kinematics.pose_with_covariance.pose.position.y + eps);
+  const Box bbox(min_corner, max_corner);
+
+  local_rtree.query(bgi::intersects(bbox), std::back_inserter(candidates));
+
+  for (const auto & box_and_lanelet : candidates) {
+    const bool is_in_lanelet = lanelet::utils::isInLanelet(
+      object.kinematics.pose_with_covariance.pose, box_and_lanelet.second, 0.0);
     if (!is_in_lanelet) {
       continue;
     }
+
     const double lane_yaw = lanelet::utils::getLaneletAngle(
-      lanelet, object.kinematics.pose_with_covariance.pose.position);
+      box_and_lanelet.second, object.kinematics.pose_with_covariance.pose.position);
     const double delta_yaw = object_velocity_yaw - lane_yaw;
     const double normalized_delta_yaw = autoware::universe_utils::normalizeRadian(delta_yaw);
     const double abs_norm_delta_yaw = std::fabs(normalized_delta_yaw);
diff --git a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp
index 3f7a53d0319be..67713e86de71e 100644
--- a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp
+++ b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp
@@ -26,12 +26,17 @@
 #include "autoware_map_msgs/msg/lanelet_map_bin.hpp"
 #include "autoware_perception_msgs/msg/detected_objects.hpp"
 
+#include <boost/geometry/index/rtree.hpp>
+
 #include <lanelet2_core/Forward.h>
+#include <lanelet2_core/geometry/Lanelet.h>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 
 #include <memory>
 #include <string>
+#include <utility>
+#include <vector>
 
 namespace autoware::detected_object_validation
 {
@@ -42,6 +47,13 @@ using autoware::universe_utils::MultiPoint2d;
 using autoware::universe_utils::Point2d;
 using autoware::universe_utils::Polygon2d;
 
+namespace bg = boost::geometry;
+namespace bgi = boost::geometry::index;
+using Point2d = bg::model::point<double, 2, bg::cs::cartesian>;
+using Box = boost::geometry::model::box<Point2d>;
+using BoxAndLanelet = std::pair<Box, lanelet::Lanelet>;
+using RtreeAlgo = bgi::rstar<16>;
+
 class ObjectLaneletFilterNode : public rclcpp::Node
 {
 public:
@@ -74,17 +86,20 @@ class ObjectLaneletFilterNode : public rclcpp::Node
   bool filterObject(
     const autoware_perception_msgs::msg::DetectedObject & transformed_object,
     const autoware_perception_msgs::msg::DetectedObject & input_object,
-    const lanelet::ConstLanelets & intersected_lanelets,
+    const bg::index::rtree<BoxAndLanelet, RtreeAlgo> & local_rtree,
     autoware_perception_msgs::msg::DetectedObjects & output_object_msg);
   LinearRing2d getConvexHull(const autoware_perception_msgs::msg::DetectedObjects &);
-  lanelet::ConstLanelets getIntersectedLanelets(const LinearRing2d &);
+  LinearRing2d getConvexHullFromObjectFootprint(
+    const autoware_perception_msgs::msg::DetectedObject & object);
+  std::vector<BoxAndLanelet> getIntersectedLanelets(const LinearRing2d &);
   bool isObjectOverlapLanelets(
     const autoware_perception_msgs::msg::DetectedObject & object,
-    const lanelet::ConstLanelets & intersected_lanelets);
-  bool isPolygonOverlapLanelets(const Polygon2d &, const lanelet::ConstLanelets &);
+    const bg::index::rtree<BoxAndLanelet, RtreeAlgo> & local_rtree);
+  bool isPolygonOverlapLanelets(
+    const Polygon2d & polygon, const bgi::rtree<BoxAndLanelet, RtreeAlgo> & local_rtree);
   bool isSameDirectionWithLanelets(
-    const lanelet::ConstLanelets & lanelets,
-    const autoware_perception_msgs::msg::DetectedObject & object);
+    const autoware_perception_msgs::msg::DetectedObject & object,
+    const bgi::rtree<BoxAndLanelet, RtreeAlgo> & local_rtree);
   geometry_msgs::msg::Polygon setFootprint(const autoware_perception_msgs::msg::DetectedObject &);
 
   std::unique_ptr<autoware::universe_utils::PublishedTimePublisher> published_time_publisher_;
diff --git a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/debugger.hpp b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/debugger.hpp
index 5886474d51b34..e097755c6c506 100644
--- a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/debugger.hpp
+++ b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/debugger.hpp
@@ -97,7 +97,7 @@ class Debugger
   pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud_within_polygon_;
 
 private:
-  inline pcl::PointCloud<pcl::PointXYZ>::Ptr toXYZ(
+  inline static pcl::PointCloud<pcl::PointXYZ>::Ptr toXYZ(
     const pcl::PointCloud<pcl::PointXY>::Ptr & pointcloud)
   {
     pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud_xyz(new pcl::PointCloud<pcl::PointXYZ>);
diff --git a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp
index 801a693905f93..6d9a11634ea08 100644
--- a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp
+++ b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp
@@ -65,7 +65,7 @@ class Validator
 
 public:
   explicit Validator(const PointsNumThresholdParam & points_num_threshold_param);
-  inline pcl::PointCloud<pcl::PointXYZ>::Ptr getDebugPointCloudWithinObject()
+  inline pcl::PointCloud<pcl::PointXYZ>::Ptr getDebugPointCloudWithinObject() const
   {
     return cropped_pointcloud_;
   }
@@ -92,7 +92,7 @@ class Validator2D : public Validator
 public:
   explicit Validator2D(PointsNumThresholdParam & points_num_threshold_param);
 
-  pcl::PointCloud<pcl::PointXYZ>::Ptr convertToXYZ(
+  static pcl::PointCloud<pcl::PointXYZ>::Ptr convertToXYZ(
     const pcl::PointCloud<pcl::PointXY>::Ptr & pointcloud_xy);
   inline pcl::PointCloud<pcl::PointXYZ>::Ptr getDebugNeighborPointCloud() override
   {
diff --git a/perception/autoware_detection_by_tracker/src/debugger/debugger.hpp b/perception/autoware_detection_by_tracker/src/debugger/debugger.hpp
index bda42aa5f0b55..39dd3db10cfa9 100644
--- a/perception/autoware_detection_by_tracker/src/debugger/debugger.hpp
+++ b/perception/autoware_detection_by_tracker/src/debugger/debugger.hpp
@@ -104,7 +104,7 @@ class Debugger
   std::unique_ptr<autoware::universe_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
   std::unique_ptr<autoware::universe_utils::DebugPublisher> processing_time_publisher_;
 
-  autoware_perception_msgs::msg::DetectedObjects removeFeature(
+  static autoware_perception_msgs::msg::DetectedObjects removeFeature(
     const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input)
   {
     autoware_perception_msgs::msg::DetectedObjects objects;
diff --git a/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.cpp b/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.cpp
index dd5ce96a858cd..4d45f5df07027 100644
--- a/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.cpp
+++ b/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.cpp
@@ -115,6 +115,8 @@ RANSACGroundFilterComponent::RANSACGroundFilterComponent(const rclcpp::NodeOptio
     std::bind(&RANSACGroundFilterComponent::paramCallback, this, _1));
 
   pcl::console::setVerbosityLevel(pcl::console::L_ALWAYS);
+
+  static_tf_buffer_ = std::make_unique<autoware::universe_utils::StaticTransformBuffer>();
 }
 
 void RANSACGroundFilterComponent::setDebugPublisher()
@@ -161,20 +163,8 @@ bool RANSACGroundFilterComponent::transformPointCloud(
     return true;
   }
 
-  geometry_msgs::msg::TransformStamped transform_stamped;
-  try {
-    transform_stamped = tf_buffer_->lookupTransform(
-      in_target_frame, in_cloud_ptr->header.frame_id, in_cloud_ptr->header.stamp,
-      rclcpp::Duration::from_seconds(1.0));
-  } catch (tf2::TransformException & ex) {
-    RCLCPP_WARN(this->get_logger(), "%s", ex.what());
-    return false;
-  }
-  // tf2::doTransform(*in_cloud_ptr, *out_cloud_ptr, transform_stamped);
-  Eigen::Matrix4f mat = tf2::transformToEigen(transform_stamped.transform).matrix().cast<float>();
-  pcl_ros::transformPointCloud(mat, *in_cloud_ptr, *out_cloud_ptr);
-  out_cloud_ptr->header.frame_id = in_target_frame;
-  return true;
+  return static_tf_buffer_->transformPointcloud(
+    this, in_target_frame, *in_cloud_ptr, *out_cloud_ptr);
 }
 
 void RANSACGroundFilterComponent::extractPointsIndices(
diff --git a/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.hpp b/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.hpp
index e173fe131602f..bdc217e80650c 100644
--- a/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.hpp
+++ b/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.hpp
@@ -17,6 +17,8 @@
 
 #include "autoware/pointcloud_preprocessor/filter.hpp"
 
+#include <autoware/universe_utils/ros/static_transform_buffer.hpp>
+
 #include <geometry_msgs/msg/pose_array.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 
@@ -35,6 +37,7 @@
 #include <tf2_ros/transform_listener.h>
 
 #include <chrono>
+#include <memory>
 #include <string>
 #include <vector>
 
@@ -80,6 +83,7 @@ class RANSACGroundFilterComponent : public autoware::pointcloud_preprocessor::Fi
   bool debug_ = false;
   bool is_initialized_debug_message_ = false;
   Eigen::Vector3d unit_vec_ = Eigen::Vector3d::UnitZ();
+  std::unique_ptr<autoware::universe_utils::StaticTransformBuffer> static_tf_buffer_{nullptr};
 
   /*!
    * Output transformed PointCloud from in_cloud_ptr->header.frame_id to in_target_frame
diff --git a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp
index ad5d0ea4b6bc6..94ef16042ff33 100644
--- a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp
+++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp
@@ -91,7 +91,14 @@ class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filt
     std::vector<float> height_list;
 
     PointsCentroid()
-    : radius_sum(0.0f), height_sum(0.0f), radius_avg(0.0f), height_avg(0.0f), point_num(0)
+    : radius_sum(0.0f),
+      height_sum(0.0f),
+      radius_avg(0.0f),
+      height_avg(0.0f),
+      height_max(0.0f),
+      height_min(10.0f),
+      point_num(0),
+      grid_id(0)
     {
     }
 
@@ -128,18 +135,18 @@ class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filt
 
     float getAverageSlope() { return std::atan2(height_avg, radius_avg); }
 
-    float getAverageHeight() { return height_avg; }
+    float getAverageHeight() const { return height_avg; }
 
-    float getAverageRadius() { return radius_avg; }
+    float getAverageRadius() const { return radius_avg; }
 
-    float getMaxHeight() { return height_max; }
+    float getMaxHeight() const { return height_max; }
 
-    float getMinHeight() { return height_min; }
+    float getMinHeight() const { return height_min; }
 
-    uint16_t getGridId() { return grid_id; }
+    uint16_t getGridId() const { return grid_id; }
 
-    pcl::PointIndices getIndices() { return pcl_indices; }
-    std::vector<float> getHeightList() { return height_list; }
+    pcl::PointIndices getIndices() const { return pcl_indices; }
+    std::vector<float> getHeightList() const { return height_list; }
 
     pcl::PointIndices & getIndicesRef() { return pcl_indices; }
     std::vector<float> & getHeightListRef() { return height_list; }
diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp
index 702b085dd31d7..cfbb2fa4a18cb 100644
--- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp
+++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp
@@ -80,7 +80,7 @@ void closest_cluster(
 
 void updateOutputFusedObjects(
   std::vector<DetectedObjectWithFeature> & output_objs, std::vector<PointCloud2> & clusters,
-  const std::vector<size_t> clusters_data_size, const PointCloud2 & in_cloud,
+  const std::vector<size_t> & clusters_data_size, const PointCloud2 & in_cloud,
   const std_msgs::msg::Header & in_roi_header, const tf2_ros::Buffer & tf_buffer,
   const int min_cluster_size, const int max_cluster_size, const float cluster_2d_tolerance,
   std::vector<DetectedObjectWithFeature> & output_fused_objects);
diff --git a/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp b/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp
index 0432242417e19..ac17c419efca4 100644
--- a/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp
+++ b/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp
@@ -118,7 +118,7 @@ void closest_cluster(
 
 void updateOutputFusedObjects(
   std::vector<DetectedObjectWithFeature> & output_objs, std::vector<PointCloud2> & clusters,
-  const std::vector<size_t> clusters_data_size, const PointCloud2 & in_cloud,
+  const std::vector<size_t> & clusters_data_size, const PointCloud2 & in_cloud,
   const std_msgs::msg::Header & in_roi_header, const tf2_ros::Buffer & tf_buffer,
   const int min_cluster_size, const int max_cluster_size, const float cluster_2d_tolerance,
   std::vector<DetectedObjectWithFeature> & output_fused_objects)
diff --git a/perception/autoware_lidar_centerpoint/README.md b/perception/autoware_lidar_centerpoint/README.md
index 83c18b76fd079..471712a637dec 100644
--- a/perception/autoware_lidar_centerpoint/README.md
+++ b/perception/autoware_lidar_centerpoint/README.md
@@ -28,6 +28,22 @@ We trained the models using <https://github.com/open-mmlab/mmdetection3d>.
 
 ## Parameters
 
+### ML Model Parameters
+
+Note that these parameters are associated with ONNX file, predefined during the training phase. Be careful to change ONNX file as well when changing this parameter. Also, whenever you update the ONNX file, do NOT forget to check these values.
+
+| Name                                   | Type         | Default Value                                    | Description                                                           |
+| -------------------------------------- | ------------ | ------------------------------------------------ | --------------------------------------------------------------------- |
+| `model_params.class_names`             | list[string] | ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] | list of class names for model outputs                                 |
+| `model_params.point_feature_size`      | int          | `4`                                              | number of features per point in the point cloud                       |
+| `model_params.max_voxel_size`          | int          | `40000`                                          | maximum number of voxels                                              |
+| `model_params.point_cloud_range`       | list[double] | [-76.8, -76.8, -4.0, 76.8, 76.8, 6.0]            | detection range [min_x, min_y, min_z, max_x, max_y, max_z] [m]        |
+| `model_params.voxel_size`              | list[double] | [0.32, 0.32, 10.0]                               | size of each voxel [x, y, z] [m]                                      |
+| `model_params.downsample_factor`       | int          | `1`                                              | downsample factor for coordinates                                     |
+| `model_params.encoder_in_feature_size` | int          | `9`                                              | number of input features to the encoder                               |
+| `model_params.has_variance`            | bool         | `false`                                          | true if the model outputs pose variance as well as pose for each bbox |
+| `model_params.has_twist`               | bool         | `false`                                          | true if the model outputs velocity as well as pose for each bbox      |
+
 ### Core Parameters
 
 | Name                                             | Type         | Default Value             | Description                                                   |
diff --git a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/postprocess/postprocess_kernel.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/postprocess/postprocess_kernel.hpp
index 4792fed3e9362..6ea9965f4fac0 100644
--- a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/postprocess/postprocess_kernel.hpp
+++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/postprocess/postprocess_kernel.hpp
@@ -19,7 +19,6 @@
 #include "autoware/lidar_centerpoint/utils.hpp"
 #include "cuda.h"
 #include "cuda_runtime_api.h"
-#include "thrust/device_vector.h"
 
 #include <vector>
 
@@ -37,8 +36,6 @@ class PostProcessCUDA
 
 private:
   CenterPointConfig config_;
-  thrust::device_vector<Box3D> boxes3d_d_;
-  thrust::device_vector<float> yaw_norm_thresholds_d_;
 };
 
 }  // namespace autoware::lidar_centerpoint
diff --git a/perception/autoware_lidar_centerpoint/lib/postprocess/postprocess_kernel.cu b/perception/autoware_lidar_centerpoint/lib/postprocess/postprocess_kernel.cu
index e1af8f36eb082..81157e07991d1 100644
--- a/perception/autoware_lidar_centerpoint/lib/postprocess/postprocess_kernel.cu
+++ b/perception/autoware_lidar_centerpoint/lib/postprocess/postprocess_kernel.cu
@@ -137,10 +137,6 @@ __global__ void generateBoxes3D_kernel(
 
 PostProcessCUDA::PostProcessCUDA(const CenterPointConfig & config) : config_(config)
 {
-  const auto num_raw_boxes3d = config.down_grid_size_y_ * config.down_grid_size_x_;
-  boxes3d_d_ = thrust::device_vector<Box3D>(num_raw_boxes3d);
-  yaw_norm_thresholds_d_ = thrust::device_vector<float>(
-    config_.yaw_norm_thresholds_.begin(), config_.yaw_norm_thresholds_.end());
 }
 
 // cspell: ignore divup
@@ -153,23 +149,26 @@ cudaError_t PostProcessCUDA::generateDetectedBoxes3D_launch(
     divup(config_.down_grid_size_y_, THREADS_PER_BLOCK),
     divup(config_.down_grid_size_x_, THREADS_PER_BLOCK));
   dim3 threads(THREADS_PER_BLOCK, THREADS_PER_BLOCK);
+  auto boxes3d_d =
+    thrust::device_vector<Box3D>(config_.down_grid_size_y_ * config_.down_grid_size_x_);
+  auto yaw_norm_thresholds_d = thrust::device_vector<float>(
+    config_.yaw_norm_thresholds_.begin(), config_.yaw_norm_thresholds_.end());
   generateBoxes3D_kernel<<<blocks, threads, 0, stream>>>(
     out_heatmap, out_offset, out_z, out_dim, out_rot, out_vel, config_.voxel_size_x_,
     config_.voxel_size_y_, config_.range_min_x_, config_.range_min_y_, config_.down_grid_size_x_,
     config_.down_grid_size_y_, config_.downsample_factor_, config_.class_size_,
-    config_.has_variance_, thrust::raw_pointer_cast(yaw_norm_thresholds_d_.data()),
-    thrust::raw_pointer_cast(boxes3d_d_.data()));
+    config_.has_variance_, thrust::raw_pointer_cast(yaw_norm_thresholds_d.data()),
+    thrust::raw_pointer_cast(boxes3d_d.data()));
 
   // suppress by score
   const auto num_det_boxes3d = thrust::count_if(
-    thrust::device, boxes3d_d_.begin(), boxes3d_d_.end(),
-    is_score_greater(config_.score_threshold_));
+    thrust::device, boxes3d_d.begin(), boxes3d_d.end(), is_score_greater(config_.score_threshold_));
   if (num_det_boxes3d == 0) {
     return cudaGetLastError();
   }
   thrust::device_vector<Box3D> det_boxes3d_d(num_det_boxes3d);
   thrust::copy_if(
-    thrust::device, boxes3d_d_.begin(), boxes3d_d_.end(), det_boxes3d_d.begin(),
+    thrust::device, boxes3d_d.begin(), boxes3d_d.end(), det_boxes3d_d.begin(),
     is_score_greater(config_.score_threshold_));
 
   // sort by score
diff --git a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/postprocess/postprocess_kernel.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/postprocess/postprocess_kernel.hpp
index ed677bc84b867..6228fec48a833 100644
--- a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/postprocess/postprocess_kernel.hpp
+++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/postprocess/postprocess_kernel.hpp
@@ -20,7 +20,6 @@
 
 #include <cuda.h>
 #include <cuda_runtime_api.h>
-#include <thrust/device_vector.h>
 
 #include <vector>
 
@@ -41,8 +40,6 @@ class PostprocessCuda
   cudaStream_t stream_;
   cudaStream_t stream_event_;
   cudaEvent_t start_, stop_;
-  thrust::device_vector<Box3D> boxes3d_d_;
-  thrust::device_vector<float> yaw_norm_thresholds_d_;
 };
 
 }  // namespace autoware::lidar_transfusion
diff --git a/perception/autoware_lidar_transfusion/lib/postprocess/postprocess_kernel.cu b/perception/autoware_lidar_transfusion/lib/postprocess/postprocess_kernel.cu
index fbdb89d59b3d1..905b4ea6f39f4 100644
--- a/perception/autoware_lidar_transfusion/lib/postprocess/postprocess_kernel.cu
+++ b/perception/autoware_lidar_transfusion/lib/postprocess/postprocess_kernel.cu
@@ -93,9 +93,6 @@ __global__ void generateBoxes3D_kernel(
 PostprocessCuda::PostprocessCuda(const TransfusionConfig & config, cudaStream_t & stream)
 : config_(config), stream_(stream)
 {
-  boxes3d_d_ = thrust::device_vector<Box3D>(config_.num_proposals_);
-  yaw_norm_thresholds_d_ = thrust::device_vector<float>(
-    config_.yaw_norm_thresholds_.begin(), config_.yaw_norm_thresholds_.end());
 }
 
 // cspell: ignore divup
@@ -106,22 +103,25 @@ cudaError_t PostprocessCuda::generateDetectedBoxes3D_launch(
   dim3 threads = {THREADS_PER_BLOCK};
   dim3 blocks = {divup(config_.num_proposals_, threads.x)};
 
+  auto boxes3d_d = thrust::device_vector<Box3D>(config_.num_proposals_);
+  auto yaw_norm_thresholds_d = thrust::device_vector<float>(
+    config_.yaw_norm_thresholds_.begin(), config_.yaw_norm_thresholds_.end());
+
   generateBoxes3D_kernel<<<blocks, threads, 0, stream>>>(
     cls_output, box_output, dir_cls_output, config_.voxel_x_size_, config_.voxel_y_size_,
     config_.min_x_range_, config_.min_y_range_, config_.num_proposals_, config_.num_classes_,
-    config_.num_point_values_, thrust::raw_pointer_cast(yaw_norm_thresholds_d_.data()),
-    thrust::raw_pointer_cast(boxes3d_d_.data()));
+    config_.num_point_values_, thrust::raw_pointer_cast(yaw_norm_thresholds_d.data()),
+    thrust::raw_pointer_cast(boxes3d_d.data()));
 
   // suppress by score
   const auto num_det_boxes3d = thrust::count_if(
-    thrust::device, boxes3d_d_.begin(), boxes3d_d_.end(),
-    is_score_greater(config_.score_threshold_));
+    thrust::device, boxes3d_d.begin(), boxes3d_d.end(), is_score_greater(config_.score_threshold_));
   if (num_det_boxes3d == 0) {
     return cudaGetLastError();
   }
   thrust::device_vector<Box3D> det_boxes3d_d(num_det_boxes3d);
   thrust::copy_if(
-    thrust::device, boxes3d_d_.begin(), boxes3d_d_.end(), det_boxes3d_d.begin(),
+    thrust::device, boxes3d_d.begin(), boxes3d_d.end(), det_boxes3d_d.begin(),
     is_score_greater(config_.score_threshold_));
 
   // sort by score
diff --git a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp
index b1402724e943d..45be1ceab08a4 100644
--- a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp
@@ -410,9 +410,9 @@ bool withinRoadLanelet(
 }
 
 boost::optional<CrosswalkEdgePoints> isReachableCrosswalkEdgePoints(
-  const TrackedObject & object, const lanelet::ConstLanelet & target_crosswalk,
-  const CrosswalkEdgePoints & edge_points, const lanelet::LaneletMapPtr & lanelet_map_ptr,
-  const double time_horizon, const double min_object_vel)
+  const TrackedObject & object, const lanelet::ConstLanelets & surrounding_lanelets,
+  const lanelet::ConstLanelets & external_surrounding_crosswalks,
+  const CrosswalkEdgePoints & edge_points, const double time_horizon, const double min_object_vel)
 {
   using Point = boost::geometry::model::d2::point_xy<double>;
 
@@ -421,9 +421,6 @@ boost::optional<CrosswalkEdgePoints> isReachableCrosswalkEdgePoints(
   const auto yaw = autoware::universe_utils::getRPY(object.kinematics.pose_with_covariance.pose).z;
 
   lanelet::BasicPoint2d obj_pos_as_lanelet(obj_pos.x, obj_pos.y);
-  if (boost::geometry::within(obj_pos_as_lanelet, target_crosswalk.polygon2d().basicPolygon())) {
-    return {};
-  }
 
   const auto & p1 = edge_points.front_center_point;
   const auto & p2 = edge_points.back_center_point;
@@ -441,17 +438,12 @@ boost::optional<CrosswalkEdgePoints> isReachableCrosswalkEdgePoints(
   const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y);
   const auto is_stop_object = estimated_velocity < stop_velocity_th;
   const auto velocity = std::max(min_object_vel, estimated_velocity);
-  const auto surrounding_lanelets = lanelet::geometry::findNearest(
-    lanelet_map_ptr->laneletLayer, obj_pos_as_lanelet, time_horizon * velocity);
 
-  const auto isAcrossAnyRoad = [&surrounding_lanelets](const Point & p_src, const Point & p_dst) {
-    const auto withinAnyCrosswalk = [&surrounding_lanelets](const Point & p) {
-      for (const auto & lanelet : surrounding_lanelets) {
-        const lanelet::Attribute attr = lanelet.second.attribute(lanelet::AttributeName::Subtype);
-        if (
-          (attr.value() == lanelet::AttributeValueString::Crosswalk ||
-           attr.value() == lanelet::AttributeValueString::Walkway) &&
-          boost::geometry::within(p, lanelet.second.polygon2d().basicPolygon())) {
+  const auto isAcrossAnyRoad = [&surrounding_lanelets, &external_surrounding_crosswalks](
+                                 const Point & p_src, const Point & p_dst) {
+    const auto withinAnyCrosswalk = [&external_surrounding_crosswalks](const Point & p) {
+      for (const auto & crosswalk : external_surrounding_crosswalks) {
+        if (boost::geometry::within(p, crosswalk.polygon2d().basicPolygon())) {
           return true;
         }
       }
@@ -470,14 +462,13 @@ boost::optional<CrosswalkEdgePoints> isReachableCrosswalkEdgePoints(
     std::vector<Point> points_of_intersect;
     const boost::geometry::model::linestring<Point> line{p_src, p_dst};
     for (const auto & lanelet : surrounding_lanelets) {
-      const lanelet::Attribute attr = lanelet.second.attribute(lanelet::AttributeName::Subtype);
+      const lanelet::Attribute attr = lanelet.attribute(lanelet::AttributeName::Subtype);
       if (attr.value() != lanelet::AttributeValueString::Road) {
         continue;
       }
 
       std::vector<Point> tmp_intersects;
-      boost::geometry::intersection(
-        line, lanelet.second.polygon2d().basicPolygon(), tmp_intersects);
+      boost::geometry::intersection(line, lanelet.polygon2d().basicPolygon(), tmp_intersects);
       for (const auto & p : tmp_intersects) {
         if (isExist(p, points_of_intersect) || withinAnyCrosswalk(p)) {
           continue;
@@ -1384,10 +1375,29 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
   }
 
   boost::optional<lanelet::ConstLanelet> crossing_crosswalk{boost::none};
-  for (const auto & crosswalk : crosswalks_) {
-    if (withinLanelet(object, crosswalk)) {
-      crossing_crosswalk = crosswalk;
-      break;
+  const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position;
+  const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear;
+  const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y);
+  const auto velocity = std::max(min_crosswalk_user_velocity_, estimated_velocity);
+  // TODO(Mamoru Sobue): 3rd argument of findNearest is the number of lanelets, not radius, so past
+  // implementation has been wrong.
+  const auto surrounding_lanelets_with_dist = lanelet::geometry::findNearest(
+    lanelet_map_ptr_->laneletLayer, lanelet::BasicPoint2d{obj_pos.x, obj_pos.y},
+    prediction_time_horizon_.pedestrian * velocity);
+  lanelet::ConstLanelets surrounding_lanelets;
+  lanelet::ConstLanelets external_surrounding_crosswalks;
+  for (const auto & [dist, lanelet] : surrounding_lanelets_with_dist) {
+    surrounding_lanelets.push_back(lanelet);
+    const auto attr = lanelet.attribute(lanelet::AttributeName::Subtype);
+    if (
+      attr.value() == lanelet::AttributeValueString::Crosswalk ||
+      attr.value() == lanelet::AttributeValueString::Walkway) {
+      const auto & crosswalk = lanelet;
+      if (withinLanelet(object, crosswalk)) {
+        crossing_crosswalk = crosswalk;
+      } else {
+        external_surrounding_crosswalks.push_back(crosswalk);
+      }
     }
   }
 
@@ -1448,8 +1458,9 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
     }
   }
 
-  // try to find the edge points for all crosswalks and generate path to the crosswalk edge
-  for (const auto & crosswalk : crosswalks_) {
+  // try to find the edge points for other surrounding crosswalks and generate path to the crosswalk
+  // edge
+  for (const auto & crosswalk : external_surrounding_crosswalks) {
     const auto crosswalk_signal_id_opt = getTrafficSignalId(crosswalk);
     if (crosswalk_signal_id_opt.has_value() && use_crosswalk_signal_) {
       if (!calcIntentionToCrossWithTrafficSignal(
@@ -1474,8 +1485,8 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
     }
 
     const auto reachable_crosswalk = isReachableCrosswalkEdgePoints(
-      object, crosswalk, edge_points, lanelet_map_ptr_, prediction_time_horizon_.pedestrian,
-      min_crosswalk_user_velocity_);
+      object, surrounding_lanelets, external_surrounding_crosswalks, edge_points,
+      prediction_time_horizon_.pedestrian, min_crosswalk_user_velocity_);
 
     if (!reachable_crosswalk) {
       continue;
diff --git a/perception/autoware_map_based_prediction/src/path_generator.cpp b/perception/autoware_map_based_prediction/src/path_generator.cpp
index dac034a6b7a7d..6f2fb8a86b502 100644
--- a/perception/autoware_map_based_prediction/src/path_generator.cpp
+++ b/perception/autoware_map_based_prediction/src/path_generator.cpp
@@ -188,7 +188,7 @@ PredictedPath PathGenerator::generatePolynomialPath(
 {
   // Get current Frenet Point
   const double ref_path_len = autoware::motion_utils::calcArcLength(ref_path);
-  const auto current_point = getFrenetPoint(object, ref_path, speed_limit, duration);
+  const auto current_point = getFrenetPoint(object, ref_path, duration, speed_limit);
 
   // Step1. Set Target Frenet Point
   // Note that we do not set position s,
diff --git a/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp b/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp
index 77618e1882be7..79dfb3720b626 100644
--- a/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp
+++ b/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp
@@ -91,7 +91,8 @@ class TrackerDebugger
   void endMeasurementTime(const rclcpp::Time & now);
   void startPublishTime(const rclcpp::Time & now);
   void endPublishTime(const rclcpp::Time & now, const rclcpp::Time & object_time);
-  void checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat);
+  void checkDelay(
+    diagnostic_updater::DiagnosticStatusWrapper & stat);  // cppcheck-suppress functionConst
 
   // Debug object
   void setObjectChannels(const std::vector<std::string> & channels)
diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp
index 45955ac983dde..89d8cbaeb3bd8 100644
--- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp
+++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp
@@ -153,9 +153,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   // If the delay compensation is enabled, the timer is used to publish the output at the correct
   // time.
   if (enable_delay_compensation) {
-    publisher_period_ = 1.0 / publish_rate;    // [s]
-    constexpr double timer_multiplier = 20.0;  // 20 times frequent for publish timing check
-    const auto timer_period = rclcpp::Rate(publish_rate * timer_multiplier).period();
+    const auto timer_period = rclcpp::Rate(publish_rate).period();
     publish_timer_ = rclcpp::create_timer(
       this, get_clock(), timer_period, std::bind(&MultiObjectTracker::onTimer, this));
   }
@@ -210,35 +208,13 @@ void MultiObjectTracker::onTrigger()
   ObjectsList objects_list;
   const bool is_objects_ready = input_manager_->getObjects(current_time, objects_list);
   if (!is_objects_ready) return;
-
   onMessage(objects_list);
-  const rclcpp::Time latest_time(objects_list.back().second.header.stamp);
-
-  // Publish objects if the timer is not enabled
-  if (publish_timer_ == nullptr) {
-    // if the delay compensation is disabled, publish the objects in the latest time
-    publish(latest_time);
-  } else {
-    // Publish if the next publish time is close
-    const double minimum_publish_interval = publisher_period_ * 0.70;  // 70% of the period
-    const rclcpp::Time publish_time = this->now();
-    if ((publish_time - last_published_time_).seconds() > minimum_publish_interval) {
-      checkAndPublish(publish_time);
-    }
-  }
 }
 
 void MultiObjectTracker::onTimer()
 {
   const rclcpp::Time current_time = this->now();
 
-  // Check the publish period
-  const auto elapsed_time = (current_time - last_published_time_).seconds();
-  // If the elapsed time is over the period, publish objects with prediction
-  constexpr double maximum_latency_ratio = 1.11;  // 11% margin
-  const double maximum_publish_latency = publisher_period_ * maximum_latency_ratio;
-  if (elapsed_time < maximum_publish_latency) return;
-
   // get objects from the input manager and run process
   ObjectsList objects_list;
   const bool is_objects_ready = input_manager_->getObjects(current_time, objects_list);
diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp
index db5eaaa88ca8c..e917acbda9fcc 100644
--- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp
+++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp
@@ -78,7 +78,6 @@ class MultiObjectTracker : public rclcpp::Node
   // publish timer
   rclcpp::TimerBase::SharedPtr publish_timer_;
   rclcpp::Time last_published_time_;
-  double publisher_period_;
 
   // internal states
   std::string world_frame_id_;  // tracking frame
diff --git a/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp
index 432a4105d815f..ee2c6910159b5 100644
--- a/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp
+++ b/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp
@@ -60,7 +60,7 @@ class InputStream
   void getObjectsOlderThan(
     const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time,
     ObjectsList & objects_list);
-  void getNames(std::string & long_name, std::string & short_name)
+  void getNames(std::string & long_name, std::string & short_name) const
   {
     long_name = long_name_;
     short_name = short_name_;
@@ -94,7 +94,7 @@ class InputStream
   std::string input_topic_;
   std::string long_name_;
   std::string short_name_;
-  bool is_spawn_enabled_;
+  bool is_spawn_enabled_{};
 
   size_t que_size_{30};
   std::deque<DetectedObjects> objects_que_;
@@ -103,11 +103,11 @@ class InputStream
 
   // bool is_time_initialized_{false};
   int initial_count_{0};
-  double expected_interval_;
-  double latency_mean_;
-  double latency_var_;
-  double interval_mean_;
-  double interval_var_;
+  double expected_interval_{};
+  double latency_mean_{};
+  double latency_var_{};
+  double interval_mean_{};
+  double interval_var_{};
 
   rclcpp::Time latest_measurement_time_;
   rclcpp::Time latest_message_time_;
@@ -136,7 +136,7 @@ class InputManager
   bool is_initialized_{false};
   rclcpp::Time latest_exported_object_time_;
 
-  size_t input_size_;
+  size_t input_size_{};
   std::vector<std::shared_ptr<InputStream>> input_streams_;
 
   std::function<void()> func_trigger_;
diff --git a/perception/autoware_object_merger/README.md b/perception/autoware_object_merger/README.md
index f9d8c91b2a4c1..c65353efa3a91 100644
--- a/perception/autoware_object_merger/README.md
+++ b/perception/autoware_object_merger/README.md
@@ -25,9 +25,9 @@ The successive shortest path algorithm is used to solve the data association pro
 
 ## Parameters
 
-{{ json_to_markdown("perception/object_merger/schema/object_association_merger.schema.json") }}
-{{ json_to_markdown("perception/object_merger/schema/data_association_matrix.schema.json") }}
-{{ json_to_markdown("perception/object_merger/schema/overlapped_judge.schema.json") }}
+{{ json_to_markdown("perception/autoware_object_merger/schema/object_association_merger.schema.json") }}
+{{ json_to_markdown("perception/autoware_object_merger/schema/data_association_matrix.schema.json") }}
+{{ json_to_markdown("perception/autoware_object_merger/schema/overlapped_judge.schema.json") }}
 
 ## Tips
 
diff --git a/perception/autoware_object_range_splitter/README.md b/perception/autoware_object_range_splitter/README.md
index 3cdfe36528094..dbb6c2a48faab 100644
--- a/perception/autoware_object_range_splitter/README.md
+++ b/perception/autoware_object_range_splitter/README.md
@@ -43,7 +43,7 @@ Example:
 
 ## Parameters
 
-{{ json_to_markdown("perception/object_range_splitter/schema/object_range_splitter.schema.json") }}
+{{ json_to_markdown("perception/autoware_object_range_splitter/schema/object_range_splitter.schema.json") }}
 
 ## Assumptions / Known limits
 
diff --git a/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp
index b940b932db0ef..518052a1a4b8c 100644
--- a/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp
+++ b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp
@@ -81,9 +81,9 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud(
     : range(_range), wx(_wx), wy(_wy)
     {
     }
-    double range;
-    double wx;
-    double wy;
+    double range{};
+    double wx{};
+    double wy{};
   };
 
   std::vector</*angle bin*/ std::vector<BinInfo>> obstacle_pointcloud_angle_bins(angle_bin_size);
diff --git a/perception/autoware_probabilistic_occupancy_grid_map/lib/fusion_policy/fusion_policy.cpp b/perception/autoware_probabilistic_occupancy_grid_map/lib/fusion_policy/fusion_policy.cpp
index 6d058b3bef6eb..993e220786de8 100644
--- a/perception/autoware_probabilistic_occupancy_grid_map/lib/fusion_policy/fusion_policy.cpp
+++ b/perception/autoware_probabilistic_occupancy_grid_map/lib/fusion_policy/fusion_policy.cpp
@@ -202,17 +202,17 @@ struct dempsterShaferOccupancy
   }
 
   // calc conflict factor K
-  double calcK(const dempsterShaferOccupancy & other)
+  double calcK(const dempsterShaferOccupancy & other) const
   {
     return (occupied * other.empty + empty * other.occupied);
   }
   // calc sum of occupied probability mass
-  double calcOccupied(const dempsterShaferOccupancy & other)
+  double calcOccupied(const dempsterShaferOccupancy & other) const
   {
     return occupied * other.occupied + occupied * other.unknown + unknown * other.occupied;
   }
   // calc sum of empty probability mass
-  double calcEmpty(const dempsterShaferOccupancy & other)
+  double calcEmpty(const dempsterShaferOccupancy & other) const
   {
     return empty * other.empty + empty * other.unknown + unknown * other.empty;
   }
@@ -240,7 +240,7 @@ struct dempsterShaferOccupancy
   }
 
   // get occupancy probability via Pignistic Probability
-  double getPignisticProbability() { return occupied + unknown / 2.0; }
+  double getPignisticProbability() const { return occupied + unknown / 2.0; }
 };
 
 /**
diff --git a/perception/autoware_radar_fusion_to_detected_object/src/include/radar_fusion_to_detected_object.hpp b/perception/autoware_radar_fusion_to_detected_object/src/include/radar_fusion_to_detected_object.hpp
index b296f52e2c139..3c95d8f068af1 100644
--- a/perception/autoware_radar_fusion_to_detected_object/src/include/radar_fusion_to_detected_object.hpp
+++ b/perception/autoware_radar_fusion_to_detected_object/src/include/radar_fusion_to_detected_object.hpp
@@ -99,9 +99,9 @@ class RadarFusionToDetectedObject
   // std::vector<DetectedObject> splitObject(
   //   const DetectedObject & object, const std::shared_ptr<std::vector<RadarInput>> & radars);
   TwistWithCovariance estimateTwist(
-    const DetectedObject & object, const std::shared_ptr<std::vector<RadarInput>> & radars);
+    const DetectedObject & object, const std::shared_ptr<std::vector<RadarInput>> & radars) const;
   bool isQualified(
-    const DetectedObject & object, const std::shared_ptr<std::vector<RadarInput>> & radars);
+    const DetectedObject & object, const std::shared_ptr<std::vector<RadarInput>> & radars) const;
   TwistWithCovariance convertDopplerToTwist(
     const DetectedObject & object, const TwistWithCovariance & twist_with_covariance);
   static bool isYawCorrect(
diff --git a/perception/autoware_radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp b/perception/autoware_radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp
index bc96ac780fd0a..25e174a44c2ec 100644
--- a/perception/autoware_radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp
+++ b/perception/autoware_radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp
@@ -205,7 +205,7 @@ RadarFusionToDetectedObject::filterRadarWithinObject(
 // (Target value is amplitude if using radar pointcloud. Target value is probability if using radar
 // objects).
 TwistWithCovariance RadarFusionToDetectedObject::estimateTwist(
-  const DetectedObject & object, const std::shared_ptr<std::vector<RadarInput>> & radars)
+  const DetectedObject & object, const std::shared_ptr<std::vector<RadarInput>> & radars) const
 {
   if (!radars || (*radars).empty()) {
     TwistWithCovariance output{};
@@ -298,7 +298,7 @@ TwistWithCovariance RadarFusionToDetectedObject::estimateTwist(
 
 // Judge whether low confidence objects that do not have some radar points/objects or not.
 bool RadarFusionToDetectedObject::isQualified(
-  const DetectedObject & object, const std::shared_ptr<std::vector<RadarInput>> & radars)
+  const DetectedObject & object, const std::shared_ptr<std::vector<RadarInput>> & radars) const
 {
   if (object.existence_probability > param_.threshold_probability) {
     return true;
diff --git a/perception/autoware_radar_tracks_msgs_converter/README.md b/perception/autoware_radar_tracks_msgs_converter/README.md
index 2883ddaecf926..5010f5f873932 100644
--- a/perception/autoware_radar_tracks_msgs_converter/README.md
+++ b/perception/autoware_radar_tracks_msgs_converter/README.md
@@ -1,6 +1,6 @@
 # radar_tracks_msgs_converter
 
-This package converts from [radar_msgs/msg/RadarTracks](https://github.com/ros-perception/radar_msgs/blob/ros2/msg/RadarTracks.msg) into [autoware_perception_msgs/msg/DetectedObject]<https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_perception_msgs/msg/DetectedObject.msg)> and [autoware_perception_msgs/msg/TrackedObject](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_perception_msgs/msg/TrackedObject.msg).
+This package converts from [radar_msgs/msg/RadarTracks](https://github.com/ros-perception/radar_msgs/blob/ros2/msg/RadarTracks.msg) into [autoware_perception_msgs/msg/DetectedObject](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_perception_msgs/msg/DetectedObject.msg) and [autoware_perception_msgs/msg/TrackedObject](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_perception_msgs/msg/TrackedObject.msg).
 
 - Calculation cost is O(n).
   - n: The number of radar objects
@@ -54,7 +54,7 @@ Autoware objects label is defined in [ObjectClassification](https://github.com/a
 
 #### Parameter Summary
 
-{{ json_to_markdown("perception/radar_tracks_msgs_converter/schema/radar_tracks_msgs_converter.schema.json") }}
+{{ json_to_markdown("perception/autoware_radar_tracks_msgs_converter/schema/radar_tracks_msgs_converter.schema.json") }}
 
 #### Parameter Description
 
diff --git a/perception/autoware_shape_estimation/CMakeLists.txt b/perception/autoware_shape_estimation/CMakeLists.txt
index b19f422c28d27..96bf946ab11db 100644
--- a/perception/autoware_shape_estimation/CMakeLists.txt
+++ b/perception/autoware_shape_estimation/CMakeLists.txt
@@ -10,13 +10,17 @@ find_package(pcl_conversions REQUIRED)
 find_package(OpenCV REQUIRED)
 find_package(Eigen3 REQUIRED)
 
+find_package(CUDA)
+find_package(CUDNN)
+find_package(TENSORRT)
+
 set(SHAPE_ESTIMATION_DEPENDENCIES
   PCL
   OpenCV
   Eigen3
 )
 
-ament_auto_add_library(${PROJECT_NAME}_lib SHARED
+set(${PROJECT_NAME}_SOURCES
   lib/shape_estimator.cpp
   lib/model/bounding_box.cpp
   lib/model/convex_hull.cpp
@@ -31,6 +35,19 @@ ament_auto_add_library(${PROJECT_NAME}_lib SHARED
   lib/corrector/no_corrector.cpp
 )
 
+if(${CUDA_FOUND} AND ${CUDNN_FOUND} AND ${TENSORRT_FOUND})
+  message("CUDA found, including CUDA-specific sources")
+  list(APPEND ${PROJECT_NAME}_SOURCES lib/tensorrt_shape_estimator.cpp)
+  add_definitions(-DUSE_CUDA)
+else()
+  message("CUDA not found, excluding CUDA-specific sources")
+endif()
+
+ament_auto_add_library(${PROJECT_NAME}_lib
+  SHARED
+  ${${PROJECT_NAME}_SOURCES}
+)
+
 ament_target_dependencies(${PROJECT_NAME}_lib ${SHAPE_ESTIMATION_DEPENDENCIES})
 
 target_include_directories(${PROJECT_NAME}_lib
@@ -39,6 +56,13 @@ target_include_directories(${PROJECT_NAME}_lib
   "${EIGEN3_INCLUDE_DIR}"
 )
 
+if(${CUDA_FOUND} AND ${CUDNN_FOUND} AND ${TENSORRT_FOUND})
+target_include_directories(${PROJECT_NAME}_lib
+  SYSTEM PUBLIC
+  "${TENSORRT_INCLUDE_DIRS}"
+)
+endif()
+
 ament_auto_add_library(${PROJECT_NAME} SHARED
   src/shape_estimation_node.cpp
 )
@@ -57,6 +81,16 @@ target_link_libraries(${PROJECT_NAME}
   ${PROJECT_NAME}_lib
 )
 
+if(${CUDA_FOUND} AND ${CUDNN_FOUND} AND ${TENSORRT_FOUND})
+  target_link_libraries(${PROJECT_NAME}
+    ${TENSORRT_LIBRARIES}
+  )
+endif()
+
+target_compile_definitions(${PROJECT_NAME} PRIVATE
+  TENSORRT_VERSION_MAJOR=${TENSORRT_VERSION_MAJOR}
+)
+
 rclcpp_components_register_node(${PROJECT_NAME}
   PLUGIN "autoware::shape_estimation::ShapeEstimationNode"
   EXECUTABLE shape_estimation_node
@@ -79,6 +113,6 @@ if(BUILD_TESTING)
   ament_add_ros_isolated_gtest(test_shape_estimation ${test_files})
 
   target_link_libraries(test_shape_estimation
-  ${PROJECT_NAME}
+    ${PROJECT_NAME}
   )
 endif()
diff --git a/perception/autoware_shape_estimation/README.md b/perception/autoware_shape_estimation/README.md
index 5f184c32f1805..d3cb88ae130d1 100644
--- a/perception/autoware_shape_estimation/README.md
+++ b/perception/autoware_shape_estimation/README.md
@@ -10,7 +10,8 @@ This node calculates a refined object shape (bounding box, cylinder, convex hull
 
 - bounding box
 
-  L-shape fitting. See reference below for details.
+  - L-shape fitting: See reference below for details
+  - ML based shape fitting: See ML Based Shape Fitting Implementation section below for details
 
 - cylinder
 
@@ -38,6 +39,118 @@ This node calculates a refined object shape (bounding box, cylinder, convex hull
 
 {{ json_to_markdown("perception/autoware_shape_estimation/schema/shape_estimation.schema.json") }}
 
+## ML Based Shape Implementation
+
+The model takes a point cloud and object label(provided by camera detections/Apollo instance segmentation) as an input and outputs the 3D bounding box of the object.
+
+ML based shape estimation algorithm uses a PointNet model as a backbone to estimate the 3D bounding box of the object. The model is trained on the NuScenes dataset with vehicle labels (Car, Truck, Bus, Trailer).
+
+The implemented model is concatenated with STN (Spatial Transformer Network) to learn the transformation of the input point cloud to the canonical space and PointNet to predict the 3D bounding box of the object.
+Bounding box estimation part of _Frustum PointNets for 3D Object Detection from RGB-D Data_ paper used as a reference.
+
+The model predicts the following outputs for each object:
+
+- x,y,z coordinates of the object center
+- object heading angle classification result(Uses 12 bins for angle classification - 30 degrees each)
+- object heading angle residuals
+- object size classification result
+- object size residuals
+
+### Training ML Based Shape Estimation Model
+
+To train the model, you need ground truth 3D bounding box annotations. When using the mmdetection3d repository for training a 3D object detection algorithm, these ground truth annotations are
+saved and utilized for data augmentation. These annotations are used as an essential dataset for training the shape estimation model effectively.
+
+### Preparing the Dataset
+
+#### Install MMDetection3D prerequisites
+
+**Step 1.** Download and install Miniconda from the [official website](https://mmpretrain.readthedocs.io/en/latest/get_started.html).
+
+**Step 2.** Create a conda virtual environment and activate it
+
+```bash
+conda create --name train-shape-estimation python=3.8 -y
+conda activate train-shape-estimation
+```
+
+**Step 3.** Install PyTorch
+
+```bash
+conda install pytorch torchvision -c pytorch
+```
+
+#### Install mmdetection3d
+
+**Step 1.** Install MMEngine, MMCV, and MMDetection using MIM
+
+```bash
+pip install -U openmim
+mim install mmengine
+mim install 'mmcv>=2.0.0rc4'
+mim install 'mmdet>=3.0.0rc5, <3.3.0'
+```
+
+**Step 2.** Install Autoware's MMDetection3D fork
+
+```bash
+git clone https://github.com/autowarefoundation/mmdetection3d.git
+cd mmdetection3d
+pip install -v -e .
+```
+
+#### Preparing NuScenes dataset for training
+
+**Step 1.** Download the NuScenes dataset from the [official website](https://www.nuscenes.org/download) and extract the dataset to a folder of your choice.
+
+**Note:** The NuScenes dataset is large and requires significant disk space. Ensure you have enough storage available before proceeding.
+
+**Step 2.** Create a symbolic link to the dataset folder
+
+```bash
+ln -s /path/to/nuscenes/dataset/ /path/to/mmdetection3d/data/nuscenes/
+```
+
+**Step 3.** Prepare the NuScenes data by running:
+
+```bash
+cd mmdetection3d
+python tools/create_data.py nuscenes --root-path ./data/nuscenes --out-dir ./data/nuscenes --extra-tag nuscenes --only-gt-database True
+```
+
+#### Clone Bounding Box Estimator model
+
+```bash
+git clone https://github.com/autowarefoundation/bbox_estimator.git
+```
+
+#### Split the dataset into training and validation sets
+
+```bash
+
+cd bbox_estimator
+python3 utils/split_dbinfos.py --dataset_path /path/to/mmdetection3d/data/nuscenes --classes 'car' 'truck' 'bus' 'trailer'  --train_ratio 0.8
+```
+
+### Training and Deploying the model
+
+#### Training the model
+
+```bash
+# Detailed training options can be found in the training script
+# For more details, run `python3 train.py --help`
+python3 train.py --dataset_path /path/to/mmdetection3d/data/nuscenes
+```
+
+#### Deploying the model
+
+```bash
+# Convert the trained model to ONNX format
+python3 onnx_converter.py --weight_path /path/to/best_checkpoint.pth --output_path /path/to/output.onnx
+```
+
+Give the output path of the ONNX model to the `model_path` parameter in the `shape_estimation` node launch file.
+
 ## Assumptions / Known limits
 
 TBD
@@ -56,3 +169,15 @@ month = {June},
 keywords = {autonomous driving, laser scanner, perception, segmentation},
 }
 ```
+
+Frustum PointNets for 3D Object Detection from RGB-D Data:
+
+````bibtex
+@inproceedings{qi2018frustum,
+title={Frustum pointnets for 3d object detection from rgb-d data},
+author={Qi, Charles R and Liu, Wei and Wu, Chenxia and Su, Hao and Guibas, Leonidas J},
+booktitle={Proceedings of the IEEE conference on computer vision and pattern recognition},
+pages={918--927},
+year={2018}
+}```
+````
diff --git a/perception/autoware_shape_estimation/config/shape_estimation.param.yaml b/perception/autoware_shape_estimation/config/shape_estimation.param.yaml
index e277d99d70edd..00af06a3703f2 100644
--- a/perception/autoware_shape_estimation/config/shape_estimation.param.yaml
+++ b/perception/autoware_shape_estimation/config/shape_estimation.param.yaml
@@ -1,8 +1,14 @@
 /**:
-    ros__parameters:
-      use_corrector: true
-      use_filter: true
-      use_vehicle_reference_yaw: false
-      use_vehicle_reference_shape_size: false
-      use_boost_bbox_optimizer: false
-      fix_filtered_objects_label_to_unknown: true
+  ros__parameters:
+    use_corrector: true
+    use_filter: true
+    use_vehicle_reference_yaw: false
+    use_vehicle_reference_shape_size: false
+    use_boost_bbox_optimizer: false
+    fix_filtered_objects_label_to_unknown: true
+    model_params:
+      use_ml_shape_estimator: false
+      minimum_points: 16
+      precision: "fp32"
+      batch_size: 32
+      build_only: false
diff --git a/perception/autoware_shape_estimation/include/autoware/shape_estimation/tensorrt_shape_estimator.hpp b/perception/autoware_shape_estimation/include/autoware/shape_estimation/tensorrt_shape_estimator.hpp
new file mode 100644
index 0000000000000..5d0b7032445c8
--- /dev/null
+++ b/perception/autoware_shape_estimation/include/autoware/shape_estimation/tensorrt_shape_estimator.hpp
@@ -0,0 +1,93 @@
+// Copyright 2018 Autoware Foundation. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__SHAPE_ESTIMATION__TENSORRT_SHAPE_ESTIMATOR_HPP_
+#define AUTOWARE__SHAPE_ESTIMATION__TENSORRT_SHAPE_ESTIMATOR_HPP_
+
+#include <cuda_utils/cuda_check_error.hpp>
+#include <cuda_utils/cuda_unique_ptr.hpp>
+#include <cuda_utils/stream_unique_ptr.hpp>
+#include <pcl_ros/transforms.hpp>
+#include <tensorrt_common/tensorrt_common.hpp>
+
+#include <autoware_perception_msgs/msg/detected_objects.hpp>
+#include <geometry_msgs/msg/transform.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/msg/point_field.hpp>
+#include <sensor_msgs/point_cloud2_iterator.hpp>
+#include <tier4_perception_msgs/msg/detected_objects_with_feature.hpp>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace autoware::shape_estimation
+{
+using cuda_utils::CudaUniquePtr;
+using cuda_utils::CudaUniquePtrHost;
+using cuda_utils::makeCudaStream;
+using cuda_utils::StreamUniquePtr;
+
+using autoware_perception_msgs::msg::DetectedObject;
+using autoware_perception_msgs::msg::DetectedObjects;
+using tier4_perception_msgs::msg::DetectedObjectsWithFeature;
+using Label = autoware_perception_msgs::msg::ObjectClassification;
+
+class TrtShapeEstimator
+{
+public:
+  TrtShapeEstimator(
+    const std::string & model_path, const std::string & precision,
+    const tensorrt_common::BatchConfig & batch_config, const size_t max_workspace_size = (1 << 30),
+    const tensorrt_common::BuildConfig build_config =
+      tensorrt_common::BuildConfig("MinMax", -1, false, false, false, 0.0));
+
+  ~TrtShapeEstimator();
+
+  bool inference(const DetectedObjectsWithFeature & input, DetectedObjectsWithFeature & output);
+
+private:
+  void preprocess(const DetectedObjectsWithFeature & input);
+
+  bool feed_forward_and_decode(
+    const DetectedObjectsWithFeature & input, DetectedObjectsWithFeature & output);
+
+  void postprocess();
+
+  static double class2angle(int pred_cls, double residual, int num_class);
+
+  StreamUniquePtr stream_{makeCudaStream()};
+  std::unique_ptr<tensorrt_common::TrtCommon> trt_common_;
+
+  std::vector<float> input_pc_h_;
+  CudaUniquePtr<float[]> input_pc_d_;
+
+  std::vector<float> input_one_hot_h_;
+  CudaUniquePtr<float[]> input_one_hot_d_;
+
+  size_t out_s1center_elem_num_;
+  size_t out_s1center_elem_num_per_batch_;
+  CudaUniquePtr<float[]> out_s1center_prob_d_;
+  CudaUniquePtrHost<float[]> out_s1center_prob_h_;
+
+  size_t out_pred_elem_num_;
+  size_t out_pred_elem_num_per_batch_;
+  CudaUniquePtr<float[]> out_pred_prob_d_;
+  CudaUniquePtrHost<float[]> out_pred_prob_h_;
+
+  std::vector<std::vector<float>> g_type_mean_size_;
+  size_t batch_size_;
+};
+}  // namespace autoware::shape_estimation
+#endif  // AUTOWARE__SHAPE_ESTIMATION__TENSORRT_SHAPE_ESTIMATOR_HPP_
diff --git a/perception/autoware_shape_estimation/launch/shape_estimation.launch.xml b/perception/autoware_shape_estimation/launch/shape_estimation.launch.xml
index 816653013c6c8..40fc0a930ac23 100644
--- a/perception/autoware_shape_estimation/launch/shape_estimation.launch.xml
+++ b/perception/autoware_shape_estimation/launch/shape_estimation.launch.xml
@@ -2,12 +2,16 @@
   <arg name="input/objects" default="labeled_clusters"/>
   <arg name="output/objects" default="shape_estimated_objects"/>
   <arg name="node_name" default="shape_estimation"/>
+
+  <arg name="data_path" default="$(env HOME)/autoware_data" description="packages data and artifacts directory path"/>
+  <arg name="model_path" default="$(var data_path)/shape_estimation/pointnet.onnx"/>
   <!-- Parameter -->
   <arg name="config_file" default="$(find-pkg-share autoware_shape_estimation)/config/shape_estimation.param.yaml"/>
 
   <node pkg="autoware_shape_estimation" exec="shape_estimation_node" name="$(var node_name)" output="screen">
     <remap from="input" to="$(var input/objects)"/>
     <remap from="objects" to="$(var output/objects)"/>
-    <param from="$(var config_file)"/>
+    <param name="model_path" value="$(var model_path)"/>
+    <param from="$(var config_file)" allow_substs="true"/>
   </node>
 </launch>
diff --git a/perception/autoware_shape_estimation/lib/tensorrt_shape_estimator.cpp b/perception/autoware_shape_estimation/lib/tensorrt_shape_estimator.cpp
new file mode 100644
index 0000000000000..a097060081dc9
--- /dev/null
+++ b/perception/autoware_shape_estimation/lib/tensorrt_shape_estimator.cpp
@@ -0,0 +1,338 @@
+// Copyright 2018 Autoware Foundation. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "autoware/shape_estimation/tensorrt_shape_estimator.hpp"
+
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+#include <tf2/utils.h>
+
+#include <iostream>
+#include <random>
+
+namespace autoware::shape_estimation
+{
+TrtShapeEstimator::TrtShapeEstimator(
+  const std::string & model_path, const std::string & precision,
+  const tensorrt_common::BatchConfig & batch_config, const size_t max_workspace_size,
+  const tensorrt_common::BuildConfig build_config)
+{
+  trt_common_ = std::make_unique<tensorrt_common::TrtCommon>(
+    model_path, precision, nullptr, batch_config, max_workspace_size, build_config);
+
+  trt_common_->setup();
+
+  if (!trt_common_->isInitialized()) {
+    std::cerr << "Failed to initialize TensorRT" << std::endl;
+    return;
+  }
+
+  const auto pc_input_dims = trt_common_->getBindingDimensions(0);
+  const auto pc_input_size = std::accumulate(
+    pc_input_dims.d + 1, pc_input_dims.d + pc_input_dims.nbDims, 1, std::multiplies<int>());
+  input_pc_d_ = cuda_utils::make_unique<float[]>(pc_input_size * batch_config[2]);
+  batch_size_ = batch_config[2];
+  const auto one_hot_input_dims = trt_common_->getBindingDimensions(1);
+  const auto one_hot_input_size = std::accumulate(
+    one_hot_input_dims.d + 1, one_hot_input_dims.d + one_hot_input_dims.nbDims, 1,
+    std::multiplies<int>());
+  input_one_hot_d_ = cuda_utils::make_unique<float[]>(one_hot_input_size * batch_config[2]);
+
+  const auto stage1_center_out_dims = trt_common_->getBindingDimensions(2);
+  out_s1center_elem_num_ = std::accumulate(
+    stage1_center_out_dims.d + 1, stage1_center_out_dims.d + stage1_center_out_dims.nbDims, 1,
+    std::multiplies<int>());
+  out_s1center_elem_num_ = out_s1center_elem_num_ * batch_config[2];
+  out_s1center_elem_num_per_batch_ = static_cast<size_t>(out_s1center_elem_num_ / batch_config[2]);
+  out_s1center_prob_d_ = cuda_utils::make_unique<float[]>(out_s1center_elem_num_);
+  out_s1center_prob_h_ =
+    cuda_utils::make_unique_host<float[]>(out_s1center_elem_num_, cudaHostAllocPortable);
+
+  const auto pred_out_dims = trt_common_->getBindingDimensions(3);
+  out_pred_elem_num_ = std::accumulate(
+    pred_out_dims.d + 1, pred_out_dims.d + pred_out_dims.nbDims, 1, std::multiplies<int>());
+  out_pred_elem_num_ = out_pred_elem_num_ * batch_config[2];
+  out_pred_elem_num_per_batch_ = static_cast<size_t>(out_pred_elem_num_ / batch_config[2]);
+  out_pred_prob_d_ = cuda_utils::make_unique<float[]>(out_pred_elem_num_);
+  out_pred_prob_h_ =
+    cuda_utils::make_unique_host<float[]>(out_pred_elem_num_, cudaHostAllocPortable);
+
+  g_type_mean_size_ = {{4.6344314, 1.9600292, 1.7375569},     {6.936331, 2.5178623, 2.8506238},
+                       {11.194943, 2.9501154, 3.4918275},     {12.275775, 2.9231303, 3.87086},
+                       {0.80057803, 0.5983815, 1.27450867},   {1.76282397, 0.59706367, 1.73698127},
+                       {16.17150617, 2.53246914, 3.53079012}, {3.64300781, 1.54298177, 1.92320313}};
+}
+
+bool TrtShapeEstimator::inference(
+  const DetectedObjectsWithFeature & input, DetectedObjectsWithFeature & output)
+{
+  if (!trt_common_->isInitialized()) {
+    return false;
+  }
+
+  bool result = false;
+
+  for (size_t i = 0; i < input.feature_objects.size(); i += batch_size_) {
+    DetectedObjectsWithFeature input_batch;
+    input_batch.header = input.header;
+
+    for (size_t j = 0; j < batch_size_ && (i + j) < input.feature_objects.size(); ++j) {
+      input_batch.feature_objects.push_back(input.feature_objects[i + j]);
+    }
+
+    preprocess(input_batch);
+    DetectedObjectsWithFeature output_batch;
+    result = feed_forward_and_decode(input_batch, output_batch);
+
+    output.feature_objects.insert(
+      output.feature_objects.end(), output_batch.feature_objects.begin(),
+      output_batch.feature_objects.end());
+  }
+
+  return result;
+}
+
+void TrtShapeEstimator::preprocess(const DetectedObjectsWithFeature & input)
+{
+  auto input_dims_pc = trt_common_->getBindingDimensions(0);
+  int batch_size = static_cast<int>(input.feature_objects.size());
+
+  const auto input_chan = static_cast<float>(input_dims_pc.d[1]);
+  const auto input_pc_size = static_cast<float>(input_dims_pc.d[2]);
+
+  auto input_dims_one_hot = trt_common_->getBindingDimensions(1);
+  const auto input_one_hot_size = static_cast<float>(input_dims_one_hot.d[1]);
+
+  int volume_pc = batch_size * input_chan * input_pc_size;
+  input_pc_h_.resize(volume_pc);
+
+  int volume_one_hot = batch_size * input_one_hot_size;
+  input_one_hot_h_.resize(volume_one_hot);
+
+  // fill point cloud
+  for (size_t i = 0; i < input.feature_objects.size(); i++) {
+    const auto & feature_object = input.feature_objects[i];
+
+    int point_size_of_cloud =
+      feature_object.feature.cluster.width * feature_object.feature.cluster.height;
+
+    if (point_size_of_cloud <= input_pc_size) {
+      int offset = 0;
+      for (sensor_msgs::PointCloud2ConstIterator<float> x_iter(feature_object.feature.cluster, "x"),
+           y_iter(feature_object.feature.cluster, "y"), z_iter(feature_object.feature.cluster, "z");
+           x_iter != x_iter.end(); ++x_iter, ++y_iter, ++z_iter) {
+        input_pc_h_[i * input_chan * input_pc_size + 0 + offset] = -1 * (*y_iter);
+        input_pc_h_[i * input_chan * input_pc_size + 512 + offset] = -1 * (*z_iter);
+        input_pc_h_[i * input_chan * input_pc_size + 1024 + offset] = *x_iter;
+        offset++;
+      }
+
+      int iter_count = static_cast<int>(input_pc_size) / point_size_of_cloud;
+      int remainer_count = static_cast<int>(input_pc_size) % point_size_of_cloud;
+
+      for (int j = 1; j < iter_count; j++) {
+        for (int k = 0; k < point_size_of_cloud; k++) {
+          input_pc_h_[i * input_chan * input_pc_size + 0 + j * point_size_of_cloud + k] =
+            input_pc_h_[i * input_chan * input_pc_size + k];
+
+          input_pc_h_[i * input_chan * input_pc_size + 512 + j * point_size_of_cloud + k] =
+            input_pc_h_[i * input_chan * input_pc_size + 512 + k];
+
+          input_pc_h_[i * input_chan * input_pc_size + 1024 + j * point_size_of_cloud + k] =
+            input_pc_h_[i * input_chan * input_pc_size + 1024 + k];
+        }
+      }
+
+      for (int j = 0; j < remainer_count; j++) {
+        input_pc_h_[i * input_chan * input_pc_size + 0 + iter_count * point_size_of_cloud + j] =
+          input_pc_h_[i * input_chan * input_pc_size + j];
+
+        input_pc_h_[i * input_chan * input_pc_size + 512 + iter_count * point_size_of_cloud + j] =
+          input_pc_h_[i * input_chan * input_pc_size + 512 + j];
+
+        input_pc_h_[i * input_chan * input_pc_size + 1024 + iter_count * point_size_of_cloud + j] =
+          input_pc_h_[i * input_chan * input_pc_size + 1024 + j];
+      }
+
+    } else {
+      std::vector<float> sampled_points;
+      std::vector<size_t> indices(
+        feature_object.feature.cluster.width * feature_object.feature.cluster.height);
+      std::iota(indices.begin(), indices.end(), 0);
+
+      std::sample(
+        indices.begin(), indices.end(), std::back_inserter(sampled_points), 512,
+        std::mt19937{std::random_device{}()});
+
+      // Create an iterator to read the points
+      sensor_msgs::PointCloud2ConstIterator<float> iter_x(feature_object.feature.cluster, "x");
+      sensor_msgs::PointCloud2ConstIterator<float> iter_y(feature_object.feature.cluster, "y");
+      sensor_msgs::PointCloud2ConstIterator<float> iter_z(feature_object.feature.cluster, "z");
+
+      int offset = 0;
+      for (const auto & index : sampled_points) {
+        auto idx = index * feature_object.feature.cluster.point_step / sizeof(float);
+        input_pc_h_[i * input_chan * input_pc_size + 0 + offset] = (-1 * iter_y[idx]);
+        input_pc_h_[i * input_chan * input_pc_size + 512 + offset] = (-1 * iter_z[idx]);
+        input_pc_h_[i * input_chan * input_pc_size + 1024 + offset] = iter_x[idx];
+        offset++;
+      }
+    }
+
+    const auto & label = feature_object.object.classification.front().label;
+
+    // Initialize all elements to 0.0f
+    input_one_hot_h_[i * 4 + 0] = 0.0f;
+    input_one_hot_h_[i * 4 + 1] = 0.0f;
+    input_one_hot_h_[i * 4 + 2] = 0.0f;
+    input_one_hot_h_[i * 4 + 3] = 0.0f;
+
+    if (label == Label::CAR) {
+      input_one_hot_h_[i * 4 + 0] = 1.0f;
+    } else if (label == Label::TRUCK) {
+      input_one_hot_h_[i * 4 + 1] = 1.0f;
+    } else if (label == Label::BUS) {
+      input_one_hot_h_[i * 4 + 2] = 1.0f;
+    } else if (label == Label::TRAILER) {
+      input_one_hot_h_[i * 4 + 3] = 1.0f;
+    }
+  }
+
+  CHECK_CUDA_ERROR(cudaMemcpy(
+    input_pc_d_.get(), input_pc_h_.data(), input_pc_h_.size() * sizeof(float),
+    cudaMemcpyHostToDevice));
+  CHECK_CUDA_ERROR(cudaMemcpy(
+    input_one_hot_d_.get(), input_one_hot_h_.data(), input_one_hot_h_.size() * sizeof(float),
+    cudaMemcpyHostToDevice));
+}
+
+bool TrtShapeEstimator::feed_forward_and_decode(
+  const DetectedObjectsWithFeature & input, DetectedObjectsWithFeature & output)
+{
+  int batch_size = static_cast<int>(input.feature_objects.size());
+  std::vector<void *> buffers = {
+    input_pc_d_.get(), input_one_hot_d_.get(), out_s1center_prob_d_.get(), out_pred_prob_d_.get()};
+  trt_common_->enqueueV2(buffers.data(), *stream_, nullptr);
+
+  CHECK_CUDA_ERROR(cudaMemcpyAsync(
+    out_s1center_prob_h_.get(), out_s1center_prob_d_.get(), out_s1center_elem_num_ * sizeof(float),
+    cudaMemcpyDeviceToHost, *stream_));
+
+  CHECK_CUDA_ERROR(cudaMemcpyAsync(
+    out_pred_prob_h_.get(), out_pred_prob_d_.get(), out_pred_elem_num_ * sizeof(float),
+    cudaMemcpyDeviceToHost, *stream_));
+
+  cudaStreamSynchronize(*stream_);
+
+  float * out_stage1_net = out_s1center_prob_h_.get();
+  float * out_pred_net = out_pred_prob_h_.get();
+
+  output.feature_objects.resize(batch_size);
+  for (int i = 0; i < batch_size; i++) {
+    output.feature_objects.at(i) = input.feature_objects.at(i);
+
+    autoware_perception_msgs::msg::Shape shape;
+    geometry_msgs::msg::Pose pose;
+
+    shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX;
+
+    pose.position.x = out_stage1_net[i * out_s1center_elem_num_per_batch_ + 2] +
+                      out_pred_net[i * out_pred_elem_num_per_batch_ + 2];
+    pose.position.y =
+      -(out_stage1_net[i * out_s1center_elem_num_per_batch_ + 0] +
+        out_pred_net[i * out_pred_elem_num_per_batch_ + 0]);
+    pose.position.z =
+      -(out_stage1_net[i * out_s1center_elem_num_per_batch_ + 1] +
+        out_pred_net[i * out_pred_elem_num_per_batch_ + 1]);
+
+    int idx = 0;
+    idx += 3;
+
+    int NUM_HEADING_BIN = 12;
+    int NUM_SIZE_CLUSTER = 8;
+
+    int heading_idx = 0;
+    float max = std::numeric_limits<float>::lowest();
+    for (int j = idx; j < idx + NUM_HEADING_BIN; j++) {
+      if (out_pred_net[i * out_pred_elem_num_per_batch_ + j] > max) {
+        max = out_pred_net[i * out_pred_elem_num_per_batch_ + j];
+        heading_idx = j - idx;
+      }
+    }
+
+    idx += NUM_HEADING_BIN;
+
+    float heading_residual =
+      out_pred_net[i * out_pred_elem_num_per_batch_ + idx + heading_idx] * (M_PI / NUM_HEADING_BIN);
+
+    idx += NUM_HEADING_BIN;
+
+    float heading = class2angle(heading_idx, heading_residual, NUM_HEADING_BIN);
+
+    tf2::Quaternion quaternion;
+    quaternion.setRPY(0.0, 0.0, -M_PI / 2 - heading);
+
+    pose.orientation.x = quaternion.x();
+    pose.orientation.y = quaternion.y();
+    pose.orientation.z = quaternion.z();
+    pose.orientation.w = quaternion.w();
+
+    int size_idx = 0;
+    max = std::numeric_limits<float>::lowest();
+    for (int j = idx; j < idx + NUM_SIZE_CLUSTER; j++) {
+      if (out_pred_net[i * out_pred_elem_num_per_batch_ + j] > max) {
+        max = out_pred_net[i * out_pred_elem_num_per_batch_ + j];
+        size_idx = j - idx;
+      }
+    }
+
+    idx += NUM_SIZE_CLUSTER;
+    float x_size = out_pred_net[i * out_pred_elem_num_per_batch_ + idx + size_idx * 3] *
+                     g_type_mean_size_[size_idx][0] +
+                   g_type_mean_size_[size_idx][0];
+
+    float y_size = out_pred_net[i * out_pred_elem_num_per_batch_ + idx + size_idx * 3 + 1] *
+                     g_type_mean_size_[size_idx][1] +
+                   g_type_mean_size_[size_idx][1];
+    float z_size = out_pred_net[i * out_pred_elem_num_per_batch_ + idx + size_idx * 3 + 2] *
+                     g_type_mean_size_[size_idx][2] +
+                   g_type_mean_size_[size_idx][2];
+
+    shape.dimensions.x = x_size;
+    shape.dimensions.y = y_size;
+    shape.dimensions.z = z_size;
+
+    output.feature_objects.at(i).object.shape = shape;
+    output.feature_objects.at(i).object.kinematics.pose_with_covariance.pose = pose;
+  }
+
+  return true;
+}
+
+double TrtShapeEstimator::class2angle(int pred_cls, double residual, int num_class)
+{
+  double angle_per_class = 2.0 * M_PI / static_cast<double>(num_class);
+  double angle_center = static_cast<double>(pred_cls) * angle_per_class;
+  double angle = angle_center + residual;
+
+  // Adjust angle to be within the range [-pi, pi]
+  while (angle > M_PI) {
+    angle -= 2.0 * M_PI;
+  }
+  while (angle < -M_PI) {
+    angle += 2.0 * M_PI;
+  }
+  return angle;
+};
+}  // namespace autoware::shape_estimation
diff --git a/perception/autoware_shape_estimation/package.xml b/perception/autoware_shape_estimation/package.xml
index ad226a5cc2f66..fec1009428113 100644
--- a/perception/autoware_shape_estimation/package.xml
+++ b/perception/autoware_shape_estimation/package.xml
@@ -6,19 +6,27 @@
   <description>This package implements a shape estimation algorithm as a ROS 2 node</description>
   <maintainer email="yukihiro.saito@tier4.jp">Yukihiro Saito</maintainer>
   <maintainer email="yoshi.ri@tier4.jp">Yoshi Ri</maintainer>
+  <maintainer email="kcolak@leodrive.ai">Kaan Colak</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
+  <buildtool_depend>cudnn_cmake_module</buildtool_depend>
+  <buildtool_depend>tensorrt_cmake_module</buildtool_depend>
+
+  <buildtool_export_depend>cudnn_cmake_module</buildtool_export_depend>
+  <buildtool_export_depend>tensorrt_cmake_module</buildtool_export_depend>
 
   <depend>autoware_perception_msgs</depend>
   <depend>autoware_universe_utils</depend>
+  <depend>cuda_utils</depend>
   <depend>eigen</depend>
   <depend>libopencv-dev</depend>
   <depend>libpcl-all-dev</depend>
   <depend>pcl_conversions</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
+  <depend>tensorrt_common</depend>
   <depend>tf2</depend>
   <depend>tf2_eigen</depend>
   <depend>tf2_geometry_msgs</depend>
diff --git a/perception/autoware_shape_estimation/schema/shape_estimation.schema.json b/perception/autoware_shape_estimation/schema/shape_estimation.schema.json
index d81bfa636a923..6a7e3d25af534 100644
--- a/perception/autoware_shape_estimation/schema/shape_estimation.schema.json
+++ b/perception/autoware_shape_estimation/schema/shape_estimation.schema.json
@@ -30,6 +30,37 @@
           "type": "boolean",
           "description": "The flag to use boost bbox optimizer",
           "default": "false"
+        },
+        "model_params": {
+          "type": "object",
+          "description": "Parameters for model configuration.",
+          "properties": {
+            "use_ml_shape_estimator": {
+              "type": "boolean",
+              "description": "The flag to apply use ml bounding box estimator.",
+              "default": "true"
+            },
+            "minimum_points": {
+              "type": "integer",
+              "description": "The minimum number of points to fit a bounding box.",
+              "default": "16"
+            },
+            "precision": {
+              "type": "string",
+              "description": "The precision of the model.",
+              "default": "fp32"
+            },
+            "batch_size": {
+              "type": "integer",
+              "description": "The batch size of the model.",
+              "default": "32"
+            },
+            "build_only": {
+              "type": "boolean",
+              "description": "The flag to build the model only.",
+              "default": "false"
+            }
+          }
         }
       },
       "required": [
diff --git a/perception/autoware_shape_estimation/src/shape_estimation_node.cpp b/perception/autoware_shape_estimation/src/shape_estimation_node.cpp
index 4a373b15d580f..9c9c996ba0c6d 100644
--- a/perception/autoware_shape_estimation/src/shape_estimation_node.cpp
+++ b/perception/autoware_shape_estimation/src/shape_estimation_node.cpp
@@ -56,6 +56,23 @@ ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_option
   estimator_ =
     std::make_unique<ShapeEstimator>(use_corrector, use_filter, use_boost_bbox_optimizer);
 
+#ifdef USE_CUDA
+  use_ml_shape_estimation_ = declare_parameter<bool>("model_params.use_ml_shape_estimator");
+  if (use_ml_shape_estimation_) {
+    std::string model_path = declare_parameter<std::string>("model_path");
+    min_points_ = declare_parameter<int>("model_params.minimum_points");
+    std::string precision = declare_parameter<std::string>("model_params.precision");
+    int batch_size = declare_parameter<int>("model_params.batch_size");
+    tensorrt_common::BatchConfig batch_config{batch_size, batch_size, batch_size};
+    tensorrt_shape_estimator_ =
+      std::make_unique<TrtShapeEstimator>(model_path, precision, batch_config);
+    if (this->declare_parameter("model_params.build_only", false)) {
+      RCLCPP_INFO(this->get_logger(), "TensorRT engine is built and shutdown node.");
+      rclcpp::shutdown();
+    }
+  }
+#endif
+
   processing_time_publisher_ =
     std::make_unique<autoware::universe_utils::DebugPublisher>(this, "shape_estimation");
   stop_watch_ptr_ =
@@ -94,6 +111,9 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared
   DetectedObjectsWithFeature output_msg;
   output_msg.header = input_msg->header;
 
+  // Create ml model input batch
+  DetectedObjectsWithFeature input_trt_batch;
+
   // Estimate shape for each object and pack msg
   for (const auto & feature_object : input_msg->feature_objects) {
     const auto & object = feature_object.object;
@@ -109,6 +129,14 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared
       continue;
     }
 
+#ifdef USE_CUDA
+    // If ml based shape estimation is enabled, add object to input batch and continue
+    if (is_vehicle && use_ml_shape_estimation_ && cluster->size() > min_points_) {
+      input_trt_batch.feature_objects.push_back(feature_object);
+      continue;
+    }
+#endif
+
     // estimate shape and pose
     autoware_perception_msgs::msg::Shape shape;
     geometry_msgs::msg::Pose pose;
@@ -138,6 +166,16 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared
     output_msg.feature_objects.back().object.kinematics.pose_with_covariance.pose = pose;
   }
 
+#ifdef USE_CUDA
+  DetectedObjectsWithFeature output_model;
+  if (use_ml_shape_estimation_ && !input_trt_batch.feature_objects.empty()) {
+    tensorrt_shape_estimator_->inference(input_trt_batch, output_model);
+  }
+  for (auto & feature_object : output_model.feature_objects) {
+    output_msg.feature_objects.push_back(feature_object);
+  }
+#endif
+
   // Publish
   pub_->publish(output_msg);
   published_time_publisher_->publish_if_subscribed(pub_, output_msg.header.stamp);
diff --git a/perception/autoware_shape_estimation/src/shape_estimation_node.hpp b/perception/autoware_shape_estimation/src/shape_estimation_node.hpp
index d9f944346611a..a98d45453086b 100644
--- a/perception/autoware_shape_estimation/src/shape_estimation_node.hpp
+++ b/perception/autoware_shape_estimation/src/shape_estimation_node.hpp
@@ -17,6 +17,12 @@
 
 #include "autoware/shape_estimation/shape_estimator.hpp"
 
+#ifdef USE_CUDA
+#include "autoware/shape_estimation/tensorrt_shape_estimator.hpp"
+
+#include <tensorrt_common/tensorrt_common.hpp>
+#endif
+
 #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>
@@ -51,6 +57,13 @@ class ShapeEstimationNode : public rclcpp::Node
   bool use_vehicle_reference_shape_size_;
   bool fix_filtered_objects_label_to_unknown_;
 
+#ifdef USE_CUDA
+  std::unique_ptr<TrtShapeEstimator> tensorrt_shape_estimator_;
+#endif
+
+  bool use_ml_shape_estimation_;
+  size_t min_points_;
+
 public:
   explicit ShapeEstimationNode(const rclcpp::NodeOptions & node_options);
 };
diff --git a/perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp b/perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp
index a327e6395b2eb..98b368ff373e8 100644
--- a/perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp
+++ b/perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp
@@ -54,7 +54,7 @@ bool fileExists(const std::string & file_name, bool verbose)
   return true;
 }
 
-std::vector<std::string> loadListFromTextFile(const std::string filename)
+std::vector<std::string> loadListFromTextFile(const std::string & filename)
 {
   assert(fileExists(filename, true));
   std::vector<std::string> list;
diff --git a/perception/autoware_tensorrt_yolox/CMakeLists.txt b/perception/autoware_tensorrt_yolox/CMakeLists.txt
index 1f54326f2b33a..a6eaf28933160 100644
--- a/perception/autoware_tensorrt_yolox/CMakeLists.txt
+++ b/perception/autoware_tensorrt_yolox/CMakeLists.txt
@@ -143,6 +143,7 @@ rclcpp_components_register_node(yolox_single_image_inference_node
 )
 
 ament_auto_add_library(${PROJECT_NAME}_node SHARED
+  src/utils.cpp
   src/tensorrt_yolox_node.cpp
 )
 
@@ -166,6 +167,12 @@ rclcpp_components_register_node(${PROJECT_NAME}_node
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
+
+  if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL)
+    ament_auto_add_gtest(test_utils
+      test/test_utils.cpp
+    )
+  endif()
 endif()
 
 ament_auto_package(INSTALL_TO_SHARE
diff --git a/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox_node.hpp b/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox_node.hpp
index 7f622b2dc867e..2e317139f27c0 100644
--- a/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox_node.hpp
+++ b/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox_node.hpp
@@ -40,6 +40,7 @@
 #include <map>
 #include <memory>
 #include <string>
+#include <utility>
 #include <vector>
 
 namespace autoware::tensorrt_yolox
@@ -81,6 +82,7 @@ class TrtYoloXNode : public rclcpp::Node
   int mapRoiLabel2SegLabel(const int32_t roi_label_index);
   image_transport::Publisher image_pub_;
   image_transport::Publisher mask_pub_;
+
   image_transport::Publisher color_mask_pub_;
 
   rclcpp::Publisher<tier4_perception_msgs::msg::DetectedObjectsWithFeature>::SharedPtr objects_pub_;
diff --git a/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/utils.hpp b/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/utils.hpp
new file mode 100644
index 0000000000000..8b86a798e798b
--- /dev/null
+++ b/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/utils.hpp
@@ -0,0 +1,28 @@
+// 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__TENSORRT_YOLOX__UTILS_HPP_
+#define AUTOWARE__TENSORRT_YOLOX__UTILS_HPP_
+#include <opencv2/opencv.hpp>
+
+#include <utility>
+#include <vector>
+
+namespace autoware::tensorrt_yolox
+{
+std::vector<std::pair<uint8_t, int>> runLengthEncoder(const cv::Mat & mask);
+cv::Mat runLengthDecoder(const std::vector<uint8_t> & rle_data, const int rows, const int cols);
+}  // namespace autoware::tensorrt_yolox
+
+#endif  // AUTOWARE__TENSORRT_YOLOX__UTILS_HPP_
diff --git a/perception/autoware_tensorrt_yolox/package.xml b/perception/autoware_tensorrt_yolox/package.xml
index 9205b0c83cbea..b627b455ec7df 100644
--- a/perception/autoware_tensorrt_yolox/package.xml
+++ b/perception/autoware_tensorrt_yolox/package.xml
@@ -32,6 +32,7 @@
 
   <exec_depend>autoware_image_transport_decompressor</exec_depend>
 
+  <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
 
diff --git a/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp
index f3c7b2552edb6..7f398ca005d67 100644
--- a/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp
+++ b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp
@@ -14,6 +14,7 @@
 
 #include "autoware/tensorrt_yolox/tensorrt_yolox_node.hpp"
 
+#include "autoware/tensorrt_yolox/utils.hpp"
 #include "object_recognition_utils/object_classification.hpp"
 
 #include <autoware_perception_msgs/msg/object_classification.hpp>
@@ -177,12 +178,19 @@ void TrtYoloXNode::onImage(const sensor_msgs::msg::Image::ConstSharedPtr msg)
       overlapSegmentByRoi(yolox_object, mask, width, height);
     }
   }
-  // TODO(badai-nguyen): consider to change to 4bits data transfer
   if (trt_yolox_->getMultitaskNum() > 0) {
     sensor_msgs::msg::Image::SharedPtr out_mask_msg =
       cv_bridge::CvImage(std_msgs::msg::Header(), sensor_msgs::image_encodings::MONO8, mask)
         .toImageMsg();
     out_mask_msg->header = msg->header;
+
+    std::vector<std::pair<uint8_t, int>> compressed_data = runLengthEncoder(mask);
+    int step = sizeof(uint8_t) + sizeof(int);
+    out_mask_msg->data.resize(static_cast<int>(compressed_data.size()) * step);
+    for (size_t i = 0; i < compressed_data.size(); ++i) {
+      std::memcpy(&out_mask_msg->data[i * step], &compressed_data.at(i).first, sizeof(uint8_t));
+      std::memcpy(&out_mask_msg->data[i * step + 1], &compressed_data.at(i).second, sizeof(int));
+    }
     mask_pub_.publish(out_mask_msg);
   }
   image_pub_.publish(in_image_ptr->toImageMsg());
diff --git a/perception/autoware_tensorrt_yolox/src/utils.cpp b/perception/autoware_tensorrt_yolox/src/utils.cpp
new file mode 100644
index 0000000000000..8d2b4a4a0527f
--- /dev/null
+++ b/perception/autoware_tensorrt_yolox/src/utils.cpp
@@ -0,0 +1,65 @@
+// 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.
+
+#include "autoware/tensorrt_yolox/utils.hpp"
+
+namespace autoware::tensorrt_yolox
+{
+
+std::vector<std::pair<uint8_t, int>> runLengthEncoder(const cv::Mat & image)
+{
+  std::vector<std::pair<uint8_t, int>> compressed_data;
+  const int rows = image.rows;
+  const int cols = image.cols;
+  compressed_data.emplace_back(image.at<uint8_t>(0, 0), 0);
+  for (int i = 0; i < rows; ++i) {
+    for (int j = 0; j < cols; ++j) {
+      uint8_t current_value = image.at<uint8_t>(i, j);
+      if (compressed_data.back().first == current_value) {
+        ++compressed_data.back().second;
+      } else {
+        compressed_data.emplace_back(current_value, 1);
+      }
+    }
+  }
+  return compressed_data;
+}
+
+cv::Mat runLengthDecoder(const std::vector<uint8_t> & rle_data, const int rows, const int cols)
+{
+  cv::Mat mask(rows, cols, CV_8UC1, cv::Scalar(0));
+  int idx = 0;
+  int step = sizeof(uint8_t) + sizeof(int);
+  for (size_t i = 0; i < rle_data.size(); i += step) {
+    uint8_t value;
+    int length;
+    std::memcpy(&value, &rle_data[i], sizeof(uint8_t));
+    std::memcpy(
+      &length, &rle_data[i + 1],
+      sizeof(
+        int));  // under the condition that we know rle_data[i] only consume 1 element of the vector
+    for (int j = 0; j < length; ++j) {
+      int row_idx = static_cast<int>(idx / cols);
+      int col_idx = static_cast<int>(idx % cols);
+      mask.at<uint8_t>(row_idx, col_idx) = value;
+      idx++;
+      if (idx > rows * cols) {
+        break;
+      }
+    }
+  }
+  return mask;
+}
+
+}  // namespace autoware::tensorrt_yolox
diff --git a/perception/autoware_tensorrt_yolox/test/test_utils.cpp b/perception/autoware_tensorrt_yolox/test/test_utils.cpp
new file mode 100644
index 0000000000000..7f61ed9141c1e
--- /dev/null
+++ b/perception/autoware_tensorrt_yolox/test/test_utils.cpp
@@ -0,0 +1,80 @@
+// 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.
+
+#include "autoware/tensorrt_yolox/utils.hpp"
+
+#include <opencv2/opencv.hpp>
+
+#include <gtest/gtest.h>
+
+using autoware::tensorrt_yolox::runLengthDecoder;
+using autoware::tensorrt_yolox::runLengthEncoder;
+
+// Test case 1: Test if the decoded image is the same as the original image
+TEST(UtilsTest, runLengthEncoderDecoderTest)
+{
+  int height = 10;
+  int width = 20;
+  uint8_t number_cls = 16;
+  // Create an image as below
+  // 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+  // 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+  // 2 2 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+  // 3 3 3 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+  // 4 4 4 4 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+  // 5 5 5 5 5 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+  // 6 6 6 6 6 6 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+  // 7 7 7 7 7 7 7 0 0 0 0 0 0 0 0 0 0 0 0 0
+  // 8 8 8 8 8 8 8 8 0 0 0 0 0 0 0 0 0 0 0 0
+  // 9 9 9 9 9 9 9 9 9 0 0 0 0 0 0 0 0 0 0 0
+  cv::Mat image = cv::Mat::zeros(10, 20, CV_8UC1);
+  for (int i = 0; i < height; ++i) {
+    for (int j = 0; j < width; ++j) {
+      if (j < i) {
+        image.at<uint8_t>(i, j) = i % number_cls;
+      } else {
+        image.at<uint8_t>(i, j) = 0;
+      }
+    }
+  }
+  // Compress the image
+  std::vector<std::pair<uint8_t, int>> compressed_data = runLengthEncoder(image);
+  int step = sizeof(uint8_t) + sizeof(int);
+  std::vector<uint8_t> data(compressed_data.size() * step);
+  for (size_t i = 0; i < compressed_data.size(); ++i) {
+    std::memcpy(&data[i * step], &compressed_data.at(i).first, sizeof(uint8_t));
+    std::memcpy(&data[i * step + sizeof(uint8_t)], &compressed_data.at(i).second, sizeof(int));
+  }
+  // Decompress the image
+  cv::Mat decoded_image = runLengthDecoder(data, height, width);
+  // Compare the original image and the decoded image
+  ASSERT_EQ(image.rows, decoded_image.rows);
+  ASSERT_EQ(image.cols, decoded_image.cols);
+  bool image_eq = true;
+  for (int i = 0; i < image.rows; ++i) {
+    for (int j = 0; j < image.cols; ++j) {
+      if (image.at<uint8_t>(i, j) != decoded_image.at<uint8_t>(i, j)) {
+        image_eq = false;
+        break;
+      }
+    }
+  }
+  EXPECT_EQ(image_eq, true);
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/perception/autoware_traffic_light_classifier/src/classifier/cnn_classifier.cpp b/perception/autoware_traffic_light_classifier/src/classifier/cnn_classifier.cpp
index 6e3ae34297510..7a6d00f0135c5 100644
--- a/perception/autoware_traffic_light_classifier/src/classifier/cnn_classifier.cpp
+++ b/perception/autoware_traffic_light_classifier/src/classifier/cnn_classifier.cpp
@@ -195,7 +195,7 @@ bool CNNClassifier::readLabelfile(std::string filepath, std::vector<std::string>
   return true;
 }
 
-bool CNNClassifier::isColorLabel(const std::string label)
+bool CNNClassifier::isColorLabel(const std::string & label)
 {
   using tier4_perception_msgs::msg::TrafficLight;
   if (
diff --git a/perception/autoware_traffic_light_classifier/src/classifier/cnn_classifier.hpp b/perception/autoware_traffic_light_classifier/src/classifier/cnn_classifier.hpp
index 5f6f086763f4e..85895cfc9e637 100644
--- a/perception/autoware_traffic_light_classifier/src/classifier/cnn_classifier.hpp
+++ b/perception/autoware_traffic_light_classifier/src/classifier/cnn_classifier.hpp
@@ -63,7 +63,7 @@ class CNNClassifier : public ClassifierInterface
   void postProcess(
     int class_index, float prob, tier4_perception_msgs::msg::TrafficLight & traffic_signal);
   bool readLabelfile(std::string filepath, std::vector<std::string> & labels);
-  bool isColorLabel(const std::string label);
+  bool isColorLabel(const std::string & label);
   void outputDebugImage(
     cv::Mat & debug_image, const tier4_perception_msgs::msg::TrafficLight & traffic_signal);
 
diff --git a/perception/autoware_traffic_light_classifier/src/traffic_light_classifier_node.cpp b/perception/autoware_traffic_light_classifier/src/traffic_light_classifier_node.cpp
index b42ded4cbffa3..88c15c7c457eb 100644
--- a/perception/autoware_traffic_light_classifier/src/traffic_light_classifier_node.cpp
+++ b/perception/autoware_traffic_light_classifier/src/traffic_light_classifier_node.cpp
@@ -84,6 +84,9 @@ void TrafficLightClassifierNodelet::imageRoiCallback(
   if (classifier_ptr_.use_count() == 0) {
     return;
   }
+  if (input_rois_msg->rois.empty()) {
+    return;
+  }
 
   cv_bridge::CvImagePtr cv_ptr;
   try {
@@ -95,41 +98,49 @@ void TrafficLightClassifierNodelet::imageRoiCallback(
   }
 
   tier4_perception_msgs::msg::TrafficLightArray output_msg;
-
   output_msg.signals.resize(input_rois_msg->rois.size());
 
   std::vector<cv::Mat> images;
   std::vector<size_t> backlight_indices;
-  for (size_t i = 0; i < input_rois_msg->rois.size(); i++) {
-    // skip if the roi is not detected
-    if (input_rois_msg->rois.at(i).roi.height == 0) {
-      break;
+  size_t idx_valid_roi = 0;
+  for (const auto & input_roi : input_rois_msg->rois) {
+    // ignore if the roi is not the type to be classified
+    if (input_roi.traffic_light_type != classify_traffic_light_type_) {
+      continue;
     }
-    if (input_rois_msg->rois.at(i).traffic_light_type != classify_traffic_light_type_) {
+    // skip if the roi size is zero
+    if (input_roi.roi.height == 0 || input_roi.roi.width == 0) {
       continue;
     }
-    output_msg.signals[images.size()].traffic_light_id =
-      input_rois_msg->rois.at(i).traffic_light_id;
-    output_msg.signals[images.size()].traffic_light_type =
-      input_rois_msg->rois.at(i).traffic_light_type;
-    const sensor_msgs::msg::RegionOfInterest & roi = input_rois_msg->rois.at(i).roi;
 
+    // set traffic light id and type
+    output_msg.signals[idx_valid_roi].traffic_light_id = input_roi.traffic_light_id;
+    output_msg.signals[idx_valid_roi].traffic_light_type = input_roi.traffic_light_type;
+
+    const sensor_msgs::msg::RegionOfInterest & roi = input_roi.roi;
     auto roi_img = cv_ptr->image(cv::Rect(roi.x_offset, roi.y_offset, roi.width, roi.height));
     if (is_harsh_backlight(roi_img)) {
-      backlight_indices.emplace_back(i);
+      backlight_indices.emplace_back(idx_valid_roi);
     }
     images.emplace_back(roi_img);
+    idx_valid_roi++;
   }
 
+  // classify the images
   output_msg.signals.resize(images.size());
-  if (!classifier_ptr_->getTrafficSignals(images, output_msg)) {
-    RCLCPP_ERROR(this->get_logger(), "failed classify image, abort callback");
-    return;
+  if (!images.empty()) {
+    if (!classifier_ptr_->getTrafficSignals(images, output_msg)) {
+      RCLCPP_ERROR(this->get_logger(), "failed classify image, abort callback");
+      return;
+    }
   }
 
   // append the undetected rois as unknown
   for (const auto & input_roi : input_rois_msg->rois) {
-    if (input_roi.roi.height == 0 && input_roi.traffic_light_type == classify_traffic_light_type_) {
+    // if the type is the target type but the roi size is zero, the roi is undetected
+    if (
+      (input_roi.roi.height == 0 || input_roi.roi.width == 0) &&
+      input_roi.traffic_light_type == classify_traffic_light_type_) {
       tier4_perception_msgs::msg::TrafficLight tlr_sig;
       tlr_sig.traffic_light_id = input_roi.traffic_light_id;
       tlr_sig.traffic_light_type = input_roi.traffic_light_type;
diff --git a/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp b/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp
index 9aae99012cc4b..381fa79ed090c 100644
--- a/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp
+++ b/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp
@@ -49,7 +49,7 @@ float calWeightedIou(
 
 namespace autoware::traffic_light
 {
-inline std::vector<float> toFloatVector(const std::vector<double> double_vector)
+inline std::vector<float> toFloatVector(const std::vector<double> & double_vector)
 {
   return std::vector<float>(double_vector.begin(), double_vector.end());
 }
diff --git a/perception/autoware_traffic_light_occlusion_predictor/src/occlusion_predictor.cpp b/perception/autoware_traffic_light_occlusion_predictor/src/occlusion_predictor.cpp
index 74878ba0b93af..7fd5ac8489226 100644
--- a/perception/autoware_traffic_light_occlusion_predictor/src/occlusion_predictor.cpp
+++ b/perception/autoware_traffic_light_occlusion_predictor/src/occlusion_predictor.cpp
@@ -148,7 +148,7 @@ void CloudOcclusionPredictor::calcRoiVector3D(
 
 void CloudOcclusionPredictor::filterCloud(
   const pcl::PointCloud<pcl::PointXYZ> & cloud_in, const std::vector<pcl::PointXYZ> & roi_tls,
-  const std::vector<pcl::PointXYZ> & roi_brs, pcl::PointCloud<pcl::PointXYZ> & cloud_out)
+  const std::vector<pcl::PointXYZ> & roi_brs, pcl::PointCloud<pcl::PointXYZ> & cloud_out) const
 {
   float min_x = 0, max_x = 0, min_y = 0, max_y = 0, min_z = 0, max_z = 0;
   for (const auto & pt : roi_tls) {
diff --git a/perception/autoware_traffic_light_occlusion_predictor/src/occlusion_predictor.hpp b/perception/autoware_traffic_light_occlusion_predictor/src/occlusion_predictor.hpp
index 435ee478eea22..701b15f7c92b8 100644
--- a/perception/autoware_traffic_light_occlusion_predictor/src/occlusion_predictor.hpp
+++ b/perception/autoware_traffic_light_occlusion_predictor/src/occlusion_predictor.hpp
@@ -73,7 +73,7 @@ class CloudOcclusionPredictor
 
   void filterCloud(
     const pcl::PointCloud<pcl::PointXYZ> & cloud_in, const std::vector<pcl::PointXYZ> & roi_tls,
-    const std::vector<pcl::PointXYZ> & roi_brs, pcl::PointCloud<pcl::PointXYZ> & cloud_out);
+    const std::vector<pcl::PointXYZ> & roi_brs, pcl::PointCloud<pcl::PointXYZ> & cloud_out) const;
 
   void sampleTrafficLightRoi(
     const pcl::PointXYZ & top_left, const pcl::PointXYZ & bottom_right,
diff --git a/perception/autoware_traffic_light_visualization/config/traffic_light_visualization.param.yaml b/perception/autoware_traffic_light_visualization/config/traffic_light_visualization.param.yaml
index 1fc3167ef857f..1354f7e619b86 100644
--- a/perception/autoware_traffic_light_visualization/config/traffic_light_visualization.param.yaml
+++ b/perception/autoware_traffic_light_visualization/config/traffic_light_visualization.param.yaml
@@ -1,3 +1,4 @@
 /**:
   ros__parameters:
     enable_fine_detection: false
+    use_image_transport: true
diff --git a/perception/autoware_traffic_light_visualization/launch/traffic_light_roi_visualizer.launch.xml b/perception/autoware_traffic_light_visualization/launch/traffic_light_roi_visualizer.launch.xml
index e96ff3fa0106d..d4af7a27636df 100644
--- a/perception/autoware_traffic_light_visualization/launch/traffic_light_roi_visualizer.launch.xml
+++ b/perception/autoware_traffic_light_visualization/launch/traffic_light_roi_visualizer.launch.xml
@@ -5,6 +5,7 @@
   <arg name="input/traffic_signals" default="~/input/traffic_signals"/>
   <arg name="output/image" default="~/debug/rois"/>
   <arg name="enable_fine_detection" default="false"/>
+  <arg name="use_image_transport" default="true"/>
 
   <node pkg="autoware_traffic_light_visualization" exec="traffic_light_visualization_node" name="traffic_light_visualization">
     <remap from="~/input/image" to="$(var input/image)"/>
@@ -13,5 +14,6 @@
     <remap from="~/input/traffic_signals" to="$(var input/traffic_signals)"/>
     <remap from="~/output/image" to="$(var output/image)"/>
     <param name="enable_fine_detection" value="$(var enable_fine_detection)"/>
+    <param name="use_image_transport" value="$(var use_image_transport)"/>
   </node>
 </launch>
diff --git a/perception/autoware_traffic_light_visualization/schema/traffic_light_visualization.schema.json b/perception/autoware_traffic_light_visualization/schema/traffic_light_visualization.schema.json
index 6ef025692904c..7058e89c3e5ea 100644
--- a/perception/autoware_traffic_light_visualization/schema/traffic_light_visualization.schema.json
+++ b/perception/autoware_traffic_light_visualization/schema/traffic_light_visualization.schema.json
@@ -10,9 +10,14 @@
           "type": "boolean",
           "description": "whether to visualize result of the traffic light fine detection",
           "default": "false"
+        },
+        "use_image_transport": {
+          "type": "boolean",
+          "description": "whether to apply image transport to compress the output debugging image in the traffic light fine detection",
+          "default": "true"
         }
       },
-      "required": ["enable_fine_detection"]
+      "required": ["enable_fine_detection", "use_image_transport"]
     }
   },
   "properties": {
diff --git a/perception/autoware_traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp b/perception/autoware_traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp
index 90b73e643ae8e..ccfaf37fd7c6d 100644
--- a/perception/autoware_traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp
+++ b/perception/autoware_traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp
@@ -38,7 +38,7 @@ namespace
 }
 
 bool isAttributeValue(
-  const lanelet::ConstPoint3d p, const std::string attr_str, const std::string value_str)
+  const lanelet::ConstPoint3d p, const std::string & attr_str, const std::string & value_str)
 {
   lanelet::Attribute attr = p.attribute(attr_str);
   if (attr.value().compare(value_str) == 0) {
@@ -49,7 +49,7 @@ bool isAttributeValue(
 
 void lightAsMarker(
   const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & node_logging,
-  lanelet::ConstPoint3d p, visualization_msgs::msg::Marker * marker, const std::string ns,
+  lanelet::ConstPoint3d p, visualization_msgs::msg::Marker * marker, const std::string & ns,
   const rclcpp::Time & current_time)
 {
   if (marker == nullptr) {
diff --git a/perception/autoware_traffic_light_visualization/src/traffic_light_map_visualizer/node.hpp b/perception/autoware_traffic_light_visualization/src/traffic_light_map_visualizer/node.hpp
index 61b5204b57592..1870a2b1bf74f 100644
--- a/perception/autoware_traffic_light_visualization/src/traffic_light_map_visualizer/node.hpp
+++ b/perception/autoware_traffic_light_visualization/src/traffic_light_map_visualizer/node.hpp
@@ -31,11 +31,11 @@ namespace autoware::traffic_light
 class TrafficLightMapVisualizerNode : public rclcpp::Node
 {
 public:
-  explicit TrafficLightMapVisualizerNode(const rclcpp::NodeOptions & options);
+  explicit TrafficLightMapVisualizerNode(const rclcpp::NodeOptions & node_options);
   ~TrafficLightMapVisualizerNode() = default;
   void trafficSignalsCallback(
     const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr
-      input_traffic_signals_msg);
+      input_traffic_signals);
   void binMapCallback(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr input_map_msg);
 
 private:
diff --git a/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.cpp b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.cpp
index 1afc048bd0f30..7482749630a48 100644
--- a/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.cpp
+++ b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.cpp
@@ -32,6 +32,7 @@ TrafficLightRoiVisualizerNode::TrafficLightRoiVisualizerNode(const rclcpp::NodeO
   using std::placeholders::_3;
   using std::placeholders::_4;
   enable_fine_detection_ = this->declare_parameter<bool>("enable_fine_detection");
+  use_image_transport_ = this->declare_parameter<bool>("use_image_transport");
 
   if (enable_fine_detection_) {
     sync_with_rough_roi_.reset(new SyncWithRoughRoi(
@@ -48,13 +49,24 @@ TrafficLightRoiVisualizerNode::TrafficLightRoiVisualizerNode(const rclcpp::NodeO
   timer_ = rclcpp::create_timer(
     this, get_clock(), 100ms, std::bind(&TrafficLightRoiVisualizerNode::connectCb, this));
 
-  image_pub_ =
-    image_transport::create_publisher(this, "~/output/image", rclcpp::QoS{1}.get_rmw_qos_profile());
+  if (use_image_transport_) {
+    image_pub_ = image_transport::create_publisher(
+      this, "~/output/image", rclcpp::QoS{1}.get_rmw_qos_profile());
+  } else {
+    auto qos = rclcpp::QoS(1);
+    simple_image_pub_ = this->create_publisher<sensor_msgs::msg::Image>("~/output/image", qos);
+  }
 }
 
 void TrafficLightRoiVisualizerNode::connectCb()
 {
-  if (image_pub_.getNumSubscribers() == 0) {
+  int num_subscribers = 0;
+  if (use_image_transport_) {
+    num_subscribers = image_pub_.getNumSubscribers();
+  } else {
+    num_subscribers = simple_image_pub_->get_subscription_count();
+  }
+  if (num_subscribers == 0) {
     image_sub_.unsubscribe();
     traffic_signals_sub_.unsubscribe();
     roi_sub_.unsubscribe();
@@ -143,7 +155,11 @@ void TrafficLightRoiVisualizerNode::imageRoiCallback(
     RCLCPP_ERROR(
       get_logger(), "Could not convert from '%s' to 'rgb8'.", input_image_msg->encoding.c_str());
   }
-  image_pub_.publish(cv_ptr->toImageMsg());
+  if (use_image_transport_) {
+    image_pub_.publish(cv_ptr->toImageMsg());
+  } else {
+    simple_image_pub_->publish(*cv_ptr->toImageMsg());
+  }
 }
 
 bool TrafficLightRoiVisualizerNode::getClassificationResult(
@@ -220,7 +236,11 @@ void TrafficLightRoiVisualizerNode::imageRoughRoiCallback(
     RCLCPP_ERROR(
       get_logger(), "Could not convert from '%s' to 'rgb8'.", input_image_msg->encoding.c_str());
   }
-  image_pub_.publish(cv_ptr->toImageMsg());
+  if (use_image_transport_) {
+    image_pub_.publish(cv_ptr->toImageMsg());
+  } else {
+    simple_image_pub_->publish(*cv_ptr->toImageMsg());
+  }
 }
 
 }  // namespace autoware::traffic_light
diff --git a/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.hpp b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.hpp
index ac16d760adaab..47a13389950a5 100644
--- a/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.hpp
+++ b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.hpp
@@ -121,6 +121,8 @@ class TrafficLightRoiVisualizerNode : public rclcpp::Node
   message_filters::Subscriber<tier4_perception_msgs::msg::TrafficLightRoiArray> rough_roi_sub_;
   message_filters::Subscriber<tier4_perception_msgs::msg::TrafficLightArray> traffic_signals_sub_;
   image_transport::Publisher image_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr simple_image_pub_;
+
   typedef message_filters::sync_policies::ApproximateTime<
     sensor_msgs::msg::Image, tier4_perception_msgs::msg::TrafficLightRoiArray,
     tier4_perception_msgs::msg::TrafficLightArray>
@@ -136,6 +138,7 @@ class TrafficLightRoiVisualizerNode : public rclcpp::Node
   std::shared_ptr<SyncWithRoughRoi> sync_with_rough_roi_;
 
   bool enable_fine_detection_;
+  bool use_image_transport_;
 };
 
 }  // namespace autoware::traffic_light
diff --git a/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp
index c44b1415a09dc..c685bc0639601 100644
--- a/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp
+++ b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp
@@ -52,7 +52,10 @@ void drawShape(
   if (
     position.x < 0 || position.y < 0 || position.x + shapeImg.cols > params.image.cols ||
     position.y + shapeImg.rows > params.image.rows) {
-    std::cerr << "Adjusted position is out of image bounds." << std::endl;
+    // TODO(KhalilSelyan): This error message may flood the terminal logs, so commented out
+    // temporarily. Need to consider a better way.
+
+    // std::cerr << "Adjusted position is out of image bounds." << std::endl;
     return;
   }
 
diff --git a/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp b/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp
index 7b4c8e3856b38..f43b4401865b2 100644
--- a/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp
+++ b/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp
@@ -60,7 +60,8 @@ void publishMandatoryTopics(
   test_manager->publishParkingScenario(test_target_node, "freespace_planner/input/scenario");
 }
 
-TEST(PlanningModuleInterfaceTest, testPlanningInterfaceWithVariousTrajectoryInput)
+// the following tests are disable because they randomly fail
+TEST(PlanningModuleInterfaceTest, DISABLED_testPlanningInterfaceWithVariousTrajectoryInput)
 {
   rclcpp::init(0, nullptr);
 
@@ -77,7 +78,7 @@ TEST(PlanningModuleInterfaceTest, testPlanningInterfaceWithVariousTrajectoryInpu
   rclcpp::shutdown();
 }
 
-TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose)
+TEST(PlanningModuleInterfaceTest, DISABLED_NodeTestWithOffTrackEgoPose)
 {
   rclcpp::init(0, nullptr);
   auto test_manager = generateTestManager();
diff --git a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp
index 3f6c3935b8a42..f7434d912d5e0 100644
--- a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp
+++ b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp
@@ -79,7 +79,7 @@ struct AstarNode
 
 struct NodeComparison
 {
-  bool operator()(const AstarNode * lhs, const AstarNode * rhs) { return lhs->fc > rhs->fc; }
+  bool operator()(const AstarNode * lhs, const AstarNode * rhs) const { return lhs->fc > rhs->fc; }
 };
 
 struct NodeUpdate
diff --git a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp
index 4ead3cb5a7423..9233ad212bfa5 100644
--- a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp
+++ b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp
@@ -155,7 +155,7 @@ void AstarSearch::setCollisionFreeDistanceMap()
   using Entry = std::pair<IndexXY, double>;
   struct CompareEntry
   {
-    bool operator()(const Entry & a, const Entry & b) { return a.second > b.second; }
+    bool operator()(const Entry & a, const Entry & b) const { return a.second > b.second; }
   };
   std::priority_queue<Entry, std::vector<Entry>, CompareEntry> heap;
   std::vector<bool> closed(col_free_distance_map_.size(), false);
diff --git a/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.cpp b/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.cpp
index c5fcf29466936..ee2eadac6f780 100644
--- a/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.cpp
+++ b/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.cpp
@@ -605,7 +605,7 @@ double AdaptiveCruiseController::calcBaseDistToForwardObstacle(
 }
 
 double AdaptiveCruiseController::calcTargetVelocity_P(
-  const double target_dist, const double current_dist)
+  const double target_dist, const double current_dist) const
 {
   const double diff_dist = current_dist - target_dist;
   double add_vel_p;
@@ -748,7 +748,7 @@ void AdaptiveCruiseController::registerQueToVelocity(
   est_vel_que_.emplace_back(new_vel);
 }
 
-double AdaptiveCruiseController::getMedianVel(const std::vector<nav_msgs::msg::Odometry> vel_que)
+double AdaptiveCruiseController::getMedianVel(const std::vector<nav_msgs::msg::Odometry> & vel_que)
 {
   if (vel_que.size() == 0) {
     RCLCPP_WARN_STREAM(node_->get_logger(), "size of vel que is 0. Something has wrong.");
diff --git a/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.hpp b/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.hpp
index 586866f9a94d6..7535986f1db85 100644
--- a/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.hpp
+++ b/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.hpp
@@ -183,14 +183,15 @@ class AdaptiveCruiseController
   };
   Param param_;
 
-  double getMedianVel(const std::vector<nav_msgs::msg::Odometry> vel_que);
-  double lowpass_filter(const double current_value, const double prev_value, const double gain);
+  double getMedianVel(const std::vector<nav_msgs::msg::Odometry> & vel_que);
+  static double lowpass_filter(
+    const double current_value, const double prev_value, const double gain);
   void calcDistanceToNearestPointOnPath(
     const TrajectoryPoints & trajectory, const int nearest_point_idx,
     const geometry_msgs::msg::Pose & self_pose, const pcl::PointXYZ & nearest_collision_point,
     const rclcpp::Time & nearest_collision_point_time, double * distance,
     const std_msgs::msg::Header & trajectory_header);
-  double calcTrajYaw(const TrajectoryPoints & trajectory, const int collision_point_idx);
+  static double calcTrajYaw(const TrajectoryPoints & trajectory, const int collision_point_idx);
   std::optional<double> estimatePointVelocityFromObject(
     const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr,
     const double traj_yaw, const pcl::PointXYZ & nearest_collision_point);
@@ -204,8 +205,8 @@ class AdaptiveCruiseController
   double calcUpperVelocity(const double dist_to_col, const double obj_vel, const double self_vel);
   double calcThreshDistToForwardObstacle(const double current_vel, const double obj_vel);
   double calcBaseDistToForwardObstacle(const double current_vel, const double obj_vel);
-  double calcTargetVelocity_P(const double target_dist, const double current_dist);
-  double calcTargetVelocity_I(const double target_dist, const double current_dist);
+  double calcTargetVelocity_P(const double target_dist, const double current_dist) const;
+  static double calcTargetVelocity_I(const double target_dist, const double current_dist);
   double calcTargetVelocity_D(const double target_dist, const double current_dist);
   double calcTargetVelocityByPID(
     const double current_vel, const double current_dist, const double obj_vel);
diff --git a/planning/autoware_obstacle_stop_planner/src/debug_marker.hpp b/planning/autoware_obstacle_stop_planner/src/debug_marker.hpp
index 4fc8386e92b44..1b4cdfd87f81b 100644
--- a/planning/autoware_obstacle_stop_planner/src/debug_marker.hpp
+++ b/planning/autoware_obstacle_stop_planner/src/debug_marker.hpp
@@ -78,7 +78,7 @@ class DebugValues
    * @param [in] type the TYPE enum for which to get the index
    * @return index of the type
    */
-  int getValuesIdx(const TYPE type) const { return static_cast<int>(type); }
+  static int getValuesIdx(const TYPE type) { return static_cast<int>(type); }
   /**
    * @brief get all the debug values as an std::array
    * @return array of all debug values
diff --git a/planning/autoware_obstacle_stop_planner/src/planner_utils.cpp b/planning/autoware_obstacle_stop_planner/src/planner_utils.cpp
index bff8f9588215e..28c5523f49620 100644
--- a/planning/autoware_obstacle_stop_planner/src/planner_utils.cpp
+++ b/planning/autoware_obstacle_stop_planner/src/planner_utils.cpp
@@ -228,7 +228,7 @@ std::string jsonDumpsPose(const Pose & pose)
   return json_dumps_pose;
 }
 
-DiagnosticStatus makeStopReasonDiag(const std::string stop_reason, const Pose & stop_pose)
+DiagnosticStatus makeStopReasonDiag(const std::string & stop_reason, const Pose & stop_pose)
 {
   DiagnosticStatus stop_reason_diag;
   KeyValue stop_reason_diag_kv;
diff --git a/planning/autoware_obstacle_stop_planner/src/planner_utils.hpp b/planning/autoware_obstacle_stop_planner/src/planner_utils.hpp
index 21943c46f5ca5..228c71d08a9b9 100644
--- a/planning/autoware_obstacle_stop_planner/src/planner_utils.hpp
+++ b/planning/autoware_obstacle_stop_planner/src/planner_utils.hpp
@@ -122,7 +122,7 @@ Polygon2d convertBoundingBoxObjectToGeometryPolygon(
 
 std::string jsonDumpsPose(const Pose & pose);
 
-DiagnosticStatus makeStopReasonDiag(const std::string stop_reason, const Pose & stop_pose);
+DiagnosticStatus makeStopReasonDiag(const std::string & stop_reason, const Pose & stop_pose);
 
 TrajectoryPoint getBackwardPointFromBasePoint(
   const TrajectoryPoint & p_from, const TrajectoryPoint & p_to, const TrajectoryPoint & p_base,
diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp
index bbc3828bb1b15..bfcd7db6b9c43 100644
--- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp
+++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp
@@ -73,7 +73,8 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase
 
   bool apply(
     const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
-    TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories) override;
+    TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories,
+    const bool publish_debug_trajs) override;
 
   TrajectoryPoints resampleTrajectory(
     const TrajectoryPoints & input, [[maybe_unused]] const double v0,
diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp
index 541ba62e95d74..0640bc950f411 100644
--- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp
+++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp
@@ -45,8 +45,8 @@ bool calcStopVelocityWithConstantJerkAccLimit(
   const std::vector<double> & times, const size_t start_index,
   TrajectoryPoints & output_trajectory);
 void updateStopVelocityStatus(
-  double v0, double a0, double jerk_acc, double jerk_dec, int type, std::vector<double> times,
-  double t, double & x, double & v, double & a, double & j);
+  double v0, double a0, double jerk_acc, double jerk_dec, int type,
+  const std::vector<double> & times, double t, double & x, double & v, double & a, double & j);
 double integ_x(double x0, double v0, double a0, double j0, double t);
 double integ_v(double v0, double a0, double j0, double t);
 double integ_a(double a0, double j0, double t);
diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp
index 06a6f2da7f30a..79dd5cda20d4b 100644
--- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp
+++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp
@@ -19,7 +19,7 @@
 #include "autoware/universe_utils/geometry/geometry.hpp"
 #include "autoware/universe_utils/system/time_keeper.hpp"
 #include "autoware/velocity_smoother/smoother/smoother_base.hpp"
-#include "osqp_interface/osqp_interface.hpp"
+#include "qp_interface/qp_interface.hpp"
 
 #include "autoware_planning_msgs/msg/trajectory_point.hpp"
 
@@ -47,7 +47,8 @@ class JerkFilteredSmoother : public SmootherBase
 
   bool apply(
     const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
-    TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories) override;
+    TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories,
+    const bool publish_debug_trajs) override;
 
   TrajectoryPoints resampleTrajectory(
     const TrajectoryPoints & input, [[maybe_unused]] const double v0,
@@ -59,7 +60,7 @@ class JerkFilteredSmoother : public SmootherBase
 
 private:
   Param smoother_param_;
-  autoware::common::osqp::OSQPInterface qp_solver_;
+  std::shared_ptr<autoware::common::QPInterface> qp_interface_;
   rclcpp::Logger logger_{rclcpp::get_logger("smoother").get_child("jerk_filtered_smoother")};
 
   TrajectoryPoints forwardJerkFilter(
diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp
index a2a07ec6909aa..8f6bbc2236eb6 100644
--- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp
+++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp
@@ -45,7 +45,8 @@ class L2PseudoJerkSmoother : public SmootherBase
 
   bool apply(
     const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
-    TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories) override;
+    TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories,
+    const bool publish_debug_trajs) override;
 
   TrajectoryPoints resampleTrajectory(
     const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose,
diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp
index 7c46a53431608..e27a4db10e748 100644
--- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp
+++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp
@@ -45,7 +45,8 @@ class LinfPseudoJerkSmoother : public SmootherBase
 
   bool apply(
     const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
-    TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories) override;
+    TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories,
+    const bool publish_debug_trajs) override;
 
   TrajectoryPoints resampleTrajectory(
     const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose,
diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp
index 6671eaa3eabe7..7206a64ea32eb 100644
--- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp
+++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp
@@ -61,7 +61,8 @@ class SmootherBase
   virtual ~SmootherBase() = default;
   virtual bool apply(
     const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
-    TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories) = 0;
+    TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories,
+    const bool publish_debug_trajs) = 0;
 
   virtual TrajectoryPoints resampleTrajectory(
     const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose,
diff --git a/planning/autoware_velocity_smoother/package.xml b/planning/autoware_velocity_smoother/package.xml
index 62a08f60ac4e6..f5cd3f2667c04 100644
--- a/planning/autoware_velocity_smoother/package.xml
+++ b/planning/autoware_velocity_smoother/package.xml
@@ -30,6 +30,7 @@
   <depend>interpolation</depend>
   <depend>nav_msgs</depend>
   <depend>osqp_interface</depend>
+  <depend>qp_interface</depend>
   <depend>rclcpp</depend>
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp
index 3420b106d29f7..a358a1b61c11b 100644
--- a/planning/autoware_velocity_smoother/src/node.cpp
+++ b/planning/autoware_velocity_smoother/src/node.cpp
@@ -629,7 +629,8 @@ bool VelocitySmootherNode::smoothVelocity(
 
   std::vector<TrajectoryPoints> debug_trajectories;
   if (!smoother_->apply(
-        initial_motion.vel, initial_motion.acc, clipped, traj_smoothed, debug_trajectories)) {
+        initial_motion.vel, initial_motion.acc, clipped, traj_smoothed, debug_trajectories,
+        publish_debug_trajs_)) {
     RCLCPP_WARN(get_logger(), "Fail to solve optimization.");
   }
 
@@ -669,15 +670,13 @@ bool VelocitySmootherNode::smoothVelocity(
       pub_trajectory_steering_rate_limited_->publish(toTrajectoryMsg(tmp));
     }
 
-    if (!debug_trajectories.empty()) {
-      for (auto & debug_trajectory : debug_trajectories) {
-        debug_trajectory.insert(
-          debug_trajectory.begin(), traj_resampled.begin(),
-          traj_resampled.begin() + traj_resampled_closest);
-        for (size_t i = 0; i < traj_resampled_closest; ++i) {
-          debug_trajectory.at(i).longitudinal_velocity_mps =
-            debug_trajectory.at(traj_resampled_closest).longitudinal_velocity_mps;
-        }
+    for (auto & debug_trajectory : debug_trajectories) {
+      debug_trajectory.insert(
+        debug_trajectory.begin(), traj_resampled.begin(),
+        traj_resampled.begin() + traj_resampled_closest);
+      for (size_t i = 0; i < traj_resampled_closest; ++i) {
+        debug_trajectory.at(i).longitudinal_velocity_mps =
+          debug_trajectory.at(traj_resampled_closest).longitudinal_velocity_mps;
       }
     }
     publishDebugTrajectories(debug_trajectories);
diff --git a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp
index 3a6bc736c96ab..fae1b3590cacd 100644
--- a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp
+++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp
@@ -103,7 +103,8 @@ AnalyticalJerkConstrainedSmoother::Param AnalyticalJerkConstrainedSmoother::getP
 
 bool AnalyticalJerkConstrainedSmoother::apply(
   const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
-  TrajectoryPoints & output, [[maybe_unused]] std::vector<TrajectoryPoints> & debug_trajectories)
+  TrajectoryPoints & output, [[maybe_unused]] std::vector<TrajectoryPoints> & debug_trajectories,
+  [[maybe_unused]] const bool publish_debug_trajs)
 {
   RCLCPP_DEBUG(logger_, "-------------------- Start --------------------");
 
diff --git a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp
index 739ba6e7cef38..9d474a9a52bfa 100644
--- a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp
+++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp
@@ -271,8 +271,8 @@ bool calcStopVelocityWithConstantJerkAccLimit(
 }
 
 void updateStopVelocityStatus(
-  double v0, double a0, double jerk_acc, double jerk_dec, int type, std::vector<double> times,
-  double t, double & x, double & v, double & a, double & j)
+  double v0, double a0, double jerk_acc, double jerk_dec, int type,
+  const std::vector<double> & times, double t, double & x, double & v, double & a, double & j)
 {
   if (type == 1) {
     if (0 <= t && t < times.at(0)) {
diff --git a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp
index d458c688d060c..3e363b8bcbab9 100644
--- a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp
+++ b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp
@@ -15,6 +15,7 @@
 #include "autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp"
 
 #include "autoware/velocity_smoother/trajectory_utils.hpp"
+#include "qp_interface/proxqp_interface.hpp"
 
 #include <Eigen/Core>
 
@@ -40,11 +41,8 @@ JerkFilteredSmoother::JerkFilteredSmoother(
   p.over_j_weight = node.declare_parameter<double>("over_j_weight");
   p.jerk_filter_ds = node.declare_parameter<double>("jerk_filter_ds");
 
-  qp_solver_.updateMaxIter(20000);
-  qp_solver_.updateRhoInterval(0);  // 0 means automatic
-  qp_solver_.updateEpsRel(1.0e-6);  // def: 1.0e-4
-  qp_solver_.updateEpsAbs(1.0e-8);  // def: 1.0e-4
-  qp_solver_.updateVerbose(false);
+  qp_interface_ =
+    std::make_shared<autoware::common::ProxQPInterface>(false, 20000, 1.0e-8, 1.0e-6, false);
 }
 
 void JerkFilteredSmoother::setParam(const Param & smoother_param)
@@ -59,7 +57,7 @@ JerkFilteredSmoother::Param JerkFilteredSmoother::getParam() const
 
 bool JerkFilteredSmoother::apply(
   const double v0, const double a0, const TrajectoryPoints & input, TrajectoryPoints & output,
-  std::vector<TrajectoryPoints> & debug_trajectories)
+  std::vector<TrajectoryPoints> & debug_trajectories, const bool publish_debug_trajs)
 {
   autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
 
@@ -116,10 +114,12 @@ bool JerkFilteredSmoother::apply(
   auto opt_resampled_trajectory = resample(filtered);
 
   // Set debug trajectories
-  debug_trajectories.resize(3);
-  debug_trajectories[0] = resample(forward_filtered);
-  debug_trajectories[1] = resample(backward_filtered);
-  debug_trajectories[2] = resample(filtered);
+  if (publish_debug_trajs) {
+    debug_trajectories.resize(3);
+    debug_trajectories[0] = resample(forward_filtered);
+    debug_trajectories[1] = resample(backward_filtered);
+    debug_trajectories[2] = resample(filtered);
+  }
 
   // Ensure terminal velocity is zero
   opt_resampled_trajectory.back().longitudinal_velocity_mps = 0.0;
@@ -129,9 +129,12 @@ bool JerkFilteredSmoother::apply(
     // No need to do optimization
     output.front().longitudinal_velocity_mps = v0;
     output.front().acceleration_mps2 = a0;
-    debug_trajectories[0] = output;
-    debug_trajectories[1] = output;
-    debug_trajectories[2] = output;
+    if (publish_debug_trajs) {
+      debug_trajectories.resize(3);
+      debug_trajectories[0] = output;
+      debug_trajectories[1] = output;
+      debug_trajectories[2] = output;
+    }
     return true;
   }
 
@@ -301,14 +304,13 @@ bool JerkFilteredSmoother::apply(
 
   // execute optimization
   time_keeper_->start_track("optimize");
-  const auto result = qp_solver_.optimize(P, A, q, lower_bound, upper_bound);
+  const auto optval = qp_interface_->optimize(P, A, q, lower_bound, upper_bound);
   time_keeper_->end_track("optimize");
-  const std::vector<double> optval = std::get<0>(result);
-  const int status_val = std::get<3>(result);
-  if (status_val != 1) {
-    RCLCPP_WARN(logger_, "optimization failed : %s", qp_solver_.getStatusMessage().c_str());
+  if (!qp_interface_->isSolved()) {
+    RCLCPP_WARN(logger_, "optimization failed : %s", qp_interface_->getStatus().c_str());
     return false;
   }
+
   const auto has_nan =
     std::any_of(optval.begin(), optval.end(), [](const auto v) { return std::isnan(v); });
   if (has_nan) {
@@ -332,16 +334,6 @@ bool JerkFilteredSmoother::apply(
     output.at(i).acceleration_mps2 = a_stop_decel;
   }
 
-  qp_solver_.logUnsolvedStatus("[autoware_velocity_smoother]");
-
-  const int status_polish = std::get<2>(result);
-  if (status_polish != 1) {
-    const auto msg = status_polish == 0    ? "unperformed"
-                     : status_polish == -1 ? "unsuccessful"
-                                           : "unknown";
-    RCLCPP_DEBUG(logger_, "osqp polish process failed : %s. The result may be inaccurate", msg);
-  }
-
   if (VERBOSE_TRAJECTORY_VELOCITY) {
     const auto s_output = trajectory_utils::calcArclengthArray(output);
 
diff --git a/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp
index f379b217187c9..678c8e01bf67e 100644
--- a/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp
+++ b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp
@@ -53,7 +53,8 @@ L2PseudoJerkSmoother::Param L2PseudoJerkSmoother::getParam() const
 
 bool L2PseudoJerkSmoother::apply(
   const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
-  TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories)
+  TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories,
+  [[maybe_unused]] const bool publish_debug_trajs)
 {
   debug_trajectories.clear();
 
diff --git a/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp
index 2ab3d6dd80dfc..60345ff0fa6f4 100644
--- a/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp
+++ b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp
@@ -53,7 +53,8 @@ LinfPseudoJerkSmoother::Param LinfPseudoJerkSmoother::getParam() const
 
 bool LinfPseudoJerkSmoother::apply(
   const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
-  TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories)
+  TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories,
+  [[maybe_unused]] const bool publish_debug_trajs)
 {
   debug_trajectories.clear();
 
diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp
index 9c85a463067de..f09196b2cc8e1 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp
@@ -47,8 +47,6 @@ class DynamicObstacleAvoidanceModuleManager : public SceneModuleManagerInterface
 
   void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;
 
-  bool isAlwaysExecutableModule() const override;
-
 private:
   std::shared_ptr<DynamicAvoidanceParameters> parameters_;
 };
diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp
index eccf2b2a1bfde..cdf878fb62c63 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp
@@ -287,10 +287,6 @@ void DynamicObstacleAvoidanceModuleManager::updateModuleParams(
   });
 }
 
-bool DynamicObstacleAvoidanceModuleManager::isAlwaysExecutableModule() const
-{
-  return true;
-}
 }  // namespace autoware::behavior_path_planner
 
 #include <pluginlib/class_list_macros.hpp>
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp
index e34cd202011d0..749ed57f95cc9 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp
@@ -228,6 +228,8 @@ class ThreadSafeData
   DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional<BehaviorModuleOutput>, last_previous_module_output)
   DEFINE_SETTER_GETTER_WITH_MUTEX(PreviousPullOverData, prev_data)
   DEFINE_SETTER_GETTER_WITH_MUTEX(CollisionCheckDebugMap, collision_check)
+  DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, static_target_objects)
+  DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, dynamic_target_objects)
 
 private:
   std::shared_ptr<PullOverPath> pull_over_path_{nullptr};
@@ -241,6 +243,8 @@ class ThreadSafeData
   std::optional<BehaviorModuleOutput> last_previous_module_output_{};
   PreviousPullOverData prev_data_{};
   CollisionCheckDebugMap collision_check_{};
+  PredictedObjects static_target_objects_{};
+  PredictedObjects dynamic_target_objects_{};
 
   std::recursive_mutex & mutex_;
   rclcpp::Clock::SharedPtr clock_;
@@ -520,9 +524,10 @@ class GoalPlannerModule : public SceneModuleInterface
     const PathWithLaneId & path,
     const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map) const;
   bool checkObjectsCollision(
-    const PathWithLaneId & path, const std::shared_ptr<const PlannerData> planner_data,
-    const GoalPlannerParameters & parameters, const double collision_check_margin,
-    const bool extract_static_objects, const bool update_debug_data = false) const;
+    const PathWithLaneId & path, const std::vector<double> & curvatures,
+    const std::shared_ptr<const PlannerData> planner_data, const GoalPlannerParameters & parameters,
+    const double collision_check_margin, const bool extract_static_objects,
+    const bool update_debug_data = false) const;
 
   // goal seach
   Pose calcRefinedGoal(const Pose & goal_pose) const;
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp
index 32d816d9a37e8..5408dce9dcdfc 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp
@@ -43,8 +43,6 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface
 
   void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;
 
-  bool isAlwaysExecutableModule() const override;
-
   bool isSimultaneousExecutableAsApprovedModule() const override;
 
   bool isSimultaneousExecutableAsCandidateModule() const override;
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp
index 05385600926f5..9899f4a5fad1e 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp
@@ -52,41 +52,29 @@ struct PullOverPath
   size_t goal_id{};
   size_t id{};
   bool decided_velocity{false};
-  double max_curvature{0.0};  // max curvature of the parking path
 
-  PathWithLaneId getFullPath() const
+  /**
+   * @brief Set paths and start/end poses
+   * By setting partial_paths, full_path, parking_path and curvature are also set at the same time
+   * @param input_partial_paths partial paths
+   * @param input_start_pose start pose
+   * @param input_end_pose end pose
+   */
+  void setPaths(
+    const std::vector<PathWithLaneId> input_partial_paths, const Pose & input_start_pose,
+    const Pose & input_end_pose)
   {
-    PathWithLaneId path{};
-    for (size_t i = 0; i < partial_paths.size(); ++i) {
-      if (i == 0) {
-        path.points.insert(
-          path.points.end(), partial_paths.at(i).points.begin(), partial_paths.at(i).points.end());
-      } else {
-        // skip overlapping point
-        path.points.insert(
-          path.points.end(), next(partial_paths.at(i).points.begin()),
-          partial_paths.at(i).points.end());
-      }
-    }
-    path.points = autoware::motion_utils::removeOverlapPoints(path.points);
+    partial_paths = input_partial_paths;
+    start_pose = input_start_pose;
+    end_pose = input_end_pose;
 
-    return path;
+    updatePathData();
   }
 
-  // path from the pull over start pose to the end pose
-  PathWithLaneId getParkingPath() const
-  {
-    const PathWithLaneId full_path = getFullPath();
-    const size_t start_idx =
-      autoware::motion_utils::findNearestIndex(full_path.points, start_pose.position);
+  // Note: return copy value (not const&) because the value is used in multi threads
+  PathWithLaneId getFullPath() const { return full_path; }
 
-    PathWithLaneId parking_path{};
-    std::copy(
-      full_path.points.begin() + start_idx, full_path.points.end(),
-      std::back_inserter(parking_path.points));
-
-    return parking_path;
-  }
+  PathWithLaneId getParkingPath() const { return parking_path; }
 
   PathWithLaneId getCurrentPath() const
   {
@@ -108,6 +96,82 @@ struct PullOverPath
   }
 
   bool isValidPath() const { return type != PullOverPlannerType::NONE; }
+
+  std::vector<double> getFullPathCurvatures() const { return full_path_curvatures; }
+  std::vector<double> getParkingPathCurvatures() const { return parking_path_curvatures; }
+  double getFullPathMaxCurvature() const { return full_path_max_curvature; }
+  double getParkingPathMaxCurvature() const { return parking_path_max_curvature; }
+
+private:
+  void updatePathData()
+  {
+    updateFullPath();
+    updateParkingPath();
+    updateCurvatures();
+  }
+
+  void updateFullPath()
+  {
+    PathWithLaneId path{};
+    for (size_t i = 0; i < partial_paths.size(); ++i) {
+      if (i == 0) {
+        path.points.insert(
+          path.points.end(), partial_paths.at(i).points.begin(), partial_paths.at(i).points.end());
+      } else {
+        // skip overlapping point
+        path.points.insert(
+          path.points.end(), next(partial_paths.at(i).points.begin()),
+          partial_paths.at(i).points.end());
+      }
+    }
+    full_path.points = autoware::motion_utils::removeOverlapPoints(path.points);
+  }
+
+  void updateParkingPath()
+  {
+    if (full_path.points.empty()) {
+      updateFullPath();
+    }
+    const size_t start_idx =
+      autoware::motion_utils::findNearestIndex(full_path.points, start_pose.position);
+
+    PathWithLaneId path{};
+    std::copy(
+      full_path.points.begin() + start_idx, full_path.points.end(),
+      std::back_inserter(path.points));
+    parking_path = path;
+  }
+
+  void updateCurvatures()
+  {
+    const auto calculateCurvaturesAndMax =
+      [](const auto & path) -> std::pair<std::vector<double>, double> {
+      std::vector<double> curvatures = autoware::motion_utils::calcCurvature(path.points);
+      double max_curvature = 0.0;
+      if (!curvatures.empty()) {
+        max_curvature = std::abs(*std::max_element(
+          curvatures.begin(), curvatures.end(),
+          [](const double & a, const double & b) { return std::abs(a) < std::abs(b); }));
+      }
+      return std::make_pair(curvatures, max_curvature);
+    };
+    std::tie(full_path_curvatures, full_path_max_curvature) =
+      calculateCurvaturesAndMax(getFullPath());
+    std::tie(parking_path_curvatures, parking_path_max_curvature) =
+      calculateCurvaturesAndMax(getParkingPath());
+  }
+
+  // curvatures
+  std::vector<double> full_path_curvatures{};
+  std::vector<double> parking_path_curvatures{};
+  std::vector<double> current_path_curvatures{};
+  double parking_path_max_curvature{0.0};
+  double full_path_max_curvature{0.0};
+  double current_path_max_curvature{0.0};
+
+  // path
+  PathWithLaneId full_path{};
+  PathWithLaneId parking_path{};
 };
 
 class PullOverPlannerBase
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp
index 98a513d519b43..3c717a90c93ef 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp
@@ -42,6 +42,7 @@ using geometry_msgs::msg::Twist;
 using tier4_planning_msgs::msg::PathWithLaneId;
 using visualization_msgs::msg::Marker;
 using visualization_msgs::msg::MarkerArray;
+using Shape = autoware_perception_msgs::msg::Shape;
 using Polygon2d = autoware::universe_utils::Polygon2d;
 
 lanelet::ConstLanelets getPullOverLanes(
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/freespace_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/freespace_pull_over.cpp
index 1f79f330be676..57c35fc4dd8ba 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/freespace_pull_over.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/freespace_pull_over.cpp
@@ -58,7 +58,12 @@ std::optional<PullOverPath> FreespacePullOver::plan(const Pose & goal_pose)
   const Pose end_pose =
     use_back_ ? goal_pose
               : autoware::universe_utils::calcOffsetPose(goal_pose, -straight_distance, 0.0, 0.0);
-  if (!planner_->makePlan(current_pose, end_pose)) {
+
+  try {
+    if (!planner_->makePlan(current_pose, end_pose)) {
+      return {};
+    }
+  } catch (const std::exception & e) {
     return {};
   }
 
@@ -129,10 +134,8 @@ std::optional<PullOverPath> FreespacePullOver::plan(const Pose & goal_pose)
   }
 
   PullOverPath pull_over_path{};
-  pull_over_path.partial_paths = partial_paths;
   pull_over_path.pairs_terminal_velocity_and_accel = pairs_terminal_velocity_and_accel;
-  pull_over_path.start_pose = current_pose;
-  pull_over_path.end_pose = goal_pose;
+  pull_over_path.setPaths(partial_paths, current_pose, goal_pose);
   pull_over_path.type = getPlannerType();
 
   return pull_over_path;
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/geometric_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/geometric_pull_over.cpp
index ffae76bff1abe..3baa603f4d00c 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/geometric_pull_over.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/geometric_pull_over.cpp
@@ -72,10 +72,8 @@ std::optional<PullOverPath> GeometricPullOver::plan(const Pose & goal_pose)
 
   PullOverPath pull_over_path{};
   pull_over_path.type = getPlannerType();
-  pull_over_path.partial_paths = planner_.getPaths();
   pull_over_path.pairs_terminal_velocity_and_accel = planner_.getPairsTerminalVelocityAndAccel();
-  pull_over_path.start_pose = planner_.getStartPose();
-  pull_over_path.end_pose = planner_.getArcEndPose();
+  pull_over_path.setPaths(planner_.getPaths(), planner_.getStartPose(), planner_.getArcEndPose());
 
   return pull_over_path;
 }
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 1a711adb133eb..419f3bd2dda47 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
@@ -123,6 +123,13 @@ GoalPlannerModule::GoalPlannerModule(
   if (parameters_->safety_check_params.enable_safety_check) {
     initializeSafetyCheckParameters();
   }
+
+  /**
+   * NOTE: Add `universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);` to functions called
+   * from the main thread only.
+   * If you want to see the processing time tree in console, uncomment the following line
+   */
+  // time_keeper_->add_reporter(&std::cerr);
 }
 
 bool GoalPlannerModule::hasPreviousModulePathShapeChanged(
@@ -299,12 +306,6 @@ void GoalPlannerModule::onTimer()
 
       // calculate absolute maximum curvature of parking path(start pose to end pose) for path
       // priority
-      const std::vector<double> curvatures =
-        autoware::motion_utils::calcCurvature(pull_over_path->getParkingPath().points);
-      pull_over_path->max_curvature = std::abs(*std::max_element(
-        curvatures.begin(), curvatures.end(),
-        [](const double & a, const double & b) { return std::abs(a) < std::abs(b); }));
-
       path_candidates.push_back(*pull_over_path);
       // calculate closest pull over start pose for stop path
       const double start_arc_length =
@@ -447,6 +448,24 @@ void GoalPlannerModule::updateObjectsFilteringParams(
 
 void GoalPlannerModule::updateData()
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
+  // extract static and dynamic objects in expanded pull over lanes
+  {
+    const auto & p = parameters_;
+    const auto & rh = *(planner_data_->route_handler);
+    const auto objects = *(planner_data_->dynamic_object);
+    const auto static_target_objects =
+      goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes(
+        rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length,
+        p->detection_bound_offset, objects, p->th_moving_object_velocity);
+    const auto dynamic_target_objects = goal_planner_utils::extractObjectsInExpandedPullOverLanes(
+      rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length,
+      p->detection_bound_offset, objects);
+    thread_safe_data_.set_static_target_objects(static_target_objects);
+    thread_safe_data_.set_dynamic_target_objects(dynamic_target_objects);
+  }
+
   // In PlannerManager::run(), it calls SceneModuleInterface::setData and
   // SceneModuleInterface::setPreviousModuleOutput before module_ptr->run().
   // Then module_ptr->run() invokes GoalPlannerModule::updateData and then
@@ -755,16 +774,17 @@ bool GoalPlannerModule::canReturnToLaneParking()
     return false;
   }
 
-  if (!thread_safe_data_.get_lane_parking_pull_over_path()) {
+  const auto lane_parking_path = thread_safe_data_.get_lane_parking_pull_over_path();
+  if (!lane_parking_path) {
     return false;
   }
 
-  const PathWithLaneId path = thread_safe_data_.get_lane_parking_pull_over_path()->getFullPath();
-
+  const PathWithLaneId path = lane_parking_path->getFullPath();
+  const std::vector<double> curvatures = lane_parking_path->getFullPathCurvatures();
   if (
     parameters_->use_object_recognition &&
     checkObjectsCollision(
-      path, planner_data_, *parameters_,
+      path, curvatures, planner_data_, *parameters_,
       parameters_->object_recognition_collision_check_hard_margins.back(),
       /*extract_static_objects=*/false)) {
     return false;
@@ -789,6 +809,8 @@ bool GoalPlannerModule::canReturnToLaneParking()
 
 GoalCandidates GoalPlannerModule::generateGoalCandidates() const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   // calculate goal candidates
   const auto & route_handler = planner_data_->route_handler;
   if (utils::isAllowedGoalModification(route_handler)) {
@@ -809,6 +831,8 @@ GoalCandidates GoalPlannerModule::generateGoalCandidates() const
 
 BehaviorModuleOutput GoalPlannerModule::plan()
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   if (utils::isAllowedGoalModification(planner_data_->route_handler)) {
     return planPullOver();
   }
@@ -821,6 +845,8 @@ std::vector<PullOverPath> GoalPlannerModule::sortPullOverPathCandidatesByGoalPri
   const std::vector<PullOverPath> & pull_over_path_candidates,
   const GoalCandidates & goal_candidates) const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   // ==========================================================================================
   // print path priority for debug
   const auto debugPrintPathPriority =
@@ -855,7 +881,8 @@ std::vector<PullOverPath> GoalPlannerModule::sortPullOverPathCandidatesByGoalPri
            << ", path_id: " << path.id << ", goal_id: " << path.goal_id
            << ", goal_priority: " << (is_safe_goal ? std::to_string(goal_priority) : "unsafe")
            << ", margin: " << path_id_to_margin_map.at(path.id)
-           << (isSoftMargin(path) ? " (soft)" : " (hard)") << ", curvature: " << path.max_curvature
+           << (isSoftMargin(path) ? " (soft)" : " (hard)")
+           << ", curvature: " << path.getParkingPathMaxCurvature()
            << (isHighCurvature(path) ? " (high)" : " (low)");
         ss << "\n";
       }
@@ -911,11 +938,11 @@ std::vector<PullOverPath> GoalPlannerModule::sortPullOverPathCandidatesByGoalPri
         return 0.0;
       }
       // check path collision margin
-      const auto resampled_path =
-        utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5);
+      const PathWithLaneId parking_path = pull_over_path.getParkingPath();
+      const std::vector<double> parking_path_curvatures = pull_over_path.getParkingPathCurvatures();
       for (const auto & margin : margins) {
         if (!checkObjectsCollision(
-              resampled_path, planner_data_, *parameters_, margin,
+              parking_path, parking_path_curvatures, planner_data_, *parameters_, margin,
               /*extract_static_objects=*/true)) {
           return margin;
         }
@@ -942,7 +969,7 @@ std::vector<PullOverPath> GoalPlannerModule::sortPullOverPathCandidatesByGoalPri
     // (3) Sort by curvature
     // If the curvature is less than the threshold, prioritize the path.
     const auto isHighCurvature = [&](const PullOverPath & path) -> bool {
-      return path.max_curvature >= parameters_->high_curvature_threshold;
+      return path.getParkingPathMaxCurvature() >= parameters_->high_curvature_threshold;
     };
 
     const auto isSoftMargin = [&](const PullOverPath & path) -> bool {
@@ -1013,6 +1040,8 @@ std::optional<std::pair<PullOverPath, GoalCandidate>> GoalPlannerModule::selectP
   const std::vector<PullOverPath> & pull_over_path_candidates,
   const GoalCandidates & goal_candidates, const double collision_check_margin) const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   const auto & goal_pose = planner_data_->route_handler->getOriginalGoalPose();
   const double backward_length =
     parameters_->backward_goal_search_length + parameters_->decide_path_distance;
@@ -1048,11 +1077,14 @@ std::optional<std::pair<PullOverPath, GoalCandidate>> GoalPlannerModule::selectP
       continue;
     }
 
-    const auto resampled_path = utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5);
+    // const auto resampled_path = utils::resamplePathWithSpline(pull_over_path.getParkingPath(),
+    // 0.5);
+    const PathWithLaneId parking_path = pull_over_path.getParkingPath();
+    const std::vector<double> parking_path_curvatures = pull_over_path.getParkingPathCurvatures();
     if (
       parameters_->use_object_recognition &&
       checkObjectsCollision(
-        resampled_path, planner_data_, *parameters_, collision_check_margin,
+        parking_path, parking_path_curvatures, planner_data_, *parameters_, collision_check_margin,
         /*extract_static_objects=*/true,
         /*update_debug_data=*/true)) {
       continue;
@@ -1060,7 +1092,7 @@ std::optional<std::pair<PullOverPath, GoalCandidate>> GoalPlannerModule::selectP
 
     if (
       parameters_->use_occupancy_grid_for_path_collision_check &&
-      checkOccupancyGridCollision(resampled_path, occupancy_grid_map_)) {
+      checkOccupancyGridCollision(parking_path, occupancy_grid_map_)) {
       continue;
     }
 
@@ -1072,6 +1104,8 @@ std::optional<std::pair<PullOverPath, GoalCandidate>> GoalPlannerModule::selectP
 
 std::vector<DrivableLanes> GoalPlannerModule::generateDrivableLanes() const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes(
     planner_data_, parameters_->backward_goal_search_length,
     parameters_->forward_goal_search_length,
@@ -1084,6 +1118,8 @@ std::vector<DrivableLanes> GoalPlannerModule::generateDrivableLanes() const
 
 void GoalPlannerModule::setOutput(BehaviorModuleOutput & output)
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   output.reference_path = getPreviousModuleOutput().reference_path;
 
   if (!thread_safe_data_.foundPullOverPath()) {
@@ -1135,6 +1171,8 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output)
 
 void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE) {
     const double drivable_area_margin = planner_data_->parameters.vehicle_width;
     output.drivable_area_info.drivable_margin =
@@ -1201,6 +1239,8 @@ bool GoalPlannerModule::hasDecidedPath(
   const std::shared_ptr<SafetyCheckParams> & safety_check_params,
   const std::shared_ptr<GoalSearcherBase> goal_searcher) const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   return checkDecidingPathStatus(
            planner_data, occupancy_grid_map, parameters, ego_predicted_path_params,
            objects_filtering_params, safety_check_params, goal_searcher)
@@ -1216,6 +1256,8 @@ bool GoalPlannerModule::hasNotDecidedPath(
   const std::shared_ptr<SafetyCheckParams> & safety_check_params,
   const std::shared_ptr<GoalSearcherBase> goal_searcher) const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   return checkDecidingPathStatus(
            planner_data, occupancy_grid_map, parameters, ego_predicted_path_params,
            objects_filtering_params, safety_check_params, goal_searcher)
@@ -1231,6 +1273,8 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus(
   const std::shared_ptr<SafetyCheckParams> & safety_check_params,
   const std::shared_ptr<GoalSearcherBase> goal_searcher) const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   const auto & prev_status = thread_safe_data_.get_prev_data().deciding_path_status;
   const bool enable_safety_check = parameters.safety_check_params.enable_safety_check;
 
@@ -1278,11 +1322,13 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus(
     }
 
     // check current parking path collision
-    const auto parking_path = utils::resamplePathWithSpline(pull_over_path->getParkingPath(), 0.5);
+    const auto parking_path = pull_over_path->getParkingPath();
+    const std::vector<double> parking_path_curvatures = pull_over_path->getParkingPathCurvatures();
     const double margin =
       parameters.object_recognition_collision_check_hard_margins.back() * hysteresis_factor;
     if (checkObjectsCollision(
-          parking_path, planner_data, parameters, margin, /*extract_static_objects=*/false)) {
+          parking_path, parking_path_curvatures, planner_data, parameters, margin,
+          /*extract_static_objects=*/false)) {
       RCLCPP_DEBUG(
         getLogger(),
         "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path has collision with objects");
@@ -1345,6 +1391,8 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus(
 
 void GoalPlannerModule::decideVelocity()
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   const double current_vel = planner_data_->self_odometry->twist.twist.linear.x;
 
   auto & first_path = thread_safe_data_.get_pull_over_path()->partial_paths.front();
@@ -1357,6 +1405,8 @@ void GoalPlannerModule::decideVelocity()
 
 BehaviorModuleOutput GoalPlannerModule::planPullOver()
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   if (!hasDecidedPath(
         planner_data_, occupancy_grid_map_, *parameters_, ego_predicted_path_params_,
         objects_filtering_params_, safety_check_params_, goal_searcher_)) {
@@ -1368,6 +1418,8 @@ BehaviorModuleOutput GoalPlannerModule::planPullOver()
 
 BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate()
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   // if pull over path candidates generation is not finished, use previous module output
   if (thread_safe_data_.get_pull_over_path_candidates().empty()) {
     return getPreviousModuleOutput();
@@ -1398,6 +1450,8 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate()
 
 BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput()
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   // if pull over path candidates generation is not finished, use previous module output
   if (thread_safe_data_.get_pull_over_path_candidates().empty()) {
     return getPreviousModuleOutput();
@@ -1475,6 +1529,8 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput()
 
 void GoalPlannerModule::postProcess()
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   if (!thread_safe_data_.foundPullOverPath()) {
     return;
   }
@@ -1502,6 +1558,8 @@ void GoalPlannerModule::postProcess()
 
 void GoalPlannerModule::updatePreviousData()
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   // for the next loop setOutput().
   // this is used to determine whether to generate a new stop path or keep the current stop path.
   // TODO(Mamoru Sobue): put prev_data_ out of  ThreadSafeData
@@ -1537,6 +1595,8 @@ void GoalPlannerModule::updatePreviousData()
 
 BehaviorModuleOutput GoalPlannerModule::planWaitingApproval()
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   if (utils::isAllowedGoalModification(planner_data_->route_handler)) {
     return planPullOverAsCandidate();
   }
@@ -1547,6 +1607,8 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApproval()
 
 std::pair<double, double> GoalPlannerModule::calcDistanceToPathChange() const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   if (!thread_safe_data_.foundPullOverPath()) {
     return {std::numeric_limits<double>::max(), std::numeric_limits<double>::max()};
   }
@@ -1581,6 +1643,8 @@ void GoalPlannerModule::setParameters(const std::shared_ptr<GoalPlannerParameter
 
 PathWithLaneId GoalPlannerModule::generateStopPath() const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   const auto & route_handler = planner_data_->route_handler;
   const auto & current_pose = planner_data_->self_odometry->pose.pose;
   const auto & common_parameters = planner_data_->parameters;
@@ -1688,6 +1752,8 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const
 
 PathWithLaneId GoalPlannerModule::generateFeasibleStopPath(const PathWithLaneId & path) const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   // calc minimum stop distance under maximum deceleration
   const auto min_stop_distance = calcFeasibleDecelDistance(
     planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0);
@@ -1763,13 +1829,16 @@ bool GoalPlannerModule::isStuck(
     return false;
   }
 
-  if (
-    parameters.use_object_recognition &&
-    checkObjectsCollision(
-      thread_safe_data_.get_pull_over_path()->getCurrentPath(), planner_data, parameters,
-      /*extract_static_objects=*/false,
-      parameters.object_recognition_collision_check_hard_margins.back())) {
-    return true;
+  const auto pull_over_path = thread_safe_data_.get_pull_over_path();
+  if (parameters.use_object_recognition) {
+    const auto path = pull_over_path->getCurrentPath();
+    const auto curvatures = autoware::motion_utils::calcCurvature(path.points);
+    if (checkObjectsCollision(
+          path, curvatures, planner_data, parameters,
+          parameters.object_recognition_collision_check_hard_margins.back(),
+          /*extract_static_objects=*/false)) {
+      return true;
+    }
   }
 
   if (
@@ -1784,6 +1853,8 @@ bool GoalPlannerModule::isStuck(
 
 bool GoalPlannerModule::hasFinishedCurrentPath()
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   if (!last_approval_data_) {
     return false;
   }
@@ -1834,6 +1905,8 @@ bool GoalPlannerModule::isOnModifiedGoal(
 
 TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo()
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   const auto path = thread_safe_data_.get_pull_over_path()->getFullPath();
   if (path.points.empty()) return getPreviousModuleOutput().turn_signal_info;
 
@@ -1913,23 +1986,29 @@ bool GoalPlannerModule::checkOccupancyGridCollision(
 }
 
 bool GoalPlannerModule::checkObjectsCollision(
-  const PathWithLaneId & path, const std::shared_ptr<const PlannerData> planner_data,
-  const GoalPlannerParameters & parameters, const double collision_check_margin,
-  const bool extract_static_objects, const bool update_debug_data) const
-{
-  const auto target_objects = std::invoke([&]() {
-    const auto & p = parameters;
-    const auto & rh = *(planner_data->route_handler);
-    const auto objects = *(planner_data->dynamic_object);
-    if (extract_static_objects) {
-      return goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes(
-        rh, left_side_parking_, p.backward_goal_search_length, p.forward_goal_search_length,
-        p.detection_bound_offset, objects, p.th_moving_object_velocity);
-    }
-    return goal_planner_utils::extractObjectsInExpandedPullOverLanes(
-      rh, left_side_parking_, p.backward_goal_search_length, p.forward_goal_search_length,
-      p.detection_bound_offset, objects);
-  });
+  const PathWithLaneId & path, const std::vector<double> & curvatures,
+  const std::shared_ptr<const PlannerData> planner_data, const GoalPlannerParameters & parameters,
+  const double collision_check_margin, const bool extract_static_objects,
+  const bool update_debug_data) const
+{
+  const auto target_objects = extract_static_objects
+                                ? thread_safe_data_.get_static_target_objects()
+                                : thread_safe_data_.get_dynamic_target_objects();
+  if (target_objects.objects.empty()) {
+    return false;
+  }
+
+  // check collision roughly with {min_distance, max_distance} between ego footprint and objects
+  // footprint
+  std::pair<bool, bool> has_collision_rough =
+    utils::path_safety_checker::checkObjectsCollisionRough(
+      path, target_objects, collision_check_margin, planner_data_->parameters, false);
+  if (!has_collision_rough.first) {
+    return false;
+  }
+  if (has_collision_rough.second) {
+    return true;
+  }
 
   std::vector<Polygon2d> obj_polygons;
   for (const auto & object : target_objects.objects) {
@@ -1941,18 +2020,22 @@ bool GoalPlannerModule::checkObjectsCollision(
    *   - `extra_stopping_margin` adds stopping margin under deceleration constraints forward.
    *   - `extra_lateral_margin` adds the lateral margin on curves.
    */
+  if (path.points.size() != curvatures.size()) {
+    RCLCPP_WARN_THROTTLE(
+      getLogger(), *clock_, 5000,
+      "path.points.size() != curvatures.size() in checkObjectsCollision(). judge as non collision");
+    return false;
+  }
   std::vector<Polygon2d> ego_polygons_expanded{};
-  const auto curvatures = autoware::motion_utils::calcCurvature(path.points);
   for (size_t i = 0; i < path.points.size(); ++i) {
     const auto p = path.points.at(i);
-
     const double extra_stopping_margin = std::min(
       std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / parameters.maximum_deceleration,
       parameters.object_recognition_collision_check_max_extra_stopping_margin);
 
     // The square is meant to imply centrifugal force, but it is not a very well-founded formula.
-    // TODO(kosuke55): It is needed to consider better way because there is an inherently different
-    // conception of the inside and outside margins.
+    // TODO(kosuke55): It is needed to consider better way because there is an inherently
+    // different conception of the inside and outside margins.
     const double extra_lateral_margin = std::min(
       extra_stopping_margin,
       std::abs(curvatures.at(i) * std::pow(p.point.longitudinal_velocity_mps, 2)));
@@ -2072,6 +2155,8 @@ void GoalPlannerModule::deceleratePath(PullOverPath & pull_over_path) const
 
 void GoalPlannerModule::decelerateForTurnSignal(const Pose & stop_pose, PathWithLaneId & path) const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   const double time = planner_data_->parameters.turn_signal_search_time;
   const Pose & current_pose = planner_data_->self_odometry->pose.pose;
 
@@ -2111,6 +2196,8 @@ void GoalPlannerModule::decelerateForTurnSignal(const Pose & stop_pose, PathWith
 void GoalPlannerModule::decelerateBeforeSearchStart(
   const Pose & search_start_offset_pose, PathWithLaneId & path) const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   const double pull_over_velocity = parameters_->pull_over_velocity;
   const Pose & current_pose = planner_data_->self_odometry->pose.pose;
 
@@ -2130,6 +2217,8 @@ void GoalPlannerModule::decelerateBeforeSearchStart(
 bool GoalPlannerModule::isCrossingPossible(
   const lanelet::ConstLanelet & start_lane, const lanelet::ConstLanelet & end_lane) const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   if (start_lane.centerline().empty() || end_lane.centerline().empty()) {
     return false;
   }
@@ -2279,6 +2368,8 @@ std::pair<bool, bool> GoalPlannerModule::isSafePath(
   const std::shared_ptr<ObjectsFilteringParams> & objects_filtering_params,
   const std::shared_ptr<SafetyCheckParams> & safety_check_params) const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   if (!thread_safe_data_.get_pull_over_path()) {
     return {false, false};
   }
@@ -2415,6 +2506,8 @@ std::pair<bool, bool> GoalPlannerModule::isSafePath(
 
 void GoalPlannerModule::setDebugData()
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   debug_marker_.markers.clear();
 
   using autoware::motion_utils::createStopVirtualWallMarker;
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 8a4f3ab202a67..00b27d43087f5 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
@@ -827,23 +827,8 @@ void GoalPlannerModuleManager::updateModuleParams(
   });
 }
 
-bool GoalPlannerModuleManager::isAlwaysExecutableModule() const
-{
-  // enable AlwaysExecutable whenever goal modification is not allowed
-  // because only minor path refinements are made for fixed goals
-  if (!utils::isAllowedGoalModification(planner_data_->route_handler)) {
-    return true;
-  }
-
-  return false;
-}
-
 bool GoalPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const
 {
-  if (isAlwaysExecutableModule()) {
-    return true;
-  }
-
   // enable SimultaneousExecutable whenever goal modification is not allowed
   // because only minor path refinements are made for fixed goals
   if (!utils::isAllowedGoalModification(planner_data_->route_handler)) {
@@ -855,10 +840,6 @@ bool GoalPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const
 
 bool GoalPlannerModuleManager::isSimultaneousExecutableAsCandidateModule() const
 {
-  if (isAlwaysExecutableModule()) {
-    return true;
-  }
-
   // enable SimultaneousExecutable whenever goal modification is not allowed
   // because only minor path refinements are made for fixed goals
   if (!utils::isAllowedGoalModification(planner_data_->route_handler)) {
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp
index 9570621f0f717..23ca76432859d 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp
@@ -246,10 +246,11 @@ std::optional<PullOverPath> ShiftPullOver::generatePullOverPath(
   // set pull over path
   PullOverPath pull_over_path{};
   pull_over_path.type = getPlannerType();
-  pull_over_path.partial_paths.push_back(shifted_path.path);
+  std::vector<PathWithLaneId> partial_paths{shifted_path.path};
+  pull_over_path.setPaths(
+    partial_paths, path_shifter.getShiftLines().front().start,
+    path_shifter.getShiftLines().front().end);
   pull_over_path.pairs_terminal_velocity_and_accel.push_back(std::make_pair(pull_over_velocity, 0));
-  pull_over_path.start_pose = path_shifter.getShiftLines().front().start;
-  pull_over_path.end_pose = path_shifter.getShiftLines().front().end;
   pull_over_path.debug_poses.push_back(shift_end_pose_prev_module_path);
   pull_over_path.debug_poses.push_back(actual_shift_end_pose);
   pull_over_path.debug_poses.push_back(goal_pose);
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt
index b0d9967e65ddd..509f38d52dd45 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt
+++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt
@@ -9,6 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/interface.cpp
   src/manager.cpp
   src/scene.cpp
+  src/utils/calculation.cpp
   src/utils/markers.cpp
   src/utils/utils.cpp
 )
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 63f5a2ec05bf1..e42e6e2d47738 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
@@ -618,6 +618,61 @@ The last behavior will also occur if the ego vehicle has departed from the curre
 
 ![stop](./images/lane_change-cant_cancel_no_abort.png)
 
+## Lane change completion checks
+
+To determine if the ego vehicle has successfully changed lanes, one of two criteria must be met: either the longitudinal or the lateral criteria.
+
+For the longitudinal criteria, the ego vehicle must pass the lane-changing end pose and be within the `finish_judge_buffer` distance from it. The module then checks if the ego vehicle is in the target lane. If true, the module returns success. This check ensures that the planner manager updates the root lanelet correctly based on the ego vehicle's current pose. Without this check, if the ego vehicle is changing lanes while avoiding an obstacle and its current pose is in the original lane, the planner manager might set the root lanelet as the original lane. This would force the ego vehicle to perform the lane change again. With the target lane check, the ego vehicle is confirmed to be in the target lane, and the planner manager can correctly update the root lanelets.
+
+If the longitudinal criteria are not met, the module evaluates the lateral criteria. For the lateral criteria, the ego vehicle must be within `finish_judge_lateral_threshold` distance from the target lane's centerline, and the angle deviation must be within `finish_judge_lateral_angle_deviation` degrees. The angle deviation check ensures there is no sudden steering. If the angle deviation is set too high, the ego vehicle's orientation could deviate significantly from the centerline, causing the trajectory follower to aggressively correct the steering to return to the centerline. Keeping the angle deviation value as small as possible avoids this issue.
+
+The process of determining lane change completion is shown in the following diagram.
+
+```plantuml
+@startuml
+skinparam defaultTextAlignment center
+skinparam backgroundColor #WHITE
+
+title Lane change completion judge
+
+start
+
+:Calculate distance from current ego pose to lane change end pose;
+
+if (Is ego velocity < 1.0?) then (<color:green><b>YES</b></color>)
+  :Set <b>finish_judge_buffer</b> to 0.0;
+else (<color:red><b>NO</b></color>)
+  :Set <b>finish_judge_buffer</b> to lane_change_finish_judge_buffer;
+endif
+
+if (ego has passed the end_pose and ego is <b>finish_judge_buffer</b> meters away from end_pose?) then (<color:green><b>YES</b></color>)
+  if (Current ego pose is in target lanes' polygon?) then (<color:green><b>YES</b></color>)
+    :Lane change is <color:green><b>completed</b></color>;
+    stop
+  else (<color:red><b>NO</b></color>)
+:Lane change is <color:red><b>NOT</b></color> completed;
+stop
+  endif
+else (<color:red><b>NO</b></color>)
+endif
+
+if (ego's yaw deviation to centerline exceeds finish_judge_lateral_angle_deviation?) then (<color:red><b>YES</b></color>)
+  :Lane change is <color:red><b>NOT</b></color> completed;
+  stop
+else (<color:green><b>NO</b></color>)
+  :Calculate distance to the target lanes' centerline;
+  if (abs(distance to the target lanes' centerline) is less than finish_judge_lateral_threshold?) then (<color:green><b>YES</b></color>)
+    :Lane change is <color:green><b>completed</b></color>;
+    stop
+  else (<color:red><b>NO</b></color>)
+    :Lane change is <color:red><b>NOT</b></color> completed;
+    stop
+  endif
+endif
+
+@enduml
+```
+
 ## Parameters
 
 ### Essential lane change parameters
@@ -631,7 +686,6 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi
 | `backward_length_buffer_for_end_of_lane`     | [m]    | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change                                  | 3.0                |
 | `backward_length_buffer_for_blocking_object` | [m]    | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change when there is an object in front | 3.0                |
 | `lane_change_finish_judge_buffer`            | [m]    | double | The additional buffer used to confirm lane change process completion                                                   | 2.0                |
-| `finish_judge_lateral_threshold`             | [m]    | double | Lateral distance threshold to confirm lane change process completion                                                   | 0.2                |
 | `lane_changing_lateral_jerk`                 | [m/s3] | double | Lateral jerk value for lane change path generation                                                                     | 0.5                |
 | `minimum_lane_changing_velocity`             | [m/s]  | double | Minimum speed during lane changing process.                                                                            | 2.78               |
 | `prediction_time_resolution`                 | [s]    | double | Time resolution for object's path interpolation and collision check.                                                   | 0.5                |
@@ -647,6 +701,16 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi
 | `lateral_acceleration.min_values`            | [m/ss] | double | Min lateral acceleration values corresponding to velocity (look up table)                                              | [0.4, 0.4, 0.4]    |
 | `lateral_acceleration.max_values`            | [m/ss] | double | Max lateral acceleration values corresponding to velocity (look up table)                                              | [0.65, 0.65, 0.65] |
 
+### Parameter to judge if lane change is completed
+
+The following parameters are used to judge lane change completion.
+
+| Name                                   | Unit  | Type   | Description                                                                                                            | Default value |
+| :------------------------------------- | ----- | ------ | ---------------------------------------------------------------------------------------------------------------------- | ------------- |
+| `lane_change_finish_judge_buffer`      | [m]   | double | The longitudinal distance starting from the lane change end pose.                                                      | 2.0           |
+| `finish_judge_lateral_threshold`       | [m]   | double | The lateral distance from targets lanes' centerline. Used in addition with `finish_judge_lateral_angle_deviation`      | 0.1           |
+| `finish_judge_lateral_angle_deviation` | [deg] | double | Ego angle deviation with reference to target lanes' centerline. Used in addition with `finish_judge_lateral_threshold` | 2.0           |
+
 ### Lane change regulations
 
 | Name                       | Unit | Type    | Description                                                | Default value |
@@ -709,6 +773,18 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi
 | `safety_check.execution.longitudinal_distance_min_threshold` | [m]     | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe.      | 3.0           |
 | `safety_check.cancel.longitudinal_velocity_delta_time`       | [m]     | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance)                                         | 0.8           |
 
+#### safety constraints specifically for stopped or parked vehicles
+
+| Name                                                      | Unit    | Type   | Description                                                                                                                                                    | Default value |
+| :-------------------------------------------------------- | ------- | ------ | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- |
+| `safety_check.parked.expected_front_deceleration`         | [m/s^2] | double | The front object's maximum deceleration when the front vehicle perform sudden braking. (\*1)                                                                   | -1.0          |
+| `safety_check.parked.expected_rear_deceleration`          | [m/s^2] | double | The rear object's maximum deceleration when the rear vehicle perform sudden braking. (\*1)                                                                     | -2.0          |
+| `safety_check.parked.rear_vehicle_reaction_time`          | [s]     | double | The reaction time of the rear vehicle driver which starts from the driver noticing the sudden braking of the front vehicle until the driver step on the brake. | 1.0           |
+| `safety_check.parked.rear_vehicle_safety_time_margin`     | [s]     | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking.                                                        | 0.8           |
+| `safety_check.parked.lateral_distance_max_threshold`      | [m]     | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe.                | 1.0           |
+| `safety_check.parked.longitudinal_distance_min_threshold` | [m]     | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe.      | 3.0           |
+| `safety_check.parked.longitudinal_velocity_delta_time`    | [m]     | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance)                                         | 0.8           |
+
 ##### safety constraints to cancel lane change path
 
 | Name                                                      | Unit    | Type   | Description                                                                                                                                                    | Default value |
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 d2f695071649a..7a9c466fec260 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
@@ -6,7 +6,6 @@
 
       backward_length_buffer_for_end_of_lane: 3.0 # [m]
       backward_length_buffer_for_blocking_object: 3.0 # [m]
-      lane_change_finish_judge_buffer: 2.0      # [m]
 
       lane_changing_lateral_jerk: 0.5              # [m/s3]
 
@@ -39,6 +38,14 @@
           lateral_distance_max_threshold: 2.0
           longitudinal_distance_min_threshold: 3.0
           longitudinal_velocity_delta_time: 0.8
+        parked:
+          expected_front_deceleration: -1.0
+          expected_rear_deceleration: -2.0
+          rear_vehicle_reaction_time: 1.0
+          rear_vehicle_safety_time_margin: 0.8
+          lateral_distance_max_threshold: 1.0
+          longitudinal_distance_min_threshold: 3.0
+          longitudinal_velocity_delta_time: 0.0
         cancel:
           expected_front_deceleration: -1.0
           expected_rear_deceleration: -2.0
@@ -109,7 +116,9 @@
         overhang_tolerance: 0.0             # [m]
         unsafe_hysteresis_threshold: 10     # [/]
 
-      finish_judge_lateral_threshold: 0.2        # [m]
+      lane_change_finish_judge_buffer: 2.0      # [m]
+      finish_judge_lateral_threshold: 0.1        # [m]
+      finish_judge_lateral_angle_deviation: 1.0 # [deg]
 
       # debug
       publish_debug_marker: false
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp
index a1284f355a87d..605e0499adcdb 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp
@@ -184,6 +184,24 @@ class NormalLaneChange : public LaneChangeBase
 
   bool isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const;
 
+  /**
+   * @brief Checks if the given pose is a valid starting point for a lane change.
+   *
+   * This function determines whether the given pose (position) of the vehicle is within
+   * the area of either the target neighbor lane or the target lane itself. It uses geometric
+   * checks to see if the start point of the lane change is covered by the polygons representing
+   * these lanes.
+   *
+   * @param common_data_ptr Shared pointer to a CommonData structure, which should include:
+   *  - Non-null `lanes_polygon_ptr` that contains the polygon data for lanes.
+   * @param pose The current pose of the vehicle
+   *
+   * @return `true` if the pose is within either the target neighbor lane or the target lane;
+   * `false` otherwise.
+   */
+  bool is_valid_start_point(
+    const lane_change::CommonDataPtr & common_data_ptr, const Pose & pose) const;
+
   bool check_prepare_phase() const;
 
   double calcMaximumLaneChangeLength(
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp
new file mode 100644
index 0000000000000..421b54db9f67a
--- /dev/null
+++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp
@@ -0,0 +1,45 @@
+// 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__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_
+#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_
+
+#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp"
+
+namespace autoware::behavior_path_planner::utils::lane_change::calculation
+{
+using behavior_path_planner::lane_change::CommonDataPtr;
+
+/**
+ * @brief Calculates the distance from the ego vehicle to the terminal point.
+ *
+ * This function computes the distance from the current position of the ego vehicle
+ * to either the goal pose or the end of the current lane, depending on whether
+ * the vehicle's current lane is within the goal section.
+ *
+ * @param common_data_ptr Shared pointer to a CommonData structure, which should include:
+ *  - Non null `lanes_ptr` that points to the current lanes data.
+ *  - Non null `self_odometry_ptr` that contains the current pose of the ego vehicle.
+ *  - Non null `route_handler_ptr` that contains the goal pose of the route.
+ *
+ * @return The distance to the terminal point (either the goal pose or the end of the current lane)
+ * in meters.
+ */
+double calc_ego_dist_to_terminal_end(const CommonDataPtr & common_data_ptr);
+
+double calc_dist_from_pose_to_terminal_end(
+  const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes,
+  const Pose & src_pose);
+}  // namespace autoware::behavior_path_planner::utils::lane_change::calculation
+
+#endif  // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_
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 495fe9012ecd0..7962c878d7d64 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
@@ -19,6 +19,7 @@
 
 #include <autoware/behavior_path_planner_common/parameters.hpp>
 #include <autoware/route_handler/route_handler.hpp>
+#include <autoware/universe_utils/math/unit_conversion.hpp>
 #include <interpolation/linear_interpolation.hpp>
 
 #include <nav_msgs/msg/odometry.hpp>
@@ -109,7 +110,6 @@ struct Parameters
   double lane_changing_lateral_jerk{0.5};
   double minimum_lane_changing_velocity{5.6};
   double lane_change_prepare_duration{4.0};
-  double lane_change_finish_judge_buffer{3.0};
   LateralAccelerationMap lane_change_lat_acc_map;
 
   // parked vehicle
@@ -151,13 +151,17 @@ struct Parameters
   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_parked{};
   utils::path_safety_checker::RSSparams rss_params_for_abort{};
   utils::path_safety_checker::RSSparams rss_params_for_stuck{};
 
   // abort
   CancelParameters cancel{};
 
+  // finish judge parameter
+  double lane_change_finish_judge_buffer{3.0};
   double finish_judge_lateral_threshold{0.2};
+  double finish_judge_lateral_angle_deviation{autoware::universe_utils::deg2rad(3.0)};
 
   // debug marker
   bool publish_debug_marker{false};
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp
index 7bb6a06116a53..d566aceba413b 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp
@@ -314,6 +314,8 @@ bool is_ahead_of_ego(
 bool is_before_terminal(
   const CommonDataPtr & common_data_ptr, const PathWithLaneId & path,
   const PredictedObject & object);
+
+double calc_angle_to_lanelet_segment(const lanelet::ConstLanelets & lanelets, const Pose & pose);
 }  // namespace autoware::behavior_path_planner::utils::lane_change
 
 namespace autoware::behavior_path_planner::utils::lane_change::debug
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml
index 74d11dbcb1e13..20fc894f7fc58 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml
+++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml
@@ -13,6 +13,7 @@
   <maintainer email="shumpei.wakabayashi@tier4.jp">Shumpei Wakabayashi</maintainer>
   <maintainer email="tomohito.ando@tier4.jp">Tomohito Ando</maintainer>
   <maintainer email="alqudah.mohammad@tier4.jp">Alqudah Mohammad</maintainer>
+  <maintainer email="maxime.clement@tier4.jp">Maxime Clement</maintainer>
 
   <license>Apache License 2.0</license>
 
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 22c93f7d4bfd5..f3f371ec02a9b 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
@@ -122,6 +122,23 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node)
   p.rss_params.lateral_distance_max_threshold = getOrDeclareParameter<double>(
     *node, parameter("safety_check.execution.lateral_distance_max_threshold"));
 
+  p.rss_params_for_parked.longitudinal_distance_min_threshold = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.parked.longitudinal_distance_min_threshold"));
+  p.rss_params_for_parked.longitudinal_distance_min_threshold = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.parked.longitudinal_distance_min_threshold"));
+  p.rss_params_for_parked.longitudinal_velocity_delta_time = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.parked.longitudinal_velocity_delta_time"));
+  p.rss_params_for_parked.front_vehicle_deceleration = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.parked.expected_front_deceleration"));
+  p.rss_params_for_parked.rear_vehicle_deceleration = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.parked.expected_rear_deceleration"));
+  p.rss_params_for_parked.rear_vehicle_reaction_time = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.parked.rear_vehicle_reaction_time"));
+  p.rss_params_for_parked.rear_vehicle_safety_time_margin = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.parked.rear_vehicle_safety_time_margin"));
+  p.rss_params_for_parked.lateral_distance_max_threshold = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.parked.lateral_distance_max_threshold"));
+
   p.rss_params_for_abort.longitudinal_distance_min_threshold = getOrDeclareParameter<double>(
     *node, parameter("safety_check.cancel.longitudinal_distance_min_threshold"));
   p.rss_params_for_abort.longitudinal_velocity_delta_time = getOrDeclareParameter<double>(
@@ -166,8 +183,6 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node)
     getOrDeclareParameter<double>(*node, parameter("minimum_lane_changing_velocity"));
   p.minimum_lane_changing_velocity =
     std::min(p.minimum_lane_changing_velocity, max_acc * p.lane_change_prepare_duration);
-  p.lane_change_finish_judge_buffer =
-    getOrDeclareParameter<double>(*node, parameter("lane_change_finish_judge_buffer"));
 
   if (p.backward_length_buffer_for_end_of_lane < 1.0) {
     RCLCPP_WARN_STREAM(
@@ -224,8 +239,15 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node)
   p.cancel.unsafe_hysteresis_threshold =
     getOrDeclareParameter<int>(*node, parameter("cancel.unsafe_hysteresis_threshold"));
 
+  // finish judge parameters
+  p.lane_change_finish_judge_buffer =
+    getOrDeclareParameter<double>(*node, parameter("lane_change_finish_judge_buffer"));
   p.finish_judge_lateral_threshold =
     getOrDeclareParameter<double>(*node, parameter("finish_judge_lateral_threshold"));
+  const auto finish_judge_lateral_angle_deviation =
+    getOrDeclareParameter<double>(*node, parameter("finish_judge_lateral_angle_deviation"));
+  p.finish_judge_lateral_angle_deviation =
+    autoware::universe_utils::deg2rad(finish_judge_lateral_angle_deviation);
 
   // debug marker
   p.publish_debug_marker = getOrDeclareParameter<bool>(*node, parameter("publish_debug_marker"));
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 97eab5a03fcaa..1bab71737da00 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
@@ -14,6 +14,7 @@
 
 #include "autoware/behavior_path_lane_change_module/scene.hpp"
 
+#include "autoware/behavior_path_lane_change_module/utils/calculation.hpp"
 #include "autoware/behavior_path_lane_change_module/utils/utils.hpp"
 #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp"
 #include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp"
@@ -23,6 +24,7 @@
 #include "autoware/behavior_path_planner_common/utils/utils.hpp"
 
 #include <autoware/universe_utils/geometry/boost_polygon_utils.hpp>
+#include <autoware/universe_utils/math/unit_conversion.hpp>
 #include <autoware/universe_utils/system/time_keeper.hpp>
 #include <autoware_lanelet2_extension/utility/message_conversion.hpp>
 #include <autoware_lanelet2_extension/utility/utilities.hpp>
@@ -44,6 +46,7 @@ using utils::lane_change::calcMinimumLaneChangeLength;
 using utils::lane_change::create_lanes_polygon;
 using utils::path_safety_checker::isPolygonOverlapLanelet;
 using utils::traffic_light::getDistanceToNextTrafficLight;
+namespace calculation = utils::lane_change::calculation;
 
 NormalLaneChange::NormalLaneChange(
   const std::shared_ptr<LaneChangeParameters> & parameters, LaneChangeModuleType type,
@@ -391,16 +394,9 @@ void NormalLaneChange::insertStopPoint(
   const auto target_objects = filterObjects();
   double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer;
 
-  const auto is_valid_start_point = std::invoke([&]() -> bool {
-    auto lc_start_point = lanelet::utils::conversion::toLaneletPoint(
-      status_.lane_change_path.info.lane_changing_start.position);
-    const auto & target_neighbor_preferred_lane_poly_2d =
-      common_data_ptr_->lanes_polygon_ptr->target_neighbor;
-    return boost::geometry::covered_by(
-      lanelet::traits::to2D(lc_start_point), target_neighbor_preferred_lane_poly_2d);
-  });
+  const auto & lc_start_point = status_.lane_change_path.info.lane_changing_start;
 
-  if (!is_valid_start_point) {
+  if (!is_valid_start_point(common_data_ptr_, lc_start_point)) {
     const auto stop_point = utils::insertStopPoint(stopping_distance, path);
     setStopPose(stop_point.point.pose);
 
@@ -689,21 +685,33 @@ bool NormalLaneChange::hasFinishedLaneChange() const
   const auto & lane_change_end = status_.lane_change_path.info.shift_line.end;
   const auto & target_lanes = get_target_lanes();
   const double dist_to_lane_change_end =
-    utils::getSignedDistance(current_pose, lane_change_end, get_target_lanes());
-  double finish_judge_buffer = lane_change_parameters_->lane_change_finish_judge_buffer;
+    utils::getSignedDistance(current_pose, lane_change_end, target_lanes);
 
-  // If ego velocity is low, relax finish judge buffer
-  const double ego_velocity = getEgoVelocity();
-  if (std::abs(ego_velocity) < 1.0) {
-    finish_judge_buffer = 0.0;
-  }
+  const auto finish_judge_buffer = std::invoke([&]() {
+    const double ego_velocity = getEgoVelocity();
+    // If ego velocity is low, relax finish judge buffer
+    if (std::abs(ego_velocity) < 1.0) {
+      return 0.0;
+    }
+    return lane_change_parameters_->lane_change_finish_judge_buffer;
+  });
 
-  const auto reach_lane_change_end = dist_to_lane_change_end + finish_judge_buffer < 0.0;
+  const auto has_passed_end_pose = dist_to_lane_change_end + finish_judge_buffer < 0.0;
 
   lane_change_debug_.distance_to_lane_change_finished =
     dist_to_lane_change_end + finish_judge_buffer;
 
-  if (!reach_lane_change_end) {
+  if (has_passed_end_pose) {
+    const auto & lanes_polygon = common_data_ptr_->lanes_polygon_ptr->target;
+    return !boost::geometry::disjoint(
+      lanes_polygon.value(),
+      lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(current_pose.position)));
+  }
+
+  const auto yaw_deviation_to_centerline =
+    utils::lane_change::calc_angle_to_lanelet_segment(target_lanes, current_pose);
+
+  if (yaw_deviation_to_centerline > lane_change_parameters_->finish_judge_lateral_angle_deviation) {
     return false;
   }
 
@@ -1220,8 +1228,6 @@ PathWithLaneId NormalLaneChange::getTargetSegment(
         std::min(dist_from_start, dist_from_end), s_start + std::numeric_limits<double>::epsilon());
     });
 
-  RCLCPP_DEBUG(logger_, "in %s start: %f, end: %f", __func__, s_start, s_end);
-
   PathWithLaneId target_segment = route_handler.getCenterLinePath(target_lanes, s_start, s_end);
   for (auto & point : target_segment.points) {
     point.point.longitudinal_velocity_mps =
@@ -1357,7 +1363,7 @@ bool NormalLaneChange::getLaneChangePaths(
     route_handler.getLateralIntervalsToPreferredLane(target_lanes.back()));
 
   const auto dist_to_end_of_current_lanes =
-    utils::getDistanceToEndOfLane(getEgoPose(), current_lanes);
+    calculation::calc_ego_dist_to_terminal_end(common_data_ptr_);
 
   const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes);
 
@@ -1385,12 +1391,6 @@ bool NormalLaneChange::getLaneChangePaths(
 
   for (const auto & prepare_duration : prepare_durations) {
     for (const auto & sampled_longitudinal_acc : longitudinal_acc_sampling_values) {
-      const auto debug_print = [&](const auto & s) {
-        RCLCPP_DEBUG_STREAM(
-          logger_, "  -  " << s << " : prep_time = " << prepare_duration
-                           << ", lon_acc = " << sampled_longitudinal_acc);
-      };
-
       // get path on original lanes
       const auto prepare_velocity = std::clamp(
         current_velocity + sampled_longitudinal_acc * prepare_duration,
@@ -1404,13 +1404,35 @@ bool NormalLaneChange::getLaneChangePaths(
       const auto prepare_length = utils::lane_change::calcPhaseLength(
         current_velocity, getCommonParam().max_vel, longitudinal_acc_on_prepare, prepare_duration);
 
+      const auto ego_dist_to_terminal_start = dist_to_end_of_current_lanes - lane_change_buffer;
+      if (prepare_length > ego_dist_to_terminal_start) {
+        RCLCPP_DEBUG(
+          logger_,
+          "Reject: Prepare length exceed distance to terminal start. prep_len: %.5f,  ego dist to "
+          "terminal start: %.5f",
+          prepare_length, ego_dist_to_terminal_start);
+        continue;
+      }
+
       auto prepare_segment = getPrepareSegment(current_lanes, backward_path_length, prepare_length);
 
+      const auto debug_print = [&](const auto & s) {
+        RCLCPP_DEBUG(
+          logger_, "%s | prep_time: %.5f | lon_acc sampled: %.5f, actual: %.5f | prep_len: %.5f", s,
+          prepare_duration, sampled_longitudinal_acc, longitudinal_acc_on_prepare, prepare_length);
+      };
+
       if (prepare_segment.points.empty()) {
         debug_print("prepare segment is empty...? Unexpected.");
         continue;
       }
 
+      if (!is_valid_start_point(common_data_ptr_, prepare_segment.points.back().point.pose)) {
+        debug_print(
+          "Reject: lane changing start point is not within the preferred lanes or its neighbors");
+        continue;
+      }
+
       // lane changing start getEgoPose() is at the end of prepare segment
       const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose;
       const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet(
@@ -1441,13 +1463,8 @@ bool NormalLaneChange::getLaneChangePaths(
       }
       RCLCPP_DEBUG(logger_, "  -  sampling num for lat_acc: %lu", sample_lat_acc.size());
 
+      debug_print("Prepare path satisfy constraints");
       for (const auto & lateral_acc : sample_lat_acc) {
-        const auto debug_print_lat = [&](const auto & s) {
-          RCLCPP_DEBUG_STREAM(
-            logger_, "    -  " << s << " : prep_time = " << prepare_duration << ", lon_acc = "
-                               << sampled_longitudinal_acc << ", lat_acc = " << lateral_acc);
-        };
-
         const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk(
           shift_length, lane_change_parameters_->lane_changing_lateral_jerk, lateral_acc);
         const double longitudinal_acc_on_lane_changing =
@@ -1463,6 +1480,14 @@ bool NormalLaneChange::getLaneChangePaths(
         utils::lane_change::setPrepareVelocity(
           prepare_segment, current_velocity, terminal_lane_changing_velocity);
 
+        const auto debug_print_lat = [&](const auto & s) {
+          RCLCPP_DEBUG(
+            logger_,
+            "    -  %s | lc_time: %.5f | lon_acc sampled: %.5f, actual: %.5f | lc_len: %.5f", s,
+            lane_changing_time, sampled_longitudinal_acc, longitudinal_acc_on_lane_changing,
+            lane_changing_length);
+        };
+
         if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) {
           debug_print_lat("Reject: length of lane changing path is longer than length to goal!!");
           continue;
@@ -1497,16 +1522,6 @@ bool NormalLaneChange::getLaneChangePaths(
           continue;
         }
 
-        const lanelet::BasicPoint2d lc_start_point(
-          prepare_segment.points.back().point.pose.position.x,
-          prepare_segment.points.back().point.pose.position.y);
-
-        const auto target_lane_poly_2d = common_data_ptr_->lanes_polygon_ptr->target.value();
-
-        const auto is_valid_start_point =
-          boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) ||
-          boost::geometry::covered_by(lc_start_point, target_lane_poly_2d);
-
         LaneChangeInfo lane_change_info;
         lane_change_info.longitudinal_acceleration =
           LaneChangePhaseInfo{longitudinal_acc_on_prepare, longitudinal_acc_on_lane_changing};
@@ -1519,13 +1534,6 @@ bool NormalLaneChange::getLaneChangePaths(
         lane_change_info.lateral_acceleration = lateral_acc;
         lane_change_info.terminal_lane_changing_velocity = terminal_lane_changing_velocity;
 
-        if (!is_valid_start_point) {
-          debug_print_lat(
-            "Reject: lane changing points are not inside of the target preferred lanes or its "
-            "neighbors");
-          continue;
-        }
-
         const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval(
           lane_changing_length, initial_lane_changing_velocity);
         const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane(
@@ -2069,9 +2077,14 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
     const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj(
       obj, lane_change_parameters_->use_all_predicted_path);
     auto is_safe = true;
+    const auto selected_rss_param =
+      (obj.initial_twist.twist.linear.x <=
+       lane_change_parameters_->prepare_segment_ignore_object_velocity_thresh)
+        ? lane_change_parameters_->rss_params_for_parked
+        : rss_params;
     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,
+        path, ego_predicted_path, obj, obj_path, common_parameters, selected_rss_param, 1.0,
         get_max_velocity_for_safety_check(), collision_check_yaw_diff_threshold,
         current_debug_data.second);
 
@@ -2209,6 +2222,18 @@ bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lan
   return is_vehicle_stuck;
 }
 
+bool NormalLaneChange::is_valid_start_point(
+  const lane_change::CommonDataPtr & common_data_ptr, const Pose & pose) const
+{
+  const lanelet::BasicPoint2d lc_start_point(pose.position.x, pose.position.y);
+
+  const auto & target_neighbor_poly = common_data_ptr->lanes_polygon_ptr->target_neighbor;
+  const auto & target_lane_poly = common_data_ptr_->lanes_polygon_ptr->target.value();
+
+  return boost::geometry::covered_by(lc_start_point, target_neighbor_poly) ||
+         boost::geometry::covered_by(lc_start_point, target_lane_poly);
+}
+
 void NormalLaneChange::setStopPose(const Pose & stop_pose)
 {
   lane_change_stop_pose_ = stop_pose;
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp
new file mode 100644
index 0000000000000..521c30d406e7a
--- /dev/null
+++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp
@@ -0,0 +1,45 @@
+// 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.
+
+#include <autoware/behavior_path_lane_change_module/utils/calculation.hpp>
+#include <autoware/behavior_path_planner_common/utils/utils.hpp>
+
+namespace autoware::behavior_path_planner::utils::lane_change::calculation
+{
+double calc_ego_dist_to_terminal_end(const CommonDataPtr & common_data_ptr)
+{
+  const auto & lanes_ptr = common_data_ptr->lanes_ptr;
+  const auto & current_lanes = lanes_ptr->current;
+  const auto & current_pose = common_data_ptr->get_ego_pose();
+
+  return calc_dist_from_pose_to_terminal_end(common_data_ptr, current_lanes, current_pose);
+}
+
+double calc_dist_from_pose_to_terminal_end(
+  const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes,
+  const Pose & src_pose)
+{
+  if (lanes.empty()) {
+    return 0.0;
+  }
+
+  const auto & lanes_ptr = common_data_ptr->lanes_ptr;
+  const auto & goal_pose = common_data_ptr->route_handler_ptr->getGoalPose();
+
+  if (lanes_ptr->current_lane_in_goal_section) {
+    return utils::getSignedDistance(src_pose, goal_pose, lanes);
+  }
+  return utils::getDistanceToEndOfLane(src_pose, lanes);
+}
+}  // namespace autoware::behavior_path_planner::utils::lane_change::calculation
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp
index 176a7ece0084e..7e93365a59e7b 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp
@@ -1343,6 +1343,17 @@ bool is_before_terminal(
   }
   return current_max_dist >= 0.0;
 }
+
+double calc_angle_to_lanelet_segment(const lanelet::ConstLanelets & lanelets, const Pose & pose)
+{
+  lanelet::ConstLanelet closest_lanelet;
+
+  if (!lanelet::utils::query::getClosestLanelet(lanelets, pose, &closest_lanelet)) {
+    return autoware::universe_utils::deg2rad(180);
+  }
+  const auto closest_pose = lanelet::utils::getClosestCenterPose(closest_lanelet, pose.position);
+  return std::abs(autoware::universe_utils::calcYawDeviation(closest_pose, pose));
+}
 }  // namespace autoware::behavior_path_planner::utils::lane_change
 
 namespace autoware::behavior_path_planner::utils::lane_change::debug
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/config/scene_module_manager.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_planner/config/scene_module_manager.param.yaml
index ef6cb82d1412a..f94657c1418e4 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner/config/scene_module_manager.param.yaml
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner/config/scene_module_manager.param.yaml
@@ -3,80 +3,80 @@
 # NOTE: The smaller the priority number is, the higher the module priority is.
 /**:
   ros__parameters:
+    # NOTE: modules which are not set true in the preset is ignored in the slot configuration
+    slots:
+      # NOTE: array of array is not supported
+      - slot1
+      - slot2
+      - slot3
+      - slot4
+    slot1:
+      - "start_planner"
+    slot2:
+      - "side_shift"
+      - "avoidance_by_lane_change"
+      - "static_obstacle_avoidance"
+      - "lane_change_left"
+      - "lane_change_right"
+      - "external_request_lane_change_left"
+      - "external_request_lane_change_right"
+    slot3:
+      - "goal_planner"
+    slot4:
+      - "dynamic_obstacle_avoidance"
+
     external_request_lane_change_left:
       enable_rtc: false
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: true
-      keep_last: false
-      priority: 6
 
     external_request_lane_change_right:
       enable_rtc: false
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: true
-      keep_last: false
-      priority: 6
 
     lane_change_left:
       enable_rtc: false
       enable_simultaneous_execution_as_approved_module: true
       enable_simultaneous_execution_as_candidate_module: true
-      keep_last: false
-      priority: 5
 
     lane_change_right:
       enable_rtc: false
       enable_simultaneous_execution_as_approved_module: true
       enable_simultaneous_execution_as_candidate_module: true
-      keep_last: false
-      priority: 5
 
     start_planner:
       enable_rtc: false
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: false
-      keep_last: false
-      priority: 0
 
     side_shift:
       enable_rtc: false
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: false
-      keep_last: false
-      priority: 2
 
     goal_planner:
       enable_rtc: false
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: false
-      keep_last: true
-      priority: 1
 
     static_obstacle_avoidance:
       enable_rtc: false
       enable_simultaneous_execution_as_approved_module: true
       enable_simultaneous_execution_as_candidate_module: false
-      keep_last: false
-      priority: 4
 
     avoidance_by_lane_change:
       enable_rtc: false
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: false
-      keep_last: false
-      priority: 3
 
     dynamic_obstacle_avoidance:
       enable_rtc: false
       enable_simultaneous_execution_as_approved_module: true
       enable_simultaneous_execution_as_candidate_module: true
-      keep_last: false
-      priority: 7
 
     sampling_planner:
       enable_module: true
       enable_rtc: false
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: false
-      keep_last: true
-      priority: 16
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md b/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md
index 09627d2d5d91e..68d81f4d257c1 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md
@@ -46,6 +46,31 @@ Additionally, the manager generates root reference path, and if any other module
 
 ![manager_overview](../image/manager/manager_overview.svg)
 
+### Slot
+
+The manager owns several containers of sub-managers, namely _slots_, that holds/runs several sub-managers and send the output to the next slot. Given the initial reference path, each slot processes the input path and the output path is processed by the next slot. The final slot output is utilized as the output of the manager. The slot passes following information
+
+```cpp
+struct SlotOutput
+{
+  BehaviorModuleOutput valid_output;
+
+  // if candidate module is running, valid_output contains the planning by candidate module. In
+  // that case, downstream slots will just run aproved modules and do not try to launch new
+  // modules
+  bool is_upstream_candidate_exclusive{false};
+
+  // if this slot failed, downstream slots need to refresh approved/candidate modules and just
+  // forward valid_output of this slot output
+  bool is_upstream_failed_approved{false};
+
+  // if the approved module in this slot returned to isWaitingApproval, downstream slots need to
+  // refresh candidate once
+  bool is_upstream_waiting_approved{false};
+};
+
+```
+
 ### Sub-managers
 
 The sub-manager's main task is
@@ -136,13 +161,13 @@ There are 6 steps in one process:
 
 At first, the manager set latest planner data, and run all approved modules and get output path. At this time, the manager checks module status and removes expired modules from approved modules stack.
 
-![process_step1](../image/manager/process_step1.svg)
+![process_step1](../image/manager/process_step1.drawio.svg)
 
 ### Step2
 
 Input approved modules output and necessary data to all registered modules, and the modules judge the necessity of path modification based on it. The manager checks which module makes execution request.
 
-![process_step2](../image/manager/process_step2.svg)
+![process_step2](../image/manager/process_step2.drawio.svg)
 
 ### Step3
 
@@ -152,19 +177,19 @@ Check request module existence.
 
 The manager decides which module to execute as candidate modules from the modules that requested to execute path modification.
 
-![process_step4](../image/manager/process_step4.svg)
+![process_step4](../image/manager/process_step4.drawio.svg)
 
 ### Step5
 
 Decides the priority order of execution among candidate modules. And, run all candidate modules. Each modules outputs reference path and RTC cooperate status.
 
-![process_step5](../image/manager/process_step5.svg)
+![process_step5](../image/manager/process_step5.drawio.svg)
 
 ### Step6
 
 Move approved module to approved modules stack from candidate modules stack.
 
-![process_step6](../image/manager/process_step6.svg)
+![process_step6](../image/manager/process_step6.drawio.svg)
 
 ---
 
@@ -377,20 +402,20 @@ If a module that doesn't support simultaneous execution exists in approved modul
 
 For example, if approved module's setting of `enable_simultaneous_execution_as_approved_module` is **ENABLE**, then only modules whose the setting is **ENABLE** proceed to the next step.
 
-![request_step2](../image/manager/request_step2.svg)
+![request_step2](../image/manager/request_step2.drawio.svg)
 
 Other examples:
 
-| Process                                                  | Description                                                                                                                                                             |
-| :------------------------------------------------------- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
-| ![request_step2-2](../image/manager/request_step2-2.svg) | If approved modules stack is empty, then all request modules proceed to the next step, regardless of the setting of `enable_simultaneous_execution_as_approved_module`. |
-| ![request_step2-3](../image/manager/request_step2-3.svg) | If approved module's setting of `enable_simultaneous_execution_as_approved_module` is **DISABLE**, then all request modules are discarded.                              |
+| Process                                                         | Description                                                                                                                                                             |
+| :-------------------------------------------------------------- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| ![request_step2-2](../image/manager/request_step2-2.drawio.svg) | If approved modules stack is empty, then all request modules proceed to the next step, regardless of the setting of `enable_simultaneous_execution_as_approved_module`. |
+| ![request_step2-3](../image/manager/request_step2-3.drawio.svg) | If approved module's setting of `enable_simultaneous_execution_as_approved_module` is **DISABLE**, then all request modules are discarded.                              |
 
 ### Step3
 
 Sort `request_modules` by priority.
 
-![request_step3](../image/manager/request_step3.svg)
+![request_step3](../image/manager/request_step3.drawio.svg)
 
 ### Step4
 
@@ -444,26 +469,26 @@ Executable or not:
 
 For example, if the highest priority module's setting of `enable_simultaneous_execution_as_candidate_module` is **DISABLE**, then all modules after the second priority are discarded.
 
-![request_step4](../image/manager/request_step4.svg)
+![request_step4](../image/manager/request_step4.drawio.svg)
 
 Other examples:
 
-| Process                                                  | Description                                                                                                                                                          |
-| :------------------------------------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
-| ![request_step4-2](../image/manager/request_step4-2.svg) | If a module with a higher priority exists, lower priority modules whose setting of `enable_simultaneous_execution_as_candidate_module` is **DISABLE** are discarded. |
-| ![request_step4-3](../image/manager/request_step4-3.svg) | If all modules' setting of `enable_simultaneous_execution_as_candidate_module` is **ENABLE**, then all modules proceed to the next step.                             |
+| Process                                                         | Description                                                                                                                                                          |
+| :-------------------------------------------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| ![request_step4-2](../image/manager/request_step4-2.drawio.svg) | If a module with a higher priority exists, lower priority modules whose setting of `enable_simultaneous_execution_as_candidate_module` is **DISABLE** are discarded. |
+| ![request_step4-3](../image/manager/request_step4-3.drawio.svg) | If all modules' setting of `enable_simultaneous_execution_as_candidate_module` is **ENABLE**, then all modules proceed to the next step.                             |
 
 ### Step5
 
 Run all candidate modules.
 
-![request_step5](../image/manager/request_step5.svg)
+![request_step5](../image/manager/request_step5.drawio.svg)
 
 ## How to decide which module's output to use?
 
 Sometimes, multiple candidate modules are running simultaneously.
 
-![multiple_candidates](../image/manager/multiple_candidates.svg)
+![multiple_candidates](../image/manager/multiple_candidates.drawio.svg)
 
 In this case, the manager selects a candidate modules which output path is used as `behavior_path_planner` output by approval condition in the following rules.
 
@@ -482,11 +507,11 @@ In this case, the manager selects a candidate modules which output path is used
     The smaller the number is, the higher the priority is.
 
 <figure markdown>
-  ![module_select](../image/manager/module_select.svg){width=1000}
+  ![module_select](../image/manager/module_select.drawio.svg){width=1000}
   <figcaption>module priority</figcaption>
 </figure>
 
-![output_module](../image/manager/output_module.svg)
+![output_module](../image/manager/output_module.drawio.svg)
 
 Additionally, the manager moves the highest priority module to approved modules stack if it is already approved.
 
@@ -502,7 +527,9 @@ This is because module C is planning output path with the output of module B as
 
 As a result, the module A's output is used as approved modules stack.
 
-![waiting_approve](../image/manager/waiting_approve.svg)
+![waiting_approve](../image/manager/waiting_approve.drawio.svg)
+
+If this case happened in the slot, `is_upstream_waiting_approved` is set to true.
 
 ### Failure modules
 
@@ -510,24 +537,45 @@ The failure modules return the status `ModuleStatus::FAILURE`. The manager remov
 
 As a result, the module A's output is used as approved modules stack.
 
-![failure_modules](../image/manager/failure_modules.svg)
+![failure_modules](../image/manager/failure_modules.drawio.svg)
+
+If this case happened in the slot, `is_upstream_failed_approved` is set to true.
 
 ### Succeeded modules
 
 The succeeded modules return the status `ModuleStatus::SUCCESS`. The manager removes those modules based on **Last In First Out** policy. In other words, if a module added later to approved modules stack is still running (is in `ModuleStatus::RUNNING`), the manager doesn't remove the succeeded module. The reason for this is the same as in removal for waiting approval modules, and is to prevent sudden changes of the running module's output.
 
-![success_modules_remove](../image/manager/success_modules_remove.svg)
+![success_modules_remove](../image/manager/success_modules_remove.drawio.svg)
 
-![success_modules_skip](../image/manager/success_modules_skip.svg)
+![success_modules_skip](../image/manager/success_modules_skip.drawio.svg)
 
 As an exception, if **Lane Change** module returns status `ModuleStatus::SUCCESS`, the manager doesn't remove any modules until all modules is in status `ModuleStatus::SUCCESS`. This is because when the manager removes the **Lane Change** (normal LC, external LC, avoidance by LC) module as succeeded module, the manager updates the information of the lane Ego is currently driving in, so root reference path (= module A's input path) changes significantly at that moment.
 
-![lane_change_remove](../image/manager/lane_change_remove.svg)
+![lane_change_remove](../image/manager/lane_change_remove.drawio.svg)
 
-![lane_change_skip](../image/manager/lane_change_skip.svg)
+![lane_change_skip](../image/manager/lane_change_skip.drawio.svg)
 
 When the manager removes succeeded modules, the last added module's output is used as approved modules stack.
 
+## Slot output propagation
+
+As the initial solution, following _SlotOutput_ is passed to the first slot.
+
+```cpp
+  SlotOutput result_output = SlotOutput{
+    getReferencePath(data),
+    false,
+    false,
+    false,
+  };
+```
+
+If a slot turned out to be `is_upstream_failed_approved`, then all the subsequent slots are refreshed and have all of their approved_modules and candidate_modules cleared. The valid_output is just transferred to the end without any modification.
+
+If a slot turned out to be `is_upstream_waiting_approved`, then all the subsequent slots clear their candidate_modules once and apply their approved_modules to obtain the slot output.
+
+If a slot turned out to be `is_upstream_candidate_exclusive`, it means that a not `simultaneously_executable_as_candidate` module is running in that slot. Then all the subsequent modules just apply their approved_modules without trying to launch new candidate_modules.
+
 ## Reference path generation
 
 The reference path is generated from the centerline of the **lanelet sequence** obtained from the **current route lanelet**, and it is not only used as an input to the first added module of approved modules stack, but also used as the output of `behavior_path_planner` if none of the modules are running.
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/failure_modules.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/failure_modules.drawio.svg
similarity index 73%
rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/failure_modules.svg
rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/failure_modules.drawio.svg
index 78653e8f9170b..afe1842e46595 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/failure_modules.svg
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/failure_modules.drawio.svg
@@ -1,25 +1,26 @@
 <?xml version="1.0" encoding="UTF-8"?>
+<!-- Do not edit this file with editors other than diagrams.net -->
 <!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
 <svg
   xmlns="http://www.w3.org/2000/svg"
   xmlns:xlink="http://www.w3.org/1999/xlink"
   version="1.1"
-  width="1764px"
-  height="444px"
-  viewBox="-0.5 -0.5 1764 444"
-  style="background-color: rgb(255, 255, 255);"
+  width="1765px"
+  height="445px"
+  viewBox="-0.5 -0.5 1765 445"
+  content="&lt;mxfile host=&quot;Electron&quot; modified=&quot;2024-07-31T07:00:19.628Z&quot; agent=&quot;5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36&quot; etag=&quot;_AQt1OO1r5noe6rH3EUf&quot; version=&quot;20.3.0&quot; type=&quot;device&quot;&gt;&lt;diagram id=&quot;FbG0muvkg23WBpYGoVLW&quot; name=&quot;ページ7&quot;&gt;7Vttb6M4EP41+biVwUDgY5omeyd1pW6j1V0/rdzgJtwSjMBJk/v1Z8C8eUhCNtCk7aWVigfs4HlmnnmBDvB4tf0akXD5jbnUH+jI3Q7w3UDXv1hDU/xJJDspsU0pWUSem8m0UjDz/qVSiKR07bk0rl3IGfO5F9aFcxYEdM5rMhJF7LV+2Qvz698akgUFgtmc+FD6l+fyZSa19WEp/4N6i2X+zZrlZGdWJL9Y7iReEpe9VkR4MsDjiDGeHa22Y+on2sv1ks2b7jlb3FhEA95mgqG7j/Rlhpfu9++7H9Ols4hfvoidZOtsiL+WW5a3y3e5DiK2DlyaLIMG+PZ16XE6C8k8OfsqYBeyJV/5YqSJwxfP98fMZ1E6F4/Tj5DHPGK/Ch0aQgI3IPe0oRGn24pIbugrZSvKo524RJ41LUNqV9qXhXN1v1bQQlK2rCBl5DOJtJBFsXqpRHEg9XiSTvF71qlh2deoUwPolIRhxDZCh8LZmLv2BU0kKibzX2dqmwV8Juc2aH86neqp9hOVeoIpRr63CMQ5zsKOEBgiTUHAgAgUqNQQQP0hYAIE5iRwPZdwel0Q1N1C64hqNEA11wCKBRRNXRHA5JBFfMkWLCD+pJTe1qEor7lnifpShf1DOd/JaEzWnNXhoVuP/105fkqWujHl6G4rV04Hu3wQiA0Xk5JBZVYyLKelo3xeHUz9EJgxW0dzekhdjmRmTqIF5QcVK/km0eZB84ioT7i3qacLTUCnU0dRRHaVC0LmBTyurPyQCCpWh1QqMNQwf3SGaSDF0LK7KM2u2M45ljgE9BBGdOOxdVywgzhgax6u+SWNtjTUp6qdHjbaip0+VW14j9Gq/OWTZ+rfClpcpNuoUdmdY1ld27ltt7Xz3CO6s/MzCc0GZhTPaUCrNjTqNrioQWTluW5qckrUST7TqVygHo2mqfwECE9JBgzVnxGMO7rREHfs/sKO8wGdHcL3G258TnBCrYOTfV1Om9/5Iae9vYzTjpyxMbntyxWtfJHLuaKjHdf93fvXvampVamVV9XHdD+0e9M9rOvfkttOyL67zS76yY1BKiv8TcXcUvwo40o5r4Tz1DTbGOrKNylJc4sZ+b31m2Y7sLcE3H18YXdX8iOUfvqiYGxfPBtyYG/qW4rFjBMuMiI8Er/T0Z/3Px4nAJp4ScLkUCjYF/nScXhCGnnixmlUTnooRQC9hqymDpvjSHj2pbUhiz3usUBPeQanxUrGTTqqnE7PGrgjnG1Lr+NsNOCsmQ7E2ewPZ9gB+7A9SFPQ2Y2puNoVNLwcC2DwmbqQFnKcq4RleMlM6P31ISVvtejPmL3kWif3IR1IB0rC02rOwV5kwwoGWCHvfxa3nmkbpIDdZVyw1/ERi1tziCDCDQ/+3ja3wqhFa+EDlLeWhgGvX77AxfmjhPdY4Ja9+9OI/hxaN1vTutMLrUMGtWC+0FcRbVr2Mb5vNedNCmmMdEAswNbz2uzFp9tR8jJRaiyuPLyb+ySOvXndeOvWr1DG8VL4qEFUyMBs4IJcdq7dmGrrxVTf7dgTdxuWUjt3YKk9Btgd1i2aJhd6qDSxkh9YfTvpp7GMyCSVa22U/PSXBlz+gRNGsMVyLW2vlgi+IV6Xb4lhBFslPbAr0GzBr/t6XFfPu001U8uKByxmIVg+gcV65174Xsj/lvCblmBoRTLXhS2AUrp3W4AvdwBbyGn3PnnW/lD2lp8Z52y1t9tWsYzcmlbbRfJS+s0zEcZzE7Cf8W71zPymRl53DygsZEI9N8TPJjru7e1ZjGAP45Gu2IbeAP2LfXJFnzXfCligBkEpIhIQEZezRxF7I2hTJFby5low7iBMOnmRXATJYcPzBN2EqODeUNFgb+MzuENTOvm27qDBdyY+uzsYPbqDGJb/5pIFk/K/hfDkPw==&lt;/diagram&gt;&lt;/mxfile&gt;"
 >
   <defs/>
   <g>
-    <rect x="962" y="2" width="800" height="440" fill="#cccccc" stroke="rgb(0, 0, 0)" stroke-width="4" pointer-events="all"/>
-    <rect x="2" y="2" width="800" height="440" fill="#cccccc" stroke="rgb(0, 0, 0)" stroke-width="4" pointer-events="all"/>
-    <rect x="23" y="22" width="320" height="400" fill="#fff2cc" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="961" y="1" width="800" height="440" fill="#cccccc" stroke="rgb(0, 0, 0)" stroke-width="4" pointer-events="all"/>
+    <rect x="1" y="1" width="800" height="440" fill="#cccccc" stroke="rgb(0, 0, 0)" stroke-width="4" pointer-events="all"/>
+    <rect x="22" y="21" width="320" height="400" fill="#fff2cc" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 318px; height: 1px; padding-top: 29px; margin-left: 24px;"
+            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 318px; height: 1px; padding-top: 28px; margin-left: 23px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -30,16 +31,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="183" y="41" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">approved modules stack</text>
+        <text x="182" y="40" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">approved modules stack</text>
       </switch>
     </g>
-    <rect x="462" y="22" width="320" height="400" fill="#fff2cc" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="461" y="21" width="320" height="400" fill="#fff2cc" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 318px; height: 1px; padding-top: 29px; margin-left: 463px;"
+            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 318px; height: 1px; padding-top: 28px; margin-left: 462px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -50,17 +51,17 @@
             </div>
           </div>
         </foreignObject>
-        <text x="622" y="41" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">candidate modules stack</text>
+        <text x="621" y="40" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">candidate modules stack</text>
       </switch>
     </g>
-    <path d="M 303 102 L 323 102 L 323 222 L 334.76 222" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 340.76 222 L 332.76 226 L 334.76 222 L 332.76 218 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
-    <path d="M 183 182 L 183 150.24" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 183 144.24 L 187 152.24 L 183 150.24 L 179 152.24 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 302 101 L 322 101 L 322 221 L 333.76 221" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 339.76 221 L 331.76 225 L 333.76 221 L 331.76 217 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 182 181 L 182 149.24" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 182 143.24 L 186 151.24 L 182 149.24 L 178 151.24 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 162px; margin-left: 183px;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 161px; margin-left: 182px;">
             <div data-drawio-colors="color: rgb(0, 0, 0); background-color: #FFD966; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
                 style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; background-color: rgb(255, 217, 102); white-space: nowrap;"
@@ -70,16 +71,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="183" y="165" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">previous module output</text>
+        <text x="182" y="164" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">previous module output</text>
       </switch>
     </g>
-    <rect x="63" y="182" width="240" height="80" fill="#3333ff" stroke="rgb(0, 0, 0)" stroke-width="2" pointer-events="all"/>
+    <rect x="62" y="181" width="240" height="80" fill="#3333ff" stroke="rgb(0, 0, 0)" stroke-width="2" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 222px; margin-left: 64px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 221px; margin-left: 63px;"
           >
             <div data-drawio-colors="color: #FFFFFF; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -90,15 +91,15 @@
             </div>
           </div>
         </foreignObject>
-        <text x="183" y="226" fill="#FFFFFF" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module A</text>
+        <text x="182" y="225" fill="#FFFFFF" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module A</text>
       </switch>
     </g>
-    <path d="M 183 303 L 183 270.24" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 183 264.24 L 187 272.24 L 183 270.24 L 179 272.24 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 182 302 L 182 269.24" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 182 263.24 L 186 271.24 L 182 269.24 L 178 271.24 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 282px; margin-left: 183px;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 282px; margin-left: 182px;">
             <div data-drawio-colors="color: rgb(0, 0, 0); background-color: #FFD966; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
                 style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; background-color: rgb(255, 217, 102); white-space: nowrap;"
@@ -108,16 +109,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="183" y="285" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">previous module output</text>
+        <text x="182" y="285" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">previous module output</text>
       </switch>
     </g>
-    <rect x="63" y="303" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="62" y="302" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 343px; margin-left: 64px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 342px; margin-left: 63px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -128,16 +129,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="183" y="347" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module B</text>
+        <text x="182" y="346" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module B</text>
       </switch>
     </g>
-    <rect x="502" y="304" width="240" height="78" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="501" y="303" width="240" height="78" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 343px; margin-left: 503px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 342px; margin-left: 502px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -148,18 +149,18 @@
             </div>
           </div>
         </foreignObject>
-        <text x="622" y="347" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module D</text>
+        <text x="621" y="346" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module D</text>
       </switch>
     </g>
-    <path d="M 23 222 L 42 222 L 42 342 L 53.76 342" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 59.76 342 L 51.76 346 L 53.76 342 L 51.76 338 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
-    <rect x="63" y="62" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <path d="M 22 221 L 41 221 L 41 341 L 52.76 341" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 58.76 341 L 50.76 345 L 52.76 341 L 50.76 337 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <rect x="62" y="61" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 102px; margin-left: 64px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 101px; margin-left: 63px;"
           >
             <div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -170,16 +171,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="183" y="106" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module C</text>
+        <text x="182" y="105" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module C</text>
       </switch>
     </g>
-    <path d="M 184 162 L 343 162 L 343 192 L 272.37 192 L 241.24 212 L 252.37 192 L 184 192 Z" fill="#990000" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 183 161 L 342 161 L 342 191 L 271.37 191 L 240.24 211 L 251.37 191 L 183 191 Z" fill="#990000" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 157px; height: 1px; padding-top: 177px; margin-left: 185px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 157px; height: 1px; padding-top: 176px; margin-left: 184px;"
           >
             <div data-drawio-colors="color: #FFFFFF; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -190,16 +191,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="264" y="181" fill="#FFFFFF" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">ModuleStatus::FAILURE</text>
+        <text x="263" y="180" fill="#FFFFFF" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">ModuleStatus::FAILURE</text>
       </switch>
     </g>
-    <rect x="982.5" y="22" width="320" height="400" fill="#fff2cc" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="981.5" y="21" width="320" height="400" fill="#fff2cc" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 318px; height: 1px; padding-top: 29px; margin-left: 984px;"
+            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 318px; height: 1px; padding-top: 28px; margin-left: 983px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -210,16 +211,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="1143" y="41" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">approved modules stack</text>
+        <text x="1142" y="40" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">approved modules stack</text>
       </switch>
     </g>
-    <rect x="1421.5" y="22" width="320" height="400" fill="#fff2cc" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="1420.5" y="21" width="320" height="400" fill="#fff2cc" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 318px; height: 1px; padding-top: 29px; margin-left: 1423px;"
+            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 318px; height: 1px; padding-top: 28px; margin-left: 1422px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -230,18 +231,18 @@
             </div>
           </div>
         </foreignObject>
-        <text x="1582" y="41" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">candidate modules stack</text>
+        <text x="1581" y="40" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">candidate modules stack</text>
       </switch>
     </g>
-    <path d="M 1262.5 342 L 1282.5 342 L 1282.5 222 L 1294.26 222" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 1300.26 222 L 1292.26 226 L 1294.26 222 L 1292.26 218 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
-    <rect x="1022.5" y="302" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <path d="M 1261.5 341 L 1281.33 341 L 1281.33 221 L 1293.26 221" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 1299.26 221 L 1291.26 225 L 1293.26 221 L 1291.26 217 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <rect x="1021.5" y="301" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 342px; margin-left: 1024px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 341px; margin-left: 1023px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -252,16 +253,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="1143" y="346" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module B</text>
+        <text x="1142" y="345" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module B</text>
       </switch>
     </g>
-    <rect x="1461.5" y="304" width="240" height="78" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="1460.5" y="303" width="240" height="78" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 343px; margin-left: 1463px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 342px; margin-left: 1462px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -272,19 +273,19 @@
             </div>
           </div>
         </foreignObject>
-        <text x="1582" y="347" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module D</text>
+        <text x="1581" y="346" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module D</text>
       </switch>
     </g>
-    <path d="M 982.5 222 L 1002.5 222 L 1002.5 342 L 1014.26 342" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 1020.26 342 L 1012.26 346 L 1014.26 342 L 1012.26 338 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
-    <path d="M 862.5 227 L 862.5 217 L 882.5 217 L 882.5 206.5 L 901.5 222 L 882.5 237.5 L 882.5 227 Z" fill="#000000" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
-    <rect x="1022.5" y="182" width="240" height="80" fill="#e6e6e6" stroke="#808080" pointer-events="all"/>
+    <path d="M 981.5 221 L 1001.33 221 L 1001.33 341 L 1013.26 341" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 1019.26 341 L 1011.26 345 L 1013.26 341 L 1011.26 337 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 861.5 226 L 861.5 216 L 881.5 216 L 881.5 205.5 L 900.5 221 L 881.5 236.5 L 881.5 226 Z" fill="#000000" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
+    <rect x="1021.5" y="181" width="240" height="80" fill="#e6e6e6" stroke="#808080" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 222px; margin-left: 1024px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 221px; margin-left: 1023px;"
           >
             <div data-drawio-colors="color: #999999; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -295,16 +296,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="1143" y="226" fill="#999999" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module A</text>
+        <text x="1142" y="225" fill="#999999" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module A</text>
       </switch>
     </g>
-    <rect x="1022.5" y="62" width="240" height="80" fill="#e6e6e6" stroke="#808080" pointer-events="all"/>
+    <rect x="1021.5" y="61" width="240" height="80" fill="#e6e6e6" stroke="#808080" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 102px; margin-left: 1024px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 101px; margin-left: 1023px;"
           >
             <div data-drawio-colors="color: #999999; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -315,25 +316,25 @@
             </div>
           </div>
         </foreignObject>
-        <text x="1143" y="106" fill="#999999" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module C</text>
+        <text x="1142" y="105" fill="#999999" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module C</text>
       </switch>
     </g>
-    <path d="M 1283 207 L 1283 197 L 1343 197 L 1343 186.5 L 1362 202 L 1343 217.5 L 1343 207 Z" fill="#990000" stroke="#000000" stroke-miterlimit="10" pointer-events="all"/>
-    <path d="M 1282.97 106.5 L 1283.03 96.5 L 1343.03 96.88 L 1343.1 86.38 L 1362 102 L 1342.9 117.38 L 1342.97 106.88 Z" fill="#990000" stroke="#000000" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 1282 206 L 1282 196 L 1342 196 L 1342 185.5 L 1361 201 L 1342 216.5 L 1342 206 Z" fill="#990000" stroke="#000000" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 1281.97 105.5 L 1282.03 95.5 L 1342.03 95.88 L 1342.1 85.38 L 1361 101 L 1341.9 116.38 L 1341.97 105.88 Z" fill="#990000" stroke="#000000" stroke-miterlimit="10" pointer-events="all"/>
     <path
-      d="M 1372.5 102 C 1372.5 90.95 1381.45 82 1392.5 82 C 1403.55 82 1412.5 90.95 1412.5 102 C 1412.5 113.05 1403.55 122 1392.5 122 C 1381.45 122 1372.5 113.05 1372.5 102 Z M 1404.08 109.88 C 1407.88 104.32 1407.19 96.84 1402.43 92.07 C 1397.66 87.31 1390.18 86.62 1384.62 90.42 Z M 1380.96 94.12 C 1377.17 99.67 1377.86 107.14 1382.61 111.9 C 1387.35 116.66 1394.82 117.37 1400.38 113.6 Z"
+      d="M 1371.5 101 C 1371.5 89.95 1380.45 81 1391.5 81 C 1402.55 81 1411.5 89.95 1411.5 101 C 1411.5 112.05 1402.55 121 1391.5 121 C 1380.45 121 1371.5 112.05 1371.5 101 Z M 1403.08 108.88 C 1406.88 103.32 1406.19 95.84 1401.43 91.07 C 1396.66 86.31 1389.18 85.62 1383.62 89.42 Z M 1379.96 93.12 C 1376.17 98.67 1376.86 106.14 1381.61 110.9 C 1386.35 115.66 1393.82 116.37 1399.38 112.6 Z"
       fill="#ff0000"
       stroke="rgb(0, 0, 0)"
       stroke-miterlimit="10"
       pointer-events="all"
     />
-    <rect x="1300" y="52" width="125" height="30" fill="none" stroke="none" pointer-events="all"/>
+    <rect x="1299" y="51" width="125" height="30" fill="none" stroke="none" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 123px; height: 1px; padding-top: 67px; margin-left: 1301px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 123px; height: 1px; padding-top: 66px; margin-left: 1300px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -344,23 +345,23 @@
             </div>
           </div>
         </foreignObject>
-        <text x="1363" y="71" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">Remove.</text>
+        <text x="1362" y="70" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">Remove.</text>
       </switch>
     </g>
     <path
-      d="M 1372.5 202 C 1372.5 190.95 1381.45 182 1392.5 182 C 1403.55 182 1412.5 190.95 1412.5 202 C 1412.5 213.05 1403.55 222 1392.5 222 C 1381.45 222 1372.5 213.05 1372.5 202 Z M 1404.08 209.88 C 1407.88 204.32 1407.19 196.84 1402.43 192.07 C 1397.66 187.31 1390.18 186.62 1384.62 190.42 Z M 1380.96 194.12 C 1377.17 199.67 1377.86 207.14 1382.61 211.9 C 1387.35 216.66 1394.82 217.37 1400.38 213.6 Z"
+      d="M 1371.5 201 C 1371.5 189.95 1380.45 181 1391.5 181 C 1402.55 181 1411.5 189.95 1411.5 201 C 1411.5 212.05 1402.55 221 1391.5 221 C 1380.45 221 1371.5 212.05 1371.5 201 Z M 1403.08 208.88 C 1406.88 203.32 1406.19 195.84 1401.43 191.07 C 1396.66 186.31 1389.18 185.62 1383.62 189.42 Z M 1379.96 193.12 C 1376.17 198.67 1376.86 206.14 1381.61 210.9 C 1386.35 215.66 1393.82 216.37 1399.38 212.6 Z"
       fill="#ff0000"
       stroke="rgb(0, 0, 0)"
       stroke-miterlimit="10"
       pointer-events="all"
     />
-    <rect x="1300" y="152" width="125" height="30" fill="none" stroke="none" pointer-events="all"/>
+    <rect x="1299" y="151" width="125" height="30" fill="none" stroke="none" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 123px; height: 1px; padding-top: 167px; margin-left: 1301px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 123px; height: 1px; padding-top: 166px; margin-left: 1300px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -371,7 +372,7 @@
             </div>
           </div>
         </foreignObject>
-        <text x="1363" y="171" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">Remove.</text>
+        <text x="1362" y="170" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">Remove.</text>
       </switch>
     </g>
   </g>
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/lane_change_remove.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/lane_change_remove.drawio.svg
similarity index 89%
rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/lane_change_remove.svg
rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/lane_change_remove.drawio.svg
index 26dd05637b5da..32cc053064913 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/lane_change_remove.svg
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/lane_change_remove.drawio.svg
@@ -1,13 +1,14 @@
 <?xml version="1.0" encoding="UTF-8"?>
+<!-- Do not edit this file with editors other than diagrams.net -->
 <!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
 <svg
   xmlns="http://www.w3.org/2000/svg"
   xmlns:xlink="http://www.w3.org/1999/xlink"
   version="1.1"
   width="2124px"
-  height="444px"
-  viewBox="-0.5 -0.5 2124 444"
-  style="background-color: rgb(255, 255, 255);"
+  height="445px"
+  viewBox="-0.5 -0.5 2124 445"
+  content="&lt;mxfile host=&quot;Electron&quot; modified=&quot;2024-07-31T07:05:15.897Z&quot; agent=&quot;5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36&quot; etag=&quot;sPF6lB9DNdZJFR430NuP&quot; version=&quot;20.3.0&quot; type=&quot;device&quot;&gt;&lt;diagram id=&quot;FbG0muvkg23WBpYGoVLW&quot; name=&quot;ページ7&quot;&gt;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&lt;/diagram&gt;&lt;/mxfile&gt;"
 >
   <defs/>
   <g>
@@ -53,8 +54,8 @@
         <text x="821" y="41" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">candidate modules stack</text>
       </switch>
     </g>
-    <path d="M 502 102 L 522 102 L 522 222 L 533.76 222" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 539.76 222 L 531.76 226 L 533.76 222 L 531.76 218 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 502 102 L 522.14 102.14 L 522.14 222.14 L 533.76 222.06" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 539.76 222.02 L 531.79 226.07 L 533.76 222.06 L 531.74 218.07 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <path d="M 382 182 L 382 150.24" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
     <path d="M 382 144.24 L 386 152.24 L 382 150.24 L 378 152.24 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
@@ -98,7 +99,7 @@
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 282px; margin-left: 382px;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 283px; margin-left: 382px;">
             <div data-drawio-colors="color: rgb(0, 0, 0); background-color: #FFD966; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
                 style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; background-color: rgb(255, 217, 102); white-space: nowrap;"
@@ -108,7 +109,7 @@
             </div>
           </div>
         </foreignObject>
-        <text x="382" y="285" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">previous module output</text>
+        <text x="382" y="286" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">previous module output</text>
       </switch>
     </g>
     <rect x="262" y="303" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
@@ -151,8 +152,8 @@
         <text x="821" y="347" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module D</text>
       </switch>
     </g>
-    <path d="M 222 222 L 241 222 L 241 342 L 252.76 342" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 258.76 342 L 250.76 346 L 252.76 342 L 250.76 338 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 222 222 L 241 222.14 L 241 342.14 L 252.76 342.06" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 258.76 342.02 L 250.79 346.07 L 252.76 342.06 L 250.74 338.07 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <rect x="262" y="62" width="240" height="80" fill="#3333ff" stroke="rgb(0, 0, 0)" stroke-width="2" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
@@ -349,7 +350,7 @@
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 158px; height: 1px; padding-top: 114px; margin-left: 1px;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 158px; height: 1px; padding-top: 112px; margin-left: 1px;">
             <div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
                 style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
@@ -359,7 +360,7 @@
             </div>
           </div>
         </foreignObject>
-        <text x="80" y="117" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">driving lane data</text>
+        <text x="80" y="116" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">driving lane data</text>
       </switch>
     </g>
     <path d="M 1301 222 L 1333.76 222" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
@@ -399,7 +400,7 @@
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 158px; height: 1px; padding-top: 114px; margin-left: 1121px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 158px; height: 1px; padding-top: 112px; margin-left: 1121px;"
           >
             <div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -410,7 +411,7 @@
             </div>
           </div>
         </foreignObject>
-        <text x="1200" y="117" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">driving lane data</text>
+        <text x="1200" y="116" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">driving lane data</text>
       </switch>
     </g>
     <path d="M 382 162 L 541 162 L 541 192 L 470.37 192 L 439.24 212 L 450.37 192 L 382 192 Z" fill="#990000" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
@@ -453,8 +454,8 @@
         <text x="461" y="301" fill="#FFFFFF" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">ModuleStatus::SUCCESS</text>
       </switch>
     </g>
-    <path d="M 1342.64 229.2 L 1342.6 222 L 1653.76 222" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 1659.76 222 L 1651.76 226 L 1653.76 222 L 1651.76 218 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 1342.64 229.2 L 1342.71 222.14 L 1653.76 222" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 1659.76 222 L 1651.77 226 L 1653.76 222 L 1651.76 218 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <path d="M 1142 22 L 1301 22 L 1301 52 L 1230.37 52 L 1199.24 72 L 1210.37 52 L 1142 52 Z" fill="#990000" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/lane_change_skip.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/lane_change_skip.drawio.svg
similarity index 92%
rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/lane_change_skip.svg
rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/lane_change_skip.drawio.svg
index c3741351798c1..f35efebea0695 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/lane_change_skip.svg
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/lane_change_skip.drawio.svg
@@ -1,4 +1,5 @@
 <?xml version="1.0" encoding="UTF-8"?>
+<!-- Do not edit this file with editors other than diagrams.net -->
 <!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
 <svg
   xmlns="http://www.w3.org/2000/svg"
@@ -7,7 +8,7 @@
   width="2124px"
   height="444px"
   viewBox="-0.5 -0.5 2124 444"
-  style="background-color: rgb(255, 255, 255);"
+  content="&lt;mxfile host=&quot;Electron&quot; modified=&quot;2024-07-31T07:05:42.999Z&quot; agent=&quot;5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36&quot; etag=&quot;l5WZnx8D4OvCObnDHVVf&quot; version=&quot;20.3.0&quot; type=&quot;device&quot;&gt;&lt;diagram id=&quot;FbG0muvkg23WBpYGoVLW&quot; name=&quot;ページ7&quot;&gt;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&lt;/diagram&gt;&lt;/mxfile&gt;"
 >
   <defs/>
   <g>
@@ -98,7 +99,7 @@
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 282px; margin-left: 382px;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 283px; margin-left: 382px;">
             <div data-drawio-colors="color: rgb(0, 0, 0); background-color: #FFD966; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
                 style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; background-color: rgb(255, 217, 102); white-space: nowrap;"
@@ -108,7 +109,7 @@
             </div>
           </div>
         </foreignObject>
-        <text x="382" y="285" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">previous module output</text>
+        <text x="382" y="286" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">previous module output</text>
       </switch>
     </g>
     <rect x="262" y="303" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
@@ -269,7 +270,7 @@
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 158px; height: 1px; padding-top: 114px; margin-left: 1px;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 158px; height: 1px; padding-top: 106px; margin-left: 1px;">
             <div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
                 style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
@@ -279,7 +280,7 @@
             </div>
           </div>
         </foreignObject>
-        <text x="80" y="117" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">driving lane data</text>
+        <text x="80" y="110" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">driving lane data</text>
       </switch>
     </g>
     <path d="M 1301 222 L 1333.76 222" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
@@ -359,7 +360,7 @@
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 158px; height: 1px; padding-top: 114px; margin-left: 1121px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 158px; height: 1px; padding-top: 106px; margin-left: 1121px;"
           >
             <div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -370,7 +371,7 @@
             </div>
           </div>
         </foreignObject>
-        <text x="1200" y="117" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">driving lane data</text>
+        <text x="1200" y="110" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">driving lane data</text>
       </switch>
     </g>
     <rect x="1342" y="22" width="320" height="400" fill="#fff2cc" stroke="rgb(0, 0, 0)" pointer-events="all"/>
@@ -443,7 +444,7 @@
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 282px; margin-left: 1502px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 283px; margin-left: 1502px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); background-color: #FFD966; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -454,7 +455,7 @@
             </div>
           </div>
         </foreignObject>
-        <text x="1502" y="285" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">previous module output</text>
+        <text x="1502" y="286" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">previous module output</text>
       </switch>
     </g>
     <rect x="1382" y="303" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/module_select.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/module_select.drawio.svg
similarity index 84%
rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/module_select.svg
rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/module_select.drawio.svg
index 5033a54e480e6..88e9365128a1a 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/module_select.svg
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/module_select.drawio.svg
@@ -1,6 +1,15 @@
 <?xml version="1.0" encoding="UTF-8"?>
+<!-- Do not edit this file with editors other than diagrams.net -->
 <!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
-<svg xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" width="841px" height="181px" viewBox="-0.5 -0.5 841 181" style="background-color: rgb(255, 255, 255);">
+<svg
+  xmlns="http://www.w3.org/2000/svg"
+  xmlns:xlink="http://www.w3.org/1999/xlink"
+  version="1.1"
+  width="841px"
+  height="181px"
+  viewBox="-0.5 -0.5 841 181"
+  content="&lt;mxfile host=&quot;Electron&quot; modified=&quot;2024-07-31T06:51:29.027Z&quot; agent=&quot;5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36&quot; etag=&quot;CLroQnpwpTmbNlKeBFLM&quot; version=&quot;20.3.0&quot; type=&quot;device&quot;&gt;&lt;diagram id=&quot;FbG0muvkg23WBpYGoVLW&quot; name=&quot;ページ7&quot;&gt;5Vhdb9sgFP01eazkrybxo5vPat1aNdWyV2aIzUaMi3Ht7NcPbBwbkzatkk6dqkgJPsCFe+65XJyBO9mWCwbS+CuFiAwcC5YDdzpwnLHniW8J7GrgwrH9yxqKGIY1aLfACv9BCrQUmmOIMm0gp5RwnOpgSJMEhVzDAGO00IdtKNFXTUGEDGAVAmKiawx5rBxzRi2+RDiKm5XtoV/3bEEzWHmSxQDSogO5s4E7YZTyurUtJ4hI9hpe6nnzZ3r3G2Mo4a+ZsCTzovxV5j+szcOX+3VMVzy9sEfKkSdAcuUyQ485yrj0gcKcCPbr/fNdQwqjeQKRtGsN3KsixhytUhDK3kLoQGAx3xLxZIvmhiZ8peZWz5iQCSWUVbbc2VB+BP6EGMeC+IDgKBF9nEpDppfKcTkclR1Ieb1AdIs424khqtf1LBWCXRO9Big6IfUUFnfDOVYgUDKK9tZbpkVDkf0m4scG8VmIErSnXTSE90PCFYeVyBvaho+51I0Y4G42liX9aaFhpH6ruT9ZHxEbrg3WsEgdiDmmgvTgtAWt4O7u/vb7bPrMSuaWUoYpw3xXLW2dsrr9an/PK+e+bLcYQtnZ13ngT7zZlcAzzujv/WHinEvjTl/jY1PjziGNv6PE/eMSn5ph0/VorYPrh+tvi726gpt/IqPPIBp//PFE01juiMbgXpTSVDY3BJWBLPKCD5RA1ZyGBGQZDivaAOMm3ImMHsYe/VIic+HNVcQAxILoTt98Xgmo7ZtiJi4glWynhayfL8QIQe3WYUaow79tHSpMDcgQARw/6beVQ1FRa9xRXCXF8+fGpa8byWjOQqTmde8XPVOec9SUiEaEuGGqEsre+VO0Yxva0U+G5fViaahJZAvXRVFnWxPshCb91FQQUOkrDjWO2Et5feh86ElPOyLOURFG/XDYvpHc9qHkdt8vuZ0jAbq5XX+S+HiW+wHj4x6v2BO9vJ502TypVL/5snlo47r8fP//vUVKk73aVdWnc5wlI+MN6h0vCuKxfSuua0P754I7+ws=&lt;/diagram&gt;&lt;/mxfile&gt;"
+>
   <defs>
     <linearGradient x1="100%" y1="0%" x2="0%" y2="0%" id="mx-gradient-ff0000-1-0000ff-1-e-0">
       <stop offset="0%" style="stop-color: rgb(0, 0, 255); stop-opacity: 1;"/>
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step5.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/multiple_candidates.drawio.svg
similarity index 82%
rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step5.svg
rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/multiple_candidates.drawio.svg
index 54b5b7840995b..6f666aed8f1e4 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step5.svg
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/multiple_candidates.drawio.svg
@@ -1,13 +1,14 @@
 <?xml version="1.0" encoding="UTF-8"?>
+<!-- Do not edit this file with editors other than diagrams.net -->
 <!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
 <svg
   xmlns="http://www.w3.org/2000/svg"
   xmlns:xlink="http://www.w3.org/1999/xlink"
   version="1.1"
-  width="1130px"
+  width="1132px"
   height="182px"
-  viewBox="-0.5 -0.5 1130 182"
-  style="background-color: rgb(255, 255, 255);"
+  viewBox="-0.5 -0.5 1132 182"
+  content="&lt;mxfile host=&quot;Electron&quot; modified=&quot;2024-07-31T06:51:58.146Z&quot; agent=&quot;5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36&quot; etag=&quot;teygj-KPaj47XfDNO2wQ&quot; version=&quot;20.3.0&quot; type=&quot;device&quot;&gt;&lt;diagram id=&quot;FbG0muvkg23WBpYGoVLW&quot; name=&quot;ページ7&quot;&gt;3Vldb5swFP01PLYC8/2YpGWTtknTMmndowsOeHMwMqYh+/WzwQQcSJOoyVItD5F97WvwPcf3HieGvVjXHxgssi80QcQAZlIb9oMBgOUC8S0N29ZwB0zgtKaU4UTN6g1L/Acpo6msFU5QqU3klBKOC90Y0zxHMddskDG60aetKNGfWsAUjQzLGJKx9QdOeNZaA+D39o8Ip1n3ZMsL25E17CarnZQZTOhmYLIfDXvBKOVta10vEJHR6+LS+kUHRncvxlDOT3H4SKJN/auunszV90/ffmR0yYs7y1Vv9wJJpbYMi4LRF5TITdCkIiL84u05jH+rjfBtFx1GqzxB8gGmYc83GeZoWcBYjm4EIYQt42siepZormjOl8q36WNCFpRQ1qxlR1EEFgthf0GMY4HAjOA0F2OcyoXG21URkNNRPTCp7X9AdI0424opatR2AuWj6OgHgdr+psfW7gDLhrh2E6HiU7pbvQ+5aKion4UAGCEQwzzBCeToH0OwEB/TPAhByRn9vTsG4EKghF283xUozgiUMkZ5D4hozC6LxX7M1zhJ5OA+SLNw4TzOLxR8F+ydiNAbBx84E8G/YuyD47Gf/wex34X6HcW+e4NBYFEiCqLqUsYzmtIcksfeOtdD38/5TGXaaAL8C3G+VdUdVpzqcKAa8yflLts/ZfveVb2HejD0sO06udjw07Az8JLd3q3pdX5nJLGSVixGp+RuDlmK+CmkltF8lR0MEcjxiy4/poBuXGeMwe1gQkFxzsvByl+lYUg6WyddYJp7suFcD9Fo36Kn3W47b2Gic1Mm3oehr7HR8/xX+TjmlRz+ihgWoUBMzXoL18yrMGgCcHAA8N0iLd2V317KuQj27nFRKhq04kXFJ2nyGT6Lm4gGLVS5XZSSBo/DSZ+hEv+Bz816EjR1qMTi7txwHybqB5GPmwt5ljYMHFSMOfC8KJpE/nXeqwuNeg1jd404vcDcmffA920NS5Wv3kgRy9MXBfoCdLUq0ZWY4Y+YkQppwFqt3Fym5O5WSIQ5Rk3AxXm8iVaIIk98LqQV7H2RHE6IZGtKJDvX0wrhCItDJzJPZvIuLk8fgWWJYz3EOh5dDraM09XAOPuefkQ9fzYPp4+odWLKHYDgTmDQ2d547BxgHsvMbRkZZebxUo5z6yTv37bAvy+peWtZ6PvWNB0OysJjHrosnPD3DvifzWU7MI8tdXUuu7fk8jmJ8l1cm7oq/s9Yf5g7TnCplGq7N9fNvjeqyKhGccVxno4IWmawkE0hZYio28eFUDG4yyinwfVmVHAnSrIukMLQNJtfG6Wn/kNw1EjmgpaYY5qDhqG21FJlexaAORhuRh37MkrLMQN3T2k5Jyot93ylJbr9HwAtDfr/UezHvw==&lt;/diagram&gt;&lt;/mxfile&gt;"
 >
   <defs/>
   <g>
@@ -93,12 +94,12 @@
     </g>
     <path d="M 600 91 L 600 121 L 610 121 L 631.76 121" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
     <path d="M 637.76 121 L 629.76 125 L 631.76 121 L 629.76 117 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
-    <path d="M 480.04 121.06 L 540.02 121.1 L 591.76 121.01" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 597.76 121 L 589.77 125.02 L 591.76 121.01 L 589.76 117.02 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 480.04 121.06 L 540.67 121 L 591.76 121" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 597.76 121 L 589.76 125 L 591.76 121 L 589.76 117 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 142px; margin-left: 539px;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 143px; margin-left: 541px;">
             <div data-drawio-colors="color: rgb(0, 0, 0); background-color: #B266FF; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
                 style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; background-color: rgb(178, 102, 255); white-space: nowrap;"
@@ -108,7 +109,7 @@
             </div>
           </div>
         </foreignObject>
-        <text x="539" y="145" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">approved module output</text>
+        <text x="541" y="146" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">approved module output</text>
       </switch>
     </g>
     <rect x="0" y="101" width="120" height="40" fill="#ff6666" stroke="rgb(0, 0, 0)" pointer-events="all"/>
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/output_module.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/output_module.drawio.svg
similarity index 85%
rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/output_module.svg
rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/output_module.drawio.svg
index ec3079114f4ea..f3bf8866d7f00 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/output_module.svg
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/output_module.drawio.svg
@@ -1,13 +1,14 @@
 <?xml version="1.0" encoding="UTF-8"?>
+<!-- Do not edit this file with editors other than diagrams.net -->
 <!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
 <svg
   xmlns="http://www.w3.org/2000/svg"
   xmlns:xlink="http://www.w3.org/1999/xlink"
   version="1.1"
-  width="1130px"
+  width="1132px"
   height="401px"
-  viewBox="-0.5 -0.5 1130 401"
-  style="background-color: rgb(255, 255, 255);"
+  viewBox="-0.5 -0.5 1132 401"
+  content="&lt;mxfile host=&quot;Electron&quot; modified=&quot;2024-07-31T06:46:39.547Z&quot; agent=&quot;5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36&quot; etag=&quot;TchrNhc6hGMy20aLiLFw&quot; version=&quot;20.3.0&quot; type=&quot;device&quot;&gt;&lt;diagram id=&quot;FbG0muvkg23WBpYGoVLW&quot; name=&quot;ページ7&quot;&gt;7VrLbts4FP0aL1PobWlpO1EDTAcoxgO0s2Ql2mJDiwJFxXa/fkiJ1Du2VMuxETSbkJe8FHnu4eHLM3O1O3ymIIn+JiHEM0MLDzPzcWYYD6Zl8H/CclQWT3MK05aisDDqlWGNfkFp1KQ1QyFMGxUZIZihpGkMSBzDgDVsgFKyb1bbENz8agK2sGNYBwB3rd9QyKLC6hrzyv4M0TZSX9YdryjZAVVZjiSNQEj2NZP5NDNXlBBWpHaHFcQCPoVL4ee/UVp2jMKYDXF4xv7+8POQfdc2//71z7eIrFnyoLtW0c4rwJkcMkgSSl5hKAZBwgxz+HnvGQhe5EDYUaFDSRaHUHxAm5nLfYQYXCcgEKV7zghui9gO85zOkxsSs7X0zfMI4xXBhOZtmb7vG6sVt79CyhCPwAKjbczLGBENdYcrERDV4aFmksP/DMkOMnrkVWSpabnSR9LRtVVw9lVsTWWLanG1NGkEkk/bsvUKcp6QqI+KgNOJQADiEIWAwfsKQcooeSmngT5RUDyF910FZd4BGoZcGGSWUBaRLYkBfqqsy2YoqjpfiIAvB+wnZOwoVQ5kjDTDAw+Ifa+l/xNNfbJl7vEgW84zR5WJ+YBLJ5GpeYls5ZbnlF8zmMapYKYkowE8CZcrpRnQLWRD9EageZIeFGLA0GtThvsCnbsuKAXHWoWEoJiltZa/CkONdXO3xbp5Wz7HevBE0YuKduVwLmGi25GHNIBxJQ08sZhWFdqzf4fCMCd4Sy4W3sp6Wk4kA7bR0mbH7coAn/JdGXCvpgKefh775QfA3nPaEuzcHnvjPParj4j9PfDePI/940fE3tWMYdjP3athb91y56G2F6N2HmXmFjsPZ+jOQwnKrXcenjl253HO40o7D8/+w8TrMNG8EyZao5nY9nAt/T2Y6Pxh4nWYqH8YJjrau2jiTe8FxjCxy6jf5eYlTLSGMlEdcydmYpc4dvu6ybVa++iis9KvYtRoUtv6WFKf87gWqd1bknrMZde0Mnmdq6gerWpTToX1HOWmi7B8lDh1y88TJGNJxnrJ8AX8gLgZQCCPavxkyCA9dYajMEW/wI+8PRFGOXV44/ZyZj/2HAex+NwSBC/bnGe1A+DScBzf7438aXbLFyLZjVn5LjP8vPigfTLmc7MRS3mouJAiutNs1Gg2QDabFF6FGYa6Qr//uX8PWytDU2+QN7jovuheoex5TQK2MIa0eGXKnyFFXzaQoxLAHB8O2E3ueHzf4X8T3fGYHfHVuvdrev/z0tVi0b3bfEt643AhXrGFzGKQpihoQtyMxzRL6nAtduaLpdevxYNX4cFHkFq47J5oKduli7XVfuYpN2WqkWJMncW605RlvXVGea91n/ehQ7RnDhlMRTcTighF7NghXRqBRCT5VMWcl+cnegIp4n0VmwDl9LUydQjVQ7mmAHiexv+kZ/N92s/X/oSkiCESGznBTaEVabHWGFqtOC+1zGmUxNLUhVjJDGugktjjlYRnq5+GFHyofmJjPv0P&lt;/diagram&gt;&lt;/mxfile&gt;"
 >
   <defs/>
   <g>
@@ -135,8 +136,8 @@
     </g>
     <path d="M 600 200 L 610 200 L 631.76 200" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
     <path d="M 637.76 200 L 629.76 204 L 631.76 200 L 629.76 196 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
-    <path d="M 600 200 L 620 200 L 620 321 L 631.76 321" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 637.76 321 L 629.76 325 L 631.76 321 L 629.76 317 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 600 200 L 620 200 L 620 321.14 L 631.76 321.06" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 637.76 321.02 L 629.79 325.07 L 631.76 321.06 L 629.74 317.07 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <path d="M 600 200 L 620 200 L 620 80 L 631.76 80" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
     <path d="M 637.76 80 L 629.76 84 L 631.76 80 L 629.76 76 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <path d="M 161 200 L 190 200 L 192.76 200" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
@@ -156,7 +157,7 @@
             </div>
           </div>
         </foreignObject>
-        <text x="541" y="224" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">approved module output</text>
+        <text x="541" y="225" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">approved module output</text>
       </switch>
     </g>
     <path d="M 120 200 L 152.76 200" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step1.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step1.drawio.svg
new file mode 100644
index 0000000000000..4e72c4a87faea
--- /dev/null
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step1.drawio.svg
@@ -0,0 +1,554 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!-- Do not edit this file with editors other than diagrams.net -->
+<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
+<svg
+  xmlns="http://www.w3.org/2000/svg"
+  xmlns:xlink="http://www.w3.org/1999/xlink"
+  version="1.1"
+  width="1165px"
+  height="1084px"
+  viewBox="-0.5 -0.5 1165 1084"
+  content="&lt;mxfile host=&quot;Electron&quot; modified=&quot;2024-07-31T05:51:24.115Z&quot; agent=&quot;5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36&quot; etag=&quot;PKy4hILTIa3tcIotgjnr&quot; version=&quot;20.3.0&quot; type=&quot;device&quot;&gt;&lt;diagram id=&quot;FbG0muvkg23WBpYGoVLW&quot; name=&quot;page1&quot;&gt;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&lt;/diagram&gt;&lt;/mxfile&gt;"
+>
+  <defs/>
+  <g>
+    <rect x="32" y="432" width="600" height="420" fill="none" stroke="#cc0000" stroke-width="3" stroke-dasharray="9 9" pointer-events="all"/>
+    <rect x="2" y="2" width="1160" height="320" fill="#cccccc" stroke="rgb(0, 0, 0)" stroke-width="4" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 1158px; height: 1px; padding-top: 9px; margin-left: 3px;"
+          >
+            <div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 16px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                <b>manager</b>
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="582" y="25" fill="#000000" font-family="Helvetica" font-size="16px" text-anchor="middle">manager</text>
+      </switch>
+    </g>
+    <rect x="22" y="122" width="1120" height="180" fill="#e6e6e6" stroke="rgb(0, 0, 0)" stroke-width="2" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 1118px; height: 1px; padding-top: 129px; margin-left: 23px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 16px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                <b>Slots</b>
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="582" y="145" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="16px" text-anchor="middle">Slots</text>
+      </switch>
+    </g>
+    <rect x="42" y="202" width="240" height="80" fill="#9999ff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 242px; margin-left: 43px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div
+                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
+              >
+                Slot 1
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="162" y="246" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">Slot 1</text>
+      </switch>
+    </g>
+    <rect x="322" y="202" width="240" height="80" fill="#9999ff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 242px; margin-left: 323px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div
+                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
+              >
+                Slot 2
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="442" y="246" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">Slot 2</text>
+      </switch>
+    </g>
+    <rect x="602" y="202" width="240" height="80" fill="#9999ff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 242px; margin-left: 603px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div
+                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
+              >
+                Slot 3
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="722" y="246" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">Slot 3</text>
+      </switch>
+    </g>
+    <rect x="882" y="202" width="240" height="80" fill="#9999ff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 242px; margin-left: 883px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div
+                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
+              >
+                Slot 4
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="1002" y="246" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">Slot 4</text>
+      </switch>
+    </g>
+    <path
+      d="M 1027.95 181 L 980.05 181 L 980.05 136.8 L 943.89 136.8 L 1004 103 L 1064.11 136.8 L 1027.95 136.8 Z"
+      fill="#003366"
+      stroke="rgb(0, 0, 0)"
+      stroke-width="2"
+      stroke-miterlimit="10"
+      pointer-events="all"
+    />
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 142px; margin-left: 1004px;"
+          >
+            <div data-drawio-colors="color: #FFFFFF; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 14px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: nowrap;">
+                Output
+                <br/>
+                path
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="1004" y="146" fill="#FFFFFF" font-family="Helvetica" font-size="14px" text-anchor="middle" font-weight="bold">Output...</text>
+      </switch>
+    </g>
+    <rect x="50" y="52" width="230" height="30" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 228px; height: 1px; padding-top: 67px; margin-left: 51px;">
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div
+                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
+              >
+                The manager passes lateest
+                <br/>
+                planner data and initial path
+                <br/>
+                to the slots
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="165" y="71" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">The manager passes lateest...</text>
+      </switch>
+    </g>
+    <rect x="890" y="52" width="230" height="30" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 228px; height: 1px; padding-top: 67px; margin-left: 891px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div
+                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
+              >
+                The manager receives the output path as the final solution
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="1005" y="71" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">The manager receives the output path a...</text>
+      </switch>
+    </g>
+    <rect x="2" y="402" width="1160" height="680" fill="#cccccc" stroke="rgb(0, 0, 0)" stroke-width="4" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 1158px; height: 1px; padding-top: 409px; margin-left: 3px;"
+          >
+            <div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 16px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                <b>Slot</b>
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="582" y="425" fill="#000000" font-family="Helvetica" font-size="16px" text-anchor="middle">Slot</text>
+      </switch>
+    </g>
+    <rect x="22" y="882" width="1120" height="180" fill="#e6e6e6" stroke="rgb(0, 0, 0)" stroke-width="2" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 1118px; height: 1px; padding-top: 889px; margin-left: 23px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 16px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                <b>sub-managers</b>
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="582" y="905" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="16px" text-anchor="middle">sub-managers</text>
+      </switch>
+    </g>
+    <rect x="42" y="962" width="240" height="80" fill="#9999ff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 1002px; margin-left: 43px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div
+                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
+              >
+                sub-manager A
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="162" y="1006" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">sub-manager A</text>
+      </switch>
+    </g>
+    <rect x="322" y="962" width="240" height="80" fill="#9999ff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 1002px; margin-left: 323px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div
+                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
+              >
+                sub-manager B
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="442" y="1006" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">sub-manager B</text>
+      </switch>
+    </g>
+    <rect x="203" y="442" width="320" height="400" fill="#cccc00" stroke="rgb(0, 0, 0)" stroke-width="2" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 318px; height: 1px; padding-top: 449px; margin-left: 204px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div
+                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
+              >
+                approved modules stack
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="363" y="461" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">approved modules stack</text>
+      </switch>
+    </g>
+    <rect x="602" y="962" width="240" height="80" fill="#9999ff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 1002px; margin-left: 603px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div
+                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
+              >
+                sub-manager C
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="722" y="1006" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">sub-manager C</text>
+      </switch>
+    </g>
+    <rect x="882" y="962" width="240" height="80" fill="#9999ff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 1002px; margin-left: 883px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div
+                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
+              >
+                sub-manager D
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="1002" y="1006" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">sub-manager D</text>
+      </switch>
+    </g>
+    <rect x="642" y="442" width="320" height="400" fill="#fff2cc" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 318px; height: 1px; padding-top: 449px; margin-left: 643px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div
+                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
+              >
+                candidate modules stack
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="802" y="461" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">candidate modules stack</text>
+      </switch>
+    </g>
+    <path d="M 483 602 L 503 602 L 503 642 L 514.76 642" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 520.76 642 L 512.76 646 L 514.76 642 L 512.76 638 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <rect x="243" y="562" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 602px; margin-left: 244px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div
+                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
+              >
+                scene module A
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="363" y="606" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module A</text>
+      </switch>
+    </g>
+    <path d="M 363 723 L 363 650.24" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 363 644.24 L 367 652.24 L 363 650.24 L 359 652.24 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 683px; margin-left: 363px;">
+            <div data-drawio-colors="color: rgb(0, 0, 0); background-color: #FFD966; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div
+                style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; background-color: rgb(255, 217, 102); white-space: nowrap;"
+              >
+                previous module output
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="363" y="686" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">previous module output</text>
+      </switch>
+    </g>
+    <rect x="243" y="723" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 763px; margin-left: 244px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div
+                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
+              >
+                scene module B
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="363" y="767" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module B</text>
+      </switch>
+    </g>
+    <rect x="682" y="724" width="240" height="78" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 763px; margin-left: 683px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div
+                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
+              >
+                scene module D
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="802" y="767" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module D</text>
+      </switch>
+    </g>
+    <path d="M 203 642 L 222 642 L 222 762 L 233.76 762" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 239.76 762 L 231.76 766 L 233.76 762 L 231.76 758 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 152 642 L 194.76 642" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 200.76 642 L 192.76 646 L 194.76 642 L 192.76 638 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <rect x="32" y="622" width="120" height="40" fill="#003366" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 118px; height: 1px; padding-top: 642px; margin-left: 33px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div
+                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
+              >
+                previous slot
+                <br/>
+                output
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="92" y="646" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">previous slot...</text>
+      </switch>
+    </g>
+    <rect x="248" y="502" width="230" height="30" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 228px; height: 1px; padding-top: 517px; margin-left: 249px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div
+                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
+              >
+                The manager run all approved modules.
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="363" y="521" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">The manager run all approved modules.</text>
+      </switch>
+    </g>
+    <rect x="522" y="627" width="120" height="30" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 118px; height: 1px; padding-top: 642px; margin-left: 523px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div
+                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
+              >
+                Get approved modules' output.
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="582" y="646" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">Get approved modules...</text>
+      </switch>
+    </g>
+    <path
+      d="M 139.05 103 L 186.95 103 L 186.95 147.2 L 223.11 147.2 L 163 181 L 102.89 147.2 L 139.05 147.2 Z"
+      fill="#003366"
+      stroke="rgb(0, 0, 0)"
+      stroke-width="2"
+      stroke-miterlimit="10"
+      pointer-events="all"
+    />
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 142px; margin-left: 163px;">
+            <div data-drawio-colors="color: #FFFFFF; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 14px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: nowrap;">
+                Initial
+                <br/>
+                path
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="163" y="146" fill="#FFFFFF" font-family="Helvetica" font-size="14px" text-anchor="middle" font-weight="bold">Initial...</text>
+      </switch>
+    </g>
+    <path
+      d="M 282.47 246.76 L 282.53 236.76 L 302.53 236.88 L 302.59 226.38 L 321.5 242 L 302.41 257.38 L 302.47 246.88 Z"
+      fill="#003366"
+      stroke="rgb(0, 0, 0)"
+      stroke-miterlimit="10"
+      pointer-events="all"
+    />
+    <path
+      d="M 562.47 246.76 L 562.53 236.76 L 582.53 236.88 L 582.59 226.38 L 601.5 242 L 582.41 257.38 L 582.47 246.88 Z"
+      fill="#003366"
+      stroke="rgb(0, 0, 0)"
+      stroke-miterlimit="10"
+      pointer-events="all"
+    />
+    <path
+      d="M 842.47 246.76 L 842.53 236.76 L 862.53 236.88 L 862.59 226.38 L 881.5 242 L 862.41 257.38 L 862.47 246.88 Z"
+      fill="#003366"
+      stroke="rgb(0, 0, 0)"
+      stroke-miterlimit="10"
+      pointer-events="all"
+    />
+  </g>
+  <switch>
+    <g requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"/>
+    <a transform="translate(0,-5)" xlink:href="https://www.diagrams.net/doc/faq/svg-export-text-problems" target="_blank">
+      <text text-anchor="middle" font-size="10px" x="50%" y="100%">Text is not SVG - cannot display</text>
+    </a>
+  </switch>
+</svg>
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step1.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step1.svg
deleted file mode 100644
index c8bc12a070d0d..0000000000000
--- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step1.svg
+++ /dev/null
@@ -1,322 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
-<svg
-  xmlns="http://www.w3.org/2000/svg"
-  xmlns:xlink="http://www.w3.org/1999/xlink"
-  version="1.1"
-  width="1164px"
-  height="684px"
-  viewBox="-0.5 -0.5 1164 684"
-  style="background-color: rgb(255, 255, 255);"
->
-  <defs/>
-  <g>
-    <rect x="2" y="2" width="1160" height="680" fill="#cccccc" stroke="rgb(0, 0, 0)" stroke-width="4" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
-      <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div
-            xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 1158px; height: 1px; padding-top: 9px; margin-left: 3px;"
-          >
-            <div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-              <div style="display: inline-block; font-size: 16px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
-                <b>manager</b>
-              </div>
-            </div>
-          </div>
-        </foreignObject>
-        <text x="582" y="25" fill="#000000" font-family="Helvetica" font-size="16px" text-anchor="middle">manager</text>
-      </switch>
-    </g>
-    <rect x="22" y="482" width="1120" height="180" fill="#e6e6e6" stroke="rgb(0, 0, 0)" stroke-width="2" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
-      <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div
-            xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 1118px; height: 1px; padding-top: 489px; margin-left: 23px;"
-          >
-            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-              <div style="display: inline-block; font-size: 16px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
-                <b>sub-managers</b>
-              </div>
-            </div>
-          </div>
-        </foreignObject>
-        <text x="582" y="505" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="16px" text-anchor="middle">sub-managers</text>
-      </switch>
-    </g>
-    <rect x="42" y="562" width="240" height="80" fill="#9999ff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
-      <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div
-            xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 602px; margin-left: 43px;"
-          >
-            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-              <div
-                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
-              >
-                sub-manager A
-              </div>
-            </div>
-          </div>
-        </foreignObject>
-        <text x="162" y="606" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">sub-manager A</text>
-      </switch>
-    </g>
-    <rect x="322" y="562" width="240" height="80" fill="#9999ff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
-      <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div
-            xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 602px; margin-left: 323px;"
-          >
-            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-              <div
-                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
-              >
-                sub-manager B
-              </div>
-            </div>
-          </div>
-        </foreignObject>
-        <text x="442" y="606" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">sub-manager B</text>
-      </switch>
-    </g>
-    <rect x="203" y="42" width="320" height="400" fill="#cccc00" stroke="rgb(0, 0, 0)" stroke-width="2" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
-      <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div
-            xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 318px; height: 1px; padding-top: 49px; margin-left: 204px;"
-          >
-            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-              <div
-                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
-              >
-                approved modules stack
-              </div>
-            </div>
-          </div>
-        </foreignObject>
-        <text x="363" y="61" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">approved modules stack</text>
-      </switch>
-    </g>
-    <rect x="602" y="562" width="240" height="80" fill="#9999ff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
-      <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div
-            xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 602px; margin-left: 603px;"
-          >
-            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-              <div
-                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
-              >
-                sub-manager C
-              </div>
-            </div>
-          </div>
-        </foreignObject>
-        <text x="722" y="606" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">sub-manager C</text>
-      </switch>
-    </g>
-    <rect x="882" y="562" width="240" height="80" fill="#9999ff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
-      <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div
-            xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 602px; margin-left: 883px;"
-          >
-            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-              <div
-                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
-              >
-                sub-manager D
-              </div>
-            </div>
-          </div>
-        </foreignObject>
-        <text x="1002" y="606" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">sub-manager D</text>
-      </switch>
-    </g>
-    <rect x="642" y="42" width="320" height="400" fill="#fff2cc" stroke="rgb(0, 0, 0)" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
-      <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div
-            xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 318px; height: 1px; padding-top: 49px; margin-left: 643px;"
-          >
-            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-              <div
-                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
-              >
-                candidate modules stack
-              </div>
-            </div>
-          </div>
-        </foreignObject>
-        <text x="802" y="61" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">candidate modules stack</text>
-      </switch>
-    </g>
-    <path d="M 483 202 L 503 202 L 503 242 L 514.76 242" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 520.76 242 L 512.76 246 L 514.76 242 L 512.76 238 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
-    <rect x="243" y="162" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
-      <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div
-            xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 202px; margin-left: 244px;"
-          >
-            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-              <div
-                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
-              >
-                scene module A
-              </div>
-            </div>
-          </div>
-        </foreignObject>
-        <text x="363" y="206" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module A</text>
-      </switch>
-    </g>
-    <path d="M 363 323 L 363 250.24" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 363 244.24 L 367 252.24 L 363 250.24 L 359 252.24 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
-      <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 282px; margin-left: 363px;">
-            <div data-drawio-colors="color: rgb(0, 0, 0); background-color: #FFD966; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-              <div
-                style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; background-color: rgb(255, 217, 102); white-space: nowrap;"
-              >
-                previous module output
-              </div>
-            </div>
-          </div>
-        </foreignObject>
-        <text x="363" y="285" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">previous module output</text>
-      </switch>
-    </g>
-    <rect x="243" y="323" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
-      <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div
-            xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 363px; margin-left: 244px;"
-          >
-            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-              <div
-                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
-              >
-                scene module B
-              </div>
-            </div>
-          </div>
-        </foreignObject>
-        <text x="363" y="367" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module B</text>
-      </switch>
-    </g>
-    <rect x="682" y="324" width="240" height="78" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
-      <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div
-            xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 363px; margin-left: 683px;"
-          >
-            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-              <div
-                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
-              >
-                scene module D
-              </div>
-            </div>
-          </div>
-        </foreignObject>
-        <text x="802" y="367" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module D</text>
-      </switch>
-    </g>
-    <path d="M 203 242 L 222 242 L 222 362 L 233.76 362" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 239.76 362 L 231.76 366 L 233.76 362 L 231.76 358 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
-    <path d="M 162 242 L 194.76 242" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 200.76 242 L 192.76 246 L 194.76 242 L 192.76 238 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
-    <rect x="42" y="222" width="120" height="40" fill="#ff6666" stroke="rgb(0, 0, 0)" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
-      <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div
-            xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 118px; height: 1px; padding-top: 242px; margin-left: 43px;"
-          >
-            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-              <div
-                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
-              >
-                generate root reference path
-              </div>
-            </div>
-          </div>
-        </foreignObject>
-        <text x="102" y="246" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">generate root refere...</text>
-      </switch>
-    </g>
-    <rect x="248" y="102" width="230" height="30" fill="none" stroke="none" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
-      <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div
-            xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 228px; height: 1px; padding-top: 117px; margin-left: 249px;"
-          >
-            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-              <div
-                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
-              >
-                The manager run all approved modules.
-              </div>
-            </div>
-          </div>
-        </foreignObject>
-        <text x="363" y="121" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">The manager run all approved modules.</text>
-      </switch>
-    </g>
-    <rect x="522" y="227" width="120" height="30" fill="none" stroke="none" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
-      <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div
-            xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 118px; height: 1px; padding-top: 242px; margin-left: 523px;"
-          >
-            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-              <div
-                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
-              >
-                Get approved modules' output.
-              </div>
-            </div>
-          </div>
-        </foreignObject>
-        <text x="582" y="246" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">Get approved modules...</text>
-      </switch>
-    </g>
-    <rect x="32" y="32" width="600" height="420" fill="none" stroke="#cc0000" stroke-width="3" stroke-dasharray="9 9" pointer-events="all"/>
-  </g>
-  <switch>
-    <g requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"/>
-    <a transform="translate(0,-5)" xlink:href="https://www.diagrams.net/doc/faq/svg-export-text-problems" target="_blank">
-      <text text-anchor="middle" font-size="10px" x="50%" y="100%">Text is not SVG - cannot display</text>
-    </a>
-  </switch>
-</svg>
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step2.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step2.drawio.svg
similarity index 75%
rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step2.svg
rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step2.drawio.svg
index b387c73e3165f..b21b6a68f3fae 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step2.svg
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step2.drawio.svg
@@ -1,41 +1,42 @@
 <?xml version="1.0" encoding="UTF-8"?>
+<!-- Do not edit this file with editors other than diagrams.net -->
 <!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
 <svg
   xmlns="http://www.w3.org/2000/svg"
   xmlns:xlink="http://www.w3.org/1999/xlink"
   version="1.1"
-  width="1164px"
-  height="684px"
-  viewBox="-0.5 -0.5 1164 684"
-  style="background-color: rgb(255, 255, 255);"
+  width="1165px"
+  height="685px"
+  viewBox="-0.5 -0.5 1165 685"
+  content="&lt;mxfile host=&quot;Electron&quot; modified=&quot;2024-07-31T05:50:57.842Z&quot; agent=&quot;5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36&quot; etag=&quot;THv3C5M6q3_ZZiHmWhp5&quot; version=&quot;20.3.0&quot; type=&quot;device&quot;&gt;&lt;diagram id=&quot;FbG0muvkg23WBpYGoVLW&quot; name=&quot;page1&quot;&gt;7VpNd6M2FP0ts/CZVXJAgIBl4sTTRXvOTN2e6SwVkA0djKgQid1fXwnEl0Rsp4aQpsMikZ4+wPc+Pd0nWFjL3f4TRVn0CwlxsgBGuF9YdwsATMu1+T9hOVSWK+CbTmXa0jiU3VrDOv4bS6MhrUUc4rzXkRGSsDjrGwOSpjhgPRuilDz1u21I0r9rhrZYM6wDlOjWr3HIosrqAbe1/4TjbVTf2YR+1bJDdWf5S/IIheSpY7LuF9aSEsKq0m6/xImAr8alGrd6prV5MIpTds4AG4S/4s3aisIvXw6/ryJ/m2+ubFhN84iSQv7iBYAJn/D2gRe2orBO+CNKI5++scufxQ41VpQUaYjF7Qze/BTFDK8zFIjWJ+4f3BaxXcJrJi9uSMrWcqzovomTZEkSQsu5rGV5cfsjpizmfNwk8TblbYxk9ejKWUwo653RRnlxe84o+d4wZzdP3YVNIiluhPcdk4TxEyY7zOiBd5GtQMxdjpFu7ThQGp5aH+GuII1Rx0GgJ41IOua2mb7ljhckfS+h0j1NZV48XO1Qyp2W5rNQWpJyHqV96sBY1IE+ddCwh6gDA9SZ01HnadR1mOINN+MRYw4QsIvDUDSqjPn8Wq3GQt5WkG9A7iAP7AHgp8PdP4H7iAtiNtwt1ePnx90xNNxRllHyyIHlOycJi4Tv+QJ3FHwflwIF6dVqBY5sM2PgDwyzv1lAQ8ffGgo4tjEZAaZGQN/xl+/A8aHx9hwfnMD97h3g7nlvD3dLwz1LEM8YaCORaK2BQsSQxgIX75kobhK8vxFpBe+I01AW74IE5Xkc9MnoM8d711rGBVW9VjuiWkPjDQgnyD3ZshpJVLeEeIOK8uF1qaQ6QoIecHLLo+m2fKp6ipSkWJFetq6mV+V1zDtw2MuidN/oMO8MEF/bKE4Qix/7udeQN8g7fCYxf5LOXmcY10Z79QMvbLRfPWNOChpgOUk3dRqYV3FpNTQzRLeYaVOV3trgcIED25oDBygNY+6s+P1tmVCRim9iy3Q0VIXX10ARyiKyJSlK7lurEgLaPj8TgVWJ7p+YsYNcfKhgpM8F3sfsj075m5jq2pG1u30dXETl0EQa/nubQaLSGSWq7bCyVo97QcJVLZxjaMlzhWpZnCEFT0aQl4UGHpjRodMhEwszPxI5HFWluVA5Tjk5wjMMxcuqpxg3EOgHNnmA0zYIzJct3vhL+/52LNVsq3zMLyL0A5aM4seYFHkXfVKwrGBzBos2QHzrxofBYPGvpUO5E9z5EF4WJ7xz4wQcO05c5gsDJzbqMpzp8GDaZeh55szLEOpSSIN+pjRqVOihkkZx6MF50LveRNC7OvQ15G8gi7qG7olESqRRsD1b/pFIPSuHPHgskdKyn7MTKQ8qiZSWk02cSLn+rDvzC2T8qHJ8GpWt8wvUzM0/k98XC3YA1PBonBDs2ggfvIJgr4P2fyhvbCqvnzd61rl6cHSPvmhn9AYOGNvcIO+8QW+3yGcShVdRKs1OOIZSUd/sOa6n52qDr1TtqUSipyuVC4Hu4Cc3+xDlUTnc1BRF+bZ78BMEayLMoWEOYG54A6Bru/d4qDsa6r9FpTBvXnBkXOLxf3xN4lwE5+YY3qgUo4HSUPzVXgd+bFLra41JDhrr09Wno1ZnOodIrhOeQDD+FEcW0JB/9D2ovxbH4NgB147C8tCHJu4AydZkHOsHUCrHFAeYR9z2VOSjKOI9DgoWk7Ts8VdR8V/RXe72mP5vmPW11Ts7ryYEGrH3OmUfPnzQSKrTOg5uwpfo6WCaYRrzpxa01IM+tyZtUxvIytTXlzLWPpdvZSSPxe8ApaCxyhSwkk7A6DSXrfZIIdpSvxJr06YuyUMJvDMdyfq++IPkS1ayq31dMyHJvNp+L1olKO13t9b9Pw==&lt;/diagram&gt;&lt;/mxfile&gt;"
 >
   <defs/>
   <g>
-    <rect x="2" y="2" width="1160" height="680" fill="#cccccc" stroke="rgb(0, 0, 0)" stroke-width="4" pointer-events="all"/>
+    <rect x="1" y="1" width="1160" height="680" fill="#cccccc" stroke="rgb(0, 0, 0)" stroke-width="4" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 1158px; height: 1px; padding-top: 9px; margin-left: 3px;"
+            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 1158px; height: 1px; padding-top: 8px; margin-left: 2px;"
           >
             <div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div style="display: inline-block; font-size: 16px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
-                <b>manager</b>
+                <b>Slot</b>
               </div>
             </div>
           </div>
         </foreignObject>
-        <text x="582" y="25" fill="#000000" font-family="Helvetica" font-size="16px" text-anchor="middle">manager</text>
+        <text x="581" y="24" fill="#000000" font-family="Helvetica" font-size="16px" text-anchor="middle">Slot</text>
       </switch>
     </g>
-    <rect x="22" y="482" width="1120" height="180" fill="#cccc00" stroke="rgb(0, 0, 0)" stroke-width="2" pointer-events="all"/>
+    <rect x="21" y="481" width="1120" height="180" fill="#cccc00" stroke="rgb(0, 0, 0)" stroke-width="2" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 1118px; height: 1px; padding-top: 489px; margin-left: 23px;"
+            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 1118px; height: 1px; padding-top: 488px; margin-left: 22px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div style="display: inline-block; font-size: 16px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
@@ -44,16 +45,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="582" y="505" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="16px" text-anchor="middle">sub-managers</text>
+        <text x="581" y="504" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="16px" text-anchor="middle">sub-managers</text>
       </switch>
     </g>
-    <rect x="42" y="562" width="240" height="80" fill="#9999ff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="41" y="561" width="240" height="80" fill="#9999ff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 602px; margin-left: 43px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 601px; margin-left: 42px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -64,16 +65,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="162" y="606" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">sub-manager A</text>
+        <text x="161" y="605" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">sub-manager A</text>
       </switch>
     </g>
-    <rect x="322" y="562" width="240" height="80" fill="#9999ff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="321" y="561" width="240" height="80" fill="#9999ff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 602px; margin-left: 323px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 601px; margin-left: 322px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -84,16 +85,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="442" y="606" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">sub-manager B</text>
+        <text x="441" y="605" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">sub-manager B</text>
       </switch>
     </g>
-    <rect x="203" y="42" width="320" height="400" fill="#fff2cc" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="202" y="41" width="320" height="400" fill="#fff2cc" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 318px; height: 1px; padding-top: 49px; margin-left: 204px;"
+            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 318px; height: 1px; padding-top: 48px; margin-left: 203px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -104,16 +105,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="363" y="61" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">approved modules stack</text>
+        <text x="362" y="60" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">approved modules stack</text>
       </switch>
     </g>
-    <rect x="602" y="562" width="240" height="80" fill="#9999ff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="601" y="561" width="240" height="80" fill="#9999ff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 602px; margin-left: 603px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 601px; margin-left: 602px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -124,16 +125,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="722" y="606" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">sub-manager C</text>
+        <text x="721" y="605" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">sub-manager C</text>
       </switch>
     </g>
-    <rect x="882" y="562" width="240" height="80" fill="#9999ff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="881" y="561" width="240" height="80" fill="#9999ff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 602px; margin-left: 883px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 601px; margin-left: 882px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -144,14 +145,14 @@
             </div>
           </div>
         </foreignObject>
-        <text x="1002" y="606" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">sub-manager D</text>
+        <text x="1001" y="605" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">sub-manager D</text>
       </switch>
     </g>
-    <path d="M 261.5 463 L 342.5 463 L 342.5 503 L 379.5 503 L 302 541 L 224.5 503 L 261.5 503 Z" fill="#660033" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 260.5 462 L 341.5 462 L 341.5 502 L 378.5 502 L 301 540 L 223.5 502 L 260.5 502 Z" fill="#660033" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 502px; margin-left: 302px;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 501px; margin-left: 301px;">
             <div data-drawio-colors="color: #FFFFFF; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div style="display: inline-block; font-size: 14px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: nowrap;">
                 planner
@@ -161,16 +162,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="302" y="506" fill="#FFFFFF" font-family="Helvetica" font-size="14px" text-anchor="middle" font-weight="bold">planner...</text>
+        <text x="301" y="505" fill="#FFFFFF" font-family="Helvetica" font-size="14px" text-anchor="middle" font-weight="bold">planner...</text>
       </switch>
     </g>
-    <rect x="642" y="42" width="320" height="400" fill="#fff2cc" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="641" y="41" width="320" height="400" fill="#fff2cc" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 318px; height: 1px; padding-top: 49px; margin-left: 643px;"
+            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 318px; height: 1px; padding-top: 48px; margin-left: 642px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -181,18 +182,18 @@
             </div>
           </div>
         </foreignObject>
-        <text x="802" y="61" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">candidate modules stack</text>
+        <text x="801" y="60" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">candidate modules stack</text>
       </switch>
     </g>
-    <path d="M 483 202 L 503 202 L 503 242 L 514.76 242" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 520.76 242 L 512.76 246 L 514.76 242 L 512.76 238 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
-    <rect x="243" y="162" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <path d="M 482 201 L 502.06 201.06 L 502.06 241.06 L 513.76 241.02" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 519.76 241.01 L 511.78 245.03 L 513.76 241.02 L 511.75 237.03 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <rect x="242" y="161" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 202px; margin-left: 244px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 201px; margin-left: 243px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -203,15 +204,15 @@
             </div>
           </div>
         </foreignObject>
-        <text x="363" y="206" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module A</text>
+        <text x="362" y="205" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module A</text>
       </switch>
     </g>
-    <path d="M 363 323 L 363 250.24" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 363 244.24 L 367 252.24 L 363 250.24 L 359 252.24 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 362 322 L 362 249.24" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 362 243.24 L 366 251.24 L 362 249.24 L 358 251.24 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 282px; margin-left: 363px;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 282px; margin-left: 362px;">
             <div data-drawio-colors="color: rgb(0, 0, 0); background-color: #FFD966; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
                 style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; background-color: rgb(255, 217, 102); white-space: nowrap;"
@@ -221,16 +222,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="363" y="285" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">previous module output</text>
+        <text x="362" y="285" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">previous module output</text>
       </switch>
     </g>
-    <rect x="243" y="323" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="242" y="322" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 363px; margin-left: 244px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 362px; margin-left: 243px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -241,16 +242,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="363" y="367" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module B</text>
+        <text x="362" y="366" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module B</text>
       </switch>
     </g>
-    <rect x="682" y="324" width="240" height="78" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="681" y="323" width="240" height="78" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 363px; margin-left: 683px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 362px; margin-left: 682px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -261,11 +262,11 @@
             </div>
           </div>
         </foreignObject>
-        <text x="802" y="367" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module D</text>
+        <text x="801" y="366" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module D</text>
       </switch>
     </g>
     <path
-      d="M 902.5 541 L 821.5 541 L 821.5 503.01 L 784.5 503.01 L 862 463 L 939.5 503.01 L 902.5 503.01 Z"
+      d="M 901.5 540 L 820.5 540 L 820.5 502.01 L 783.5 502.01 L 861 462 L 938.5 502.01 L 901.5 502.01 Z"
       fill="#003366"
       stroke="rgb(0, 0, 0)"
       stroke-width="2"
@@ -275,7 +276,7 @@
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 502px; margin-left: 862px;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 501px; margin-left: 861px;">
             <div data-drawio-colors="color: #FFFFFF; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div style="display: inline-block; font-size: 14px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: nowrap;">
                 module
@@ -285,41 +286,43 @@
             </div>
           </div>
         </foreignObject>
-        <text x="862" y="506" fill="#FFFFFF" font-family="Helvetica" font-size="14px" text-anchor="middle" font-weight="bold">module...</text>
+        <text x="861" y="505" fill="#FFFFFF" font-family="Helvetica" font-size="14px" text-anchor="middle" font-weight="bold">module...</text>
       </switch>
     </g>
-    <path d="M 203 242 L 222 242 L 222 362 L 233.76 362" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 239.76 362 L 231.76 366 L 233.76 362 L 231.76 358 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
-    <path d="M 162 242 L 194.76 242" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 200.76 242 L 192.76 246 L 194.76 242 L 192.76 238 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
-    <rect x="42" y="222" width="120" height="40" fill="#ff6666" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <path d="M 202 241 L 221 241.06 L 221 361.06 L 232.76 361.02" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 238.76 361.01 L 230.78 365.03 L 232.76 361.02 L 230.75 357.03 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 161 241 L 193.76 241" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 199.76 241 L 191.76 245 L 193.76 241 L 191.76 237 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <rect x="41" y="221" width="120" height="40" fill="#003366" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 118px; height: 1px; padding-top: 242px; margin-left: 43px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 118px; height: 1px; padding-top: 241px; margin-left: 42px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
                 style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
               >
-                generate root reference path
+                previous slot
+                <br/>
+                output
               </div>
             </div>
           </div>
         </foreignObject>
-        <text x="102" y="246" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">generate root refere...</text>
+        <text x="101" y="245" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">previous slot...</text>
       </switch>
     </g>
-    <rect x="42" y="452" width="1080" height="100" fill="none" stroke="#cc0000" stroke-width="3" stroke-dasharray="9 9" pointer-events="all"/>
-    <rect x="54.5" y="502" width="170" height="30" fill="none" stroke="none" pointer-events="all"/>
+    <rect x="41" y="451" width="1080" height="100" fill="none" stroke="#cc0000" stroke-width="3" stroke-dasharray="9 9" pointer-events="all"/>
+    <rect x="53.5" y="501" width="170" height="30" fill="none" stroke="none" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 168px; height: 1px; padding-top: 517px; margin-left: 56px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 168px; height: 1px; padding-top: 516px; margin-left: 55px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -330,16 +333,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="140" y="521" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">The manager pass latest plan...</text>
+        <text x="139" y="520" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">The manager pass latest plan...</text>
       </switch>
     </g>
-    <rect x="942" y="502" width="170" height="30" fill="none" stroke="none" pointer-events="all"/>
+    <rect x="941" y="501" width="170" height="30" fill="none" stroke="none" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 168px; height: 1px; padding-top: 517px; margin-left: 943px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 168px; height: 1px; padding-top: 516px; margin-left: 942px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -350,16 +353,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="1027" y="521" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">The manager receives module'...</text>
+        <text x="1026" y="520" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">The manager receives module'...</text>
       </switch>
     </g>
-    <path d="M 1002 542 L 1142 542 L 1142 572 L 1082.2 572 L 1052.4 592 L 1062.2 572 L 1002 572 Z" fill="#990000" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 1001 541 L 1141 541 L 1141 571 L 1081.2 571 L 1051.4 591 L 1061.2 571 L 1001 571 Z" fill="#990000" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 138px; height: 1px; padding-top: 557px; margin-left: 1003px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 138px; height: 1px; padding-top: 556px; margin-left: 1002px;"
           >
             <div data-drawio-colors="color: #FFFFFF; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -370,16 +373,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="1072" y="561" fill="#FFFFFF" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">Execution request!!!</text>
+        <text x="1071" y="560" fill="#FFFFFF" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">Execution request!!!</text>
       </switch>
     </g>
-    <path d="M 722 542 L 862 542 L 862 572 L 802.2 572 L 772.4 592 L 782.2 572 L 722 572 Z" fill="#990000" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 721 541 L 861 541 L 861 571 L 801.2 571 L 771.4 591 L 781.2 571 L 721 571 Z" fill="#990000" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 138px; height: 1px; padding-top: 557px; margin-left: 723px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 138px; height: 1px; padding-top: 556px; margin-left: 722px;"
           >
             <div data-drawio-colors="color: #FFFFFF; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -390,7 +393,7 @@
             </div>
           </div>
         </foreignObject>
-        <text x="792" y="561" fill="#FFFFFF" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">Execution request!!!</text>
+        <text x="791" y="560" fill="#FFFFFF" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">Execution request!!!</text>
       </switch>
     </g>
   </g>
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step4.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step4.drawio.svg
similarity index 90%
rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step4.svg
rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step4.drawio.svg
index 06536181e6334..880bb4449ec77 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step4.svg
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step4.drawio.svg
@@ -1,4 +1,5 @@
 <?xml version="1.0" encoding="UTF-8"?>
+<!-- Do not edit this file with editors other than diagrams.net -->
 <!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
 <svg
   xmlns="http://www.w3.org/2000/svg"
@@ -7,7 +8,7 @@
   width="1164px"
   height="684px"
   viewBox="-0.5 -0.5 1164 684"
-  style="background-color: rgb(255, 255, 255);"
+  content="&lt;mxfile host=&quot;Electron&quot; modified=&quot;2024-07-31T06:00:16.866Z&quot; agent=&quot;5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36&quot; etag=&quot;nhwcjOulp_enDhUtfPq8&quot; version=&quot;20.3.0&quot; type=&quot;device&quot;&gt;&lt;diagram id=&quot;FbG0muvkg23WBpYGoVLW&quot; name=&quot;ページ7&quot;&gt;7VpZk6M2EP41fpwtEIfN4/jaPEyqduNKJfsoGxmUxYgSwmPvr4+ExSXhqwyDJxXmYVDrMHxfd6u70cia7Q5fKUzC34mPohEw/MPImo8AeBlbNv8nJEcpscyxdxIFFPsnoVkJVvgXkkJDSjPso7QxkBESMZw0hRsSx2jDGjJIKXlvDtuSqPmrCQyQJlhtYKRL/8I+C0/SCRhX8t8QDsLil01Xvt8OFoPlm6Qh9Ml7TWQtRtaMEsJOd7vDDEUCvgKX07zlmd7ywSiK2S0TbOD/gbYrK/S/fz/+uQy9IN2+mMWL7GGUyVceATfiK07X/CYQN6uIP6MU8vVLuXwvdizAoiSLfSR+z+Dd7yFmaJXAjeh95wrCZSHbRbxl8tstidlKzhXDtziKZiQiNF/LmuUXl+8RZZgT8hrhIOZ9jCTF7JO2mK5s12Yb+cXlKaPkZ0mdXT51HTcJpfghdKiJJI5fEdkhRo98iOy1rIJUqdf2xJCC90pJuC5IYVjTEHcihVBqZlAuX5HHbyR/93Dpmi1cPkRRjZKYxHzQ1IdpmE83S3AbpLXCbnUEu2d7CuyWDrtlmzrqNugNdTC5bkFptn7ZwZg7C5p+uCUtXPF3syU1qQNdWYzdtBgHTNosBrRYjNmfxQBP465GFe947Y4Zs4WBHfZ90alS5vFruewKeleBvhTUoAd2C/L9AV/Y7VngOzSJwYB3VZ1/BuD1PQImCSV7jiwPWoifRTzcEsDDzc9uOVCgXi6X4MIG3wUBTjGn3C/slv2izefYRn8MgCuqP/sPqL4HnlD1rSvAzz8/8DzOeULgHQ34DYx97EOGPtjpiJwiD1DbnU4/sY/nqtnCM7ghVwMa+Tz1lU1CWUgCEsNoUUmnTSqqMW9EwJcD/g9i7CjjSZgx0qQHHTD7u3b/Qyz1xZGt+UGunDeORSPmL1xOEo3aLNGspuWtYt4dZKYkoxt0ES6ZJzNIA8QujpRrCjQvqgdFEWR43yw0tBGdT32lFB5rAxKCY5bWVv4mBDWtmyibn2MYSoHg+gzbUBTt9BSV2pWv84gm6iWIdIPiyjUMF4e/ejN7Me0qGnGVaMR7AtesJ68JRXtMsrQOP8lYkrEhHUblJH7UfUSrw9ANX+U/gmsUTflmE+QP2IhK557rPugrvJt9xbhrX/GgOrTkw6opDpSZ9WqKjlksMpwp2npmpmE/UGogrjxCVaqsy/zqK26yDUONm1pqRh/NUksSobI0UB7xJCyVccaQLOkZx7Ow1KUf07E3AbgN+/GkL+ydQROLIhi4JbHoNkHoJ+zXo3RHLeeWn5mKRU5xh5xX0Xl3BuGo9UslH7hhRvFs/WYQznhIlbsnl/2cKqcWMEpFuKZy3TGs5yla2fpynvImQv4mgVC6ar4zMEQv+XCKUvwLrssvbNJ0+OLOdOTM78owpsB1z5T7Lmu3POIgH2NUHiy4fb94Mb6A8dhqcCn3iwdVxHSbi4LmAmS7TVFPmuF9Ftsv61hlY4A6VmHJQ9SxHjzdoH8yrJUq0toZlTUtvquf8QcfEugZhmVdriQ88g3XAHqQ3fr1XPXUHRKiB9lvMIs3oYZ3GsJE3HLUIk7JdcwTRDF/SuGUi0nfKpFGSUvBR/3EIs+lnEuNEpJihkkMcou08nMQJ9sHRq0777U7Os5im46a37YUBCctnDr9cer+z2m3nHotNYuOOOXN6vjgaUOtjmFai38B&lt;/diagram&gt;&lt;/mxfile&gt;"
 >
   <defs/>
   <g>
@@ -21,12 +22,12 @@
           >
             <div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div style="display: inline-block; font-size: 16px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
-                <b>manager</b>
+                <b>Slot</b>
               </div>
             </div>
           </div>
         </foreignObject>
-        <text x="582" y="25" fill="#000000" font-family="Helvetica" font-size="16px" text-anchor="middle">manager</text>
+        <text x="582" y="25" fill="#000000" font-family="Helvetica" font-size="16px" text-anchor="middle">Slot</text>
       </switch>
     </g>
     <rect x="631" y="32" width="341" height="420" fill="none" stroke="#cc0000" stroke-width="3" stroke-dasharray="9 9" pointer-events="all"/>
@@ -310,7 +311,7 @@
     </g>
     <path d="M 162 242 L 194.76 242" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
     <path d="M 200.76 242 L 192.76 246 L 194.76 242 L 192.76 238 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
-    <rect x="42" y="222" width="120" height="40" fill="#ff6666" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="42" y="222" width="120" height="40" fill="#003366" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
@@ -322,12 +323,14 @@
               <div
                 style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
               >
-                generate root reference path
+                previous slot
+                <br/>
+                output
               </div>
             </div>
           </div>
         </foreignObject>
-        <text x="102" y="246" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">generate root refere...</text>
+        <text x="102" y="246" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">previous slot...</text>
       </switch>
     </g>
     <path d="M 832 62 L 912 62 L 912 92 L 886.4 92 L 860.8 112 L 866.4 92 L 832 92 Z" fill="#990000" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step5.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step5.drawio.svg
similarity index 90%
rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step5.svg
rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step5.drawio.svg
index 1d886e47d9ad4..997814acd9fcc 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step5.svg
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step5.drawio.svg
@@ -1,13 +1,14 @@
 <?xml version="1.0" encoding="UTF-8"?>
+<!-- Do not edit this file with editors other than diagrams.net -->
 <!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
 <svg
   xmlns="http://www.w3.org/2000/svg"
   xmlns:xlink="http://www.w3.org/1999/xlink"
   version="1.1"
-  width="1171px"
+  width="1172px"
   height="684px"
-  viewBox="-0.5 -0.5 1171 684"
-  style="background-color: rgb(255, 255, 255);"
+  viewBox="-0.5 -0.5 1172 684"
+  content="&lt;mxfile host=&quot;Electron&quot; modified=&quot;2024-07-31T06:03:43.260Z&quot; agent=&quot;5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36&quot; etag=&quot;txIZmv6wBq4-WE_aLvOP&quot; version=&quot;20.3.0&quot; type=&quot;device&quot;&gt;&lt;diagram id=&quot;FbG0muvkg23WBpYGoVLW&quot; name=&quot;ページ7&quot;&gt;7VtNc6M4EP01PiYFAmQ4xnY8c5itmlnX1u4cFSMbdjCihJzY8+tXAvEl4a8xGGc2ziGoRcvw3qOlbuSRNd3sPlGUBH8QH0cjYPi7kTUbAWC6tsH/Ccs+tzyY0AW5aU1DX55WGRbhTyyN0nO9DX2cNk5khEQsTJrGJYljvGQNG6KUvDVPW5Go+a0JWmPNsFiiSLf+HfosyK0uGFf2zzhcB8U3m9DLezaoOFneSRogn7zVTNbzyJpSQlh+tNlNcSTgK3DJ/eYHessLozhm5zjYwP8TrxZW4H/7tv9rHnjrdMX5kDfyiqKtvOURgBEfcfLCD9biYBHxa5RGPn5pl/fF9gVYlGxjH4vvM3j3WxAyvEjQUvS+cYFwW8A2EW+Z/HBFYraQvuL0VRhFUxIRmo1lTbMPt79iykJOyFMUrmPex0hSeOdqMaFs17yN7MPtKaPkR0mdXV51HTcJpfgivKuZJI6fMNlgRvf8FNlrWQWpUteOA6XhrRIJ14I0BjWFQFcakVTmuhy+Io8fSP4u4tI9zWW6fXnYoJjLlqY35/QZir+zOW1yB7rizm5yBw27jTvQwp3ZI3eexl2NKt7x1B0zZgsDm9D3RadKmcc/83lX0EMF+hLlGvTAbkG+P+DHxgngO3wkBgMeqpq/B+BNDXiUJJS8cmT59En8bcQnfgE8Wv7olgMF6vl8Do5MNV0Q4BQ+xYQBDZ0Aqy3m2EZfDACjbe6/CucarjGJhap9lAaZu1kG9MYk3zpNWx2h7lrqNO3pqEOrDXXQn+7BiYAz/Q0CjgfuMOBYJ4CfvX/guW7vEHhHA36JYj/0EcM3DvUip8gCTnuo72fF6SnLnnsI/uYYakBjn6e+skkoC8iaxCh6rqyTJhXVOV+IgC8D/F/M2F6u4tGWkSY9eBeyf2rH38VQj45szXZy5KyxLxoxv+HSSTRqXqJZuWWtwu8CMlOypUt8FC45VzJE15gdPVOOKdA8Kg+KI8TC12ahoY3ozPWJUrSvnZCQMGZpbeSvwlCf/NQlxxgqBYKTHq5hKELLr6KSXXk71yhRX4akSxxXoWG47OfJm9rPk67WgFAl5A5Cs14ySCh+Dck2rcNPtizZsiEDRhUkvtdjRGvA0B98lf8IveBowiebdXaBjVxg5kF4Zazwzo4V465jxZVyaKlCqI/iQPlwv4+i65pDP4qung9r2A+UGnSJvW0Y6mqopf52a+xbUgMV+9mD+TuiP26pXN8afT0/aEH/2trEXaLvFq/DTqFfTNTdo+9Zg87rv5IIlI0hEgHn3Mm9CCpDJwLeWBWdcSIROOXRUyLg2R9K7EeJzp0o0b1YiaqHJ6+qZyU6H0rsR4l3UhzpQInFi/yelThome4SJXarqH50otPqqLVZT33zk4ta+lV8Xyw5x75UcpqHB24iufGQkrukMvw+Jae9DnDVKv8ByXXHsF710169H6/6fREFtCaBSCZSPHNjmB7LsChOw5/opdwlJB8dPrgzGTmzi+p1EwDhgZdnx9UtNwzKyxiV2/TOz+YejEcwLl4qSi5lNnelREzYHBQ0ByCrVYp7Uob3Xp79e1j4lC9bh3grdOXeD33bU0vhX24epMXuwAMR4SaFGMOwrOOV+St2ojljVy+Dte4BtPsqgwEDaJQcisCx/yT2NotoG6E0DZdNiJt8dDOznh+S4fhp4h3dz3B6Mj47T6jR5bSwVdiuXiZqG349RQf5PWlztjaUbR9KJG41/QPD/r8I7U7kY4NDa/8O5HPr1SMw9HL9h3xuKx+eCD46XQmoZbDeJQQ1CX3msOFUXGhCQ0JDttfklAYoEYd8sRBxxZ1eaiSYhvxaRTZSOH2tTJpUWsSk7tST21WVX5vMs0+WyaQhC0kMMula2Y8Y8kUvMGrdWa/d0S5X21R3ucKWfSWtaxnn8rUMb1Y/Wcr1UP30y3r+Dw==&lt;/diagram&gt;&lt;/mxfile&gt;"
 >
   <defs/>
   <g>
@@ -21,12 +22,12 @@
           >
             <div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div style="display: inline-block; font-size: 16px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
-                <b>manager</b>
+                <b>Slot</b>
               </div>
             </div>
           </div>
         </foreignObject>
-        <text x="582" y="25" fill="#000000" font-family="Helvetica" font-size="16px" text-anchor="middle">manager</text>
+        <text x="582" y="25" fill="#000000" font-family="Helvetica" font-size="16px" text-anchor="middle">Slot</text>
       </switch>
     </g>
     <rect x="22" y="482" width="1120" height="180" fill="#e6e6e6" stroke="rgb(0, 0, 0)" stroke-width="2" pointer-events="all"/>
@@ -195,7 +196,7 @@
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 282px; margin-left: 363px;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 283px; margin-left: 363px;">
             <div data-drawio-colors="color: rgb(0, 0, 0); background-color: #FFD966; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
                 style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; background-color: rgb(255, 217, 102); white-space: nowrap;"
@@ -205,7 +206,7 @@
             </div>
           </div>
         </foreignObject>
-        <text x="363" y="285" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">previous module output</text>
+        <text x="363" y="286" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">previous module output</text>
       </switch>
     </g>
     <rect x="243" y="323" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
@@ -301,7 +302,7 @@
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 263px; margin-left: 583px;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 263px; margin-left: 584px;">
             <div data-drawio-colors="color: rgb(0, 0, 0); background-color: #B266FF; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
                 style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; background-color: rgb(178, 102, 255); white-space: nowrap;"
@@ -311,12 +312,12 @@
             </div>
           </div>
         </foreignObject>
-        <text x="583" y="266" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">approved module output</text>
+        <text x="584" y="266" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">approved module output</text>
       </switch>
     </g>
     <path d="M 162 242 L 194.76 242" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
     <path d="M 200.76 242 L 192.76 246 L 194.76 242 L 192.76 238 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
-    <rect x="42" y="222" width="120" height="40" fill="#ff6666" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="42" y="222" width="120" height="40" fill="#003366" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
@@ -328,12 +329,14 @@
               <div
                 style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
               >
-                generate root reference path
+                previous module
+                <br/>
+                output
               </div>
             </div>
           </div>
         </foreignObject>
-        <text x="102" y="246" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">generate root refere...</text>
+        <text x="102" y="246" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">previous module...</text>
       </switch>
     </g>
     <path d="M 922 122 L 1153.76 122" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step6.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step6.drawio.svg
similarity index 90%
rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step6.svg
rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step6.drawio.svg
index cf0001737acd1..ad0fe7b843342 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step6.svg
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step6.drawio.svg
@@ -1,13 +1,14 @@
 <?xml version="1.0" encoding="UTF-8"?>
+<!-- Do not edit this file with editors other than diagrams.net -->
 <!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
 <svg
   xmlns="http://www.w3.org/2000/svg"
   xmlns:xlink="http://www.w3.org/1999/xlink"
   version="1.1"
-  width="1164px"
-  height="684px"
-  viewBox="-0.5 -0.5 1164 684"
-  style="background-color: rgb(255, 255, 255);"
+  width="1165px"
+  height="685px"
+  viewBox="-0.5 -0.5 1165 685"
+  content="&lt;mxfile host=&quot;Electron&quot; modified=&quot;2024-07-31T06:07:49.300Z&quot; agent=&quot;5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36&quot; etag=&quot;Zgan5-_MrXWK2yMkZlfY&quot; version=&quot;20.3.0&quot; type=&quot;device&quot;&gt;&lt;diagram id=&quot;FbG0muvkg23WBpYGoVLW&quot; name=&quot;ページ7&quot;&gt;3VrLkqM2FP0aL7sLEMawdPsxWWSqMnGlkl6qQTZkMCJCuO18fSSQQCD86DE0nuCFpasHcM7R414xAYv98QuBafgVByieWEZwnIDlxLKeTNMB7J+bTsJkT71ZadqRKCiNZm3YRP8iYTSENY8ClDUqUoxjGqVNo4+TBPm0YYOE4PdmtS2Om3dN4Q5pho0PY936ZxTQsLS61qy2/4KiXSjvbDpeWbKHsrJ4kyyEAX5XTGA1AQuCMS1T++MCxRw/iUvZbn2mtHowghJ6SwPbCn5H2w0Ig2/fTn+sQ2+XbZ8swy37OcA4F688sZyY9fjyxhI7ntjE7BmFkfVf2cV70ZMEi+A8CRC/n8GK38OIok0KfV76zhTCbCHdxyxnsuQWJ3Qj2vLq2yiOFzjGpOgLLIqL2Q+I0IgRMo+jXcLKKE5l61ItpiPySmujuJg9owR/r6izq6dWcRNQ8huho2ISOH5BeI8oObEqohQASarQtVMZ3muRMC0IY6goxHGFEQpl7qrua/JYQvD3IS6961xm+dvTHiZMtiT7dE5XDv/dzGmTO6sv7uwWd67RxZ3VwZ05HHdyvlO4U6hiBfP+mDE7GNhHQcAL25R57Fqv+4LeaUPv6tBbdgfyAwJvXgG+xyExGvCOpvkHAN7SgIdpSvCBIcuWTxzkMVv4OfDQ/94vBy2o1+u1dWGp6YOAqWxTLRiOTgDomnNsYzgG7CvSX/wPpO+1l+pHkP70CvDLnx942zIeEHhHA96HSRAFkKJPnnT47rbYoXZPOsPsfbz2AvwQ09BMAxoFzAkTWUxoiHc4gfGqtr40qajr/Io5fAXgfyNKT2I/CXOKm/SgY0T/UtKvvKvnqcgtj6LnInOSmYS9cNWIZ5RWPFs3K3Ky3QfIzHBOfHQJLiA0TCHZIXoRWLHCcjQvyoOgGNLo0HR5u4gums4JgSelQoqjhGZKz79xg6I6t7342XbLVb3aYuoYLaGVT1HLrnqdO5QIZtr0kBJ0iHCeVbMDS+CcpjkdU7S1UF9VnV4WraLTV1XDZ0Tbnr9i+IbiFzYt7orXaOyflp7zQYftus5N91adyxHRn87vnND0mErmowSpGhrJnZt7C3v10tem1mmP0o4oyGev8HoM5Ocfwvq4+oHBecdQlLTesOS4jzUUrY6wSnsojuTgDzsUHdsceyhaHZGVNvYjeZhVIFIfWaVFqftS/PQQs2vwX0/OktF2luyOoORn86cHaDT+lk/mOAz2OXp09B9hIQMa+ncirQCY4IRjGsAsLJqbXcLn/mnHGQroa8oy277oVAd95naAbnrDaV5HvUPz9zLxkJp3bOs2zc/cwdAfNRIgt123RAL69XSG8dN1t3raDv9oh5HlDk+0q+n8sMs/bR84tBz4G1rIZxvY5TfGlNwPBZ+qzBjBJ/kNxgjBp/vmFqDvRjscQ3FaTuRx+Bk/8VOme8MA4LLnds/RazXe1FPv7sjvYJTo0fhHcRD4VZyEtDb96+Iayml7gE0/0GNXX/GBM1EfztaDROHF+SfnXzIVgD1lxcQ3ZxVMKz0WcMlyObKunbVkIUx5chuj45x/yFVMbIFILv0YZlnkN9luSkM73BLbWdZLtZe1yrz86gQ8A769lfDb7u2hFrGlPqcX9dsWflMoBMj0ThG5JKirE6gilmmHVqTt3s2DN2u7qMB7nja7KRcQbfvQEeAH1zs7sxe5vvSzbP1ZXVm9/j4RrP4D&lt;/diagram&gt;&lt;/mxfile&gt;"
 >
   <defs/>
   <g>
@@ -21,12 +22,12 @@
           >
             <div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div style="display: inline-block; font-size: 16px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
-                <b>manager</b>
+                <b>Slot</b>
               </div>
             </div>
           </div>
         </foreignObject>
-        <text x="582" y="25" fill="#000000" font-family="Helvetica" font-size="16px" text-anchor="middle">manager</text>
+        <text x="582" y="25" fill="#000000" font-family="Helvetica" font-size="16px" text-anchor="middle">Slot</text>
       </switch>
     </g>
     <rect x="22" y="482" width="1120" height="180" fill="#e6e6e6" stroke="rgb(0, 0, 0)" stroke-width="2" pointer-events="all"/>
@@ -310,7 +311,7 @@
     <path d="M 239.76 362 L 231.76 366 L 233.76 362 L 231.76 358 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <path d="M 162 242 L 194.76 242" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
     <path d="M 200.76 242 L 192.76 246 L 194.76 242 L 192.76 238 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
-    <rect x="42" y="222" width="120" height="40" fill="#ff6666" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="42" y="222" width="120" height="40" fill="#003366" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
@@ -322,12 +323,14 @@
               <div
                 style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
               >
-                generate root reference path
+                previous module
+                <br/>
+                output
               </div>
             </div>
           </div>
         </foreignObject>
-        <text x="102" y="246" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">generate root refere...</text>
+        <text x="102" y="246" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">previous module...</text>
       </switch>
     </g>
     <rect x="243" y="82" width="240" height="80" fill="#3333ff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step1.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step1.drawio.svg
similarity index 78%
rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step1.svg
rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step1.drawio.svg
index a3e3489f0cb7f..5243deb6375cb 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step1.svg
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step1.drawio.svg
@@ -1,38 +1,39 @@
 <?xml version="1.0" encoding="UTF-8"?>
+<!-- Do not edit this file with editors other than diagrams.net -->
 <!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
 <svg
   xmlns="http://www.w3.org/2000/svg"
   xmlns:xlink="http://www.w3.org/1999/xlink"
   version="1.1"
   width="1164px"
-  height="444px"
-  viewBox="-0.5 -0.5 1164 444"
-  style="background-color: rgb(255, 255, 255);"
+  height="445px"
+  viewBox="-0.5 -0.5 1164 445"
+  content="&lt;mxfile host=&quot;Electron&quot; modified=&quot;2024-07-31T06:13:14.342Z&quot; agent=&quot;5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36&quot; etag=&quot;dxoMVGSIqdgdq3a1mdpv&quot; version=&quot;20.3.0&quot; type=&quot;device&quot;&gt;&lt;diagram id=&quot;FbG0muvkg23WBpYGoVLW&quot; name=&quot;ページ7&quot;&gt;7Zpdc5s6EIZ/Sy58mQxIgJ3LxLHbmTOd6dSdaW8VkI1OZcQRInb667sCYT5E4rSF2CcTchGx+kDs8+Jd1p7g+Xb/QZI0/iQiyifIifYTfDdByHV8H/5py2NpufRQcF2aNpJFZlhtWLGftJprrDmLaNYaqITgiqVtYyiShIaqZSNSil172Frw9lVTsqGWYRUSblu/sUjFpXWGprX9I2WbuLqyW93fllSDzZ1kMYnErmHCiwmeSyFU2dru55Rr91V+Kectn+g9bEzSRL1kwke+3O3/3effnfXXf758i8VKpZfI7PaB8Nzc8QQFHBa8vYfGRjdWHLZojLD8wW5uSz1WvpIiTyKqL+dA9y5miq5SEureHegDbLHacjhzobkWiVqZuXr4mnE+F1zIYi08Lw6wP1CpGPC44WyTQJ8SaTW7FIsbmPPGbKc4wJ4pKX4cyHmHXTfdZjypL0T3DZNx4wcqtlTJRxhiepFeu5hjZB3gCvKu1ghIwRjjhkA8zxiJEebmsHzNDhoG32+gxM5xlFl+f7klCYhWZidBWkB5GdI2OjQUOtRB5wd96FAPOnc2GjrXQtcgBR03w4FxewBsWRTpzi6xaziWy6E873U8H3i255HX4/jx/I6O+H3AB+KP/D7OM4C7z8AZkMBHSMzfJImgG0jOgIR3hMTdmyQxm50fCTs3k/S/nGZKp5ciyjkkxoOy6Lh8Eei/JwP2IB9FVibVE45nfW4fLxp7djRe7GmYKyYSMBsGFxcXlvMhxU91E1zFRa6OA0ipZLBpKutJn2uTxceWvvWQmLy3kw8vi0NfUGRM3wfSArnCRZ5V5lzIaXQXvR4eCLJnRf6+dLkPsj8aYzvyvzP+G8ZTK6c4PWM7p3hn/BeMsfXaewaMe7KVkCb0ECFPl8TfXM+9xe3rJfHeyRMWzz8O40R5/MgwrDz+DGAEx2GcKJUfGYaVyp8BjJkFg5M8CeMnQ8+a0/2Nrl+DU2gSmeZdyEmWsbDNoQ0NRlcenaLyvCqroatgqrGa7llPkc5xMA7q8lvVE9E1yYtyYU/s6siAk3vKb0n4Y1NsrFoiEQntlPm8ZyLck0qgUatib+ugQdl/JhZJyoliD+06fx95c4XPgiWqmdXi6yunPtxOaHQ7WspELkNqFkGNMn3Puu2lfNxZShG5ocpaqlDmwQ9/LtbKQe9ifTtinbrjiBXWPbFY7Zf1d7H+v8Wq3zPGEKv9/vLqYrWrDiRNpXgAiTVKeqBaYDxsbtYRIygBPfP16hBpmPVNW19pD01ftbTnv+AN5UTftR2S4kF8j/2278dMgeG0/hVD+ajUvwbBi18=&lt;/diagram&gt;&lt;/mxfile&gt;"
 >
   <defs/>
   <g>
     <rect x="2" y="2" width="1160" height="440" fill="#cccccc" stroke="rgb(0, 0, 0)" stroke-width="4" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
+    <g transform="translate(-0.5 -0.5)scale(0.9999999999999999)">
       <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+        <foreignObject pointer-events="none" width="101%" height="101%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
             style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 1158px; height: 1px; padding-top: 9px; margin-left: 3px;"
           >
             <div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div style="display: inline-block; font-size: 16px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
-                <b>manager</b>
+                <b>Slot</b>
               </div>
             </div>
           </div>
         </foreignObject>
-        <text x="582" y="25" fill="#000000" font-family="Helvetica" font-size="16px" text-anchor="middle">manager</text>
+        <text x="582" y="25" fill="#000000" font-family="Helvetica" font-size="16px" text-anchor="middle">Slot</text>
       </switch>
     </g>
     <rect x="22" y="242" width="1120" height="180" fill="#cccc00" stroke="rgb(0, 0, 0)" stroke-width="2" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
+    <g transform="translate(-0.5 -0.5)scale(0.9999999999999999)">
       <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+        <foreignObject pointer-events="none" width="101%" height="101%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
             style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 1118px; height: 1px; padding-top: 249px; margin-left: 23px;"
@@ -48,9 +49,9 @@
       </switch>
     </g>
     <rect x="42" y="322" width="240" height="80" fill="#9999ff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
+    <g transform="translate(-0.5 -0.5)scale(0.9999999999999999)">
       <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+        <foreignObject pointer-events="none" width="101%" height="101%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
             style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 362px; margin-left: 43px;"
@@ -68,9 +69,9 @@
       </switch>
     </g>
     <rect x="322" y="322" width="240" height="80" fill="#9999ff" stroke="rgb(0, 0, 0)" stroke-width="2" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
+    <g transform="translate(-0.5 -0.5)scale(0.9999999999999999)">
       <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+        <foreignObject pointer-events="none" width="101%" height="101%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
             style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 362px; margin-left: 323px;"
@@ -88,9 +89,9 @@
       </switch>
     </g>
     <rect x="602" y="322" width="240" height="80" fill="#9999ff" stroke="rgb(0, 0, 0)" stroke-width="2" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
+    <g transform="translate(-0.5 -0.5)scale(0.9999999999999999)">
       <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+        <foreignObject pointer-events="none" width="101%" height="101%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
             style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 362px; margin-left: 603px;"
@@ -108,9 +109,9 @@
       </switch>
     </g>
     <rect x="882" y="322" width="240" height="80" fill="#9999ff" stroke="rgb(0, 0, 0)" stroke-width="2" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
+    <g transform="translate(-0.5 -0.5)scale(0.9999999999999999)">
       <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+        <foreignObject pointer-events="none" width="101%" height="101%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
             style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 362px; margin-left: 883px;"
@@ -128,9 +129,9 @@
       </switch>
     </g>
     <rect x="302" y="42" width="840" height="180" fill="#e6e6e6" stroke="rgb(0, 0, 0)" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
+    <g transform="translate(-0.5 -0.5)scale(0.9999999999999999)">
       <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+        <foreignObject pointer-events="none" width="101%" height="101%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
             style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 838px; height: 1px; padding-top: 49px; margin-left: 303px;"
@@ -148,9 +149,9 @@
       </switch>
     </g>
     <path d="M 442 302 L 582 302 L 582 332 L 522.2 332 L 492.4 352 L 502.2 332 L 442 332 Z" fill="#990000" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
+    <g transform="translate(-0.5 -0.5)scale(0.9999999999999999)">
       <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+        <foreignObject pointer-events="none" width="101%" height="101%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
             style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 138px; height: 1px; padding-top: 317px; margin-left: 443px;"
@@ -168,9 +169,9 @@
       </switch>
     </g>
     <path d="M 722 302 L 862 302 L 862 332 L 802.2 332 L 772.4 352 L 782.2 332 L 722 332 Z" fill="#990000" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
+    <g transform="translate(-0.5 -0.5)scale(0.9999999999999999)">
       <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+        <foreignObject pointer-events="none" width="101%" height="101%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
             style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 138px; height: 1px; padding-top: 317px; margin-left: 723px;"
@@ -188,9 +189,9 @@
       </switch>
     </g>
     <path d="M 1002 302 L 1142 302 L 1142 332 L 1082.2 332 L 1052.4 352 L 1062.2 332 L 1002 332 Z" fill="#990000" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
+    <g transform="translate(-0.5 -0.5)scale(0.9999999999999999)">
       <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+        <foreignObject pointer-events="none" width="101%" height="101%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
             style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 138px; height: 1px; padding-top: 317px; margin-left: 1003px;"
@@ -208,9 +209,9 @@
       </switch>
     </g>
     <rect x="322" y="122" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" stroke-width="2" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
+    <g transform="translate(-0.5 -0.5)scale(0.9999999999999999)">
       <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+        <foreignObject pointer-events="none" width="101%" height="101%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
             style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 162px; margin-left: 323px;"
@@ -228,9 +229,9 @@
       </switch>
     </g>
     <rect x="602" y="122" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" stroke-width="2" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
+    <g transform="translate(-0.5 -0.5)scale(0.9999999999999999)">
       <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+        <foreignObject pointer-events="none" width="101%" height="101%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
             style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 162px; margin-left: 603px;"
@@ -248,9 +249,9 @@
       </switch>
     </g>
     <rect x="882" y="122" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" stroke-width="2" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
+    <g transform="translate(-0.5 -0.5)scale(0.9999999999999999)">
       <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+        <foreignObject pointer-events="none" width="101%" height="101%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
             style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 162px; margin-left: 883px;"
@@ -275,9 +276,9 @@
       stroke-miterlimit="10"
       pointer-events="all"
     />
-    <g transform="translate(-0.5 -0.5)">
+    <g transform="translate(-0.5 -0.5)scale(0.9999999999999999)">
       <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+        <foreignObject pointer-events="none" width="101%" height="101%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 252px; margin-left: 441px;">
             <div data-drawio-colors="color: #FFFFFF; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div style="display: inline-block; font-size: 14px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: nowrap;">
@@ -297,9 +298,9 @@
       stroke-miterlimit="10"
       pointer-events="all"
     />
-    <g transform="translate(-0.5 -0.5)">
+    <g transform="translate(-0.5 -0.5)scale(0.9999999999999999)">
       <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+        <foreignObject pointer-events="none" width="101%" height="101%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 252px; margin-left: 721px;">
             <div data-drawio-colors="color: #FFFFFF; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div style="display: inline-block; font-size: 14px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: nowrap;">
@@ -319,9 +320,9 @@
       stroke-miterlimit="10"
       pointer-events="all"
     />
-    <g transform="translate(-0.5 -0.5)">
+    <g transform="translate(-0.5 -0.5)scale(0.9999999999999999)">
       <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+        <foreignObject pointer-events="none" width="101%" height="101%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
             style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 252px; margin-left: 1002px;"
@@ -337,9 +338,9 @@
       </switch>
     </g>
     <rect x="22" y="42" width="270" height="180" fill="#fff2cc" stroke="rgb(0, 0, 0)" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
+    <g transform="translate(-0.5 -0.5)scale(0.9999999999999999)">
       <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+        <foreignObject pointer-events="none" width="101%" height="101%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
             style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 268px; height: 1px; padding-top: 49px; margin-left: 23px;"
@@ -357,9 +358,9 @@
       </switch>
     </g>
     <rect x="37" y="122" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
+    <g transform="translate(-0.5 -0.5)scale(0.9999999999999999)">
       <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+        <foreignObject pointer-events="none" width="101%" height="101%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
             style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 162px; margin-left: 38px;"
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2-2.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2-2.drawio.svg
similarity index 88%
rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2-2.svg
rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2-2.drawio.svg
index f93ae15728181..79cf0c73223c9 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2-2.svg
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2-2.drawio.svg
@@ -1,4 +1,5 @@
 <?xml version="1.0" encoding="UTF-8"?>
+<!-- Do not edit this file with editors other than diagrams.net -->
 <!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
 <svg
   xmlns="http://www.w3.org/2000/svg"
@@ -7,7 +8,7 @@
   width="1121px"
   height="253px"
   viewBox="-0.5 -0.5 1121 253"
-  style="background-color: rgb(255, 255, 255);"
+  content="&lt;mxfile host=&quot;Electron&quot; modified=&quot;2024-07-31T06:20:54.323Z&quot; agent=&quot;5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36&quot; etag=&quot;10K82ngjZljqxW7kt2Pg&quot; version=&quot;20.3.0&quot; type=&quot;device&quot;&gt;&lt;diagram id=&quot;FbG0muvkg23WBpYGoVLW&quot; name=&quot;ページ7&quot;&gt;7Zhhc5owGMc/jS+7A4KoLxW13W3d9Wbv2r1kECVrJDQEpfv0S0hASGhdr+q8nXqnyT/wkDy/J3lCesBfF9c0SONbEkHcc6yo6IFpz3GubMcd8X8hvSipP7Q9Ka0oiqRo74QF+g2VaCk1RxHMWhcyQjBDaVsMSZLAkLW0gFKybV+2JLj91DRYQUNYhAE21QcUsViqQ2ew028gWsXVk21PDXkdVBerkWRxEJFtQwKzHvApIUyW1oUPsfBf5Rd53/yV1rpjFCbsb264wfNt8avIH63l/ZfvDzFZsPTKthxpZxPgXA2ZwuccZkyMgUQ55t6X/WcvlVMoyZMICrtWD0y2MWJwkQahaN3ySOBazNaY12xeXJKELdS9ZR1h7BNMaGkLzDzx5foGUoa448cYrRLexogwZI5SDVxcDouGpEZ9DckaMvrCL1GtwLMUAhWFA9dVwraBtNLiJs6hEgMVRqva+s7TvKCc/S7HA8PxWQgTWLudF/joPcy7MvlJeWklShla55gFCSQ552LBAoY5QySRfeQ/aUrJRrAZ89rs23jydXZYfjqnNYoi0aiDHY98dzYRXWaUPNWzxzkUVEeD2ndMqE4X1CMydfcz9T/KtLpfcCkXvsrj3nMu1hLuXbBcWpYI+p0knzT9vJABIU3wUUorsvW/jJORPvnPIU76++Nkepn7rzF162X5nJh6BtOaRyOTchRB+HTUjDqfzx3fP2pGBfri25VRncGJM+rAIDC7vbv/8W+mQL2vkVOg0TIqP8p0l34QQqB/flNkaAAy2PBdciqKSwyLsdi/c3/AJFLFaYiDLENhG1Gbp8bBKj9veRVGrVcA06cNj/U7HFZpFOKAoU37xaHLi+oJdwSVGbxCNgCjTxo0z9ZgZCSnIVR3Njf7hjFXm6FeXzPFArqCzDBVoq0H/xHaowvt12m7ln042q6lr8cnp111/UK7E5AzOiBtoG9pT0/bNmjfE15PYMFK5jA16DPR1mLbzowJSfQ0qqRApVq+RWaQvpWDu3K5FkH63knU5amTY5k5eX+EveeV1dNDwHOMnGx3hSE4Wk62zfOfC8m9LyB8YTg/kuaB0oXk/uMB94QkeXV33isX493BOZj9AQ==&lt;/diagram&gt;&lt;/mxfile&gt;"
 >
   <defs/>
   <g>
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2-3.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2-3.drawio.svg
similarity index 89%
rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2-3.svg
rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2-3.drawio.svg
index 38ef0df67f3f2..82d2d400f0ddf 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2-3.svg
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2-3.drawio.svg
@@ -1,4 +1,5 @@
 <?xml version="1.0" encoding="UTF-8"?>
+<!-- Do not edit this file with editors other than diagrams.net -->
 <!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
 <svg
   xmlns="http://www.w3.org/2000/svg"
@@ -7,7 +8,7 @@
   width="1121px"
   height="253px"
   viewBox="-0.5 -0.5 1121 253"
-  style="background-color: rgb(255, 255, 255);"
+  content="&lt;mxfile host=&quot;Electron&quot; modified=&quot;2024-07-31T06:21:13.512Z&quot; agent=&quot;5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36&quot; etag=&quot;Eo3hK1ntTLZxHAryUL8f&quot; version=&quot;20.3.0&quot; type=&quot;device&quot;&gt;&lt;diagram id=&quot;FbG0muvkg23WBpYGoVLW&quot; name=&quot;ページ7&quot;&gt;7Zjvb+IgGMf/Gl/u0kJX15dadbvcj1zOS7a3XIuWG5aOUu3urz8QWi3o3DJnzMUt2eiXlsL388AD7cF4Ud9yVGTfWIppD3hp3YOjHgCBF4Tyn1KetXIF+rCvpTknqRb9jTAlf7ERPaNWJMVl50bBGBWk6IoJy3OciI6GOGer7m0zRrtvLdAcO8I0QdRV70kqMq3egP5Gv8NknjVv9sNI1yxQc7MZSZmhlK22JDjuwZgzJnRpUceYKvsaX/Rzkz21bcc4zsVrHrijk1X9p64evNmvLz/vMzYVxVVkOrdEtDIj5vipwqVQQ2BpRaX5uvviufGEsypPsWrW68HhKiMCTwuUqNqVjAOpZWJB5ZUvizOWi6l5dn1NKI0ZZXzdFhyH6lfqS8wFkb4PKJnnsk4w1ZA7SDNudTuutyQz6FvMFljwZ3mLqYWhZwZpgrDvN0hWW0QDo2XbNG+MiEwUzdvWN0bLgvH6Lb77ju9lgnPcui4LcvAhlT0Z/uayNFelkiwqKlCOWSWxeLjGSSUIy3UX5Z+i4Gyp0Azk1fj7YPh1fFx8NqYFSVNVaXMdRHEwHqouC84e27kDjsUUWEyB5zIFu5h+HFJwGGn8XqTN8wrLetVrDA+fKrWQSHPhbOZ5yo2NpN80+jzV8aCbkIPUreja/zJMInvqn0GYwMNhMrrM/H1Ig3ZNPiOkgYO0xbGVRSUJlDx+aDadTCYgjj80m0J75d2VTUH/tNn0+vCcGlyW3j3z9ChRAa/PblaGTlA4Rss9eaGKM4rrgTotSDtwnpriKKGoLEnS9bsLxzLVW/+8ZCpOOwcO19Itw653+NVoHFMkyLJ7TNllonnDD0bWcdsQ68Pok80ssliUrOIJNk9uHy2cxgJrUYDQakogPsfCaWpNth38O2D3L7D3ww48/3iwA8/OACeHfXOB/QJsEB0RNrR30CeHHTmwR6RMEE8d5jJZiS5RvQVtKOYst/OgkZDJlXLTIDB/KYnuSsZW3NhbNHWtv2uBpr6zZTsUV285FocWeBgAJw37u4LPpnq0NOw3AXQB+IrTjVwFzg+g+63qAnDvF4fghADl5eb7sV5wN9/h4fgf&lt;/diagram&gt;&lt;/mxfile&gt;"
 >
   <defs/>
   <g>
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2.drawio.svg
similarity index 89%
rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2.svg
rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2.drawio.svg
index b51d1a62cd35d..9f4097d048c22 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2.svg
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2.drawio.svg
@@ -1,4 +1,5 @@
 <?xml version="1.0" encoding="UTF-8"?>
+<!-- Do not edit this file with editors other than diagrams.net -->
 <!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
 <svg
   xmlns="http://www.w3.org/2000/svg"
@@ -7,7 +8,7 @@
   width="1121px"
   height="253px"
   viewBox="-0.5 -0.5 1121 253"
-  style="background-color: rgb(255, 255, 255);"
+  content="&lt;mxfile host=&quot;Electron&quot; modified=&quot;2024-07-31T06:20:32.977Z&quot; agent=&quot;5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36&quot; etag=&quot;ceBylg-TBRtQLYqXA_4-&quot; version=&quot;20.3.0&quot; type=&quot;device&quot;&gt;&lt;diagram id=&quot;FbG0muvkg23WBpYGoVLW&quot; name=&quot;ページ7&quot;&gt;7Zhbb9owFMc/DY+dcoFAHiFAO+2iaVTqXr3kQLyaOHUcSPfpZ8cOuZjSVqWIVbRSax9fYv9/xz62e26wLq4ZSuNvNALSc6yo6LnTnuM4nuuJf9LyqCxXrjuwlGnFcKSMdm1Y4L+gjVW1HEeQtSpySgnHadsY0iSBkLdsiDG6bVdbUtL+aopWYBgWISKm9Q5HPFbWkTOs7TeAV3H1ZdvzVckaVZX1TLIYRXTbMLmznhswSrlKrYsAiJSv0kW1mz9RuhsYg4S/pMENmW+LP0X+y1refvl5F9MFT68GGtAGkVzPmMFDDhmXU6BRToT4avj8sdKE0TyJQHZr9dzJNsYcFikKZelW+IGwxXxNRM4WySVN+EK3LfOYkIASysq+3Jknf4V9A4xjofuY4FUiyjiVHZmT1POW1aFomPSkr4GugbNHUaXyQmekCWgn9PwKybZBtK9tcZNm1RJpL1rteq+FFgmt9Wt0Hxq6ZyEksFNdJMTkPSJGMvnNRGolUxle54SjBGgusFhQQJhzTBM1RPEnTRndSDRjkZt9H0++zo6Lr4tpjaNIFna5jv2gP5vIIXNG73drxzkSU9dqMx1alsnU2cf0/ZCOnkcavBVp1V5iKXe9SnDvIZcbiRDXXS6tUo3apL40/bxQ/qC6EJNUvajSD+kmg9H5uYn/vJtMLyv/SaQj7+yQViNqIN3haERRQQKF9+8aTefzuRME7xpNLesF0VRQOWU09WwDgLGmxh9iTR2FoD04uxXkGAANocX5OZXJJYFiLE/2Qg5IIp2chgRlGQ7berfhdES1yp9DokLUuhyYkjYEG+zRq7IxIIjjTftKsU9E/YUfFJfhvSLWt/1PXWZ+h0VGcxaCbtm8BhidOZ0N1HY7XXHEVsCNrkqyu8m/AbZ7gX0AtucfEfawe04+Oez+BfYB2P7wiLD97mn35LAHBuxbKvIJFLxEDqkBn8uyFlp1bqxwJjTpBkRtQjpoikjPgR2KpvuicseBuucqmVePUc5BR3rNnbXfIW33HSPu2vu8rYvxeHHXfP65AKvXk3V+wMx3oynOQsSi/5STzLcuMM/t3K/g5zkn5Cey9Vuu2lDrN3F39g8=&lt;/diagram&gt;&lt;/mxfile&gt;"
 >
   <defs/>
   <g>
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step3.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step3.drawio.svg
similarity index 84%
rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step3.svg
rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step3.drawio.svg
index 631cf38245b17..092fce6099d95 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step3.svg
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step3.drawio.svg
@@ -1,13 +1,14 @@
 <?xml version="1.0" encoding="UTF-8"?>
+<!-- Do not edit this file with editors other than diagrams.net -->
 <!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
 <svg
   xmlns="http://www.w3.org/2000/svg"
   xmlns:xlink="http://www.w3.org/1999/xlink"
   version="1.1"
-  width="1121px"
+  width="1122px"
   height="181px"
-  viewBox="-0.5 -0.5 1121 181"
-  style="background-color: rgb(255, 255, 255);"
+  viewBox="-0.5 -0.5 1122 181"
+  content="&lt;mxfile host=&quot;Electron&quot; modified=&quot;2024-07-31T06:22:47.291Z&quot; agent=&quot;5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36&quot; etag=&quot;TiixoC-ztFzSnSofO4JU&quot; version=&quot;20.3.0&quot; type=&quot;device&quot;&gt;&lt;diagram id=&quot;FbG0muvkg23WBpYGoVLW&quot; name=&quot;ページ7&quot;&gt;3VbBjpswEP0ajisBTkhyTFiSVG1VqayUXr3ggLsGs44JpF/fMTYEwlZppWRbNRxivxkPnveeLSzkZ/VG4CL9zGPCLNeOaws9Wq7rTL0J/CnkpJGHqTNBGkoEjU3aGQjpD2JA26AljclhkCg5Z5IWQzDieU4iOcCwELwapu05G761wAkZAWGE2Rjd0VimGp27szO+JTRJ2zc73kJHMtwmm04OKY551YNQYCFfcC71KKt9whR9LS963foX0W5jguTydxZs2bqqv9flN3v/9PHrLuWhLB68uS5zxKw0HQvyWpKDVC3wuGRAvt6+PLWcCF7mMVFlbQutqpRKEhY4UtEKfABYKjMGMweGe57L0Kxt5pQxnzMumloo8NQD+JEISYH3JaNJDjHJVaFxk6ZvlU7qHmSa3hCeESlOkGKioJZRwJhw1gFVT9GJwdK+mm0iNi5KuupnomFguP4T3hcj3g8RyUnHOgygeY/BTlbPAkaJGhWCckElNLK0lNw6rihujkDLqvdaKldBAbTf2/DrQ7rS9sNmG4RPbQloQlfR0dtKfiltRuNYBS+9sFz4k2AF+EEK/tKdN/dGPkD2hQ+QN/aB+5YP7maDmXPdBo/XbPDpy65R8j+UbO79e5K5I8lwUQh+BKp7VyYogaOXu16d6/Xa9f27Xp325ZF56+p0Z+96dc7Q9TOz/Mun4SbcO9P38z5Mz18iTaz3RYeCnw==&lt;/diagram&gt;&lt;/mxfile&gt;"
 >
   <defs/>
   <g>
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4-2.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4-2.drawio.svg
similarity index 58%
rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4-2.svg
rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4-2.drawio.svg
index d35c8c60ec78f..4aaacf92c0f49 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4-2.svg
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4-2.drawio.svg
@@ -1,6 +1,15 @@
 <?xml version="1.0" encoding="UTF-8"?>
+<!-- Do not edit this file with editors other than diagrams.net -->
 <!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
-<svg xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" width="841px" height="261px" viewBox="-0.5 -0.5 841 261" style="background-color: rgb(255, 255, 255);">
+<svg
+  xmlns="http://www.w3.org/2000/svg"
+  xmlns:xlink="http://www.w3.org/1999/xlink"
+  version="1.1"
+  width="1122px"
+  height="261px"
+  viewBox="-0.5 -0.5 1122 261"
+  content="&lt;mxfile host=&quot;Electron&quot; modified=&quot;2024-07-31T06:28:49.877Z&quot; agent=&quot;5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36&quot; etag=&quot;BhVRcjq88FkmsO-IQyEW&quot; version=&quot;20.3.0&quot; type=&quot;device&quot;&gt;&lt;diagram id=&quot;FbG0muvkg23WBpYGoVLW&quot; name=&quot;ページ7&quot;&gt;7Vldk5owFP01Pm6HD0F4VNR1p9t2p3Zm+5qFCOlGwobgR399AwQFQrV2YXVn1QfDTXJJzjm5yY093VlubimIgi/Eg7inKd6mp497mqYqhsF/Uss2t9wYfd3KTT5Fnmi2N8zRb1j0FdYEeTCuNGSEYIaiqtElYQhdVrEBSsm62mxBcPWtEfChZJi7AMvWR+SxILda2mBvn0HkB8WbVdPOa5agaCxmEgfAI+uSSZ/0dIcSwvLScuNAnMJX4JL3m/6ldjcwCkP2Lx1meLre/NokP5XFj8/fHwMyZ9GNqvZzPyuAEzFlCl8SGLN0DsRLMEc/Hz/bFqBQkoQeTP0qPX20DhCD8wi4ae2aC4HbArbE/EnlxQUJ2Vz0zZ4Rxg7BhGa+9ImZfrl9BSlDHPghRn7I6xhJHcmzFBNPm8NNySRmfQvJEjK65U1ELadLUCBUOLAKw7pEaV/YgjKdRUMgZOTvvO+R5gUB9knAGxLwsQtDuIOdF/jsTcwEhpnIC9jMlyTVDW+gLxYK/5RNpi9+s75PtG7hA84d5uYYLRPMQAhJwqlW4Aa6CUMkzKfN3wpCD3mA8VEO+ePk63B0P3mTkbUrvLrAlsjz0sq6Ioe205+MUmAYJc+7Za+1pEZdqanRNmU1ak1q7FCM5nExjmXaTlTOazQzvpuXZfcRhGKZFyiUgSQUEEWUrDjWpf2CUwHc5073jel0qjlOp/uGUl+pTfuGNnjjfcM6vlSHZ14PrYBfbJCXpH5bwl5Cmh/zorS4wHAzTA+gHA8YeqI4djGIY+RWAa+yU0NVyT6HUIVe5QwrY1pCzGgArLBRiAFDq+rJtwlF8YYHgrJAXlDWV+1PVdIspW9X3cQkoS4UPcunVcmZptRcWTVXDFAfMslVRu1u8q9guxjAle1GguxBi2zb1tnZViW2H5I44JanbCurn332Z5vj+x6PeayqgfzEUPAekrAeToUJiJDLIzyD9FAsborpNaXVN9T0OU+4tYOKOyXVsms8qooqR22zQZZ6Z1G7WLYlZscodgH13ilR6XPlJHQsZJxAoK0YxwlUm+JKhwTqLQZivkIpe118nvLZjHwKPASbiSjqxohCN0uIONPplUpLYV1Vms6cSkuBXU5Q9f8M67p61FXnYV2+ZokoIhSxbZ6Uzu5uZ+8iDrRx9WDUExrblhd305m6w8UtXz1UCbr/9vhR+LHVC+RHzvilfNO53lOeM93u5vrJqN+aX0ACrl0T8EM7t2m3mJINlHOnZLqcgF9TstPX8eANUzL+uP9HMxfC/p9hffIH&lt;/diagram&gt;&lt;/mxfile&gt;"
+>
   <defs>
     <linearGradient x1="100%" y1="0%" x2="0%" y2="0%" id="mx-gradient-ff0000-1-0000ff-1-e-0">
       <stop offset="0%" style="stop-color: rgb(0, 0, 255); stop-opacity: 1;"/>
@@ -8,13 +17,13 @@
     </linearGradient>
   </defs>
   <g>
-    <rect x="0" y="0" width="840" height="180" fill="#e6e6e6" stroke="rgb(0, 0, 0)" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
+    <rect x="280" y="0" width="840" height="180" fill="#e6e6e6" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)scale(0.9999999999999999)">
       <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+        <foreignObject pointer-events="none" width="101%" height="101%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 838px; height: 1px; padding-top: 7px; margin-left: 1px;"
+            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 838px; height: 1px; padding-top: 7px; margin-left: 281px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -25,16 +34,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="420" y="19" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">request modules</text>
+        <text x="700" y="19" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">request modules</text>
       </switch>
     </g>
-    <rect x="20" y="80" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" stroke-width="2" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
+    <rect x="300" y="80" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" stroke-width="2" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)scale(0.9999999999999999)">
       <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+        <foreignObject pointer-events="none" width="101%" height="101%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 120px; margin-left: 21px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 120px; margin-left: 301px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -52,16 +61,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="140" y="124" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module Bsimultaneous execution as...</text>
+        <text x="420" y="124" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module Bsimultaneous execution as...</text>
       </switch>
     </g>
-    <rect x="580" y="80" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" stroke-width="2" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
+    <rect x="860" y="80" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" stroke-width="2" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)scale(0.9999999999999999)">
       <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+        <foreignObject pointer-events="none" width="101%" height="101%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 120px; margin-left: 581px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 120px; margin-left: 861px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -75,30 +84,70 @@
             </div>
           </div>
         </foreignObject>
-        <text x="700" y="124" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module D...</text>
+        <text x="980" y="124" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module D...</text>
+      </switch>
+    </g>
+    <rect x="0" y="0" width="270" height="180" fill="#fff2cc" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)scale(0.9999999999999999)">
+      <switch>
+        <foreignObject pointer-events="none" width="101%" height="101%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 268px; height: 1px; padding-top: 7px; margin-left: 1px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div
+                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
+              >
+                approved modules stack
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="135" y="19" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">approved modules stack</text>
+      </switch>
+    </g>
+    <rect x="15" y="80" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)scale(0.9999999999999999)">
+      <switch>
+        <foreignObject pointer-events="none" width="101%" height="101%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 120px; margin-left: 16px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div
+                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
+              >
+                scene module A
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="135" y="124" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module A</text>
       </switch>
     </g>
     <path
-      d="M 134.51 169.56 L 144.51 169.44 L 144.76 189.44 L 155.26 189.31 L 139.99 208.5 L 124.26 189.7 L 134.76 189.56 Z"
+      d="M 414.51 169.56 L 424.51 169.44 L 424.76 189.44 L 435.26 189.31 L 419.99 208.5 L 404.26 189.7 L 414.76 189.56 Z"
       fill="#000000"
       stroke="rgb(0, 0, 0)"
       stroke-miterlimit="10"
       pointer-events="all"
     />
     <path
-      d="M 694.51 169.56 L 704.51 169.44 L 704.76 189.44 L 715.26 189.31 L 699.99 208.5 L 684.26 189.7 L 694.76 189.56 Z"
+      d="M 974.51 169.56 L 984.51 169.44 L 984.76 189.44 L 995.26 189.31 L 979.99 208.5 L 964.26 189.7 L 974.76 189.56 Z"
       fill="#000000"
       stroke="rgb(0, 0, 0)"
       stroke-miterlimit="10"
       pointer-events="all"
     />
-    <rect x="10" y="221" width="260" height="30" fill="none" stroke="none" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
+    <rect x="290" y="221" width="260" height="30" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)scale(0.9999999999999999)">
       <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+        <foreignObject pointer-events="none" width="101%" height="101%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 258px; height: 1px; padding-top: 236px; margin-left: 11px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 258px; height: 1px; padding-top: 236px; margin-left: 291px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -111,16 +160,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="140" y="242" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="20px" text-anchor="middle" font-weight="bold">Push back...</text>
+        <text x="420" y="242" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="20px" text-anchor="middle" font-weight="bold">Push back...</text>
       </switch>
     </g>
-    <rect x="625" y="221" width="150" height="30" fill="none" stroke="none" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
+    <rect x="905" y="221" width="150" height="30" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)scale(0.9999999999999999)">
       <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+        <foreignObject pointer-events="none" width="101%" height="101%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 148px; height: 1px; padding-top: 236px; margin-left: 626px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 148px; height: 1px; padding-top: 236px; margin-left: 906px;"
           >
             <div data-drawio-colors="color: #FF0000; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -131,21 +180,24 @@
             </div>
           </div>
         </foreignObject>
-        <text x="700" y="242" fill="#FF0000" font-family="Helvetica" font-size="20px" text-anchor="middle" font-weight="bold">Discard</text>
+        <text x="980" y="242" fill="#FF0000" font-family="Helvetica" font-size="20px" text-anchor="middle" font-weight="bold">Discard</text>
       </switch>
     </g>
     <path
-      d="M 39.5 64 L 39.5 74.5 L 20.5 59 L 39.5 43.5 L 39.5 54 L 800.5 54 L 800.5 43.5 L 819.5 59 L 800.5 74.5 L 800.5 64 Z"
+      d="M 319.5 64 L 319.5 74.5 L 300.5 59 L 319.5 43.5 L 319.5 54 L 1080.5 54 L 1080.5 43.5 L 1099.5 59 L 1080.5 74.5 L 1080.5 64 Z"
       fill="url(#mx-gradient-ff0000-1-0000ff-1-e-0)"
       stroke="rgb(0, 0, 0)"
       stroke-miterlimit="10"
       pointer-events="all"
     />
-    <rect x="70" y="19" width="140" height="30" fill="none" stroke="none" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
+    <rect x="350" y="19" width="140" height="30" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)scale(0.9999999999999999)">
       <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 138px; height: 1px; padding-top: 34px; margin-left: 71px;">
+        <foreignObject pointer-events="none" width="101%" height="101%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 138px; height: 1px; padding-top: 34px; margin-left: 351px;"
+          >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
                 style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
@@ -155,16 +207,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="140" y="38" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">priority: HIGH</text>
+        <text x="420" y="38" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">priority: HIGH</text>
       </switch>
     </g>
-    <rect x="630" y="19" width="140" height="30" fill="none" stroke="none" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
+    <rect x="910" y="19" width="140" height="30" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)scale(0.9999999999999999)">
       <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+        <foreignObject pointer-events="none" width="101%" height="101%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 138px; height: 1px; padding-top: 34px; margin-left: 631px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 138px; height: 1px; padding-top: 34px; margin-left: 911px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -175,16 +227,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="700" y="38" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">priority: LOW</text>
+        <text x="980" y="38" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">priority: LOW</text>
       </switch>
     </g>
-    <rect x="300" y="80" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" stroke-width="2" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
+    <rect x="580" y="80" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" stroke-width="2" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)scale(0.9999999999999999)">
       <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+        <foreignObject pointer-events="none" width="101%" height="101%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 120px; margin-left: 301px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 120px; margin-left: 581px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -202,23 +254,23 @@
             </div>
           </div>
         </foreignObject>
-        <text x="420" y="124" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module Csimultaneous execution as...</text>
+        <text x="700" y="124" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module Csimultaneous execution as...</text>
       </switch>
     </g>
     <path
-      d="M 414.51 169.56 L 424.51 169.44 L 424.76 189.44 L 435.26 189.31 L 419.99 208.5 L 404.26 189.7 L 414.76 189.56 Z"
+      d="M 694.51 169.56 L 704.51 169.44 L 704.76 189.44 L 715.26 189.31 L 699.99 208.5 L 684.26 189.7 L 694.76 189.56 Z"
       fill="#000000"
       stroke="rgb(0, 0, 0)"
       stroke-miterlimit="10"
       pointer-events="all"
     />
-    <rect x="290" y="221" width="260" height="30" fill="none" stroke="none" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
+    <rect x="570" y="221" width="260" height="30" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)scale(0.9999999999999999)">
       <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+        <foreignObject pointer-events="none" width="101%" height="101%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 258px; height: 1px; padding-top: 236px; margin-left: 291px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 258px; height: 1px; padding-top: 236px; margin-left: 571px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -231,7 +283,7 @@
             </div>
           </div>
         </foreignObject>
-        <text x="420" y="242" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="20px" text-anchor="middle" font-weight="bold">Push back...</text>
+        <text x="700" y="242" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="20px" text-anchor="middle" font-weight="bold">Push back...</text>
       </switch>
     </g>
   </g>
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4-3.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4-3.drawio.svg
similarity index 88%
rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4-3.svg
rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4-3.drawio.svg
index 78240c9f5a307..02fc096b54fa4 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4-3.svg
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4-3.drawio.svg
@@ -1,6 +1,15 @@
 <?xml version="1.0" encoding="UTF-8"?>
+<!-- Do not edit this file with editors other than diagrams.net -->
 <!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
-<svg xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" width="841px" height="261px" viewBox="-0.5 -0.5 841 261" style="background-color: rgb(255, 255, 255);">
+<svg
+  xmlns="http://www.w3.org/2000/svg"
+  xmlns:xlink="http://www.w3.org/1999/xlink"
+  version="1.1"
+  width="841px"
+  height="261px"
+  viewBox="-0.5 -0.5 841 261"
+  content="&lt;mxfile host=&quot;Electron&quot; modified=&quot;2024-07-31T06:29:51.430Z&quot; agent=&quot;5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36&quot; etag=&quot;_wfvhglN6HLrCwBnDSWe&quot; version=&quot;20.3.0&quot; type=&quot;device&quot;&gt;&lt;diagram id=&quot;FbG0muvkg23WBpYGoVLW&quot; name=&quot;ページ7&quot;&gt;7Vldd9ogGP41XnYnX0Zz6WfdWbf1zJ3T3dIEE1YMlpAa9+sHCWgC1s42dl2nXggP8Abe5+HlJXbc0bK4pGCVfCYRxB3HioqOO+44jmM5Lv8RyKZCLnzL8yoopiiqQHsHzNEvKEFLojmKYNboyAjBDK2aYEjSFIasgQFKybrZbUFw86krEEMDmIcAm+gNilhSoX2nt8NnEMWJerLtB1XLEqjOciVZAiKyrkHupOOOKCGsKi2LEcTCfcov1bjpI63biVGYsj8ZMMPTdfGzyH9Yi++fvt0kZM5WF7Yr7TwAnMslU3ifw4yJNZAox9z71fzZRjmFkjyNoLBrddzhOkEMzlcgFK1rLgSOJWyJec3mxQVJ2VyOLesI4xHBhJa23Ikvvhx/gJQh7vgBRnHK2xgRhsxVqgnz7rCoQXLVl5AsIaMb3kXJ0OlLCjaKPUsC6xqlnsSSOp1qJJAyirfWd57mBensoxzvGI7PQpjCrdt5ga/ex0z6sBS5cpt/nwvd8A7uYmFZYj07yI/lbzn2luoIn3BlsIIztMwxAykkOafaggUMc4ZIWi2bPxWkEYoA47Mc8Orky2B4NXmVmbUrPF1gSxRFolFX5CAYeZOhcAyj5G677Z2W1Ohauhr7phqdfWo8oRjdp8U4Nml7pnLeIal9/w2S6hmkGr7nh9JKFBcYFgNxXHJ/wDSSxXGIQZahsElBky/Nz1b5OeRVGDVOXNOnNY919zhMYRRiwNBD85ze50X5hGuCylClKPPs4EO3SZqrDm9lJiM5DaEcWT9bDWOOxr9naaYYoDFkhqmS2u3iX8J298z2AbaDXotsB3pC8fps+wbb13mWcOQWhHdmpN5F4lpWx6VRdtZUwmMea2qgipmK95SkeoCVEJBBmB8dDNJD0XlflNeUpqeNol5dD5yDijsmMQx0HpWRetT298jSPV3U7re4jznBlL1se0/5aoYxBRHijq61Tady66u2MaL8AiZOf86ouD+0FBVsa19ibrUUF8xsrPvMqODaT5o6eVQIDO2sKCIUsU2Vgc0+Xs7+if3eRp7d1emwA2Nz2/tSstNtbvW0Rwm6+nrzv/AT2G+QH/NtiHEPGp0v5e/v/tY1XhH9/fubZ74hen4m8P4yej9oMaPv6Wf3q2f0nvkK5pzRH7+Pe28vo/fM9zBnZo9mtq+nc6dklld3f8xUW3z3B5c7+Q0=&lt;/diagram&gt;&lt;/mxfile&gt;"
+>
   <defs>
     <linearGradient x1="100%" y1="0%" x2="0%" y2="0%" id="mx-gradient-ff0000-1-0000ff-1-e-0">
       <stop offset="0%" style="stop-color: rgb(0, 0, 255); stop-opacity: 1;"/>
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4.drawio.svg
similarity index 88%
rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4.svg
rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4.drawio.svg
index c4b1d8d385122..0dd387d460cd9 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4.svg
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4.drawio.svg
@@ -1,13 +1,14 @@
 <?xml version="1.0" encoding="UTF-8"?>
+<!-- Do not edit this file with editors other than diagrams.net -->
 <!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
 <svg
   xmlns="http://www.w3.org/2000/svg"
   xmlns:xlink="http://www.w3.org/1999/xlink"
   version="1.1"
-  width="1121px"
+  width="1122px"
   height="261px"
-  viewBox="-0.5 -0.5 1121 261"
-  style="background-color: rgb(255, 255, 255);"
+  viewBox="-0.5 -0.5 1122 261"
+  content="&lt;mxfile host=&quot;Electron&quot; modified=&quot;2024-07-31T06:28:20.483Z&quot; agent=&quot;5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36&quot; etag=&quot;WNlWx0M9GbhQ2u9ww_f5&quot; version=&quot;20.3.0&quot; type=&quot;device&quot;&gt;&lt;diagram id=&quot;FbG0muvkg23WBpYGoVLW&quot; name=&quot;ページ7&quot;&gt;7Vhdd5owGP41XnaHL0EurR+1Z93WM3dOd5tClKyR0BAU9+uXQFBImNYWu3YbXhiefJA8z5uHN/Ts0Sq/oiCJPpEQ4p5lhHnPHvcsy/EGJv8TyLZELmzLk9CSorAEa8Ac/YQSNCSaoRCmjYaMEMxQ0gQDEscwYA0MUEo2zWYLgptPTcASasA8AFhH71DIohIdWN4en0G0jKonm65f1qxA1ViuJI1ASDY1yJ707BElhJWlVT6CWNBX8VL2m/6mdjcxCmP2lA4zPN3kP/Lsu7H49vHrXUTmLLmoBFoDnMkVU/iYwZSJJZAww5z8cvpsW3FCSRaHUAxr9OzLTYQYnCcgELUbHgcci9gK8zuTFxckZnPZt7hHGI8IJrQYy5644sfxNaQMcd6HGC1jXseIGEhfZDVh3hzmNUgu+gqSFWR0y5vIWq6WVEAGodevBtnUFHVko6iuZtUTyCha7kbfE80LkutTeLc03tMAxnDHOi/wxbuYSQqLEK9Ycx8zETW8gb1YGPyqQ+5S/hd976mK8PmWA5ZwilYZZiCGJONKGzCHQcYQictV86eCOEQhYHyWw54IsefPaXw9H17eTJ48tW4DTw2wFQpDUalG5NAfOZNLwQyj5GG3662OotE2lGh0DT0arbZoPF8w2seDcayrdmLkTD6X4v+Fmg7ct6epo2kKkoSSNae65uxcCRA8nNXhp9OpNRqd1eENdU+1Obzlva7D949vquEf3g2dcG/231zsuxr1GtE8G0tEcYFhPhR5IqcDxqEsjgMM0hQFTb6b4iikGsV1iFQYNlJNndIaYf0WviqMQgwYWjcT1DYS5RNuCSre1ZVijul/UDXzFS1SktEAyp71pFIbzFK2nmcrQzFAl5BpQxXK7hb/ArG9/2IfENv3OhTbVzPpVxd7oIl9m6URR+6L15iaouxTkOPvPG54rBkCZbJQyR6TWPVSCQHpt9zeGaSHjLjN0JVAU1+m4r48FVsHA+6UA5GvyuhYumW7LVGpyt2dZfuasGOUBoCG71Qncd9Igo4Zxgn6+Ub/uH5mm6ucTT/T0dOd59sw36CUvcydp3w1l0sKQgTbhajqxojCoDi2cKXFd4+OTN002tJNoyNb106Rfc9/nqnb5tGhzmzqpqPnawlFhCK2LY+Os+ur2bvwgS6+D/RVOWxf39xt+fQZN7eeYzUFuvly96/o45uvqA+/3X8gLvfb/kO7PfkF&lt;/diagram&gt;&lt;/mxfile&gt;"
 >
   <defs>
     <linearGradient x1="100%" y1="0%" x2="0%" y2="0%" id="mx-gradient-ff0000-1-0000ff-1-e-0">
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/multiple_candidates.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step5.drawio.svg
similarity index 51%
rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/multiple_candidates.svg
rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step5.drawio.svg
index 650b0b76548d1..29fb69f77be40 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/multiple_candidates.svg
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step5.drawio.svg
@@ -1,23 +1,24 @@
 <?xml version="1.0" encoding="UTF-8"?>
+<!-- Do not edit this file with editors other than diagrams.net -->
 <!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
 <svg
   xmlns="http://www.w3.org/2000/svg"
   xmlns:xlink="http://www.w3.org/1999/xlink"
   version="1.1"
-  width="1130px"
-  height="401px"
-  viewBox="-0.5 -0.5 1130 401"
-  style="background-color: rgb(255, 255, 255);"
+  width="1131px"
+  height="183px"
+  viewBox="-0.5 -0.5 1131 183"
+  content="&lt;mxfile host=&quot;Electron&quot; modified=&quot;2024-07-31T06:35:13.925Z&quot; agent=&quot;5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36&quot; etag=&quot;YxpN8ty9i5xn-1O3FjJt&quot; version=&quot;20.3.0&quot; type=&quot;device&quot;&gt;&lt;diagram id=&quot;FbG0muvkg23WBpYGoVLW&quot; name=&quot;ページ7&quot;&gt;3Vldb5swFP01PLYy3/CYpGWTtknTMmndowsOeHMwMqYh+/WzwQQcSJOoyVItL7GvfQ2+5/je48SwF+v6A4NF9oUmiBgWSGrDfjAs684EwBTf0rRVJtsL/NaUMpy0RrM3LPEfpIxAWSucoFKbyCklHBe6MaZ5jmKu2SBjdKNPW1GiP7WAKRoZljEkY+sPnPCstQaW39s/Ipxm3ZNNL2xH1rCbrHZSZjChm4HJfjTsBaOUt611vUBExq+LS+sXHRjdvRhDOT/F4SOJNvWvunoCq++fvv3I6JIXd6ar3u4FkkptGRYFoy8okZugSUVE+MXbcxj/Vhvh2y46jFZ5guQDgGHPNxnmaFnAWI5uBCWELeNrInqmaK5ozpfKt+ljQhaUUNasZUdRZC0Wwv6CGMcCgRnBaS7GOJULjberIiCno3pgUtv/gOgacbYVU9So7QTKR9HRDwK1/U2Prd0Blg1x7SZCxad0t3ofctFQUT8LAWuEQAzzBCeQo38MwUJ8ADgIQckZ/b07BtaFQAm7eL8rUJwRKGWM8h4Q0ZhdFov9mK9xksjBfZBm4cJ5nF8o+K61dyJCbxx8y5kI/hVjHxyP/fw/iP0u1O8o9t0bDAKLElEQVZcyntGU5pA89ta5Hvp+zmcq00YT4F+I862q7rDiVIcD1Zg/KXfZ/inb967qPdSDoYdt18nFhp+GnYGX7PZuTa/zOyOJlbRiMTold3PIUsRPIbWM5qvsYIhAjl90+TEFdOM6YwxuBxMKinNeDlb+Kg1D0tk66QIA9mTDuR6i0b5FT7vddt7CROemTLwPQ19jo+f5r/JxzCs5/BUxLEKBmJr1Fq6BqzBoAnDrAOC7RVq6K7+9lHMR7N3jolQ0aMWLik/S5DN8FncRDVqocrsoJQ0eh5M+QyX+A5+b9SRo6lCJxd254T5M1A8iHzcX8ixtGDioGHPL86JoEvnXea8uNOo1jN014vQCcwfuLd+3NSxVvnojRUxPX9TSF6CrVYmuxAx/xIxUSAPWauXmMiV3t0IizDFqAi7O4020QhR54nMhrWDvi+RwQiSbUyLZuZ5WCEdYHDqReTKTd3F5+ggsSxzrIdbx6HKwaZyuBsbZ9/Qj6vmzeTh9RM0TU+4ABHcCg872xmPnWOBYZm7LyCgzj5dynFsnef+2Bf59Sc1by0LfN6fpcFAWHvPQZeGEv3fA/2wu2wE4ttTVuezeksvnJMp3cW3qqvg/Y/1h7jjBpVKq7d5cN/veqCKjGsUVx3k6ImiZwUI2hZQhom4fF0LF4C6jnAbXm1HBnSjJukAKQwCaXxulp/5DcNRI5oKWmGOaWw1DbamlyvYsWGAw3Iw69mWUlgMCd09pOScqLfd8pSW6/R8ALQ36f1Lsx78=&lt;/diagram&gt;&lt;/mxfile&gt;"
 >
   <defs/>
   <g>
-    <rect x="161" y="0" width="320" height="400" fill="#fff2cc" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="161" y="1" width="320" height="180" fill="#fff2cc" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 318px; height: 1px; padding-top: 7px; margin-left: 162px;"
+            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 318px; height: 1px; padding-top: 8px; margin-left: 162px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -28,16 +29,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="321" y="19" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">approved modules stack</text>
+        <text x="321" y="20" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">approved modules stack</text>
       </switch>
     </g>
-    <rect x="600" y="0" width="320" height="400" fill="#fff2cc" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="600" y="1" width="320" height="180" fill="#cccc00" stroke="rgb(0, 0, 0)" stroke-width="2" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 318px; height: 1px; padding-top: 7px; margin-left: 601px;"
+            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 318px; height: 1px; padding-top: 8px; margin-left: 601px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -48,18 +49,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="760" y="19" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">candidate modules stack</text>
+        <text x="760" y="20" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">candidate modules stack</text>
       </switch>
     </g>
-    <path d="M 441 200 L 460 200 L 472.76 200" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 478.76 200 L 470.76 204 L 472.76 200 L 470.76 196 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
-    <rect x="201" y="160" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="201" y="81" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 200px; margin-left: 202px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 121px; margin-left: 202px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -70,16 +69,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="321" y="204" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module A</text>
+        <text x="321" y="125" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module A</text>
       </switch>
     </g>
-    <rect x="640" y="40" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="640" y="81" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 80px; margin-left: 641px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 121px; margin-left: 641px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -90,63 +89,17 @@
             </div>
           </div>
         </foreignObject>
-        <text x="760" y="84" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module B</text>
+        <text x="760" y="125" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module B</text>
       </switch>
     </g>
-    <rect x="640" y="160" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <path d="M 600 91 L 600.11 121.11 L 610.11 121.11 L 631.76 121.03" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 637.76 121.01 L 629.78 125.04 L 631.76 121.03 L 629.75 117.04 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 480.04 121.06 L 540.56 121.11 L 591.76 121.02" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 597.76 121 L 589.77 125.02 L 591.76 121.02 L 589.76 117.02 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div
-            xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 200px; margin-left: 641px;"
-          >
-            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-              <div
-                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
-              >
-                scene module C
-              </div>
-            </div>
-          </div>
-        </foreignObject>
-        <text x="760" y="204" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module C</text>
-      </switch>
-    </g>
-    <rect x="640" y="282" width="240" height="78" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
-      <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div
-            xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 321px; margin-left: 641px;"
-          >
-            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-              <div
-                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
-              >
-                scene module D
-              </div>
-            </div>
-          </div>
-        </foreignObject>
-        <text x="760" y="325" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module D</text>
-      </switch>
-    </g>
-    <path d="M 600 200 L 610 200 L 631.76 200" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 637.76 200 L 629.76 204 L 631.76 200 L 629.76 196 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
-    <path d="M 600 200 L 620 200 L 620 321 L 631.76 321" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 637.76 321 L 629.76 325 L 631.76 321 L 629.76 317 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
-    <path d="M 600 200 L 620 200 L 620 80 L 631.76 80" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 637.76 80 L 629.76 84 L 631.76 80 L 629.76 76 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
-    <path d="M 161 200 L 190 200 L 192.76 200" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 198.76 200 L 190.76 204 L 192.76 200 L 190.76 196 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
-    <path d="M 481 200 L 591.76 200" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 597.76 200 L 589.76 204 L 591.76 200 L 589.76 196 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
-      <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 221px; margin-left: 541px;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 142px; margin-left: 542px;">
             <div data-drawio-colors="color: rgb(0, 0, 0); background-color: #B266FF; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
                 style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; background-color: rgb(178, 102, 255); white-space: nowrap;"
@@ -156,16 +109,14 @@
             </div>
           </div>
         </foreignObject>
-        <text x="541" y="224" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">approved module output</text>
+        <text x="542" y="146" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">approved module output</text>
       </switch>
     </g>
-    <path d="M 120 200 L 152.76 200" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 158.76 200 L 150.76 204 L 152.76 200 L 150.76 196 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
-    <rect x="0" y="180" width="120" height="40" fill="#ff6666" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="0" y="101" width="120" height="40" fill="#ff6666" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 118px; height: 1px; padding-top: 200px; margin-left: 1px;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 118px; height: 1px; padding-top: 121px; margin-left: 1px;">
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
                 style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
@@ -175,35 +126,17 @@
             </div>
           </div>
         </foreignObject>
-        <text x="60" y="204" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">generate root refere...</text>
-      </switch>
-    </g>
-    <path d="M 880 80 L 1111.76 80" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/>
-    <path d="M 1117.76 80 L 1109.76 84 L 1111.76 80 L 1109.76 76 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
-      <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 80px; margin-left: 1000px;">
-            <div data-drawio-colors="color: rgb(0, 0, 0); background-color: #67AB9F; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-              <div
-                style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; background-color: rgb(103, 171, 159); white-space: nowrap;"
-              >
-                output
-              </div>
-            </div>
-          </div>
-        </foreignObject>
-        <text x="1000" y="83" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">output</text>
+        <text x="60" y="125" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">generate root refere...</text>
       </switch>
     </g>
-    <path d="M 880 200 L 1111.76 200" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/>
-    <path d="M 1117.76 200 L 1109.76 204 L 1111.76 200 L 1109.76 196 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 880 121 L 1111.76 121" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 1117.76 121 L 1109.76 125 L 1111.76 121 L 1109.76 117 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 200px; margin-left: 1000px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 121px; margin-left: 1000px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); background-color: #67AB9F; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -214,28 +147,31 @@
             </div>
           </div>
         </foreignObject>
-        <text x="1000" y="203" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">output</text>
+        <text x="1000" y="124" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">output</text>
       </switch>
     </g>
-    <path d="M 880 320.5 L 1111.76 320.5" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/>
-    <path d="M 1117.76 320.5 L 1109.76 324.5 L 1111.76 320.5 L 1109.76 316.5 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 441 121 L 451 121.11 L 472.76 121.03" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 478.76 121.01 L 470.78 125.04 L 472.76 121.03 L 470.75 117.04 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 120 121 L 191.76 121" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 197.76 121 L 189.76 125 L 191.76 121 L 189.76 117 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 765 61 L 885 61 L 885 91 L 836.6 91 L 808.2 111 L 816.6 91 L 765 91 Z" fill="#990000" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 321px; margin-left: 1000px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 118px; height: 1px; padding-top: 76px; margin-left: 766px;"
           >
-            <div data-drawio-colors="color: rgb(0, 0, 0); background-color: #67AB9F; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+            <div data-drawio-colors="color: #FFFFFF; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
-                style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; background-color: rgb(103, 171, 159); white-space: nowrap;"
+                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(255, 255, 255); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
               >
-                output
+                executing
               </div>
             </div>
           </div>
         </foreignObject>
-        <text x="1000" y="324" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">output</text>
+        <text x="825" y="80" fill="#FFFFFF" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">executing</text>
       </switch>
     </g>
   </g>
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/success_modules_remove.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/success_modules_remove.drawio.svg
similarity index 85%
rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/success_modules_remove.svg
rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/success_modules_remove.drawio.svg
index 7b85f38c21e92..eefd36856e39a 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/success_modules_remove.svg
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/success_modules_remove.drawio.svg
@@ -1,4 +1,5 @@
 <?xml version="1.0" encoding="UTF-8"?>
+<!-- Do not edit this file with editors other than diagrams.net -->
 <!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
 <svg
   xmlns="http://www.w3.org/2000/svg"
@@ -7,7 +8,7 @@
   width="1764px"
   height="444px"
   viewBox="-0.5 -0.5 1764 444"
-  style="background-color: rgb(255, 255, 255);"
+  content="&lt;mxfile host=&quot;Electron&quot; modified=&quot;2024-07-31T07:01:30.863Z&quot; agent=&quot;5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36&quot; etag=&quot;iXM8eH3GR_-vDcYm_9RE&quot; version=&quot;20.3.0&quot; type=&quot;device&quot;&gt;&lt;diagram id=&quot;FbG0muvkg23WBpYGoVLW&quot; name=&quot;ページ7&quot;&gt;7Vtdk6I4FP01Pm4XED4fbT9mH2aqesaa2u2nrbSkhR0kFERb99dvgEQgFxVHGO3exa4uciGR3HPuzeGCIzRZ7z6lOAm+UJ9EI0PzdyM0HRmG7vB/eXsv2iYqDas09IWpMizCf4gwasK6CX2SNU5klEYsTJrGJY1jsmQNG05T+tY87ZVGzW9N8IoAw2KJI2j9I/RZUFpdw6nsv5NwFchv1m2vPLLG8mQxkyzAPn2rmdBshCYppazcW+8mJMpdJ/1S9psfOXq4sJTErEsH0/C/kdcFCvyvX/ff54G3yl5/Q7pRjrPF0UZMWVwu20sfpHQT+yQfRhuhx7cgZGSR4GV+9I1jzm0BW0e8pfPd1zCKJjSiadEXTYqN2zOW0h8HH5rcAicg5rQlKSO7mklM6BOha8LSPT9FHLVsU/QR9LJdU7j7rYaWJmxBDSlTnogFQ1aH0Ssn8h3hx4t8it6zT03bvUefmsCnOElSuuU+5MFG/U3E00TuYrz8caW3acwWom+L9+fzuVF4P3dpyDPFOApXMT/GaNITAo5mKAjYEAFktCGgDYeABRBY4tgPfczIfUHQDAu9p1Sjg1RzD6DYwNHE5wuYaNKUBXRFYxzNKutjE4rqnM80d1/hsL8JY3uxGuMNo014yC5kf9b2n/OhHizRmu7EyEVjLxsxn/ChU96o9cqbVbeiJfs1wTROgZnRTbokp9xliMzMcLoi7KRjRb7JvXmSHimJMAu3TbnQBnTRdZymeF87IaFhzLLayE+5ocY6TU0Fnqks8+d6OJqtKUQrr6Ki3WE61zDRAekhSck2pJvskB34Dt2wZMNuSdqKqM91np4mbY2nz3UOHyGtmr8i/EKiR54WV8U0Gqls6tl23zzX3a48lxHRH8+vTGguoFG2JDGpc2jc7+KiLiLr0PcLyimrztibmLNHMUDNrhXbUOuO6ZhqPBtw3THMlnXHHW7Z8T5gsMMI/IkwvmZx0jovTu59Ba288lNB+3jjoB0kFA+a8HaheJjGCd9P37/vLV25K+W+N7r53nEH8z28r/+Vue0C9d2vuhhGGwMpy+NNxVyNozJXin4VnJfKbNMxlG9SRHOHHvLahpXZBqwtgXCf3Cbc820+h/poXmyXkfCKpGx72s31kQGrVV8KdBYMM66R0Jj/Lb5PJrPFAoCVBTjJd7nLI66gzgOWkDTkF07SqtNTZQJ4tuicJpCeJwTtMSATmoUspLFRZB5U3L6U2crQaoeLoybqCWfXRk2cXRfirFsexNkaDmdYE/uwVUnLtvUHS4HgDkpghg0w+C/VJW1d0+4SFueW2ujdVSbNzjd/MufcujLpSZJ1lUxnezQlU0t/EyQgT33iVHoaCMLe9JcUGx+q8vGzZc6bV0w6lzklTe6mYtKhzPkBKiaWo6khy+/bby/PYfnyI9ZMbO5q6P2bV02QBlz7bqomVaa8TCtck+Sszsqg97LwkYXYAZJzsMqMZbsgg5wVGi19fk11BsFiLOC6vL1/jchunL+hVpDFF7vTZYSzLFw2ydtkv5IyDs+fjpLsLCFqycBqyQXSdi1vLLWeJ5E8J99ahlLLwWCoIwTsD2t47z8A1mUKakP7WNHm7lngwTqChzxpupQJttbhrmBwLsAaBOCCVACfc0n8VJXIXihjdH20aFBjhmTTerfKX7V9eMGcPA8x/Svbr19o1FaPOMeJC8SEZkE/tzyJbtMSw70WiOBbJ9/Imm7JA/A/nydT/NmIrZjGqh4TJiwA4RKxrKgeFXNtolDJ3Q1d2IPC9hyvCYrrtZRFpYioo4IGQ0Xi/f9LHCfug+7gNQ4TPqa4l4dJMzv/QJy8YmtbGF0t/wyG16CPlXiz+iFAuSpVP6ZAs38B&lt;/diagram&gt;&lt;/mxfile&gt;"
 >
   <defs/>
   <g>
@@ -53,8 +54,8 @@
         <text x="622" y="41" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">candidate modules stack</text>
       </switch>
     </g>
-    <path d="M 303 102 L 323 102 L 323 222 L 334.76 222" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 340.76 222 L 332.76 226 L 334.76 222 L 332.76 218 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 303 102 L 323.2 102.2 L 323.2 222.2 L 334.76 222.08" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 340.76 222.02 L 332.8 226.1 L 334.76 222.08 L 332.72 218.1 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <path d="M 183 182 L 183 150.24" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
     <path d="M 183 144.24 L 187 152.24 L 183 150.24 L 179 152.24 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
@@ -98,7 +99,7 @@
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 282px; margin-left: 183px;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 283px; margin-left: 183px;">
             <div data-drawio-colors="color: rgb(0, 0, 0); background-color: #FFD966; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
                 style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; background-color: rgb(255, 217, 102); white-space: nowrap;"
@@ -108,7 +109,7 @@
             </div>
           </div>
         </foreignObject>
-        <text x="183" y="285" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">previous module output</text>
+        <text x="183" y="286" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">previous module output</text>
       </switch>
     </g>
     <rect x="63" y="303" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
@@ -151,8 +152,8 @@
         <text x="622" y="347" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module D</text>
       </switch>
     </g>
-    <path d="M 23 222 L 42 222 L 42 342 L 53.76 342" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 59.76 342 L 51.76 346 L 53.76 342 L 51.76 338 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 23 222 L 42 222.2 L 42 342.2 L 53.76 342.08" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 59.76 342.02 L 51.8 346.1 L 53.76 342.08 L 51.72 338.1 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <rect x="63" y="62" width="240" height="80" fill="#3333ff" stroke="rgb(0, 0, 0)" stroke-width="2" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
@@ -233,8 +234,8 @@
         <text x="1582" y="41" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">candidate modules stack</text>
       </switch>
     </g>
-    <path d="M 1262.5 222 L 1281 222 L 1294.26 222" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 1300.26 222 L 1292.26 226 L 1294.26 222 L 1292.26 218 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 1262.5 222 L 1281.2 222.2 L 1294.26 222.08" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 1300.26 222.02 L 1292.3 226.1 L 1294.26 222.08 L 1292.23 218.1 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <path d="M 1142.5 302 L 1142.5 270.24" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
     <path d="M 1142.5 264.24 L 1146.5 272.24 L 1142.5 270.24 L 1138.5 272.24 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
@@ -296,8 +297,8 @@
         <text x="1582" y="347" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module D</text>
       </switch>
     </g>
-    <path d="M 982.5 222 L 1002.5 222 L 1002.5 342 L 1014.26 342" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 1020.26 342 L 1012.26 346 L 1014.26 342 L 1012.26 338 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 982.5 222 L 1002.4 222.2 L 1002.4 342.2 L 1014.26 342.08" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 1020.26 342.02 L 1012.3 346.1 L 1014.26 342.08 L 1012.22 338.1 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <path d="M 862.5 227 L 862.5 217 L 882.5 217 L 882.5 206.5 L 901.5 222 L 882.5 237.5 L 882.5 227 Z" fill="#000000" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
     <path d="M 1282.97 106.5 L 1283.03 96.5 L 1343.03 96.88 L 1343.1 86.38 L 1362 102 L 1342.9 117.38 L 1342.97 106.88 Z" fill="#990000" stroke="#000000" stroke-miterlimit="10" pointer-events="all"/>
     <path
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/success_modules_skip.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/success_modules_skip.drawio.svg
similarity index 77%
rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/success_modules_skip.svg
rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/success_modules_skip.drawio.svg
index d54de3ddcc148..5beb45a79c22b 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/success_modules_skip.svg
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/success_modules_skip.drawio.svg
@@ -1,25 +1,26 @@
 <?xml version="1.0" encoding="UTF-8"?>
+<!-- Do not edit this file with editors other than diagrams.net -->
 <!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
 <svg
   xmlns="http://www.w3.org/2000/svg"
   xmlns:xlink="http://www.w3.org/1999/xlink"
   version="1.1"
-  width="1764px"
-  height="444px"
-  viewBox="-0.5 -0.5 1764 444"
-  style="background-color: rgb(255, 255, 255);"
+  width="1765px"
+  height="445px"
+  viewBox="-0.5 -0.5 1765 445"
+  content="&lt;mxfile host=&quot;Electron&quot; modified=&quot;2024-07-31T07:02:11.758Z&quot; agent=&quot;5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36&quot; etag=&quot;huRqWh6m-Lqcjou3bF8p&quot; version=&quot;20.3.0&quot; type=&quot;device&quot;&gt;&lt;diagram id=&quot;FbG0muvkg23WBpYGoVLW&quot; name=&quot;ページ7&quot;&gt;7VtZc6M4EP41fpwpLoF5dHxkpmonNTOu1E4etUY22sGIEnJi769fAcIcjY+MISaHK1WRGiSj7+uL7mRgjtfbW44j/xvzSDAwNG87MCcDw/hkO0j+SiQ7JTE0w8pEK069TKgXgjn9jyihpqQb6pG4cqNgLBA0qgoXLAzJQlRkmHP2VL1tyYLqt0Z4RYBgvsABlP5NPeFn0qHhFPIvhK78/Jt1282urHF+szpJ7GOPPZVE5nRgjjljIhutt2MSJPDluGTrZgeu7h+Mk1Ccs8AyvJ9kOTd978eP3f3Md1fx8pNpKYYecbBRR1aPK3Y5BpxtQo8k22gD8+bJp4LMI7xIrj5J3qXMF+tAznQ5XNIgGLOA8XStOU4/Uh4Lzn7vMbSkBB5AnemRcEG2JZE60C1hayL4Tt6iriLbUmuUgjmWpuB+KrGVy/wSU5alhFhpyGq/ewGiHCgcn4Wp/ZoxtexhHzF1AKY4ijh7lBhKY2PeJpBuIoEYL35fiDYLxVytbUB/NpsZKfoJpFR6ilFAV6G8JljUEgOOZtQYMCADptHEgNYdA0PAwAKHHvWwIP2ioGoWekuuRgeupg+kuABo4skApqaMC5+tWIiDaSG9qVJR3PMXS+BLAfuXCLFT0RhvBKvSQ7ZU/CqNH5KtPiM1m2zVzulkl09CeeD9omRSWpVMi2XpLF9XJdM4RmbMNnxBjsGFlGcWmK+IOAqs8jcJmkfVg5MAC/pYTReaiE6XjjjHu9INEaOhiEs7f08EJa3T6q4AabUwf3KFXU0M5CB7ikLt9se5QBORBtxDxMkjZZt47x3kgG1EtBHXVNpCUR/KenpcaUt6+lDW4QNKW/dfAf6HBDfSLa7SY1Rc2cS17db1XD9Xz3OLaE/PL3No+ZOX1ChekJCUdWjUbnCpB5E19bxU5WpRJ/nMZmqDajSapfJO4o4l40zNAwxh3DGshrgz7CzsIOMNGju0wD8w40uM1jzbaPWeGa152mhvrmO0I3dsTW+6MkVH069uitZp7CevH3uk199K9y9Jp7B3hp1hj67q256RfbeaXXSUG4NUVtpbnfN6gSHzlWpdQedz02zLMWrfZNeraSdX5M/WcZoNa0vA3MdXNvdafqSln5fLjyz7+vkRrFZ9S9mZCyxkjmSO5M/P+7u7r3e3gKzYx1EylJAHMoM6TVhEOJUPTnix6HshAnw25DlVIl1XEXYo0Y1YTAVloZF6HjN9fcm8laGVLqdXLbMlnoe2WePZgjzryIU8o+54dgHP76kmZutaEnh6VxWzz8hH30BOZOsWgL8HWZF9Rksp93HLgGxHSZsuLaF4ajhZBDiO6aIKeZWfGq77IHMQ15PZSgkx1ABYLrswqUGontTkqcZ+kywDA0lNw1b1nBhsdSA/ai0dcWC5Bga6+f14PJ3PPwLdHwY61JDQvHCgc2DB582235Bt9zKqOSYA9qPXcwQuZSCny2m5cl+71+NCvTvZ7WlY8zL9HgfWnV5/Cfi19nvyv7M6Q9fdtnX9QqcGk8X33e9BDnyn6kHHx4F1p9dv7tfu+DjO2WaLema2sMD1Fjs+TcbYg55P/u7+zvoOHeVpMKmyXRew3lHnAdkO+K5TvYemNS/TfXBgvfN9dx+aHEQP+g/5zh/9h9bKMmho6IDp63cghh8FuJdgugcluCEswU2YnIdM+DRcfQbsyuOKKn0ZHznOIQvrflSJsPK10rVnxB50wk36UqvTV3SiBXZcxwEBU0OQHQNBdsznsyOnxf/oZIG0+F8nc/o/&lt;/diagram&gt;&lt;/mxfile&gt;"
 >
   <defs/>
   <g>
-    <rect x="962" y="2" width="800" height="440" fill="#cccccc" stroke="rgb(0, 0, 0)" stroke-width="4" pointer-events="all"/>
-    <rect x="2" y="2" width="800" height="440" fill="#cccccc" stroke="rgb(0, 0, 0)" stroke-width="4" pointer-events="all"/>
-    <rect x="23" y="22" width="320" height="400" fill="#fff2cc" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="961" y="2" width="800" height="440" fill="#cccccc" stroke="rgb(0, 0, 0)" stroke-width="4" pointer-events="all"/>
+    <rect x="1" y="2" width="800" height="440" fill="#cccccc" stroke="rgb(0, 0, 0)" stroke-width="4" pointer-events="all"/>
+    <rect x="22" y="22" width="320" height="400" fill="#fff2cc" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 318px; height: 1px; padding-top: 29px; margin-left: 24px;"
+            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 318px; height: 1px; padding-top: 29px; margin-left: 23px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -30,16 +31,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="183" y="41" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">approved modules stack</text>
+        <text x="182" y="41" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">approved modules stack</text>
       </switch>
     </g>
-    <rect x="462" y="22" width="320" height="400" fill="#fff2cc" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="461" y="22" width="320" height="400" fill="#fff2cc" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 318px; height: 1px; padding-top: 29px; margin-left: 463px;"
+            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 318px; height: 1px; padding-top: 29px; margin-left: 462px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -50,17 +51,17 @@
             </div>
           </div>
         </foreignObject>
-        <text x="622" y="41" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">candidate modules stack</text>
+        <text x="621" y="41" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">candidate modules stack</text>
       </switch>
     </g>
-    <path d="M 303 102 L 323 102 L 323 222 L 334.76 222" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 340.76 222 L 332.76 226 L 334.76 222 L 332.76 218 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
-    <path d="M 183 182 L 183 150.24" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 183 144.24 L 187 152.24 L 183 150.24 L 179 152.24 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 302 102 L 322 102 L 322 222 L 333.76 222" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 339.76 222 L 331.76 226 L 333.76 222 L 331.76 218 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 182 182 L 182 150.24" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 182 144.24 L 186 152.24 L 182 150.24 L 178 152.24 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 162px; margin-left: 183px;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 162px; margin-left: 182px;">
             <div data-drawio-colors="color: rgb(0, 0, 0); background-color: #FFD966; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
                 style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; background-color: rgb(255, 217, 102); white-space: nowrap;"
@@ -70,16 +71,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="183" y="165" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">previous module output</text>
+        <text x="182" y="165" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">previous module output</text>
       </switch>
     </g>
-    <rect x="63" y="182" width="240" height="80" fill="#3333ff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="62" y="182" width="240" height="80" fill="#3333ff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 222px; margin-left: 64px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 222px; margin-left: 63px;"
           >
             <div data-drawio-colors="color: #FFFFFF; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -90,15 +91,15 @@
             </div>
           </div>
         </foreignObject>
-        <text x="183" y="226" fill="#FFFFFF" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module A</text>
+        <text x="182" y="226" fill="#FFFFFF" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module A</text>
       </switch>
     </g>
-    <path d="M 183 303 L 183 270.24" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 183 264.24 L 187 272.24 L 183 270.24 L 179 272.24 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 182 303 L 182 270.24" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 182 264.24 L 186 272.24 L 182 270.24 L 178 272.24 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 282px; margin-left: 183px;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 283px; margin-left: 182px;">
             <div data-drawio-colors="color: rgb(0, 0, 0); background-color: #FFD966; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
                 style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; background-color: rgb(255, 217, 102); white-space: nowrap;"
@@ -108,16 +109,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="183" y="285" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">previous module output</text>
+        <text x="182" y="286" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">previous module output</text>
       </switch>
     </g>
-    <rect x="63" y="303" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="62" y="303" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 343px; margin-left: 64px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 343px; margin-left: 63px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -128,16 +129,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="183" y="347" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module B</text>
+        <text x="182" y="347" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module B</text>
       </switch>
     </g>
-    <rect x="502" y="304" width="240" height="78" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="501" y="304" width="240" height="78" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 343px; margin-left: 503px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 343px; margin-left: 502px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -148,18 +149,18 @@
             </div>
           </div>
         </foreignObject>
-        <text x="622" y="347" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module D</text>
+        <text x="621" y="347" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module D</text>
       </switch>
     </g>
-    <path d="M 23 222 L 42 222 L 42 342 L 53.76 342" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 59.76 342 L 51.76 346 L 53.76 342 L 51.76 338 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
-    <rect x="63" y="62" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <path d="M 22 222 L 41 222 L 41 342 L 52.76 342" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 58.76 342 L 50.76 346 L 52.76 342 L 50.76 338 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <rect x="62" y="62" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 102px; margin-left: 64px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 102px; margin-left: 63px;"
           >
             <div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -170,16 +171,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="183" y="106" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module C</text>
+        <text x="182" y="106" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module C</text>
       </switch>
     </g>
-    <path d="M 184 42 L 343 42 L 343 72 L 272.37 72 L 241.24 92 L 252.37 72 L 184 72 Z" fill="#990000" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 183 42 L 342 42 L 342 72 L 271.37 72 L 240.24 92 L 251.37 72 L 183 72 Z" fill="#990000" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 157px; height: 1px; padding-top: 57px; margin-left: 185px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 157px; height: 1px; padding-top: 57px; margin-left: 184px;"
           >
             <div data-drawio-colors="color: #FFFFFF; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -190,16 +191,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="264" y="61" fill="#FFFFFF" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">ModuleStatus::RUNNING</text>
+        <text x="262" y="61" fill="#FFFFFF" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">ModuleStatus::RUNNING</text>
       </switch>
     </g>
-    <rect x="1421.5" y="22" width="320" height="400" fill="#fff2cc" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="1420.5" y="22" width="320" height="400" fill="#fff2cc" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 318px; height: 1px; padding-top: 29px; margin-left: 1423px;"
+            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 318px; height: 1px; padding-top: 29px; margin-left: 1422px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -210,16 +211,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="1582" y="41" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">candidate modules stack</text>
+        <text x="1581" y="41" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">candidate modules stack</text>
       </switch>
     </g>
-    <rect x="1461.5" y="304" width="240" height="78" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="1460.5" y="304" width="240" height="78" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 343px; margin-left: 1463px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 343px; margin-left: 1462px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -230,17 +231,17 @@
             </div>
           </div>
         </foreignObject>
-        <text x="1582" y="347" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module D</text>
+        <text x="1581" y="347" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module D</text>
       </switch>
     </g>
-    <path d="M 862.5 227 L 862.5 217 L 882.5 217 L 882.5 206.5 L 901.5 222 L 882.5 237.5 L 882.5 227 Z" fill="#000000" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
-    <path d="M 184 162 L 343 162 L 343 192 L 272.37 192 L 241.24 212 L 252.37 192 L 184 192 Z" fill="#990000" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 861.5 227 L 861.5 217 L 881.5 217 L 881.5 206.5 L 900.5 222 L 881.5 237.5 L 881.5 227 Z" fill="#000000" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 183 162 L 342 162 L 342 192 L 271.37 192 L 240.24 212 L 251.37 192 L 183 192 Z" fill="#990000" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 157px; height: 1px; padding-top: 177px; margin-left: 185px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 157px; height: 1px; padding-top: 177px; margin-left: 184px;"
           >
             <div data-drawio-colors="color: #FFFFFF; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -251,16 +252,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="264" y="181" fill="#FFFFFF" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">ModuleStatus::SUCCESS</text>
+        <text x="262" y="181" fill="#FFFFFF" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">ModuleStatus::SUCCESS</text>
       </switch>
     </g>
-    <rect x="981.5" y="22" width="320" height="400" fill="#fff2cc" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="980.5" y="22" width="320" height="400" fill="#fff2cc" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 318px; height: 1px; padding-top: 29px; margin-left: 983px;"
+            style="display: flex; align-items: unsafe flex-start; justify-content: unsafe center; width: 318px; height: 1px; padding-top: 29px; margin-left: 982px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -271,19 +272,19 @@
             </div>
           </div>
         </foreignObject>
-        <text x="1142" y="41" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">approved modules stack</text>
+        <text x="1141" y="41" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">approved modules stack</text>
       </switch>
     </g>
-    <path d="M 1261.5 102 L 1281.5 102 L 1281.5 222 L 1293.26 222" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 1299.26 222 L 1291.26 226 L 1293.26 222 L 1291.26 218 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
-    <path d="M 1141.5 182 L 1141.5 150.24" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 1141.5 144.24 L 1145.5 152.24 L 1141.5 150.24 L 1137.5 152.24 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 1260.5 102 L 1280.33 102 L 1280.33 222 L 1292.26 222" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 1298.26 222 L 1290.26 226 L 1292.26 222 L 1290.26 218 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 1140.5 182 L 1140.5 150.24" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 1140.5 144.24 L 1144.5 152.24 L 1140.5 150.24 L 1136.5 152.24 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 162px; margin-left: 1142px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 162px; margin-left: 1141px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); background-color: #FFD966; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -294,16 +295,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="1142" y="165" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">previous module output</text>
+        <text x="1141" y="165" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">previous module output</text>
       </switch>
     </g>
-    <rect x="1021.5" y="182" width="240" height="80" fill="#3333ff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="1020.5" y="182" width="240" height="80" fill="#3333ff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 222px; margin-left: 1023px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 222px; margin-left: 1022px;"
           >
             <div data-drawio-colors="color: #FFFFFF; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -314,17 +315,17 @@
             </div>
           </div>
         </foreignObject>
-        <text x="1142" y="226" fill="#FFFFFF" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module A</text>
+        <text x="1141" y="226" fill="#FFFFFF" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module A</text>
       </switch>
     </g>
-    <path d="M 1141.5 303 L 1141.5 270.24" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 1141.5 264.24 L 1145.5 272.24 L 1141.5 270.24 L 1137.5 272.24 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 1140.5 303 L 1140.5 270.24" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 1140.5 264.24 L 1144.5 272.24 L 1140.5 270.24 L 1136.5 272.24 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 282px; margin-left: 1142px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 283px; margin-left: 1141px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); background-color: #FFD966; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -335,16 +336,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="1142" y="285" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">previous module output</text>
+        <text x="1141" y="286" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">previous module output</text>
       </switch>
     </g>
-    <rect x="1021.5" y="303" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="1020.5" y="303" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 343px; margin-left: 1023px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 343px; margin-left: 1022px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -355,18 +356,18 @@
             </div>
           </div>
         </foreignObject>
-        <text x="1142" y="347" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module B</text>
+        <text x="1141" y="347" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module B</text>
       </switch>
     </g>
-    <path d="M 981.5 222 L 1000.5 222 L 1000.5 342 L 1012.26 342" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 1018.26 342 L 1010.26 346 L 1012.26 342 L 1010.26 338 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
-    <rect x="1021.5" y="62" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <path d="M 980.5 222 L 999.67 222 L 999.67 342 L 1011.26 342" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 1017.26 342 L 1009.26 346 L 1011.26 342 L 1009.26 338 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <rect x="1020.5" y="62" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 102px; margin-left: 1023px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 238px; height: 1px; padding-top: 102px; margin-left: 1022px;"
           >
             <div data-drawio-colors="color: #000000; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -377,11 +378,11 @@
             </div>
           </div>
         </foreignObject>
-        <text x="1142" y="106" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module C</text>
+        <text x="1141" y="106" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module C</text>
       </switch>
     </g>
     <path
-      d="M 1142.5 42 L 1301.5 42 L 1301.5 72 L 1230.87 72 L 1199.74 92 L 1210.87 72 L 1142.5 72 Z"
+      d="M 1141.5 42 L 1300.5 42 L 1300.5 72 L 1229.87 72 L 1198.74 92 L 1209.87 72 L 1141.5 72 Z"
       fill="#990000"
       stroke="rgb(0, 0, 0)"
       stroke-width="2"
@@ -393,7 +394,7 @@
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 157px; height: 1px; padding-top: 57px; margin-left: 1144px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 157px; height: 1px; padding-top: 57px; margin-left: 1143px;"
           >
             <div data-drawio-colors="color: #FFFFFF; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -404,11 +405,11 @@
             </div>
           </div>
         </foreignObject>
-        <text x="1222" y="61" fill="#FFFFFF" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">ModuleStatus::RUNNING</text>
+        <text x="1221" y="61" fill="#FFFFFF" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">ModuleStatus::RUNNING</text>
       </switch>
     </g>
     <path
-      d="M 1142.5 162 L 1301.5 162 L 1301.5 192 L 1230.87 192 L 1199.74 212 L 1210.87 192 L 1142.5 192 Z"
+      d="M 1141.5 162 L 1300.5 162 L 1300.5 192 L 1229.87 192 L 1198.74 212 L 1209.87 192 L 1141.5 192 Z"
       fill="#990000"
       stroke="rgb(0, 0, 0)"
       stroke-width="2"
@@ -420,7 +421,7 @@
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 157px; height: 1px; padding-top: 177px; margin-left: 1144px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 157px; height: 1px; padding-top: 177px; margin-left: 1143px;"
           >
             <div data-drawio-colors="color: #FFFFFF; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -431,16 +432,16 @@
             </div>
           </div>
         </foreignObject>
-        <text x="1222" y="181" fill="#FFFFFF" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">ModuleStatus::SUCCESS</text>
+        <text x="1221" y="181" fill="#FFFFFF" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">ModuleStatus::SUCCESS</text>
       </switch>
     </g>
-    <rect x="1298.5" y="207" width="125" height="30" fill="none" stroke="none" pointer-events="all"/>
+    <rect x="1297.5" y="207" width="125" height="30" fill="none" stroke="none" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
           <div
             xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 123px; height: 1px; padding-top: 222px; margin-left: 1300px;"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 123px; height: 1px; padding-top: 222px; margin-left: 1299px;"
           >
             <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
@@ -451,7 +452,7 @@
             </div>
           </div>
         </foreignObject>
-        <text x="1361" y="226" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">Do nothing.</text>
+        <text x="1360" y="226" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">Do nothing.</text>
       </switch>
     </g>
   </g>
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/waiting_approve.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/waiting_approve.drawio.svg
similarity index 86%
rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/waiting_approve.svg
rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/waiting_approve.drawio.svg
index 41345df4f3fb1..5177ba100b2a3 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/waiting_approve.svg
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/waiting_approve.drawio.svg
@@ -1,13 +1,14 @@
 <?xml version="1.0" encoding="UTF-8"?>
+<!-- Do not edit this file with editors other than diagrams.net -->
 <!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
 <svg
   xmlns="http://www.w3.org/2000/svg"
   xmlns:xlink="http://www.w3.org/1999/xlink"
   version="1.1"
   width="1764px"
-  height="444px"
-  viewBox="-0.5 -0.5 1764 444"
-  style="background-color: rgb(255, 255, 255);"
+  height="445px"
+  viewBox="-0.5 -0.5 1764 445"
+  content="&lt;mxfile host=&quot;Electron&quot; modified=&quot;2024-07-31T06:58:54.624Z&quot; agent=&quot;5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36&quot; etag=&quot;mtzSXlK8YEB8BHNq9_p6&quot; version=&quot;20.3.0&quot; type=&quot;device&quot;&gt;&lt;diagram id=&quot;FbG0muvkg23WBpYGoVLW&quot; name=&quot;ページ7&quot;&gt;7VtNc6M4EP01Pk6KL/FxTJx49jBTlZnU1uycthSjADsYUSAn9v76lUAYUGNjjyEmyTqHQIME9Hv9utXYM3O+2nzOcBp+pT6JZ4bmb2bm7cwwPum67fH/wrSVJtOTliCL/NKm14aH6F8ijZq0riOf5K0TGaUxi9K2cUmThCxZy4azjL60T3uicfuqKQ4IMDwscQytPyKfhaXVNZza/geJgrC68u6JV7g6WT5JHmKfvjRM5t3MnGeUsnJrtZmTWLiv8ks5brHn6O7GMpKwYwZYhv+dPD2Yof/t2/bPRegF+dMnw5HzPON4LR9Z3i7bVj7I6DrxiZhGm5k3L2HEyEOKl+LoC8ed20K2ivmezjefojie05hmxVhzXny4PWcZ/bXzocUt8AGqeyEZI5uGST7QZ0JXhGVbfoo8iuzKu5JfCLnS8NJAS5O2sIGUZUkjlgwJdrPXTuQb0o8n+VR7yz61bHuCPrWgT3GaZvSZ+5AHG/XXMZcJ4WK8/HWmt2nCHuTYDu8vFguj8L5wacSV4jqOgoQfYzQdCgFXbyNgaxAB0+hCQBsPAagUS5z4kY8ZmRYE7bDQB5IaXZWaSYAClYX4PIHJXZqxkAY0wfFdbb1pQ1Gf84UK9xUO+4cwtpXZGK8ZbcNDNhH7q7H9U0x1heTe7UbOXOxsq52EP/BukNhpjBK79bBirxrXBtM4BGZO19mSHHIXku5iOAsIO+hYCZnw5kF6ZCTGLHpulwtdQBdDr7MMbxsnpDRKWN6Y+V4YGlLgASlwlTTfO6KS6ppo5V3UtNs9zjlMNIE8pBl5jug636kD36Brlq7ZJUlbE/Vnk6eHSdvg6c8mh/eQVtWvGD+S+IbLYlA8RkvKbj3bHprnlnUsz6uIGI7nZwqaBWiUL0lCmhy6Hja5qElkFfl+QTkl64jPYiEnaGejRWE/AcITigHHUOLZsWHe2elVM++446Ud9A6DHcL3G2F8TtDaRycna2JBa/cH7c1lgvbam1t3N2OFolvl2suFIoILI+D727fve6Srq1LXNY7zveOO5nsdOPY1te2E6nvY6mKc2hiWso665PIMJY5KrZTjajhPLrMdTWWX1lNmgxGe8RpldlWvHQr3+YXDXamPtOIzlgTb1sWrIQRr1h84YlESiMsVXSocA0zyEKdik3s25oVSPy4pySJ+xySrB93XJgBbRznTxsvzJC776tmU5vwpaGIUAmMWq5RSlAytcbg4ypd/wwDsIiWuHBMBgPWuNgsaD2BY7r7b5mMdUFPqcyFYYX6k5qOtOd4EQXEuWf68ueajXb2/7G/KoFEKrFOrIuSpUuCp7xh7RxxsPnaMB012NW2XfgYV33AFltdfYH2oBpSt64r2TKABVcXSO+841PSvV72X9/0RS5B30HGAvJ9Ax8E2L5lyz+s41C9TTkvCZ6TcKpH2p9xKUMbuaSBbreTG6mkg8D2Ovp4GHPE6PQ0HCgpgebVaforJ5lp8r6ugiS83b5cxzvNo2aZtm/eKVPR3JXqp0JABdGA9ei5jkFoTuWpJv6cm6phqHyX6yDcc1vA18VTKqztb/MHyyis+nUu70tI419XE31jJfwKFl3PE+9kL9R+PxO/V0JpAZ9KBbZMRlBX4daet+zqOk9dcsI51jlyHgqlsXX1RAKYaXXMdwIKv9FlEKi/0DO1AI+0KsIVHA2tzoY1+QhM1NKUJy/DlalE2rPfGdZc+KLm8JREDBK+HnCukwNTZd0aQc+Z44Qt7Ef+H72+FL6+6K3xPDmCttyc1dgBX1zvEgyqavojvqNzXr2YeKWN0tbdd3QxkyaTVJhC/5rh6xJw4Vwn9O9+uHmnc1Qkf7sWerZmqlzuKna7sOd53zl3YZPpOVlw5P7Is2iaEZShZ5Lv172HK6Kl/V2Te/Qc=&lt;/diagram&gt;&lt;/mxfile&gt;"
 >
   <defs/>
   <g>
@@ -53,8 +54,8 @@
         <text x="622" y="41" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">candidate modules stack</text>
       </switch>
     </g>
-    <path d="M 303 102 L 323 102 L 323 222 L 334.76 222" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 340.76 222 L 332.76 226 L 334.76 222 L 332.76 218 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 303 102 L 323 102.14 L 323 222.14 L 334.76 222.06" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 340.76 222.02 L 332.79 226.07 L 334.76 222.06 L 332.74 218.07 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <path d="M 183 182 L 183 150.24" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
     <path d="M 183 144.24 L 187 152.24 L 183 150.24 L 179 152.24 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
@@ -98,7 +99,7 @@
     <g transform="translate(-0.5 -0.5)">
       <switch>
         <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 282px; margin-left: 183px;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 283px; margin-left: 183px;">
             <div data-drawio-colors="color: rgb(0, 0, 0); background-color: #FFD966; " style="box-sizing: border-box; font-size: 0px; text-align: center;">
               <div
                 style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; background-color: rgb(255, 217, 102); white-space: nowrap;"
@@ -108,7 +109,7 @@
             </div>
           </div>
         </foreignObject>
-        <text x="183" y="285" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">previous module output</text>
+        <text x="183" y="286" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle" font-weight="bold">previous module output</text>
       </switch>
     </g>
     <rect x="63" y="303" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
@@ -151,8 +152,8 @@
         <text x="622" y="347" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module D</text>
       </switch>
     </g>
-    <path d="M 23 222 L 42 222 L 42 342 L 53.76 342" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 59.76 342 L 51.76 346 L 53.76 342 L 51.76 338 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 23 222 L 42.14 222.14 L 42.14 342.14 L 53.76 342.06" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 59.76 342.02 L 51.79 346.07 L 53.76 342.06 L 51.74 338.07 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <rect x="63" y="62" width="240" height="80" fill="#a9c4eb" stroke="rgb(0, 0, 0)" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
@@ -233,8 +234,8 @@
         <text x="1581" y="41" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">candidate modules stack</text>
       </switch>
     </g>
-    <path d="M 1262 342 L 1282 342 L 1282 222 L 1293.76 222" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 1299.76 222 L 1291.76 226 L 1293.76 222 L 1291.76 218 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 1262 342 L 1282.14 342.14 L 1282.14 222.14 L 1293.76 222.06" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 1299.76 222.02 L 1291.79 226.07 L 1293.76 222.06 L 1291.74 218.07 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <rect x="1461" y="182" width="240" height="80" fill="#3333ff" stroke="rgb(0, 0, 0)" stroke-width="2" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
       <switch>
@@ -295,8 +296,8 @@
         <text x="1581" y="347" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle" font-weight="bold">scene module D</text>
       </switch>
     </g>
-    <path d="M 982 222 L 1002 222 L 1002 342 L 1013.76 342" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 1019.76 342 L 1011.76 346 L 1013.76 342 L 1011.76 338 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 982 222 L 1002.14 222.14 L 1002.14 342.14 L 1013.76 342.06" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 1019.76 342.02 L 1011.79 346.07 L 1013.76 342.06 L 1011.74 338.07 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
     <path d="M 862.5 227 L 862.5 217 L 882.5 217 L 882.5 206.5 L 901.5 222 L 882.5 237.5 L 882.5 227 Z" fill="#000000" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
     <rect x="1022" y="182" width="240" height="80" fill="#e6e6e6" stroke="#808080" pointer-events="all"/>
     <g transform="translate(-0.5 -0.5)">
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp
index 0876d44b477a9..77f2ffc26f44d 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp
@@ -49,15 +49,14 @@ using DebugPublisher = autoware::universe_utils::DebugPublisher;
 using DebugDoubleMsg = tier4_debug_msgs::msg::Float64Stamped;
 using DebugStringMsg = tier4_debug_msgs::msg::StringStamped;
 
-enum Action {
-  ADD = 0,
-  DELETE,
-  MOVE,
-};
-
-struct ModuleUpdateInfo
+struct SceneModuleUpdateInfo
 {
-  explicit ModuleUpdateInfo(
+  enum Action {
+    ADD = 0,
+    DELETE,
+    MOVE,
+  };
+  explicit SceneModuleUpdateInfo(
     const SceneModulePtr & module_ptr, const Action & action, const std::string & description)
   : status(module_ptr->getCurrentStatus()),
     action(action),
@@ -66,7 +65,7 @@ struct ModuleUpdateInfo
   {
   }
 
-  explicit ModuleUpdateInfo(
+  explicit SceneModuleUpdateInfo(
     const std::string & name, const Action & action, const ModuleStatus & status,
     const std::string & description)
   : status(status), action(action), module_name(name), description(description)
@@ -82,6 +81,19 @@ struct ModuleUpdateInfo
   std::string description;
 };
 
+enum SlotStatus {
+  NORMAL = 0,
+  UPSTREAM_APPROVED_FAILED,
+  UPSTREAM_WAITING_APPROVED,
+  UPSTREAM_EXCLUSIVE_CANDIDATE
+};
+
+struct ModuleUpdateInfo
+{
+  std::vector<SceneModuleUpdateInfo> scene_status;
+  std::vector<SlotStatus> slot_status;
+};
+
 struct SceneModuleStatus
 {
   explicit SceneModuleStatus(const std::string & n) : module_name(n) {}
@@ -94,6 +106,264 @@ struct SceneModuleStatus
   ModuleStatus status{ModuleStatus::SUCCESS};
 };
 
+struct SlotOutput
+{
+  BehaviorModuleOutput valid_output;
+
+  // if candidate module is running, valid_output contains the planning by candidate module. In
+  // that case, downstream slots will just run approved modules and do not try to launch new
+  // modules
+  bool is_upstream_candidate_exclusive{false};
+
+  // if this slot failed, downstream slots need to refresh approved/candidate modules and just
+  // forward valid_output of this slot output
+  bool is_upstream_failed_approved{false};
+
+  // if the approved module in this slot returned to isWaitingApproval, downstream slots need to
+  // refresh candidate once
+  bool is_upstream_waiting_approved{false};
+};
+
+class SubPlannerManager
+{
+public:
+  explicit SubPlannerManager(
+    std::shared_ptr<std::optional<lanelet::ConstLanelet>> lanelet,
+    std::unordered_map<std::string, double> & processing_time, ModuleUpdateInfo & debug_info)
+  : current_route_lanelet_(lanelet),
+    processing_time_(std::ref(processing_time)),
+    debug_info_(std::ref(debug_info))
+  {
+  }
+
+  void addSceneModuleManager(const SceneModuleManagerPtr module_ptr)
+  {
+    manager_ptrs_.push_back(module_ptr);
+    // NOTE(Mamoru Sobue): SceneModuleManagerPtr->name() == SceneModulePtr->name()
+    module_priorities_[module_ptr->name()] = module_priorities_.size();
+  }
+
+  const std::vector<SceneModuleManagerPtr> & getSceneModuleManager() const { return manager_ptrs_; }
+
+  /**
+   * @pre previous_slot_output.is_upstream_candidate_exclusive is false
+   * @pre previous_slot_output.is_upstream_failed_approved is false
+   */
+  SlotOutput propagateFull(
+    const std::shared_ptr<PlannerData> & planner_data, const SlotOutput & previous_slot_output);
+
+  /**
+   * @brief run approved modules without launching new modules(candidates)
+   * @pre previous_slot_output.is_upstream_candidate_exclusive is true
+   * @post is_upstream_candidate_exclusive of output is true
+   * @post candidate_module does not increase
+   */
+  SlotOutput propagateWithExclusiveCandidate(
+    const std::shared_ptr<PlannerData> & planner_data, const SlotOutput & previous_slot_output);
+
+  /**
+   * @brief delete approved/candidate modules and just forward upstream output
+   * @post candidate_module and approved_module get empty
+   */
+  void propagateWithFailedApproved();
+
+  /**
+   * @pre previous_slot_output.is_upstream_waiting_approved is true
+   * @post is_upstream_waiting_approved of output is true
+   */
+  SlotOutput propagateWithWaitingApproved(
+    const std::shared_ptr<PlannerData> & planner_data, const SlotOutput & previous_slot_output);
+
+  template <typename F>
+  bool isAnyApprovedPred(F && pred) const
+  {
+    return std::any_of(
+      approved_module_ptrs_.begin(), approved_module_ptrs_.end(),
+      [&](const auto & m) { return pred(m); });
+  }
+
+  template <typename F>
+  bool isAnyCandidatePred(F && pred) const
+  {
+    return std::any_of(
+      candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(),
+      [&](const auto & m) { return pred(m); });
+  }
+
+  const std::vector<SceneModulePtr> & approved_modules() const { return approved_module_ptrs_; }
+
+  const std::vector<SceneModulePtr> & candidate_modules() const { return candidate_module_ptrs_; }
+
+  /**
+   * @brief reset the current route lanelet to be the closest lanelet within the route
+   * @param planner data.
+   */
+  void resetCurrentRouteLanelet(const std::shared_ptr<PlannerData> & data)
+  {
+    lanelet::ConstLanelet ret{};
+    data->route_handler->getClosestLaneletWithinRoute(data->self_odometry->pose.pose, &ret);
+    *current_route_lanelet_ = ret;
+  }
+
+  void reset()
+  {
+    approved_module_ptrs_.clear();
+    candidate_module_ptrs_.clear();
+  }
+
+private:
+  /**
+   * @brief get all modules that make execution request.
+   * @param decided (=approved) path.
+   * @return request modules.
+   */
+  std::vector<SceneModulePtr> getRequestModules(
+    const BehaviorModuleOutput & previous_module_output,
+    const std::vector<SceneModulePtr> & deleted_modules) const;
+
+  /**
+   * @brief get manager pointer from module name.
+   * @param module pointer.
+   * @return manager pointer.
+   */
+  SceneModuleManagerPtr getManager(const SceneModulePtr & module_ptr) const
+  {
+    const auto itr = std::find_if(
+      manager_ptrs_.begin(), manager_ptrs_.end(),
+      [&module_ptr](const auto & m) { return m->name() == module_ptr->name(); });
+
+    if (itr == manager_ptrs_.end()) {
+      throw std::domain_error("unknown manager name.");
+    }
+
+    return *itr;
+  }
+
+  /**
+   * @brief select a module that should be execute at first.
+   * @param modules that make execution request.
+   * @return the highest priority module.
+   */
+  SceneModulePtr selectHighestPriorityModule(std::vector<SceneModulePtr> & request_modules) const;
+
+  /**
+   * @brief swap the modules order based on it's priority.
+   * @param modules.
+   * @details for now, the priority is decided in config file and doesn't change runtime.
+   */
+  void sortByPriority(std::vector<SceneModulePtr> & modules) const
+  {
+    // NOTE(Mamoru Sobue): SceneModuleManagerPtr->name() == SceneModulePtr->name()
+    std::sort(modules.begin(), modules.end(), [this](auto a, auto b) {
+      const auto a_it = module_priorities_.find(a->name());
+      const auto b_it = module_priorities_.find(b->name());
+      if (a_it == module_priorities_.end() || b_it == module_priorities_.end()) {
+        throw std::domain_error("unknown module name");
+      }
+      return a_it->second < b_it->second;
+    });
+  }
+
+  /**
+   * @brief stop and unregister the module from manager.
+   * @param module.
+   */
+  void deleteExpiredModules(SceneModulePtr & module_ptr) const
+  {
+    module_ptr->onExit();
+    module_ptr->publishObjectsOfInterestMarker();
+    module_ptr.reset();
+  }
+
+  /**
+   * @brief stop and remove all modules in candidate_module_ptrs_.
+   */
+  void clearCandidateModules()
+  {
+    std::for_each(candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), [this](auto & m) {
+      deleteExpiredModules(m);
+    });
+    candidate_module_ptrs_.clear();
+  }
+
+  /**
+   * @brief stop and remove all modules in approved_module_ptrs_.
+   */
+  void clearApprovedModules();
+
+  /**
+   * @brief update candidate_module_ptrs_ based on latest request modules.
+   * @param the highest priority module in latest request modules.
+   */
+  void updateCandidateModules(
+    const std::vector<SceneModulePtr> & request_modules,
+    const SceneModulePtr & highest_priority_module);
+
+  /**
+   * @brief run the module and publish RTC status.
+   * @param module.
+   * @param planner data.
+   * @return planning result.
+   */
+  BehaviorModuleOutput run(
+    const SceneModulePtr & module_ptr, const std::shared_ptr<PlannerData> & planner_data,
+    const BehaviorModuleOutput & previous_module_output) const;
+
+  /**
+   * @brief checks whether a path of trajectory has forward driving direction
+   * @param modules that make execution request.
+   * @param planner data.
+   * @param decided (=approved) path.
+   * @return the highest priority module in request modules, and it's planning result.
+   */
+  std::pair<SceneModulePtr, BehaviorModuleOutput> runRequestModules(
+    const std::vector<SceneModulePtr> & request_modules, const std::shared_ptr<PlannerData> & data,
+    const BehaviorModuleOutput & previous_module_output);
+
+  /**
+   * @brief run all modules in approved_module_ptrs_ and get a planning result as
+   * approved_modules_output.
+   * @param planner data.
+   * @return valid planning result.
+   * @details in this function, expired modules (ModuleStatus::FAILURE or ModuleStatus::SUCCESS) are
+   * removed from approved_module_ptrs_.
+   */
+  SlotOutput runApprovedModules(
+    const std::shared_ptr<PlannerData> & data, const BehaviorModuleOutput & upstream_slot_output);
+
+  /**
+   * @brief push back to approved_module_ptrs_.
+   * @param approved module pointer.
+   */
+  void addApprovedModule(const SceneModulePtr & module_ptr)
+  {
+    approved_module_ptrs_.push_back(module_ptr);
+  }
+
+  bool isAnyCandidateExclusive() const
+  {
+    return std::any_of(
+      candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(),
+      [&](const auto & m) { return !getManager(m)->isSimultaneousExecutableAsCandidateModule(); });
+  }
+
+  std::vector<SceneModuleManagerPtr> manager_ptrs_;
+
+  std::unordered_map<std::string, size_t> module_priorities_;
+
+  // all the slots share the current_route_lanelet of PlannerManager, so if one of the slot changed
+  // it, it is reflected to later calculations as well
+  std::shared_ptr<std::optional<lanelet::ConstLanelet>> current_route_lanelet_{nullptr};
+
+  std::unordered_map<std::string, double> & processing_time_;
+
+  std::vector<SceneModulePtr> approved_module_ptrs_;
+
+  std::vector<SceneModulePtr> candidate_module_ptrs_;
+
+  ModuleUpdateInfo & debug_info_;
+};
+
 class PlannerManager
 {
 public:
@@ -112,23 +382,28 @@ class PlannerManager
    */
   void launchScenePlugin(rclcpp::Node & node, const std::string & name);
 
-  /**
-   * @brief calculate max iteration numbers.
-   * Let N be the number of scene modules. The maximum number of iterations executed in a loop is N,
-   * but after that, if there are any modules that have succeeded or failed, the approve_modules of
-   * all modules are cleared, and the loop is executed for N-1 modules. As this process repeats, it
-   * becomes N + (N-1) + (N-2) + … + 1, therefore the maximum number of iterations is N(N+1)/2.
-   * @param number of scene module
-   *
-   */
-  void calculateMaxIterationNum(const size_t scene_module_num);
+  void configureModuleSlot(const std::vector<std::vector<std::string>> & slot_configuration);
 
-  /**
-   * @brief unregister managers.
-   * @param node.
-   * @param plugin name.
-   */
-  void removeScenePlugin(rclcpp::Node & node, const std::string & name);
+  std::vector<SceneModulePtr> approved_modules() const
+  {
+    std::vector<SceneModulePtr> modules;
+    for (const auto & planner_manager_slot : planner_manager_slots_) {
+      const auto & sub_modules = planner_manager_slot.approved_modules();
+      std::copy(sub_modules.begin(), sub_modules.end(), std::back_inserter(modules));
+    }
+
+    return modules;
+  }
+
+  std::vector<SceneModulePtr> candidate_modules() const
+  {
+    std::vector<SceneModulePtr> modules;
+    for (const auto & planner_manager_slot : planner_manager_slots_) {
+      const auto & sub_modules = planner_manager_slot.candidate_modules();
+      std::copy(sub_modules.begin(), sub_modules.end(), std::back_inserter(modules));
+    }
+    return modules;
+  }
 
   /**
    * @brief update module parameters. used for dynamic reconfigure.
@@ -147,10 +422,11 @@ class PlannerManager
   void reset()
   {
     std::for_each(manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->reset(); });
-    approved_module_ptrs_.clear();
-    candidate_module_ptrs_.clear();
-    current_route_lanelet_ = std::nullopt;
+    *current_route_lanelet_ = std::nullopt;
     resetProcessingTime();
+    for (auto & planner_manager_slot : planner_manager_slots_) {
+      planner_manager_slot.reset();
+    }
   }
 
   /**
@@ -185,18 +461,20 @@ class PlannerManager
   {
     std::vector<std::shared_ptr<SceneModuleStatus>> ret;
 
-    const auto size = approved_module_ptrs_.size() + candidate_module_ptrs_.size();
+    const auto approved_module_ptrs = approved_modules();
+    const auto candidate_module_ptrs = candidate_modules();
+    const auto size = approved_module_ptrs.size() + candidate_module_ptrs.size();
 
     ret.reserve(size);
 
-    for (const auto & m : approved_module_ptrs_) {
+    for (const auto & m : approved_module_ptrs) {
       auto s = std::make_shared<SceneModuleStatus>(m->name());
       s->is_waiting_approval = m->isWaitingApproval();
       s->status = m->getCurrentStatus();
       ret.push_back(s);
     }
 
-    for (const auto & m : candidate_module_ptrs_) {
+    for (const auto & m : candidate_module_ptrs) {
       auto s = std::make_shared<SceneModuleStatus>(m->name());
       s->is_waiting_approval = m->isWaitingApproval();
       s->status = m->getCurrentStatus();
@@ -216,40 +494,31 @@ class PlannerManager
     stop_reason_array.header.frame_id = "map";
     stop_reason_array.header.stamp = clock_.now();
 
-    std::for_each(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [&](const auto & m) {
+    const auto approved_module_ptrs = approved_modules();
+    const auto candidate_module_ptrs = candidate_modules();
+
+    std::for_each(approved_module_ptrs.begin(), approved_module_ptrs.end(), [&](const auto & m) {
       const auto reason = m->getStopReason();
       if (reason.reason != "") {
         stop_reason_array.stop_reasons.push_back(m->getStopReason());
       }
     });
 
-    std::for_each(
-      candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), [&](const auto & m) {
-        const auto reason = m->getStopReason();
-        if (reason.reason != "") {
-          stop_reason_array.stop_reasons.push_back(m->getStopReason());
-        }
-      });
+    std::for_each(candidate_module_ptrs.begin(), candidate_module_ptrs.end(), [&](const auto & m) {
+      const auto reason = m->getStopReason();
+      if (reason.reason != "") {
+        stop_reason_array.stop_reasons.push_back(m->getStopReason());
+      }
+    });
 
     return stop_reason_array;
   }
 
   /**
-   * @brief check if there are approved modules.
-   */
-  bool hasApprovedModules() const { return !approved_module_ptrs_.empty(); }
-
-  bool hasNonAlwaysExecutableApprovedModules() const
-  {
-    return std::any_of(
-      approved_module_ptrs_.begin(), approved_module_ptrs_.end(),
-      [this](const auto & m) { return !getManager(m)->isAlwaysExecutableModule(); });
-  }
-
-  /**
-   * @brief check if there are candidate modules.
+   * @brief check if re-routable approved module is running(namely except for fixed_goal_planner
+   * and dynamic_avoidance)
    */
-  bool hasCandidateModules() const { return !candidate_module_ptrs_.empty(); }
+  bool hasPossibleRerouteApprovedModules(const std::shared_ptr<PlannerData> & data) const;
 
   /**
    * @brief show planner manager internal condition.
@@ -275,43 +544,10 @@ class PlannerManager
     lanelet::ConstLanelet ret{};
     data->route_handler->getClosestLaneletWithinRoute(data->self_odometry->pose.pose, &ret);
     RCLCPP_DEBUG(logger_, "update current route lanelet. id:%ld", ret.id());
-    current_route_lanelet_ = ret;
+    *current_route_lanelet_ = ret;
   }
 
 private:
-  /**
-   * @brief run the module and publish RTC status.
-   * @param module.
-   * @param planner data.
-   * @return planning result.
-   */
-  BehaviorModuleOutput run(
-    const SceneModulePtr & module_ptr, const std::shared_ptr<PlannerData> & planner_data,
-    const BehaviorModuleOutput & previous_module_output) const
-  {
-    universe_utils::ScopedTimeTrack st(module_ptr->name() + "=>run", *module_ptr->getTimeKeeper());
-    stop_watch_.tic(module_ptr->name());
-
-    module_ptr->setData(planner_data);
-    module_ptr->setPreviousModuleOutput(previous_module_output);
-
-    module_ptr->lockRTCCommand();
-    const auto result = module_ptr->run();
-    module_ptr->unlockRTCCommand();
-
-    module_ptr->postProcess();
-
-    module_ptr->updateCurrentState();
-
-    module_ptr->publishSteeringFactor();
-
-    module_ptr->publishObjectsOfInterestMarker();
-
-    processing_time_.at(module_ptr->name()) += stop_watch_.toc(module_ptr->name(), true);
-
-    return result;
-  }
-
   /**
    * @brief find and set the closest lanelet within the route to current route lanelet
    * @param planner data.
@@ -333,39 +569,6 @@ class PlannerManager
    */
   void publishDebugRootReferencePath(const BehaviorModuleOutput & reference_path) const;
 
-  /**
-   * @brief stop and unregister the module from manager.
-   * @param module.
-   */
-  void deleteExpiredModules(SceneModulePtr & module_ptr) const
-  {
-    module_ptr->onExit();
-    module_ptr->publishObjectsOfInterestMarker();
-    module_ptr.reset();
-  }
-
-  /**
-   * @brief swap the modules order based on it's priority.
-   * @param modules.
-   * @details for now, the priority is decided in config file and doesn't change runtime.
-   */
-  void sortByPriority(std::vector<SceneModulePtr> & modules) const
-  {
-    // TODO(someone) enhance this priority decision method.
-    std::sort(modules.begin(), modules.end(), [this](auto a, auto b) {
-      return getManager(a)->getPriority() < getManager(b)->getPriority();
-    });
-  }
-
-  /**
-   * @brief push back to approved_module_ptrs_.
-   * @param approved module pointer.
-   */
-  void addApprovedModule(const SceneModulePtr & module_ptr)
-  {
-    approved_module_ptrs_.push_back(module_ptr);
-  }
-
   /**
    * @brief reset processing time.
    */
@@ -376,121 +579,30 @@ class PlannerManager
     }
   }
 
-  /**
-   * @brief stop and remove all modules in candidate_module_ptrs_.
-   */
-  void clearCandidateModules()
-  {
-    std::for_each(candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), [this](auto & m) {
-      deleteExpiredModules(m);
-    });
-    candidate_module_ptrs_.clear();
-  }
-
-  /**
-   * @brief get manager pointer from module name.
-   * @param module pointer.
-   * @return manager pointer.
-   */
-  SceneModuleManagerPtr getManager(const SceneModulePtr & module_ptr) const
-  {
-    const auto itr = std::find_if(
-      manager_ptrs_.begin(), manager_ptrs_.end(),
-      [&module_ptr](const auto & m) { return m->name() == module_ptr->name(); });
-
-    if (itr == manager_ptrs_.end()) {
-      throw std::domain_error("unknown manager name.");
-    }
-
-    return *itr;
-  }
-
-  /**
-   * @brief run all modules in approved_module_ptrs_ and get a planning result as
-   * approved_modules_output.
-   * @param planner data.
-   * @param deleted modules.
-   * @return valid planning result.
-   * @details in this function, expired modules (ModuleStatus::FAILURE or ModuleStatus::SUCCESS) are
-   * removed from approved_module_ptrs_ and added to deleted_modules.
-   */
-  BehaviorModuleOutput runApprovedModules(
-    const std::shared_ptr<PlannerData> & data, std::vector<SceneModulePtr> & deleted_modules);
-
-  /**
-   * @brief select a module that should be execute at first.
-   * @param modules that make execution request.
-   * @return the highest priority module.
-   */
-  SceneModulePtr selectHighestPriorityModule(std::vector<SceneModulePtr> & request_modules) const;
-
-  /**
-   * @brief update candidate_module_ptrs_ based on latest request modules.
-   * @param the highest priority module in latest request modules.
-   */
-  void updateCandidateModules(
-    const std::vector<SceneModulePtr> & request_modules,
-    const SceneModulePtr & highest_priority_module);
+  // NOTE: current_route_lanelet_ is shared with SubPlannerManagers
+  std::shared_ptr<std::optional<lanelet::ConstLanelet>> current_route_lanelet_;
 
-  /**
-   * @brief get all modules that make execution request.
-   * @param decided (=approved) path.
-   * @param deleted modules.
-   * @return request modules.
-   */
-  std::vector<SceneModulePtr> getRequestModules(
-    const BehaviorModuleOutput & previous_module_output,
-    const std::vector<SceneModulePtr> & deleted_modules) const;
-
-  /**
-   * @brief checks whether a path of trajectory has forward driving direction
-   * @param modules that make execution request.
-   * @param planner data.
-   * @param decided (=approved) path.
-   * @return the highest priority module in request modules, and it's planning result.
-   */
-  std::pair<SceneModulePtr, BehaviorModuleOutput> runRequestModules(
-    const std::vector<SceneModulePtr> & request_modules, const std::shared_ptr<PlannerData> & data,
-    const BehaviorModuleOutput & previous_module_output);
-
-  /**
-   * @brief run keep last approved modules
-   * @param planner data.
-   * @param previous module output.
-   * @return planning result.
-   */
-  BehaviorModuleOutput runKeepLastModules(
-    const std::shared_ptr<PlannerData> & data, const BehaviorModuleOutput & previous_output) const;
-
-  static std::string getNames(const std::vector<SceneModulePtr> & modules);
-
-  std::optional<lanelet::ConstLanelet> current_route_lanelet_{std::nullopt};
+  // NOTE: SubPlannerManager::manager_ptrs_ and manager_ptrs_ share the same SceneModuleManager
+  // instance as shared_ptr
+  std::vector<SubPlannerManager> planner_manager_slots_;
 
   std::vector<SceneModuleManagerPtr> manager_ptrs_;
 
-  std::vector<SceneModulePtr> approved_module_ptrs_;
-
-  std::vector<SceneModulePtr> candidate_module_ptrs_;
-
   std::unique_ptr<DebugPublisher> debug_publisher_ptr_;
 
   std::unique_ptr<DebugPublisher> state_publisher_ptr_;
 
   pluginlib::ClassLoader<SceneModuleManagerInterface> plugin_loader_;
 
-  mutable rclcpp::Logger logger_;
+  rclcpp::Logger logger_;
 
   mutable rclcpp::Clock clock_;
 
-  mutable StopWatch<std::chrono::milliseconds> stop_watch_;
-
-  mutable std::unordered_map<std::string, double> processing_time_;
-
-  mutable std::vector<ModuleUpdateInfo> debug_info_;
+  std::unordered_map<std::string, double> processing_time_;
 
-  mutable std::shared_ptr<SceneModuleVisitor> debug_msg_ptr_;
+  ModuleUpdateInfo debug_info_;
 
-  size_t max_iteration_num_{100};
+  std::shared_ptr<SceneModuleVisitor> debug_msg_ptr_;
 };
 }  // namespace autoware::behavior_path_planner
 
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp
index bf323e76912d4..135e420d113f3 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -68,18 +68,28 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
 
     const std::lock_guard<std::mutex> lock(mutex_manager_);  // for planner_manager_
 
+    const auto slots = declare_parameter<std::vector<std::string>>("slots");
+    std::vector<std::vector<std::string>> slot_configuration{slots.size()};
+    for (size_t i = 0; i < slots.size(); ++i) {
+      const auto & slot = slots.at(i);
+      const auto modules = declare_parameter<std::vector<std::string>>(slot);
+      for (const auto & module_name : modules) {
+        slot_configuration.at(i).push_back(module_name);
+      }
+    }
+
     planner_manager_ = std::make_shared<PlannerManager>(*this);
 
-    size_t scene_module_num = 0;
     for (const auto & name : declare_parameter<std::vector<std::string>>("launch_modules")) {
       // workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter.
       if (name == "") {
         break;
       }
       planner_manager_->launchScenePlugin(*this, name);
-      scene_module_num++;
     }
-    planner_manager_->calculateMaxIterationNum(scene_module_num);
+
+    // NOTE: this needs to be after launchScenePlugin()
+    planner_manager_->configureModuleSlot(slot_configuration);
 
     for (const auto & manager : planner_manager_->getSceneModuleManagers()) {
       path_candidate_publishers_.emplace(
@@ -419,7 +429,7 @@ void BehaviorPathPlannerNode::run()
     planner_data_->operation_mode->is_autoware_control_enabled;
   if (
     !controlled_by_autoware_autonomously &&
-    !planner_manager_->hasNonAlwaysExecutableApprovedModules())
+    !planner_manager_->hasPossibleRerouteApprovedModules(planner_data_))
     planner_manager_->resetCurrentRouteLanelet(planner_data_);
 
   // run behavior planner
@@ -548,7 +558,7 @@ void BehaviorPathPlannerNode::publish_reroute_availability() const
   // always-executable module is approved and running, rerouting will not be possible.
   RerouteAvailability is_reroute_available;
   is_reroute_available.stamp = this->now();
-  if (planner_manager_->hasNonAlwaysExecutableApprovedModules()) {
+  if (planner_manager_->hasPossibleRerouteApprovedModules(planner_data_)) {
     is_reroute_available.availability = false;
   } else {
     is_reroute_available.availability = true;
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp
index 5278ea8e76981..dc6f7552c5a61 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp
@@ -23,7 +23,7 @@
 #include <autoware_lanelet2_extension/utility/query.hpp>
 #include <magic_enum.hpp>
 
-#include <boost/format.hpp>
+#include <boost/scope_exit.hpp>
 
 #include <memory>
 #include <string>
@@ -37,6 +37,7 @@ PlannerManager::PlannerManager(rclcpp::Node & node)
   logger_(node.get_logger().get_child("planner_manager")),
   clock_(*node.get_clock())
 {
+  current_route_lanelet_ = std::make_shared<std::optional<lanelet::ConstLanelet>>(std::nullopt);
   processing_time_.emplace("total_time", 0.0);
   debug_publisher_ptr_ = std::make_unique<DebugPublisher>(&node, "~/debug");
   state_publisher_ptr_ = std::make_unique<DebugPublisher>(&node, "~/debug");
@@ -65,151 +66,123 @@ void PlannerManager::launchScenePlugin(rclcpp::Node & node, const std::string &
   }
 }
 
-void PlannerManager::calculateMaxIterationNum(const size_t scene_module_num)
+void PlannerManager::configureModuleSlot(
+  const std::vector<std::vector<std::string>> & slot_configuration)
 {
-  max_iteration_num_ = scene_module_num * (scene_module_num + 1) / 2;
-}
-
-void PlannerManager::removeScenePlugin(rclcpp::Node & node, const std::string & name)
-{
-  auto it = std::remove_if(manager_ptrs_.begin(), manager_ptrs_.end(), [&](const auto plugin) {
-    return plugin->name() == name;
-  });
+  std::unordered_map<std::string, SceneModuleManagerPtr> registered_modules;
+  for (const auto & manager_ptr : manager_ptrs_) {
+    registered_modules[manager_ptr->name()] = manager_ptr;
+  }
 
-  if (it == manager_ptrs_.end()) {
-    RCLCPP_WARN_STREAM(
-      node.get_logger(),
-      "The scene plugin '" << name << "' is not found in the registered modules.");
-  } else {
-    manager_ptrs_.erase(it, manager_ptrs_.end());
-    processing_time_.erase(name);
-    RCLCPP_INFO_STREAM(node.get_logger(), "The scene plugin '" << name << "' is unloaded.");
+  for (const auto & slot : slot_configuration) {
+    SubPlannerManager sub_manager(current_route_lanelet_, processing_time_, debug_info_);
+    for (const auto & module_name : slot) {
+      if (const auto it = registered_modules.find(module_name); it != registered_modules.end()) {
+        sub_manager.addSceneModuleManager(it->second);
+      } else {
+        // TODO(Mamoru Sobue): use LOG
+        std::cout << module_name << " registered in slot_configuration is not registered, skipping"
+                  << std::endl;
+      }
+    }
+    if (sub_manager.getSceneModuleManager().size() != 0) {
+      planner_manager_slots_.push_back(sub_manager);
+      // TODO(Mamoru Sobue): use LOG
+      std::cout << "added a slot with " << sub_manager.getSceneModuleManager().size() << " modules"
+                << std::endl;
+    }
   }
 }
 
 BehaviorModuleOutput PlannerManager::run(const std::shared_ptr<PlannerData> & data)
 {
   resetProcessingTime();
-  stop_watch_.tic("total_time");
-  debug_info_.clear();
+  StopWatch<std::chrono::milliseconds> stop_watch;
+  stop_watch.tic("total_time");
+  BOOST_SCOPE_EXIT((&processing_time_)(&stop_watch))
+  {
+    processing_time_.at("total_time") += stop_watch.toc("total_time", true);
+  }
+  BOOST_SCOPE_EXIT_END;
 
-  if (!current_route_lanelet_) resetCurrentRouteLanelet(data);
+  debug_info_.scene_status.clear();
+  debug_info_.slot_status.clear();
+
+  if (!current_route_lanelet_->has_value()) resetCurrentRouteLanelet(data);
 
   std::for_each(
     manager_ptrs_.begin(), manager_ptrs_.end(), [&data](const auto & m) { m->setData(data); });
 
-  auto result_output = [&]() {
-    const bool is_any_approved_module_running =
-      std::any_of(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [](const auto & m) {
-        return m->getCurrentStatus() == ModuleStatus::RUNNING ||
-               m->getCurrentStatus() == ModuleStatus::WAITING_APPROVAL;
+  const bool is_any_approved_module_running = std::any_of(
+    planner_manager_slots_.begin(), planner_manager_slots_.end(), [&](const auto & slot) {
+      return slot.isAnyApprovedPred([](const auto & m) {
+        const auto status = m->getCurrentStatus();
+        return status == ModuleStatus::RUNNING || status == ModuleStatus::WAITING_APPROVAL;
       });
+    });
 
-    // IDLE is a state in which an execution has been requested but not yet approved.
-    // once approved, it basically turns to running.
-    const bool is_any_candidate_module_running_or_idle =
-      std::any_of(candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), [](const auto & m) {
-        return m->getCurrentStatus() == ModuleStatus::RUNNING ||
-               m->getCurrentStatus() == ModuleStatus::WAITING_APPROVAL ||
-               m->getCurrentStatus() == ModuleStatus::IDLE;
+  // IDLE is a state in which an execution has been requested but not yet approved.
+  // once approved, it basically turns to running.
+  const bool is_any_candidate_module_running_or_idle = std::any_of(
+    planner_manager_slots_.begin(), planner_manager_slots_.end(), [](const auto & slot) {
+      return slot.isAnyCandidatePred([](const auto & m) {
+        const auto status = m->getCurrentStatus();
+        return status == ModuleStatus::RUNNING || status == ModuleStatus::WAITING_APPROVAL ||
+               status == ModuleStatus::IDLE;
       });
+    });
 
-    const bool is_any_module_running =
-      is_any_approved_module_running || is_any_candidate_module_running_or_idle;
-
-    updateCurrentRouteLanelet(data);
-
-    const bool is_out_of_route = utils::isEgoOutOfRoute(
-      data->self_odometry->pose.pose, current_route_lanelet_.value(), data->prev_modified_goal,
-      data->route_handler);
+  const bool is_any_module_running =
+    is_any_approved_module_running || is_any_candidate_module_running_or_idle;
 
-    if (!is_any_module_running && is_out_of_route) {
-      BehaviorModuleOutput output = utils::createGoalAroundPath(data);
-      generateCombinedDrivableArea(output, data);
-      RCLCPP_WARN_THROTTLE(
-        logger_, clock_, 5000,
-        "Ego is out of route, no module is running. Skip running scene modules.");
-      return output;
-    }
-    std::vector<SceneModulePtr>
-      deleted_modules;  // store the scene modules deleted from approved modules
-
-    for (size_t itr_num = 1;; ++itr_num) {
-      /**
-       * STEP1: get approved modules' output
-       */
-      auto approved_modules_output = runApprovedModules(data, deleted_modules);
-
-      /**
-       * STEP2: check modules that need to be launched
-       */
-      const auto request_modules = getRequestModules(approved_modules_output, deleted_modules);
-
-      /**
-       * STEP3: if there is no module that need to be launched, return approved modules' output
-       */
-      if (request_modules.empty()) {
-        const auto output = runKeepLastModules(data, approved_modules_output);
-        processing_time_.at("total_time") = stop_watch_.toc("total_time", true);
-        return output;
-      }
+  updateCurrentRouteLanelet(data);
 
-      /**
-       * STEP4: if there is module that should be launched, execute the module
-       */
-      const auto [highest_priority_module, candidate_modules_output] =
-        runRequestModules(request_modules, data, approved_modules_output);
-
-      /**
-       * STEP5: run keep last approved modules after running candidate modules.
-       * NOTE: if no candidate module is launched, approved_modules_output used as input for keep
-       * last modules and return the result immediately.
-       */
-      const auto output = runKeepLastModules(
-        data, highest_priority_module ? candidate_modules_output : approved_modules_output);
-      if (!highest_priority_module) {
-        processing_time_.at("total_time") = stop_watch_.toc("total_time", true);
-        return output;
-      }
+  const bool is_out_of_route = utils::isEgoOutOfRoute(
+    data->self_odometry->pose.pose, current_route_lanelet_->value(), data->prev_modified_goal,
+    data->route_handler);
 
-      /**
-       * STEP6: if the candidate module's modification is NOT approved yet, return the result.
-       * NOTE: the result is output of the candidate module, but the output path don't contains path
-       * shape modification that needs approval. On the other hand, it could include velocity
-       * profile modification.
-       */
-      if (highest_priority_module->isWaitingApproval()) {
-        processing_time_.at("total_time") = stop_watch_.toc("total_time", true);
-        return output;
-      }
+  if (!is_any_module_running && is_out_of_route) {
+    BehaviorModuleOutput result_output = utils::createGoalAroundPath(data);
+    RCLCPP_WARN_THROTTLE(
+      logger_, clock_, 5000,
+      "Ego is out of route, no module is running. Skip running scene modules.");
+    generateCombinedDrivableArea(result_output, data);
+    return result_output;
+  }
+  std::vector<SceneModulePtr>
+    deleted_modules;  // store the scene modules deleted from approved modules
+
+  SlotOutput result_output = SlotOutput{
+    getReferencePath(data),
+    false,
+    false,
+    false,
+  };
 
-      /**
-       * STEP7: if the candidate module is approved, push the module into approved_module_ptrs_
-       */
-      addApprovedModule(highest_priority_module);
-      clearCandidateModules();
-      debug_info_.emplace_back(highest_priority_module, Action::ADD, "To Approval");
-
-      if (itr_num >= max_iteration_num_) {
-        RCLCPP_WARN_THROTTLE(
-          logger_, clock_, 1000, "Reach iteration limit (max: %ld). Output current result.",
-          max_iteration_num_);
-        processing_time_.at("total_time") = stop_watch_.toc("total_time", true);
-        return output;
-      }
+  for (auto & planner_manager_slot : planner_manager_slots_) {
+    if (result_output.is_upstream_failed_approved) {
+      // clear all candidate/approved modules of all subsequent slots, and keep result_output as is
+      planner_manager_slot.propagateWithFailedApproved();
+      debug_info_.slot_status.push_back(SlotStatus::UPSTREAM_APPROVED_FAILED);
+    } else if (result_output.is_upstream_waiting_approved) {
+      result_output = planner_manager_slot.propagateWithWaitingApproved(data, result_output);
+      debug_info_.slot_status.push_back(SlotStatus::UPSTREAM_WAITING_APPROVED);
+    } else if (result_output.is_upstream_candidate_exclusive) {
+      result_output = planner_manager_slot.propagateWithExclusiveCandidate(data, result_output);
+      debug_info_.slot_status.push_back(SlotStatus::UPSTREAM_EXCLUSIVE_CANDIDATE);
+    } else {
+      result_output = planner_manager_slot.propagateFull(data, result_output);
+      debug_info_.slot_status.push_back(SlotStatus::NORMAL);
     }
-
-    return BehaviorModuleOutput{};  // something wrong.
-  }();
+  }
 
   std::for_each(manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) {
     m->updateObserver();
     m->publishRTCStatus();
   });
 
-  generateCombinedDrivableArea(result_output, data);
-
-  return result_output;
+  generateCombinedDrivableArea(result_output.valid_output, data);
+  return result_output.valid_output;
 }
 
 // NOTE: To deal with some policies about drivable area generation, currently DrivableAreaInfo is
@@ -255,93 +228,227 @@ void PlannerManager::generateCombinedDrivableArea(
   utils::extractObstaclesFromDrivableArea(output.path, di.obstacles);
 }
 
-std::vector<SceneModulePtr> PlannerManager::getRequestModules(
-  const BehaviorModuleOutput & previous_module_output,
-  const std::vector<SceneModulePtr> & deleted_modules) const
+void PlannerManager::updateCurrentRouteLanelet(const std::shared_ptr<PlannerData> & data)
 {
-  if (previous_module_output.path.points.empty()) {
-    RCLCPP_ERROR_STREAM(
-      logger_, "Current module output is null. Skip candidate module check."
-                 << "\n      - Approved  module list: " << getNames(approved_module_ptrs_)
-                 << "\n      - Candidate module list: " << getNames(candidate_module_ptrs_));
-    return {};
+  const auto & route_handler = data->route_handler;
+  const auto & pose = data->self_odometry->pose.pose;
+  const auto p = data->parameters;
+
+  constexpr double extra_margin = 10.0;
+  const auto backward_length =
+    std::max(p.backward_path_length, p.backward_path_length + extra_margin);
+
+  lanelet::ConstLanelet closest_lane{};
+
+  if (route_handler->getClosestRouteLaneletFromLanelet(
+        pose, current_route_lanelet_->value(), &closest_lane, p.ego_nearest_dist_threshold,
+        p.ego_nearest_yaw_threshold)) {
+    *current_route_lanelet_ = closest_lane;
+    return;
   }
 
-  std::vector<SceneModulePtr> request_modules{};
+  const auto lanelet_sequence = route_handler->getLaneletSequence(
+    current_route_lanelet_->value(), pose, backward_length, p.forward_path_length);
+
+  const auto could_calculate_closest_lanelet =
+    lanelet::utils::query::getClosestLaneletWithConstrains(
+      lanelet_sequence, pose, &closest_lane, p.ego_nearest_dist_threshold,
+      p.ego_nearest_yaw_threshold) ||
+    lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lane);
+
+  if (could_calculate_closest_lanelet)
+    *current_route_lanelet_ = closest_lane;
+  else
+    resetCurrentRouteLanelet(data);
+}
+
+BehaviorModuleOutput PlannerManager::getReferencePath(
+  const std::shared_ptr<PlannerData> & data) const
+{
+  const auto reference_path = utils::getReferencePath(current_route_lanelet_->value(), data);
+  publishDebugRootReferencePath(reference_path);
+  return reference_path;
+}
 
-  const auto toc = [this](const auto & name) {
-    processing_time_.at(name) += stop_watch_.toc(name, true);
+void PlannerManager::publishDebugRootReferencePath(
+  const BehaviorModuleOutput & reference_path) const
+{
+  using visualization_msgs::msg::Marker;
+  MarkerArray array;
+  Marker m = autoware::universe_utils::createDefaultMarker(
+    "map", clock_.now(), "root_reference_path", 0UL, Marker::LINE_STRIP,
+    autoware::universe_utils::createMarkerScale(1.0, 1.0, 1.0),
+    autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 1.0));
+  for (const auto & p : reference_path.path.points) m.points.push_back(p.point.pose.position);
+  array.markers.push_back(m);
+  m.points.clear();
+  m.id = 1UL;
+  for (const auto & p : current_route_lanelet_->value().polygon3d().basicPolygon())
+    m.points.emplace_back().set__x(p.x()).set__y(p.y()).set__z(p.z());
+  array.markers.push_back(m);
+  debug_publisher_ptr_->publish<MarkerArray>("root_reference_path", array);
+}
+
+bool PlannerManager::hasPossibleRerouteApprovedModules(
+  const std::shared_ptr<PlannerData> & data) const
+{
+  const auto & approved_module = approved_modules();
+  const auto not_possible_reroute_module = [&](const SceneModulePtr m) {
+    if (m->name() == "dynamic_avoidance") {
+      return false;
+    }
+    if (m->name() == "goal_planner" && !utils::isAllowedGoalModification(data->route_handler)) {
+      return false;
+    }
+    return true;
   };
 
+  return std::any_of(approved_module.begin(), approved_module.end(), not_possible_reroute_module);
+}
+
+void PlannerManager::print() const
+{
+  const auto approved_module_ptrs = approved_modules();
+  const auto candidate_module_ptrs = candidate_modules();
+
+  const auto get_status = [](const auto & m) {
+    return magic_enum::enum_name(m->getCurrentStatus());
+  };
+
+  size_t max_string_num = 0;
+
+  std::ostringstream string_stream;
+  string_stream << "\n";
+  string_stream << "***********************************************************\n";
+  string_stream << "                  planner manager status\n";
+  string_stream << "-----------------------------------------------------------\n";
+  string_stream << "registered modules: ";
+  for (const auto & m : manager_ptrs_) {
+    string_stream << "[" << m->name() << "]";
+    max_string_num = std::max(max_string_num, m->name().length());
+  }
+
+  string_stream << "\n";
+  string_stream << "approved modules  : ";
+  std::string delimiter = "";
+  for (const auto & planner_manager_slot : planner_manager_slots_) {
+    string_stream << std::exchange(delimiter, " ==> ") << "[[ ";
+    std::string delimiter_sub = "";
+    for (const auto & m : planner_manager_slot.approved_modules()) {
+      string_stream << std::exchange(delimiter_sub, "->") << "[" << m->name() << "("
+                    << get_status(m) << ")"
+                    << "]";
+    }
+    string_stream << " ]]";
+  }
+
+  string_stream << "\n";
+  string_stream << "candidate modules : ";
+  delimiter = "";
+  for (const auto & planner_manager_slot : planner_manager_slots_) {
+    string_stream << std::exchange(delimiter, " ==> ") << "[[ ";
+    std::string delimiter_sub = "";
+    for (const auto & m : planner_manager_slot.candidate_modules()) {
+      string_stream << std::exchange(delimiter_sub, "->") << "[" << m->name() << "("
+                    << get_status(m) << ")"
+                    << "]";
+    }
+    string_stream << " ]]";
+  }
+
+  string_stream << "\n";
+  string_stream << "slot_status       : ";
+  delimiter = "";
+  for (const auto & slot_status : debug_info_.slot_status) {
+    string_stream << std::exchange(delimiter, "->") << "[" << magic_enum::enum_name(slot_status)
+                  << "]";
+  }
+
+  string_stream << "\n";
+  string_stream << "update module info: ";
+  for (const auto & i : debug_info_.scene_status) {
+    string_stream << "[Module:" << i.module_name << " Status:" << magic_enum::enum_name(i.status)
+                  << " Action:" << magic_enum::enum_name(i.action)
+                  << " Description:" << i.description << "]\n"
+                  << std::setw(28);
+  }
+
+  string_stream << "\n" << std::fixed << std::setprecision(1);
+  string_stream << "processing time   : ";
+  for (const auto & t : processing_time_) {
+    string_stream << std::right << "[" << std::setw(static_cast<int>(max_string_num) + 1)
+                  << std::left << t.first << ":" << std::setw(4) << std::right << t.second
+                  << "ms]\n"
+                  << std::setw(21);
+  }
+
+  state_publisher_ptr_->publish<DebugStringMsg>("internal_state", string_stream.str());
+
+  RCLCPP_DEBUG_STREAM(logger_, string_stream.str());
+}
+
+void PlannerManager::publishProcessingTime() const
+{
+  for (const auto & t : processing_time_) {
+    std::string name = t.first + std::string("/processing_time_ms");
+    debug_publisher_ptr_->publish<DebugDoubleMsg>(name, t.second);
+  }
+}
+
+std::shared_ptr<SceneModuleVisitor> PlannerManager::getDebugMsg()
+{
+  debug_msg_ptr_ = std::make_shared<SceneModuleVisitor>();
+  const auto approved_module_ptrs = approved_modules();
+  const auto candidate_module_ptrs = candidate_modules();
+
+  for (const auto & approved_module : approved_module_ptrs) {
+    approved_module->acceptVisitor(debug_msg_ptr_);
+  }
+
+  for (const auto & candidate_module : candidate_module_ptrs) {
+    candidate_module->acceptVisitor(debug_msg_ptr_);
+  }
+  return debug_msg_ptr_;
+}
+
+std::vector<SceneModulePtr> SubPlannerManager::getRequestModules(
+  const BehaviorModuleOutput & previous_module_output,
+  const std::vector<SceneModulePtr> & deleted_modules) const
+{
+  std::vector<SceneModulePtr> request_modules{};
+  StopWatch<std::chrono::milliseconds> stop_watch;
+
   for (const auto & manager_ptr : manager_ptrs_) {
-    stop_watch_.tic(manager_ptr->name());
-    /**
-     * skip the module that is already deleted.
-     */
+    stop_watch.tic(manager_ptr->name());
+    BOOST_SCOPE_EXIT((&manager_ptr)(&processing_time_)(&stop_watch))
     {
-      const auto name = manager_ptr->name();
-      const auto find_deleted_module = [&name](const auto & m) { return m->name() == name; };
-      const auto itr =
-        std::find_if(deleted_modules.begin(), deleted_modules.end(), find_deleted_module);
-      if (itr != deleted_modules.end()) {
-        continue;
-      }
+      processing_time_.at(manager_ptr->name()) += stop_watch.toc(manager_ptr->name(), true);
     }
+    BOOST_SCOPE_EXIT_END;
 
-    /**
-     * determine the execution capability of modules based on existing approved modules.
-     */
-    // Condition 1: always executable module can be added regardless of the existence of other
-    // modules, so skip checking the existence of other modules.
-    // in other cases, need to check the existence of other modules and which module can be added.
-    if (!manager_ptr->isAlwaysExecutableModule() && hasNonAlwaysExecutableApprovedModules()) {
-      // pairs of find_block_module and is_executable
-      std::vector<std::pair<std::function<bool(const SceneModulePtr &)>, std::function<bool()>>>
-        conditions;
-
-      // Condition 2: do not add modules that are neither always nor simultaneous executable
-      // if there exists at least one approved module that is simultaneous but not always
-      // executable. (only modules that are either always executable or simultaneous executable can
-      // be added)
-      conditions.emplace_back(
-        [&](const SceneModulePtr & m) {
-          return !getManager(m)->isAlwaysExecutableModule() &&
-                 getManager(m)->isSimultaneousExecutableAsApprovedModule();
-        },
-        [&]() { return manager_ptr->isSimultaneousExecutableAsApprovedModule(); });
-
-      // Condition 3: do not add modules that are not always executable if there exists
-      // at least one approved module that is neither always nor simultaneous executable.
-      // (only modules that are always executable can be added)
-      conditions.emplace_back(
-        [&](const SceneModulePtr & m) {
-          return !getManager(m)->isAlwaysExecutableModule() &&
-                 !getManager(m)->isSimultaneousExecutableAsApprovedModule();
-        },
-        [&]() { return false; });
-
-      bool skip_module = false;
-      for (const auto & condition : conditions) {
-        const auto & find_block_module = condition.first;
-        const auto & is_executable = condition.second;
-
-        const auto itr = std::find_if(
-          approved_module_ptrs_.begin(), approved_module_ptrs_.end(), find_block_module);
-
-        if (itr != approved_module_ptrs_.end() && !is_executable()) {
-          toc(manager_ptr->name());
-          skip_module = true;
-          continue;
-        }
-      }
-      if (skip_module) {
-        continue;
-      }
+    if (const auto deleted_it = std::find_if(
+          deleted_modules.begin(), deleted_modules.end(),
+          [&](const auto & m) { return m->name() == manager_ptr->name(); });
+        deleted_it != deleted_modules.end()) {
+      continue;
+    }
+
+    // Condition 1:
+    // the approved module queue is either
+    // - consists of multiple simultaneous_executable_as_approved modules only
+    // - consists of only 1 "not simultaneous_executable_as_approved" module
+    const bool exclusive_module_exist_in_approved_pool = std::any_of(
+      approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [&](const SceneModulePtr & m) {
+        return !getManager(m)->isSimultaneousExecutableAsApprovedModule();
+      });
+    const bool is_this_exclusive = !manager_ptr->isSimultaneousExecutableAsApprovedModule();
+    const bool approved_pool_is_not_empty = !approved_module_ptrs_.empty();
+
+    const bool is_this_not_joinable =
+      exclusive_module_exist_in_approved_pool || (is_this_exclusive && approved_pool_is_not_empty);
+    if (is_this_not_joinable) {
+      continue;
     }
-    // else{
-    //   Condition 4: if none of the above conditions are met, any module can be added.
-    //   (when the approved modules are either empty or consist only of always executable modules.)
-    // }
 
     /**
      * launch new candidate module.
@@ -359,8 +466,6 @@ std::vector<SceneModulePtr> PlannerManager::getRequestModules(
             request_modules.emplace_back(manager_ptr->getIdleModule());
           }
         }
-
-        toc(manager_ptr->name());
         continue;
       }
     }
@@ -380,7 +485,6 @@ std::vector<SceneModulePtr> PlannerManager::getRequestModules(
       if (itr != candidate_module_ptrs_.end()) {
         request_modules.clear();
         request_modules.emplace_back(*itr);
-        toc(manager_ptr->name());
         break;
       }
     }
@@ -396,7 +500,6 @@ std::vector<SceneModulePtr> PlannerManager::getRequestModules(
 
       if (itr != candidate_module_ptrs_.end()) {
         request_modules.emplace_back(*itr);
-        toc(manager_ptr->name());
         continue;
       }
     }
@@ -406,112 +509,99 @@ std::vector<SceneModulePtr> PlannerManager::getRequestModules(
      */
     {
       if (!manager_ptr->canLaunchNewModule()) {
-        toc(manager_ptr->name());
         continue;
       }
 
       manager_ptr->updateIdleModuleInstance();
       if (!manager_ptr->isExecutionRequested(previous_module_output)) {
-        toc(manager_ptr->name());
         continue;
       }
 
       request_modules.emplace_back(manager_ptr->getIdleModule());
     }
-
-    toc(manager_ptr->name());
   }
 
   return request_modules;
 }
 
-BehaviorModuleOutput PlannerManager::runKeepLastModules(
-  const std::shared_ptr<PlannerData> & data, const BehaviorModuleOutput & previous_output) const
-{
-  auto output = previous_output;
-  std::for_each(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [&](const auto & m) {
-    if (getManager(m)->isKeepLast()) {
-      output = run(m, data, output);
-    }
-  });
-
-  return output;
-}
-
-void PlannerManager::updateCurrentRouteLanelet(const std::shared_ptr<PlannerData> & data)
+SceneModulePtr SubPlannerManager::selectHighestPriorityModule(
+  std::vector<SceneModulePtr> & request_modules) const
 {
-  const auto & route_handler = data->route_handler;
-  const auto & pose = data->self_odometry->pose.pose;
-  const auto p = data->parameters;
-
-  constexpr double extra_margin = 10.0;
-  const auto backward_length =
-    std::max(p.backward_path_length, p.backward_path_length + extra_margin);
-
-  lanelet::ConstLanelet closest_lane{};
-
-  if (route_handler->getClosestRouteLaneletFromLanelet(
-        pose, current_route_lanelet_.value(), &closest_lane, p.ego_nearest_dist_threshold,
-        p.ego_nearest_yaw_threshold)) {
-    current_route_lanelet_ = closest_lane;
-    return;
+  if (request_modules.empty()) {
+    return {};
   }
 
-  const auto lanelet_sequence = route_handler->getLaneletSequence(
-    current_route_lanelet_.value(), pose, backward_length, p.forward_path_length);
-
-  const auto could_calculate_closest_lanelet =
-    lanelet::utils::query::getClosestLaneletWithConstrains(
-      lanelet_sequence, pose, &closest_lane, p.ego_nearest_dist_threshold,
-      p.ego_nearest_yaw_threshold) ||
-    lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lane);
+  sortByPriority(request_modules);
 
-  if (could_calculate_closest_lanelet)
-    current_route_lanelet_ = closest_lane;
-  else
-    resetCurrentRouteLanelet(data);
+  return request_modules.front();
 }
 
-BehaviorModuleOutput PlannerManager::getReferencePath(
-  const std::shared_ptr<PlannerData> & data) const
+void SubPlannerManager::clearApprovedModules()
 {
-  const auto reference_path = utils::getReferencePath(*current_route_lanelet_, data);
-  publishDebugRootReferencePath(reference_path);
-  return reference_path;
+  std::for_each(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [this](auto & m) {
+    debug_info_.scene_status.emplace_back(
+      m, SceneModuleUpdateInfo::Action::DELETE, "From Approved");
+    deleteExpiredModules(m);
+  });
+  approved_module_ptrs_.clear();
 }
 
-void PlannerManager::publishDebugRootReferencePath(
-  const BehaviorModuleOutput & reference_path) const
+void SubPlannerManager::updateCandidateModules(
+  const std::vector<SceneModulePtr> & request_modules,
+  const SceneModulePtr & highest_priority_module)
 {
-  using visualization_msgs::msg::Marker;
-  MarkerArray array;
-  Marker m = autoware::universe_utils::createDefaultMarker(
-    "map", clock_.now(), "root_reference_path", 0UL, Marker::LINE_STRIP,
-    autoware::universe_utils::createMarkerScale(1.0, 1.0, 1.0),
-    autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 1.0));
-  for (const auto & p : reference_path.path.points) m.points.push_back(p.point.pose.position);
-  array.markers.push_back(m);
-  m.points.clear();
-  m.id = 1UL;
-  for (const auto & p : current_route_lanelet_->polygon3d().basicPolygon())
-    m.points.emplace_back().set__x(p.x()).set__y(p.y()).set__z(p.z());
-  array.markers.push_back(m);
-  debug_publisher_ptr_->publish<MarkerArray>("root_reference_path", array);
-}
+  const auto exist = [](const auto & module_ptr, const auto & module_ptrs) {
+    const auto itr = std::find_if(
+      module_ptrs.begin(), module_ptrs.end(),
+      [&module_ptr](const auto & m) { return m->name() == module_ptr->name(); });
 
-SceneModulePtr PlannerManager::selectHighestPriorityModule(
-  std::vector<SceneModulePtr> & request_modules) const
-{
-  if (request_modules.empty()) {
-    return {};
+    return itr != module_ptrs.end();
+  };
+
+  /**
+   * unregister expired modules
+   */
+  {
+    const auto candidate_to_remove = [&](auto & itr) {
+      if (!exist(itr, request_modules)) {
+        deleteExpiredModules(itr);
+        return true;
+      }
+      return itr->name() == highest_priority_module->name() &&
+             !highest_priority_module->isWaitingApproval();
+    };
+
+    candidate_module_ptrs_.erase(
+      std::remove_if(
+        candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), candidate_to_remove),
+      candidate_module_ptrs_.end());
+
+    std::for_each(
+      manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); });
   }
 
-  sortByPriority(request_modules);
+  /**
+   * register running candidate modules
+   */
+  for (const auto & m : request_modules) {
+    if (
+      m->name() == highest_priority_module->name() &&
+      !highest_priority_module->isWaitingApproval()) {
+      continue;
+    }
 
-  return request_modules.front();
+    if (!exist(m, candidate_module_ptrs_)) {
+      candidate_module_ptrs_.push_back(m);
+    }
+  }
+
+  /**
+   * sort by priority. sorted_request_modules.front() is the highest priority module.
+   */
+  sortByPriority(candidate_module_ptrs_);
 }
 
-std::pair<SceneModulePtr, BehaviorModuleOutput> PlannerManager::runRequestModules(
+std::pair<SceneModulePtr, BehaviorModuleOutput> SubPlannerManager::runRequestModules(
   const std::vector<SceneModulePtr> & request_modules, const std::shared_ptr<PlannerData> & data,
   const BehaviorModuleOutput & previous_module_output)
 {
@@ -533,62 +623,27 @@ std::pair<SceneModulePtr, BehaviorModuleOutput> PlannerManager::runRequestModule
   auto sorted_request_modules = request_modules;
   sortByPriority(sorted_request_modules);
 
-  /**
-   * remove non-executable modules.
-   */
+  // the candidate module queue is either
+  // - consists of multiple simultaneous_executable_as_candidate modules only
+  // - consists of only 1 "not simultaneous_executable_as_candidate" module
   for (const auto & module_ptr : sorted_request_modules) {
-    // Condition 1: always executable module can be added regardless of the existence of other
-    // modules.
-    if (getManager(module_ptr)->isAlwaysExecutableModule()) {
+    // any module can join if executable_modules is empty
+    const bool is_executable_modules_empty = executable_modules.empty();
+    if (is_executable_modules_empty) {
       executable_modules.push_back(module_ptr);
       continue;
     }
 
-    // Condition 4: If the executable modules are either empty or consist only of always executable
-    // modules, any module can be added.
-    const bool has_non_always_executable_module = std::any_of(
-      executable_modules.begin(), executable_modules.end(),
-      [this](const auto & m) { return !getManager(m)->isAlwaysExecutableModule(); });
-    if (!has_non_always_executable_module) {
-      executable_modules.push_back(module_ptr);
-      continue;
-    }
+    // if executable_module is not empty, only SimultaneousExecutableAsCandidate is joinable
+    const bool is_this_cooperative =
+      getManager(module_ptr)->isSimultaneousExecutableAsCandidateModule();
+    const bool any_other_cooperative = std::any_of(
+      executable_modules.begin(), executable_modules.end(), [&](const SceneModulePtr & m) {
+        return getManager(m)->isSimultaneousExecutableAsCandidateModule();
+      });
 
-    // pairs of find_block_module and is_executable
-    std::vector<std::pair<std::function<bool(const SceneModulePtr &)>, std::function<bool()>>>
-      conditions;
-
-    // Condition 3: Only modules that are always executable can be added
-    // if there exists at least one executable module that is neither always nor simultaneous
-    // executable.
-    conditions.emplace_back(
-      [this](const SceneModulePtr & m) {
-        return !getManager(m)->isAlwaysExecutableModule() &&
-               !getManager(m)->isSimultaneousExecutableAsCandidateModule();
-      },
-      [&]() { return false; });
-
-    // Condition 2: Only modules that are either always executable or simultaneous executable can be
-    // added if there exists at least one executable module that is simultaneous but not always
-    // executable.
-    conditions.emplace_back(
-      [this](const SceneModulePtr & m) {
-        return !getManager(m)->isAlwaysExecutableModule() &&
-               getManager(m)->isSimultaneousExecutableAsCandidateModule();
-      },
-      [&]() { return getManager(module_ptr)->isSimultaneousExecutableAsCandidateModule(); });
-
-    for (const auto & condition : conditions) {
-      const auto & find_block_module = condition.first;
-      const auto & is_executable = condition.second;
-
-      const auto itr =
-        std::find_if(executable_modules.begin(), executable_modules.end(), find_block_module);
-
-      if (itr != executable_modules.end() && is_executable()) {
-        executable_modules.push_back(module_ptr);
-        break;
-      }
+    if (is_this_cooperative && any_other_cooperative) {
+      executable_modules.push_back(module_ptr);
     }
   }
 
@@ -654,7 +709,7 @@ std::pair<SceneModulePtr, BehaviorModuleOutput> PlannerManager::runRequestModule
   /**
    * choice highest priority module.
    */
-  const auto module_ptr = [&]() {
+  const auto module_ptr = [&]() -> SceneModulePtr {
     if (!already_approved_modules.empty()) {
       return selectHighestPriorityModule(already_approved_modules);
     }
@@ -663,8 +718,11 @@ std::pair<SceneModulePtr, BehaviorModuleOutput> PlannerManager::runRequestModule
       return selectHighestPriorityModule(waiting_approved_modules);
     }
 
-    return SceneModulePtr();
+    return nullptr;
   }();
+  if (module_ptr == nullptr) {
+    return std::make_pair(nullptr, BehaviorModuleOutput{});
+  }
 
   /**
    * register candidate modules.
@@ -674,149 +732,116 @@ std::pair<SceneModulePtr, BehaviorModuleOutput> PlannerManager::runRequestModule
   return std::make_pair(module_ptr, results.at(module_ptr->name()));
 }
 
-BehaviorModuleOutput PlannerManager::runApprovedModules(
-  const std::shared_ptr<PlannerData> & data, std::vector<SceneModulePtr> & deleted_modules)
+BehaviorModuleOutput SubPlannerManager::run(
+  const SceneModulePtr & module_ptr, const std::shared_ptr<PlannerData> & planner_data,
+  const BehaviorModuleOutput & previous_module_output) const
 {
-  std::unordered_map<std::string, BehaviorModuleOutput> results;
-  BehaviorModuleOutput output = getReferencePath(data);
-  results.emplace("root", output);
+  StopWatch<std::chrono::milliseconds> stop_watch;
+  stop_watch.tic(module_ptr->name());
 
-  if (approved_module_ptrs_.empty()) {
-    return output;
-  }
+  module_ptr->setData(planner_data);
+  module_ptr->setPreviousModuleOutput(previous_module_output);
 
-  // move modules whose keep last flag is true to end of the approved_module_ptrs_.
-  // if there are multiple keep last modules, sort by priority
-  {
-    const auto move_to_end = [](auto & modules, const auto & module_to_move) {
-      auto itr = std::find(modules.begin(), modules.end(), module_to_move);
+  module_ptr->lockRTCCommand();
+  const auto result = module_ptr->run();
+  module_ptr->unlockRTCCommand();
 
-      if (itr != modules.end()) {
-        auto tmp = std::move(*itr);
-        modules.erase(itr);
-        modules.push_back(std::move(tmp));
-      }
-    };
+  module_ptr->postProcess();
 
-    const auto get_sorted_keep_last_modules = [this](const auto & modules) {
-      std::vector<SceneModulePtr> keep_last_modules;
+  module_ptr->updateCurrentState();
 
-      std::copy_if(
-        modules.begin(), modules.end(), std::back_inserter(keep_last_modules),
-        [this](const auto & m) { return getManager(m)->isKeepLast(); });
+  module_ptr->publishSteeringFactor();
 
-      // sort by priority (low -> high)
-      std::sort(
-        keep_last_modules.begin(), keep_last_modules.end(), [this](const auto & a, const auto & b) {
-          return getManager(a)->getPriority() < getManager(b)->getPriority();
-        });
+  module_ptr->publishObjectsOfInterestMarker();
 
-      return keep_last_modules;
-    };
+  processing_time_.at(module_ptr->name()) += stop_watch.toc(module_ptr->name(), true);
+  return result;
+}
 
-    for (const auto & module : get_sorted_keep_last_modules(approved_module_ptrs_)) {
-      move_to_end(approved_module_ptrs_, module);
-    }
+SlotOutput SubPlannerManager::runApprovedModules(
+  const std::shared_ptr<PlannerData> & data, const BehaviorModuleOutput & upstream_slot_output)
+{
+  std::unordered_map<std::string, BehaviorModuleOutput> results;
+  BehaviorModuleOutput output = upstream_slot_output;
+  results.emplace("root", output);
+
+  const bool is_candidate_plan_applied = true /* NOTE: not used in this process */;
+  bool is_this_failed = false;
+  bool is_this_waiting_approval = false;
+
+  if (approved_module_ptrs_.empty()) {
+    return SlotOutput{output, is_candidate_plan_applied, is_this_failed, is_this_waiting_approval};
   }
 
-  // lock approved modules besides last one
+  // unlock only last approved module
   std::for_each(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [&](const auto & m) {
     m->lockOutputPath();
   });
-
-  // unlock only last approved module except keep last module.
-  {
-    const auto not_keep_last_modules = std::find_if(
-      approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(),
-      [this](const auto & m) { return !getManager(m)->isKeepLast(); });
-
-    if (not_keep_last_modules != approved_module_ptrs_.rend()) {
-      (*not_keep_last_modules)->unlockOutputPath();
-    }
-  }
+  approved_module_ptrs_.back()->unlockOutputPath();
 
   /**
-   * execute approved modules except keep last modules.
+   * bootstrap approved module output
    */
   std::for_each(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [&](const auto & m) {
-    if (!getManager(m)->isKeepLast()) {
-      output = run(m, data, output);
-      results.emplace(m->name(), output);
-    }
+    output = run(m, data, output);
+    results.emplace(m->name(), output);
   });
 
-  /**
-   * pop waiting approve module. push it back candidate modules. if waiting approve module is found
-   * in iteration std::find_if, not only the module but also the next one are removed from
-   * approved_module_ptrs_ since the next module's input (= waiting approve module's output) maybe
-   * change significantly. And, only the waiting approve module is pushed back to
-   * candidate_module_ptrs_.
-   */
-  {
-    const auto not_keep_last_module = std::find_if(
-      approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(),
-      [this](const auto & m) { return !getManager(m)->isKeepLast(); });
-
-    // convert reverse iterator -> iterator
-    const auto begin_itr = not_keep_last_module != approved_module_ptrs_.rend()
-                             ? std::next(not_keep_last_module).base()
-                             : approved_module_ptrs_.begin();
-
-    const auto waiting_approval_modules_itr = std::find_if(
-      begin_itr, approved_module_ptrs_.end(),
-      [](const auto & m) { return m->isWaitingApproval(); });
+  const auto waiting_approval_modules_itr = std::find_if(
+    approved_module_ptrs_.begin(), approved_module_ptrs_.end(),
+    [](const auto & m) { return m->isWaitingApproval(); });
 
-    if (waiting_approval_modules_itr != approved_module_ptrs_.end()) {
-      clearCandidateModules();
-      candidate_module_ptrs_.push_back(*waiting_approval_modules_itr);
+  if (waiting_approval_modules_itr != approved_module_ptrs_.end()) {
+    is_this_waiting_approval = true;
 
-      debug_info_.emplace_back(
-        *waiting_approval_modules_itr, Action::MOVE, "Back To Waiting Approval");
-
-      std::for_each(
-        waiting_approval_modules_itr, approved_module_ptrs_.end(),
-        [&results](const auto & m) { results.erase(m->name()); });
+    // only keep this module as candidate
+    clearCandidateModules();
+    candidate_module_ptrs_.push_back(*waiting_approval_modules_itr);
 
-      approved_module_ptrs_.erase(waiting_approval_modules_itr);
+    // delete following result but keep the rest of the following modules
+    std::for_each(
+      waiting_approval_modules_itr, approved_module_ptrs_.end(),
+      [&results](const auto & m) { results.erase(m->name()); });
+    debug_info_.scene_status.emplace_back(
+      *waiting_approval_modules_itr, SceneModuleUpdateInfo::Action::MOVE,
+      "Back To Waiting Approval");
+    approved_module_ptrs_.erase(waiting_approval_modules_itr);
 
-      std::for_each(
-        manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); });
-    }
+    std::for_each(
+      manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); });
   }
 
   /**
    * remove failure modules. these modules' outputs are discarded as invalid plan.
    */
-  {
-    const auto itr = std::find_if(
-      approved_module_ptrs_.begin(), approved_module_ptrs_.end(),
-      [](const auto & m) { return m->getCurrentStatus() == ModuleStatus::FAILURE; });
-    if (itr != approved_module_ptrs_.end()) {
-      deleted_modules.push_back(*itr);
-    }
+  const auto failed_itr = std::find_if(
+    approved_module_ptrs_.begin(), approved_module_ptrs_.end(),
+    [](const auto & m) { return m->getCurrentStatus() == ModuleStatus::FAILURE; });
+  if (failed_itr != approved_module_ptrs_.end()) {
+    is_this_failed = true;
+
+    // clear all candidates
+    clearCandidateModules();
 
-    std::for_each(itr, approved_module_ptrs_.end(), [this](auto & m) {
-      debug_info_.emplace_back(m, Action::DELETE, "From Approved");
+    // delete both subsequent result and modules
+    std::for_each(failed_itr, approved_module_ptrs_.end(), [&](auto & m) {
+      results.erase(m->name());
+      debug_info_.scene_status.emplace_back(
+        m, SceneModuleUpdateInfo::Action::DELETE, "From Approved");
       deleteExpiredModules(m);
     });
+    approved_module_ptrs_.erase(failed_itr, approved_module_ptrs_.end());
 
     std::for_each(
       manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); });
-
-    if (itr != approved_module_ptrs_.end()) {
-      clearCandidateModules();
-    }
-
-    approved_module_ptrs_.erase(itr, approved_module_ptrs_.end());
   }
 
   if (approved_module_ptrs_.empty()) {
-    return results.at("root");
+    return SlotOutput{
+      results.at("root"), is_candidate_plan_applied, is_this_failed, is_this_waiting_approval};
   }
 
-  /**
-   * use the last module's output as approved modules planning result.
-   */
+  // use the last module's output as approved modules planning result.
   const auto approved_modules_output = [&results, this]() {
     const auto itr = std::find_if(
       approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(),
@@ -828,199 +853,124 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(
     return results.at("root");
   }();
 
-  /**
-   * remove success module immediately. if lane change module has succeeded, update current route
-   * lanelet.
-   */
-  {
-    const auto move_to_end = [](auto & modules, const auto & cond) {
-      auto itr = modules.begin();
-      while (itr != modules.end()) {
-        const auto satisfied_exit_cond =
-          std::all_of(itr, modules.end(), [&cond](const auto & m) { return cond(m); });
-
-        if (satisfied_exit_cond) {
-          return;
-        }
-
-        if (cond(*itr)) {
-          auto tmp = std::move(*itr);
-          itr = modules.erase(itr);
-          modules.insert(modules.end(), std::move(tmp));
-        } else {
-          itr++;
-        }
-      }
-    };
-    const auto success_module_cond = [](const auto & m) {
-      return m->getCurrentStatus() == ModuleStatus::SUCCESS;
-    };
-    move_to_end(approved_module_ptrs_, success_module_cond);
-
-    const auto itr =
-      std::find_if(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), success_module_cond);
-
-    if (std::any_of(itr, approved_module_ptrs_.end(), [](const auto & m) {
-          return m->isCurrentRouteLaneletToBeReset();
-        }))
-      resetCurrentRouteLanelet(data);
-
-    std::copy_if(
-      itr, approved_module_ptrs_.end(), std::back_inserter(deleted_modules), success_module_cond);
-
-    std::for_each(itr, approved_module_ptrs_.end(), [&](auto & m) {
-      debug_info_.emplace_back(m, Action::DELETE, "From Approved");
-      deleteExpiredModules(m);
-    });
-
-    approved_module_ptrs_.erase(itr, approved_module_ptrs_.end());
+  // if lane change module has succeeded, update current route lanelet.
+  if (std::any_of(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [](const auto & m) {
+        return m->getCurrentStatus() == ModuleStatus::SUCCESS &&
+               m->isCurrentRouteLaneletToBeReset();
+      }))
+    resetCurrentRouteLanelet(data);
 
-    std::for_each(
-      manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); });
+  // remove success module immediately.
+  for (auto success_itr = std::find_if(
+         approved_module_ptrs_.begin(), approved_module_ptrs_.end(),
+         [](const auto & m) { return m->getCurrentStatus() == ModuleStatus::SUCCESS; });
+       success_itr != approved_module_ptrs_.end();
+       /* success_itr++ */) {
+    if ((*success_itr)->getCurrentStatus() == ModuleStatus::SUCCESS) {
+      debug_info_.scene_status.emplace_back(
+        *success_itr, SceneModuleUpdateInfo::Action::DELETE, "From Approved");
+      deleteExpiredModules(*success_itr);
+      success_itr = approved_module_ptrs_.erase(success_itr);
+    } else {
+      success_itr++;
+    }
   }
 
-  return approved_modules_output;
+  std::for_each(
+    manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); });
+
+  return SlotOutput{
+    approved_modules_output, is_candidate_plan_applied, is_this_failed, is_this_waiting_approval};
 }
 
-void PlannerManager::updateCandidateModules(
-  const std::vector<SceneModulePtr> & request_modules,
-  const SceneModulePtr & highest_priority_module)
+SlotOutput SubPlannerManager::propagateFull(
+  const std::shared_ptr<PlannerData> & data, const SlotOutput & previous_slot_output)
 {
-  const auto exist = [](const auto & module_ptr, const auto & module_ptrs) {
-    const auto itr = std::find_if(
-      module_ptrs.begin(), module_ptrs.end(),
-      [&module_ptr](const auto & m) { return m->name() == module_ptr->name(); });
-
-    return itr != module_ptrs.end();
-  };
-
-  /**
-   * unregister expired modules
-   */
-  {
-    const auto candidate_to_remove = [&](auto & itr) {
-      if (!exist(itr, request_modules)) {
-        deleteExpiredModules(itr);
-        return true;
-      }
-      return itr->name() == highest_priority_module->name() &&
-             !highest_priority_module->isWaitingApproval();
-    };
-
-    candidate_module_ptrs_.erase(
-      std::remove_if(
-        candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), candidate_to_remove),
-      candidate_module_ptrs_.end());
+  const size_t module_size = manager_ptrs_.size();
+  const size_t max_iteration_num = static_cast<int>(module_size * (module_size + 1) / 2);
+
+  bool is_waiting_approved_slot = previous_slot_output.is_upstream_waiting_approved;
+  bool is_failed_approved_slot = false;
+  auto output_path = previous_slot_output.valid_output;
+
+  std::vector<SceneModulePtr> deleted_modules;
+  for (size_t itr_num = 0; itr_num < max_iteration_num; ++itr_num) {
+    const auto approved_module_result = runApprovedModules(data, previous_slot_output.valid_output);
+    const auto & approved_module_output = approved_module_result.valid_output;
+
+    // these status needs to be propagated to downstream slots
+    // if any of the slots returned following statuses, keep it
+    is_waiting_approved_slot =
+      is_waiting_approved_slot || approved_module_result.is_upstream_waiting_approved;
+    is_failed_approved_slot =
+      is_failed_approved_slot || approved_module_result.is_upstream_failed_approved;
+
+    const auto request_modules = getRequestModules(approved_module_output, deleted_modules);
+    if (request_modules.empty()) {
+      // there is no module that needs to be launched
+      return SlotOutput{
+        approved_module_output, isAnyCandidateExclusive(), is_failed_approved_slot,
+        is_waiting_approved_slot};
+    }
 
-    std::for_each(
-      manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); });
-  }
+    const auto [highest_priority_module, candidate_module_output] =
+      runRequestModules(request_modules, data, approved_module_output);
 
-  /**
-   * register running candidate modules
-   */
-  for (const auto & m : request_modules) {
-    if (
-      m->name() == highest_priority_module->name() &&
-      !highest_priority_module->isWaitingApproval()) {
-      continue;
+    if (!highest_priority_module) {
+      // there is no need to launch new module
+      return SlotOutput{
+        approved_module_output, isAnyCandidateExclusive(), is_failed_approved_slot,
+        is_waiting_approved_slot};
     }
 
-    if (!exist(m, candidate_module_ptrs_)) {
-      candidate_module_ptrs_.push_back(m);
+    if (highest_priority_module->isWaitingApproval()) {
+      // there is no need to launch new module
+      return SlotOutput{
+        candidate_module_output, isAnyCandidateExclusive(), is_failed_approved_slot,
+        is_waiting_approved_slot};
     }
-  }
-
-  /**
-   * sort by priority. sorted_request_modules.front() is the highest priority module.
-   */
-  sortByPriority(candidate_module_ptrs_);
-}
-
-void PlannerManager::print() const
-{
-  const auto get_status = [](const auto & m) {
-    return magic_enum::enum_name(m->getCurrentStatus());
-  };
-
-  size_t max_string_num = 0;
 
-  std::ostringstream string_stream;
-  string_stream << "\n";
-  string_stream << "***********************************************************\n";
-  string_stream << "                  planner manager status\n";
-  string_stream << "-----------------------------------------------------------\n";
-  string_stream << "registered modules: ";
-  for (const auto & m : manager_ptrs_) {
-    string_stream << "[" << m->name() << "]";
-    max_string_num = std::max(max_string_num, m->name().length());
-  }
-
-  string_stream << "\n";
-  string_stream << "approved modules  : ";
-  for (const auto & m : approved_module_ptrs_) {
-    string_stream << "[" << m->name() << "(" << get_status(m) << ")"
-                  << "]->";
-  }
-
-  string_stream << "\n";
-  string_stream << "candidate module  : ";
-  for (const auto & m : candidate_module_ptrs_) {
-    string_stream << "[" << m->name() << "(" << get_status(m) << ")"
-                  << "]->";
-  }
-
-  string_stream << "\n";
-  string_stream << "update module info: ";
-  for (const auto & i : debug_info_) {
-    string_stream << "[Module:" << i.module_name << " Status:" << magic_enum::enum_name(i.status)
-                  << " Action:" << magic_enum::enum_name(i.action)
-                  << " Description:" << i.description << "]\n"
-                  << std::setw(28);
-  }
-
-  string_stream << "\n" << std::fixed << std::setprecision(1);
-  string_stream << "processing time   : ";
-  for (const auto & t : processing_time_) {
-    string_stream << std::right << "[" << std::setw(static_cast<int>(max_string_num) + 1)
-                  << std::left << t.first << ":" << std::setw(4) << std::right << t.second
-                  << "ms]\n"
-                  << std::setw(21);
+    output_path = candidate_module_output;
+    addApprovedModule(highest_priority_module);
+    clearCandidateModules();
   }
 
-  state_publisher_ptr_->publish<DebugStringMsg>("internal_state", string_stream.str());
-
-  RCLCPP_DEBUG_STREAM(logger_, string_stream.str());
+  return SlotOutput{
+    output_path, isAnyCandidateExclusive(), is_failed_approved_slot, is_waiting_approved_slot};
 }
 
-void PlannerManager::publishProcessingTime() const
+SlotOutput SubPlannerManager::propagateWithExclusiveCandidate(
+  const std::shared_ptr<PlannerData> & data, const SlotOutput & previous_slot_output)
 {
-  for (const auto & t : processing_time_) {
-    std::string name = t.first + std::string("/processing_time_ms");
-    debug_publisher_ptr_->publish<DebugDoubleMsg>(name, t.second);
-  }
+  const auto approved_module_result = runApprovedModules(data, previous_slot_output.valid_output);
+  const auto & approved_module_output = approved_module_result.valid_output;
+
+  // these status needs to be propagated to downstream slots
+  // if any of the slots returned following statuses, keep it
+  const bool is_waiting_approved_slot = previous_slot_output.is_upstream_waiting_approved |
+                                        approved_module_result.is_upstream_waiting_approved;
+  const bool is_failed_approved_slot = previous_slot_output.is_upstream_failed_approved |
+                                       approved_module_result.is_upstream_failed_approved;
+
+  // there is no module that needs to be launched
+  return SlotOutput{
+    approved_module_output, true, is_failed_approved_slot, is_waiting_approved_slot};
 }
 
-std::shared_ptr<SceneModuleVisitor> PlannerManager::getDebugMsg()
+void SubPlannerManager::propagateWithFailedApproved()
 {
-  debug_msg_ptr_ = std::make_shared<SceneModuleVisitor>();
-  for (const auto & approved_module : approved_module_ptrs_) {
-    approved_module->acceptVisitor(debug_msg_ptr_);
-  }
-
-  for (const auto & candidate_module : candidate_module_ptrs_) {
-    candidate_module->acceptVisitor(debug_msg_ptr_);
-  }
-  return debug_msg_ptr_;
+  clearCandidateModules();
+  clearApprovedModules();
 }
 
-std::string PlannerManager::getNames(const std::vector<SceneModulePtr> & modules)
+SlotOutput SubPlannerManager::propagateWithWaitingApproved(
+  const std::shared_ptr<PlannerData> & data, const SlotOutput & previous_slot_output)
 {
-  std::stringstream ss;
-  for (const auto & m : modules) {
-    ss << "[" << m->name() << "], ";
-  }
-  return ss.str();
+  clearCandidateModules();
+
+  return previous_slot_output.is_upstream_candidate_exclusive
+           ? propagateWithExclusiveCandidate(data, previous_slot_output)
+           : propagateFull(data, previous_slot_output);
 }
 
 }  // namespace autoware::behavior_path_planner
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp
index c9adf4d60c21f..9dfd30e15cfbd 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp
@@ -217,37 +217,16 @@ class SceneModuleManagerInterface
    */
   bool canLaunchNewModule() const { return observers_.empty(); }
 
-  /**
-   * Determine if the module is always executable, regardless of the state of other modules.
-   *
-   * When this returns true:
-   * - The module can be executed even if other modules are not marked as 'simultaneously
-   * executable'.
-   * - Conversely, the presence of this module will not prevent other modules
-   *   from executing, even if they are not marked as 'simultaneously executable'.
-   */
-  virtual bool isAlwaysExecutableModule() const { return false; }
-
   virtual bool isSimultaneousExecutableAsApprovedModule() const
   {
-    if (isAlwaysExecutableModule()) {
-      return true;
-    }
-
     return config_.enable_simultaneous_execution_as_approved_module;
   }
 
   virtual bool isSimultaneousExecutableAsCandidateModule() const
   {
-    if (isAlwaysExecutableModule()) {
-      return true;
-    }
-
     return config_.enable_simultaneous_execution_as_candidate_module;
   }
 
-  bool isKeepLast() const { return config_.keep_last; }
-
   void setData(const std::shared_ptr<PlannerData> & planner_data) { planner_data_ = planner_data; }
 
   void reset()
@@ -268,8 +247,6 @@ class SceneModuleManagerInterface
     pub_debug_marker_->publish(MarkerArray{});
   }
 
-  size_t getPriority() const { return config_.priority; }
-
   std::string name() const { return name_; }
 
   std::vector<SceneModuleObserver> getSceneModuleObservers() { return observers_; }
@@ -298,8 +275,6 @@ class SceneModuleManagerInterface
         getOrDeclareParameter<bool>(*node, ns + "enable_simultaneous_execution_as_approved_module");
       config_.enable_simultaneous_execution_as_candidate_module = getOrDeclareParameter<bool>(
         *node, ns + "enable_simultaneous_execution_as_candidate_module");
-      config_.keep_last = getOrDeclareParameter<bool>(*node, ns + "keep_last");
-      config_.priority = getOrDeclareParameter<int>(*node, ns + "priority");
     }
 
     // init rtc configuration
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/parameters.hpp
index a45e8366f45d8..ecbe9b5208347 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/parameters.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/parameters.hpp
@@ -23,13 +23,10 @@ struct ModuleConfigParameters
   bool enable_rtc{false};
   bool enable_simultaneous_execution_as_approved_module{false};
   bool enable_simultaneous_execution_as_candidate_module{false};
-  bool keep_last{false};
-  uint8_t priority{0};
 };
 
 struct BehaviorPathPlannerParameters
 {
-  size_t max_iteration_num{100};
   double traffic_light_signal_timeout{1.0};
 
   double backward_path_length;
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 d0d19b6b8fed2..05f7c70fa42c2 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
@@ -27,6 +27,7 @@
 #include <geometry_msgs/msg/twist.hpp>
 
 #include <cmath>
+#include <utility>
 #include <vector>
 
 namespace autoware::behavior_path_planner::utils::path_safety_checker
@@ -156,6 +157,12 @@ bool checkSafetyWithIntegralPredictedPolygon(
   const ExtendedPredictedObjects & objects, const bool check_all_predicted_path,
   const IntegralPredictedPolygonParams & params, CollisionCheckDebugMap & debug_map);
 
+double calcObstacleMinLength(const Shape & shape);
+double calcObstacleMaxLength(const Shape & shape);
+std::pair<bool, bool> checkObjectsCollisionRough(
+  const PathWithLaneId & path, const PredictedObjects & objects, const double margin,
+  const BehaviorPathPlannerParameters & parameters, const bool use_offset_ego_point);
+
 // debug
 CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj);
 void updateCollisionCheckDebugMap(
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp
index 7afce28019c3f..dc0f13270bca2 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp
@@ -17,9 +17,11 @@
 #include "autoware/behavior_path_planner_common/utils/parking_departure/utils.hpp"
 #include "autoware/behavior_path_planner_common/utils/path_utils.hpp"
 #include "autoware/behavior_path_planner_common/utils/utils.hpp"
+#include "autoware/motion_utils/trajectory/trajectory.hpp"
 #include "autoware/universe_utils/geometry/geometry.hpp"
 #include "autoware/universe_utils/math/unit_conversion.hpp"
 
+#include <autoware_lanelet2_extension/utility/query.hpp>
 #include <autoware_lanelet2_extension/utility/utilities.hpp>
 #include <interpolation/spline_interpolation.hpp>
 
@@ -333,8 +335,20 @@ std::optional<Pose> GeometricParallelParking::calcStartPose(
   }
   const double dx_sign = is_forward ? -1 : 1;
   const double dx = 2 * std::sqrt(squared_distance_to_arc_connect) * dx_sign;
-  const Pose start_pose =
-    calcOffsetPose(goal_pose, dx + start_pose_offset, -arc_coordinates.distance, 0);
+
+  // Assuming parallel poses, calculate the approximate start pose on the centerline from the goal
+  // pose
+  const Pose approximate_start_pose = calcOffsetPose(goal_pose, dx, -arc_coordinates.distance, 0);
+  lanelet::ConstLanelet closest_road_lane{};
+
+  // Calculate start pose on the centerline, then offset it.
+  lanelet::utils::query::getClosestLanelet(road_lanes, approximate_start_pose, &closest_road_lane);
+  const Pose start_pose_no_offset =
+    lanelet::utils::getClosestCenterPose(closest_road_lane, approximate_start_pose.position);
+  const auto road_lane_path = planner_data_->route_handler->getCenterLinePath(
+    road_lanes, 0.0, std::numeric_limits<double>::max());
+  const auto start_pose = autoware::motion_utils::calcLongitudinalOffsetPose(
+    road_lane_path.points, start_pose_no_offset.position, start_pose_offset);
 
   return start_pose;
 }
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 b05d869cca6b5..8d2dd2fb86e29 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
@@ -36,6 +36,12 @@ namespace autoware::behavior_path_planner::utils::path_safety_checker
 
 namespace bg = boost::geometry;
 
+using autoware::motion_utils::calcLongitudinalOffsetPoint;
+using autoware::motion_utils::calcLongitudinalOffsetToSegment;
+using autoware::motion_utils::findNearestIndex;
+using autoware::motion_utils::findNearestSegmentIndex;
+using autoware::universe_utils::calcDistance2d;
+
 void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point)
 {
   Point2d point;
@@ -722,4 +728,96 @@ void updateCollisionCheckDebugMap(
   debug_map.insert(object_debug);
 }
 
+double calcObstacleMinLength(const Shape & shape)
+{
+  if (shape.type == Shape::BOUNDING_BOX) {
+    return std::min(shape.dimensions.x / 2.0, shape.dimensions.y / 2.0);
+  } else if (shape.type == Shape::CYLINDER) {
+    return shape.dimensions.x / 2.0;
+  } else if (shape.type == Shape::POLYGON) {
+    double min_length_to_point = std::numeric_limits<double>::max();
+    for (const auto rel_point : shape.footprint.points) {
+      const double length_to_point = std::hypot(rel_point.x, rel_point.y);
+      if (min_length_to_point > length_to_point) {
+        min_length_to_point = length_to_point;
+      }
+    }
+    return min_length_to_point;
+  }
+
+  throw std::logic_error("The shape type is not supported in obstacle_cruise_planner.");
+}
+
+double calcObstacleMaxLength(const Shape & shape)
+{
+  if (shape.type == Shape::BOUNDING_BOX) {
+    return std::hypot(shape.dimensions.x / 2.0, shape.dimensions.y / 2.0);
+  } else if (shape.type == Shape::CYLINDER) {
+    return shape.dimensions.x / 2.0;
+  } else if (shape.type == Shape::POLYGON) {
+    double max_length_to_point = 0.0;
+    for (const auto rel_point : shape.footprint.points) {
+      const double length_to_point = std::hypot(rel_point.x, rel_point.y);
+      if (max_length_to_point < length_to_point) {
+        max_length_to_point = length_to_point;
+      }
+    }
+    return max_length_to_point;
+  }
+
+  throw std::logic_error("The shape type is not supported in obstacle_cruise_planner.");
+}
+
+std::pair<bool, bool> checkObjectsCollisionRough(
+  const PathWithLaneId & path, const PredictedObjects & objects, const double margin,
+  const BehaviorPathPlannerParameters & parameters, const bool use_offset_ego_point)
+{
+  const auto & points = path.points;
+
+  std::pair<bool, bool> has_collision = {false, false};  // {min_distance, max_distance}
+  for (const auto & object : objects.objects) {
+    // calculate distance between object center and ego base_link
+    const Point & object_point = object.kinematics.initial_pose_with_covariance.pose.position;
+    const double distance = std::invoke([&]() -> double {
+      if (use_offset_ego_point) {
+        const size_t nearest_segment_idx = findNearestSegmentIndex(points, object_point);
+        const double offset_length =
+          calcLongitudinalOffsetToSegment(points, nearest_segment_idx, object_point);
+        const auto offset_point =
+          calcLongitudinalOffsetPoint(points, nearest_segment_idx, offset_length);
+        const Point ego_point =
+          offset_point ? offset_point.value()
+                       : points.at(findNearestIndex(points, object_point)).point.pose.position;
+        return autoware::universe_utils::calcDistance2d(ego_point, object_point);
+      }
+      const Point ego_point = points.at(findNearestIndex(points, object_point)).point.pose.position;
+      return autoware::universe_utils::calcDistance2d(ego_point, object_point);
+    });
+
+    // calculate min and max length from object center to edge
+    const double object_min_length = calcObstacleMinLength(object.shape);
+    const double object_max_length = calcObstacleMaxLength(object.shape);
+
+    // calculate min and max length from ego base_link to edge
+    const auto & p = parameters;
+    const double ego_min_length =
+      std::min({p.vehicle_width / 2, p.front_overhang / 2, p.rear_overhang / 2});
+    const double ego_max_length = std::max(
+      std::hypot(p.vehicle_width / 2, p.front_overhang),
+      std::hypot(p.vehicle_width / 2, p.rear_overhang));
+
+    // calculate min and max distance between object footprint and ego footprint
+    const double min_distance = distance - object_max_length - ego_max_length;
+    const double max_distance = distance - object_min_length - ego_min_length;
+
+    if (min_distance < margin) {
+      has_collision.first = true;
+    }
+    if (max_distance < margin) {
+      has_collision.second = true;
+    }
+  }
+  return has_collision;
+}
+
 }  // namespace autoware::behavior_path_planner::utils::path_safety_checker
diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp
index 145ca5175762e..e358e35ed7794 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp
@@ -16,6 +16,7 @@
 #define AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__FREESPACE_PULL_OUT_HPP_
 
 #include "autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp"
+#include "autoware/universe_utils/system/time_keeper.hpp"
 
 #include <autoware/freespace_planning_algorithms/abstract_algorithm.hpp>
 #include <autoware/freespace_planning_algorithms/astar_search.hpp>
@@ -36,7 +37,8 @@ class FreespacePullOut : public PullOutPlannerBase
 public:
   FreespacePullOut(
     rclcpp::Node & node, const StartPlannerParameters & parameters,
-    const autoware::vehicle_info_utils::VehicleInfo & vehicle_info);
+    const autoware::vehicle_info_utils::VehicleInfo & vehicle_info,
+    std::shared_ptr<universe_utils::TimeKeeper> time_keeper);
 
   PlannerType getPlannerType() const override { return PlannerType::FREESPACE; }
 
diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp
index c63dd392a5463..70ca2bdb10d37 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp
@@ -18,6 +18,7 @@
 #include "autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp"
 #include "autoware/behavior_path_start_planner_module/pull_out_path.hpp"
 #include "autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp"
+#include "autoware/universe_utils/system/time_keeper.hpp"
 
 #include <autoware/lane_departure_checker/lane_departure_checker.hpp>
 
@@ -33,7 +34,8 @@ class GeometricPullOut : public PullOutPlannerBase
   explicit GeometricPullOut(
     rclcpp::Node & node, const StartPlannerParameters & parameters,
     const std::shared_ptr<autoware::lane_departure_checker::LaneDepartureChecker>
-      lane_departure_checker);
+      lane_departure_checker,
+    std::shared_ptr<universe_utils::TimeKeeper> time_keeper);
 
   PlannerType getPlannerType() const override { return PlannerType::GEOMETRIC; };
   std::optional<PullOutPath> plan(
diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp
index 03b6e13cb2d3e..de2e57ff23576 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp
@@ -20,6 +20,7 @@
 #include "autoware/behavior_path_start_planner_module/data_structs.hpp"
 #include "autoware/behavior_path_start_planner_module/pull_out_path.hpp"
 #include "autoware/behavior_path_start_planner_module/util.hpp"
+#include "autoware/universe_utils/system/time_keeper.hpp"
 
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
@@ -37,7 +38,10 @@ using tier4_planning_msgs::msg::PathWithLaneId;
 class PullOutPlannerBase
 {
 public:
-  explicit PullOutPlannerBase(rclcpp::Node & node, const StartPlannerParameters & parameters)
+  explicit PullOutPlannerBase(
+    rclcpp::Node & node, const StartPlannerParameters & parameters,
+    std::shared_ptr<universe_utils::TimeKeeper> time_keeper)
+  : time_keeper_(time_keeper)
   {
     vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo();
     vehicle_footprint_ = vehicle_info_.createFootprint();
@@ -63,6 +67,8 @@ class PullOutPlannerBase
     autoware::behavior_path_planner::PullOutPath & pull_out_path,
     double collision_check_distance_from_end) const
   {
+    universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
     // check for collisions
     const auto & dynamic_objects = planner_data_->dynamic_object;
     const auto pull_out_lanes = start_planner_utils::getPullOutLanes(
@@ -95,6 +101,8 @@ class PullOutPlannerBase
   LinearRing2d vehicle_footprint_;
   StartPlannerParameters parameters_;
   double collision_check_margin_;
+
+  mutable std::shared_ptr<universe_utils::TimeKeeper> time_keeper_;
 };
 }  // namespace autoware::behavior_path_planner
 
diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp
index 92cad1c2eb8b8..d1b4c9dabdd7f 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp
@@ -17,6 +17,7 @@
 
 #include "autoware/behavior_path_start_planner_module/pull_out_path.hpp"
 #include "autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp"
+#include "autoware/universe_utils/system/time_keeper.hpp"
 
 #include <autoware/lane_departure_checker/lane_departure_checker.hpp>
 
@@ -34,7 +35,8 @@ class ShiftPullOut : public PullOutPlannerBase
 public:
   explicit ShiftPullOut(
     rclcpp::Node & node, const StartPlannerParameters & parameters,
-    std::shared_ptr<LaneDepartureChecker> & lane_departure_checker);
+    std::shared_ptr<LaneDepartureChecker> & lane_departure_checker,
+    std::shared_ptr<universe_utils::TimeKeeper> time_keeper);
 
   PlannerType getPlannerType() const override { return PlannerType::SHIFT; };
   std::optional<PullOutPath> plan(
diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp
index 9c9c084f823c4..c3f5e8883c284 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp
@@ -28,8 +28,10 @@ namespace autoware::behavior_path_planner
 {
 FreespacePullOut::FreespacePullOut(
   rclcpp::Node & node, const StartPlannerParameters & parameters,
-  const autoware::vehicle_info_utils::VehicleInfo & vehicle_info)
-: PullOutPlannerBase{node, parameters}, velocity_{parameters.freespace_planner_velocity}
+  const autoware::vehicle_info_utils::VehicleInfo & vehicle_info,
+  std::shared_ptr<universe_utils::TimeKeeper> time_keeper)
+: PullOutPlannerBase{node, parameters, time_keeper},
+  velocity_{parameters.freespace_planner_velocity}
 {
   autoware::freespace_planning_algorithms::VehicleShape vehicle_shape(
     vehicle_info, parameters.vehicle_shape_margin);
@@ -54,9 +56,12 @@ std::optional<PullOutPath> FreespacePullOut::plan(
 
   planner_->setMap(*planner_data_->costmap);
 
-  const bool found_path = planner_->makePlan(start_pose, end_pose);
-  if (!found_path) {
-    planner_debug_data.conditions_evaluation.emplace_back("no path found");
+  try {
+    if (!planner_->makePlan(start_pose, end_pose)) {
+      planner_debug_data.conditions_evaluation.emplace_back("no path found");
+      return {};
+    }
+  } catch (const std::exception & e) {
     return {};
   }
 
diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp
index 4470026bc8dda..13a9908b97daf 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp
@@ -33,8 +33,9 @@ using start_planner_utils::getPullOutLanes;
 GeometricPullOut::GeometricPullOut(
   rclcpp::Node & node, const StartPlannerParameters & parameters,
   const std::shared_ptr<autoware::lane_departure_checker::LaneDepartureChecker>
-    lane_departure_checker)
-: PullOutPlannerBase{node, parameters},
+    lane_departure_checker,
+  std::shared_ptr<universe_utils::TimeKeeper> time_keeper)
+: PullOutPlannerBase{node, parameters, time_keeper},
   parallel_parking_parameters_{parameters.parallel_parking_parameters},
   lane_departure_checker_(lane_departure_checker)
 {
diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp
index 18970ead153af..2b63ff6df0db2 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp
@@ -37,8 +37,9 @@ using start_planner_utils::getPullOutLanes;
 
 ShiftPullOut::ShiftPullOut(
   rclcpp::Node & node, const StartPlannerParameters & parameters,
-  std::shared_ptr<LaneDepartureChecker> & lane_departure_checker)
-: PullOutPlannerBase{node, parameters}, lane_departure_checker_{lane_departure_checker}
+  std::shared_ptr<LaneDepartureChecker> & lane_departure_checker,
+  std::shared_ptr<universe_utils::TimeKeeper> time_keeper)
+: PullOutPlannerBase{node, parameters, time_keeper}, lane_departure_checker_{lane_departure_checker}
 {
 }
 
@@ -62,6 +63,8 @@ std::optional<PullOutPath> ShiftPullOut::plan(
 
   // get safe path
   for (auto & pull_out_path : pull_out_paths) {
+    universe_utils::ScopedTimeTrack st("get safe path", *time_keeper_);
+
     // shift path is not separate but only one.
     auto & shift_path = pull_out_path.partial_paths.front();
     // check lane_departure with path between pull_out_start to pull_out_end
@@ -215,6 +218,8 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes,
   const Pose & start_pose, const Pose & goal_pose)
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   std::vector<PullOutPath> candidate_paths{};
 
   if (road_lanes.empty()) {
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 c621b16824110..5c3c62b947af2 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
@@ -65,7 +65,7 @@ StartPlannerModule::StartPlannerModule(
   vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()},
   is_freespace_planner_cb_running_{false}
 {
-  lane_departure_checker_ = std::make_shared<LaneDepartureChecker>();
+  lane_departure_checker_ = std::make_shared<LaneDepartureChecker>(time_keeper_);
   lane_departure_checker_->setVehicleInfo(vehicle_info_);
   autoware::lane_departure_checker::Param lane_departure_checker_params{};
   lane_departure_checker_params.footprint_extra_margin =
@@ -76,18 +76,19 @@ StartPlannerModule::StartPlannerModule(
   // set enabled planner
   if (parameters_->enable_shift_pull_out) {
     start_planners_.push_back(
-      std::make_shared<ShiftPullOut>(node, *parameters, lane_departure_checker_));
+      std::make_shared<ShiftPullOut>(node, *parameters, lane_departure_checker_, time_keeper_));
   }
   if (parameters_->enable_geometric_pull_out) {
     start_planners_.push_back(
-      std::make_shared<GeometricPullOut>(node, *parameters, lane_departure_checker_));
+      std::make_shared<GeometricPullOut>(node, *parameters, lane_departure_checker_, time_keeper_));
   }
   if (start_planners_.empty()) {
     RCLCPP_ERROR(getLogger(), "Not found enabled planner");
   }
 
   if (parameters_->enable_freespace_planner) {
-    freespace_planner_ = std::make_unique<FreespacePullOut>(node, *parameters, vehicle_info_);
+    freespace_planner_ =
+      std::make_unique<FreespacePullOut>(node, *parameters, vehicle_info_, time_keeper_);
     const auto freespace_planner_period_ns = rclcpp::Rate(1.0).period();
     freespace_planner_timer_cb_group_ =
       node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
@@ -211,6 +212,8 @@ void StartPlannerModule::updateObjectsFilteringParams(
 
 void StartPlannerModule::updateData()
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   // The method PlannerManager::run() calls SceneModuleInterface::setData and
   // SceneModuleInterface::setPreviousModuleOutput() before this module's run() method is called
   // with module_ptr->run(). Then module_ptr->run() invokes StartPlannerModule::updateData and,
@@ -368,6 +371,8 @@ bool StartPlannerModule::isCurrentPoseOnMiddleOfTheRoad() const
 
 bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   // prepare poses for preventing check
   // - current_pose
   // - estimated_stop_pose (The position assumed when stopped with the minimum stop distance,
@@ -397,6 +402,8 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const
 
 bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough(const Pose & ego_pose) const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   const auto & dynamic_object = planner_data_->dynamic_object;
   const auto & route_handler = planner_data_->route_handler;
   const Pose start_pose = planner_data_->route_handler->getOriginalStartPose();
@@ -646,6 +653,8 @@ bool StartPlannerModule::canTransitSuccessState()
 
 BehaviorModuleOutput StartPlannerModule::plan()
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   if (isWaitingApproval()) {
     clearWaitingApproval();
     resetPathCandidate();
@@ -662,6 +671,8 @@ BehaviorModuleOutput StartPlannerModule::plan()
   }
 
   const auto path = std::invoke([&]() {
+    universe_utils::ScopedTimeTrack st2("plan path", *time_keeper_);
+
     if (!status_.driving_forward && !status_.backward_driving_complete) {
       return status_.backward_path;
     }
@@ -775,6 +786,8 @@ PathWithLaneId StartPlannerModule::getFullPath() const
 
 BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   updatePullOutStatus();
 
   if (!status_.found_pull_out_path) {
@@ -868,6 +881,8 @@ void StartPlannerModule::planWithPriority(
   const std::vector<Pose> & start_pose_candidates, const Pose & refined_start_pose,
   const Pose & goal_pose, const std::string & search_priority)
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   if (start_pose_candidates.empty()) return;
 
   auto get_accumulated_debug_stream = [](const std::vector<PlannerDebugData> & debug_data_vector) {
@@ -884,18 +899,22 @@ void StartPlannerModule::planWithPriority(
     determinePriorityOrder(search_priority, start_pose_candidates.size());
 
   std::vector<PlannerDebugData> debug_data_vector;
-  for (const auto & collision_check_margin : parameters_->collision_check_margins) {
-    for (const auto & [index, planner] : order_priority) {
-      if (findPullOutPath(
-            start_pose_candidates[index], planner, refined_start_pose, goal_pose,
-            collision_check_margin, debug_data_vector)) {
-        debug_data_.selected_start_pose_candidate_index = index;
-        debug_data_.margin_for_start_pose_candidate = collision_check_margin;
-        if (parameters_->print_debug_info) {
-          const auto ss = get_accumulated_debug_stream(debug_data_vector);
-          DEBUG_PRINT("\nPull out path search results:\n%s", ss.str().c_str());
+  {  // create a scope for the scoped time track
+    universe_utils::ScopedTimeTrack st2("findPullOutPaths", *time_keeper_);
+
+    for (const auto & collision_check_margin : parameters_->collision_check_margins) {
+      for (const auto & [index, planner] : order_priority) {
+        if (findPullOutPath(
+              start_pose_candidates[index], planner, refined_start_pose, goal_pose,
+              collision_check_margin, debug_data_vector)) {
+          debug_data_.selected_start_pose_candidate_index = index;
+          debug_data_.margin_for_start_pose_candidate = collision_check_margin;
+          if (parameters_->print_debug_info) {
+            const auto ss = get_accumulated_debug_stream(debug_data_vector);
+            DEBUG_PRINT("\nPull out path search results:\n%s", ss.str().c_str());
+          }
+          return;
         }
-        return;
       }
     }
   }
@@ -910,6 +929,8 @@ void StartPlannerModule::planWithPriority(
 PriorityOrder StartPlannerModule::determinePriorityOrder(
   const std::string & search_priority, const size_t start_pose_candidates_num)
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   PriorityOrder order_priority;
   if (search_priority == "efficient_path") {
     for (const auto & planner : start_planners_) {
@@ -1080,6 +1101,8 @@ std::vector<DrivableLanes> StartPlannerModule::generateDrivableLanes(
 
 void StartPlannerModule::updatePullOutStatus()
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   // skip updating if enough time has not passed for preventing chattering between back and
   // start_planner
   if (!receivedNewRoute()) {
@@ -1148,6 +1171,8 @@ void StartPlannerModule::updateStatusAfterBackwardDriving()
 
 PathWithLaneId StartPlannerModule::calcBackwardPathFromStartPose() const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   const Pose start_pose = planner_data_->route_handler->getOriginalStartPose();
   const auto pull_out_lanes = start_planner_utils::getPullOutLanes(
     planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance);
@@ -1175,6 +1200,8 @@ PathWithLaneId StartPlannerModule::calcBackwardPathFromStartPose() const
 std::vector<Pose> StartPlannerModule::searchPullOutStartPoseCandidates(
   const PathWithLaneId & back_path_from_start_pose) const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   std::vector<Pose> pull_out_start_pose_candidates{};
   const auto start_pose = planner_data_->route_handler->getOriginalStartPose();
   const auto local_vehicle_footprint = vehicle_info_.createFootprint();
@@ -1239,6 +1266,8 @@ PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes(
   const double velocity_threshold, const double object_check_forward_distance,
   const double object_check_backward_distance) const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity(
     *planner_data_->dynamic_object, velocity_threshold);
 
@@ -1388,6 +1417,8 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo()
 
 bool StartPlannerModule::isSafePath() const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   // TODO(Sugahara): should safety check for backward path
 
   const auto pull_out_path = getCurrentPath();
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 0e60e3216361d..f10f871ca3cc0 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
@@ -243,6 +243,8 @@
       # For cancel maneuver
       cancel:
         enable: true                                    # [-]
+        force:
+          duration_time: 2.0                            # [s]
 
       # For yield maneuver
       yield:
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 4e14dc4886768..35b547da1f9f6 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
@@ -107,6 +107,8 @@ struct AvoidanceParameters
   // if this param is true, it reverts avoidance path when the path is no longer needed.
   bool enable_cancel_maneuver{false};
 
+  double force_deactivate_duration_time{0.0};
+
   // enable avoidance for all parking vehicle
   std::string policy_ambiguous_vehicle{"ignore"};
 
@@ -581,6 +583,8 @@ struct AvoidancePlanningData
 
   bool found_avoidance_path{false};
 
+  bool force_deactivated{false};
+
   double to_stop_line{std::numeric_limits<double>::max()};
 
   double to_start_point{std::numeric_limits<double>::lowest()};
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 56ac3d7f4f1bb..cc7254380cf0b 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
@@ -301,6 +301,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
   {
     const std::string ns = "avoidance.cancel.";
     p.enable_cancel_maneuver = getOrDeclareParameter<bool>(*node, ns + "enable");
+    p.force_deactivate_duration_time =
+      getOrDeclareParameter<double>(*node, ns + "force.duration_time");
   }
 
   // yield
diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp
index 87a4c91792649..ea12c66859a36 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp
@@ -477,6 +477,9 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface
   mutable std::vector<AvoidanceDebugMsg> debug_avoidance_initializer_for_shift_line_;
 
   mutable rclcpp::Time debug_avoidance_initializer_for_shift_line_time_;
+
+  bool force_deactivated_{false};
+  rclcpp::Time last_deactivation_triggered_time_;
 };
 
 }  // namespace autoware::behavior_path_planner
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 38a91ac39525b..2a581f90ab255 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
@@ -1263,6 +1263,16 @@
               "type": "boolean",
               "description": "Flag to enable cancel maneuver.",
               "default": "true"
+            },
+            "force": {
+              "type": "object",
+              "properties": {
+                "duration_time": {
+                  "type": "number",
+                  "description": "force deactivate duration time",
+                  "default": 2.0
+                }
+              }
             }
           },
           "required": ["enable"],
diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp
index 5655696ff086d..015a82aea3013 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp
@@ -182,6 +182,11 @@ void StaticObstacleAvoidanceModuleManager::updateModuleParams(
     updateParam<bool>(parameters, ns + "consider_rear_overhang", p->consider_rear_overhang);
   }
 
+  {
+    const std::string ns = "avoidance.cancel.";
+    updateParam<double>(parameters, ns + "force.duration_time", p->force_deactivate_duration_time);
+  }
+
   {
     const std::string ns = "avoidance.stop.";
     updateParam<double>(parameters, ns + "max_distance", p->stop_max_distance);
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 d1b78dbd6a00a..e28bd136a0681 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
@@ -570,6 +570,24 @@ void StaticObstacleAvoidanceModule::fillEgoStatus(
     return;
   }
 
+  const auto registered_sl_force_deactivated =
+    [&](const std::string & direction, const RegisteredShiftLineArray shift_line_array) {
+      return std::any_of(
+        shift_line_array.begin(), shift_line_array.end(), [&](const auto & shift_line) {
+          return rtc_interface_ptr_map_.at(direction)->isForceDeactivated(shift_line.uuid);
+        });
+    };
+
+  const auto is_force_deactivated = registered_sl_force_deactivated("left", left_shift_array_) ||
+                                    registered_sl_force_deactivated("right", right_shift_array_);
+  if (is_force_deactivated && can_yield_maneuver) {
+    data.yield_required = true;
+    data.safe_shift_line = data.new_shift_line;
+    data.force_deactivated = true;
+    RCLCPP_INFO(getLogger(), "this module is force deactivated. wait until reactivation");
+    return;
+  }
+
   /**
    * If the avoidance path is safe, use unapproved_new_sl for avoidance path generation.
    */
@@ -755,6 +773,10 @@ bool StaticObstacleAvoidanceModule::isSafePath(
   universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
   const auto & p = planner_data_->parameters;
 
+  if (force_deactivated_) {
+    return false;
+  }
+
   if (!parameters_->enable_safety_check) {
     return true;  // if safety check is disabled, it always return safe.
   }
@@ -1371,6 +1393,19 @@ void StaticObstacleAvoidanceModule::updateData()
   }
 
   safe_ = avoid_data_.safe;
+
+  if (!force_deactivated_) {
+    last_deactivation_triggered_time_ = clock_->now();
+    force_deactivated_ = avoid_data_.force_deactivated;
+    return;
+  }
+
+  if (
+    (clock_->now() - last_deactivation_triggered_time_).seconds() >
+    parameters_->force_deactivate_duration_time) {
+    RCLCPP_INFO(getLogger(), "The force deactivation is released");
+    force_deactivated_ = false;
+  }
 }
 
 void StaticObstacleAvoidanceModule::processOnEntry()
diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp
index 5e58466fa4229..57a6e340bf853 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp
@@ -827,6 +827,8 @@ AvoidLineArray ShiftLineGenerator::applyFillGapProcess(
   utils::static_obstacle_avoidance::fillAdditionalInfoFromLongitudinal(
     data, debug.step1_front_shift_line);
 
+  applySmallShiftFilter(ret, 1e-3);
+
   return ret;
 }
 
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 40373f00d4620..6038fe3daae16 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
@@ -693,6 +693,15 @@ bool isNeverAvoidanceTarget(
       }
     }
 
+    const auto right_opposite_lanes =
+      planner_data->route_handler->getRightOppositeLanelets(object.overhang_lanelet);
+    if (!right_opposite_lanes.empty() && isOnRight(object)) {
+      object.info = ObjectInfo::IS_NOT_PARKING_OBJECT;
+      RCLCPP_DEBUG(
+        rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it.");
+      return true;
+    }
+
     const auto left_lane =
       planner_data->route_handler->getLeftLanelet(object.overhang_lanelet, true, true);
     if (left_lane.has_value() && !isOnRight(object)) {
@@ -715,6 +724,15 @@ bool isNeverAvoidanceTarget(
         return true;
       }
     }
+
+    const auto left_opposite_lanes =
+      planner_data->route_handler->getLeftOppositeLanelets(object.overhang_lanelet);
+    if (!left_opposite_lanes.empty() && !isOnRight(object)) {
+      object.info = ObjectInfo::IS_NOT_PARKING_OBJECT;
+      RCLCPP_DEBUG(
+        rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it.");
+      return true;
+    }
   }
 
   if (isCloseToStopFactor(object, data, planner_data, parameters)) {
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 9cbe962e88cae..d6a1747259962 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
@@ -45,7 +45,8 @@ DetectionAreaModule::DetectionAreaModule(
   lane_id_(lane_id),
   detection_area_reg_elem_(detection_area_reg_elem),
   state_(State::GO),
-  planner_param_(planner_param)
+  planner_param_(planner_param),
+  debug_data_()
 {
   velocity_factor_.init(PlanningBehavior::USER_DEFINED_DETECTION_AREA);
 }
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp
index b749207aa2393..d93fbf271aa1d 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp
@@ -74,12 +74,12 @@ ObjectInfo::ObjectInfo(const unique_identifier_msgs::msg::UUID & uuid) : uuid_st
 
 void ObjectInfo::initialize(
   const autoware_perception_msgs::msg::PredictedObject & object,
-  std::optional<lanelet::ConstLanelet> attention_lanelet_opt_,
-  std::optional<lanelet::ConstLineString3d> stopline_opt_)
+  std::optional<lanelet::ConstLanelet> attention_lanelet_opt,
+  std::optional<lanelet::ConstLineString3d> stopline_opt)
 {
   predicted_object_ = object;
-  attention_lanelet_opt = attention_lanelet_opt_;
-  stopline_opt = stopline_opt_;
+  attention_lanelet_opt_ = attention_lanelet_opt;
+  stopline_opt_ = stopline_opt;
   unsafe_interval_ = std::nullopt;
   calc_dist_to_stopline();
 }
@@ -96,10 +96,10 @@ void ObjectInfo::update_safety(
 std::optional<geometry_msgs::msg::Point> ObjectInfo::estimated_past_position(
   const double past_duration) const
 {
-  if (!attention_lanelet_opt) {
+  if (!attention_lanelet_opt_) {
     return std::nullopt;
   }
-  const auto attention_lanelet = attention_lanelet_opt.value();
+  const auto attention_lanelet = attention_lanelet_opt_.value();
   const auto current_arc_coords = lanelet::utils::getArcCoordinates(
     {attention_lanelet}, predicted_object_.kinematics.initial_pose_with_covariance.pose);
   const auto distance = current_arc_coords.distance;
@@ -116,29 +116,29 @@ std::optional<geometry_msgs::msg::Point> ObjectInfo::estimated_past_position(
 
 void ObjectInfo::calc_dist_to_stopline()
 {
-  if (!stopline_opt || !attention_lanelet_opt) {
+  if (!stopline_opt_ || !attention_lanelet_opt_) {
     return;
   }
-  const auto attention_lanelet = attention_lanelet_opt.value();
+  const auto attention_lanelet = attention_lanelet_opt_.value();
   const auto object_arc_coords = lanelet::utils::getArcCoordinates(
     {attention_lanelet}, predicted_object_.kinematics.initial_pose_with_covariance.pose);
-  const auto stopline = stopline_opt.value();
+  const auto stopline = stopline_opt_.value();
   geometry_msgs::msg::Pose stopline_center;
   stopline_center.position.x = (stopline.front().x() + stopline.back().x()) / 2.0;
   stopline_center.position.y = (stopline.front().y() + stopline.back().y()) / 2.0;
   stopline_center.position.z = (stopline.front().z() + stopline.back().z()) / 2.0;
   const auto stopline_arc_coords =
     lanelet::utils::getArcCoordinates({attention_lanelet}, stopline_center);
-  dist_to_stopline_opt = (stopline_arc_coords.length - object_arc_coords.length);
+  dist_to_stopline_opt_ = (stopline_arc_coords.length - object_arc_coords.length);
 }
 
 bool ObjectInfo::can_stop_before_stopline(const double brake_deceleration) const
 {
-  if (!dist_to_stopline_opt) {
+  if (!dist_to_stopline_opt_) {
     return false;
   }
   const double velocity = predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x;
-  const double dist_to_stopline = dist_to_stopline_opt.value();
+  const double dist_to_stopline = dist_to_stopline_opt_.value();
   const double braking_distance = (velocity * velocity) / (2.0 * brake_deceleration);
   return dist_to_stopline > braking_distance;
 }
@@ -147,17 +147,17 @@ bool ObjectInfo::can_stop_before_ego_lane(
   const double brake_deceleration, const double tolerable_overshoot,
   lanelet::ConstLanelet ego_lane) const
 {
-  if (!dist_to_stopline_opt || !stopline_opt || !attention_lanelet_opt) {
+  if (!dist_to_stopline_opt_ || !stopline_opt_ || !attention_lanelet_opt_) {
     return false;
   }
-  const double dist_to_stopline = dist_to_stopline_opt.value();
+  const double dist_to_stopline = dist_to_stopline_opt_.value();
   const double velocity = predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x;
   const double braking_distance = (velocity * velocity) / (2.0 * brake_deceleration);
   if (dist_to_stopline > braking_distance) {
     return false;
   }
-  const auto attention_lanelet = attention_lanelet_opt.value();
-  const auto stopline = stopline_opt.value();
+  const auto attention_lanelet = attention_lanelet_opt_.value();
+  const auto stopline = stopline_opt_.value();
   const auto stopline_p1 = stopline.front();
   const auto stopline_p2 = stopline.back();
   const autoware::universe_utils::Point2d stopline_mid{
@@ -185,10 +185,10 @@ bool ObjectInfo::can_stop_before_ego_lane(
 
 bool ObjectInfo::before_stopline_by(const double margin) const
 {
-  if (!dist_to_stopline_opt) {
+  if (!dist_to_stopline_opt_) {
     return false;
   }
-  const double dist_to_stopline = dist_to_stopline_opt.value();
+  const double dist_to_stopline = dist_to_stopline_opt_.value();
   return dist_to_stopline < margin;
 }
 
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.hpp
index 0ba9947fdb318..7f7fe7429b6c9 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.hpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.hpp
@@ -126,7 +126,7 @@ class ObjectInfo
    * @brief update predicted_object_, attention_lanelet, stopline, dist_to_stopline
    */
   void initialize(
-    const autoware_perception_msgs::msg::PredictedObject & predicted_object,
+    const autoware_perception_msgs::msg::PredictedObject & object,
     std::optional<lanelet::ConstLanelet> attention_lanelet_opt,
     std::optional<lanelet::ConstLineString3d> stopline_opt);
 
@@ -134,9 +134,8 @@ class ObjectInfo
    * @brief update unsafe_knowledge
    */
   void update_safety(
-    const std::optional<CollisionInterval> & unsafe_interval_opt,
-    const std::optional<CollisionInterval> & safe_interval_opt,
-    const bool safe_under_traffic_control);
+    const std::optional<CollisionInterval> & unsafe_interval,
+    const std::optional<CollisionInterval> & safe_interval, const bool safe_under_traffic_control);
 
   /**
    * @brief find the estimated position of the object in the past
@@ -196,13 +195,13 @@ class ObjectInfo
   autoware_perception_msgs::msg::PredictedObject predicted_object_;
 
   //! null if the object in intersection_area but not in attention_area
-  std::optional<lanelet::ConstLanelet> attention_lanelet_opt{std::nullopt};
+  std::optional<lanelet::ConstLanelet> attention_lanelet_opt_{std::nullopt};
 
   //! null if the object in intersection_area but not in attention_area
-  std::optional<lanelet::ConstLineString3d> stopline_opt{std::nullopt};
+  std::optional<lanelet::ConstLineString3d> stopline_opt_{std::nullopt};
 
   //! null if the object in intersection_area but not in attention_area
-  std::optional<double> dist_to_stopline_opt{std::nullopt};
+  std::optional<double> dist_to_stopline_opt_{std::nullopt};
 
   //! store the information if judged as UNSAFE
   std::optional<CollisionInterval> unsafe_interval_{std::nullopt};
@@ -230,11 +229,12 @@ class ObjectInfoManager
 public:
   std::shared_ptr<ObjectInfo> registerObject(
     const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area,
-    const bool belong_intersection_area, const bool is_parked);
+    const bool belong_intersection_area, const bool is_parked_vehicle);
 
   void registerExistingObject(
     const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area,
-    const bool belong_intersection_area, const bool is_parked, std::shared_ptr<ObjectInfo> object);
+    const bool belong_intersection_area, const bool is_parked_vehicle,
+    std::shared_ptr<ObjectInfo> object);
 
   void clearObjects();
 
@@ -248,7 +248,7 @@ class ObjectInfoManager
   std::vector<std::shared_ptr<ObjectInfo>> allObjects() const;
 
   const std::unordered_map<unique_identifier_msgs::msg::UUID, std::shared_ptr<ObjectInfo>> &
-  getObjectsMap()
+  getObjectsMap() const
   {
     return objects_info_;
   }
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp
index ea2e1a7ae16d7..03417a8410d20 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp
@@ -590,7 +590,7 @@ class IntersectionModule : public SceneModuleInterface
    * @brief generate discretized detection lane linestring.
    */
   std::vector<lanelet::ConstLineString3d> generateDetectionLaneDivisions(
-    lanelet::ConstLanelets detection_lanelets,
+    lanelet::ConstLanelets detection_lanelets_all,
     const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) const;
   /** @} */
 
@@ -752,7 +752,7 @@ class IntersectionModule : public SceneModuleInterface
 
   void cutPredictPathWithinDuration(
     const builtin_interfaces::msg::Time & object_stamp, const double time_thr,
-    autoware_perception_msgs::msg::PredictedPath * predicted_path) const;
+    autoware_perception_msgs::msg::PredictedPath * path) const;
 
   /**
    * @brief check if there are any objects around the stoplines on the attention areas when ego
@@ -809,7 +809,7 @@ class IntersectionModule : public SceneModuleInterface
   TimeDistanceArray calcIntersectionPassingTime(
     const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized,
     const IntersectionStopLines & intersection_stoplines,
-    tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) const;
+    tier4_debug_msgs::msg::Float64MultiArrayStamped * ego_ttc_array) const;
   /** @} */
 
   mutable DebugData debug_data_;
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp
index 465fd770049e9..c3af04bbd1d1f 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp
@@ -31,6 +31,7 @@ NoDrivableLaneModule::NoDrivableLaneModule(
 : SceneModuleInterface(module_id, logger, clock),
   lane_id_(lane_id),
   planner_param_(planner_param),
+  debug_data_(),
   state_(State::INIT)
 {
   velocity_factor_.init(PlanningBehavior::NO_DRIVABLE_LANE);
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp
index 1e8e93834f89e..d9e6121a8ae51 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp
@@ -76,7 +76,7 @@ class NoDrivableLaneModule : public SceneModuleInterface
 
   PathWithNoDrivableLanePolygonIntersection path_no_drivable_lane_polygon_intersection;
   geometry_msgs::msg::Point first_intersection_point;
-  double distance_ego_first_intersection;
+  double distance_ego_first_intersection{};
 
   void handle_init_state();
   void handle_approaching_state(PathWithLaneId * path, StopReason * stop_reason);
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 0478aea34a44f..f2bcc0b5abf44 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
@@ -48,7 +48,8 @@ NoStoppingAreaModule::NoStoppingAreaModule(
 : SceneModuleInterface(module_id, logger, clock),
   lane_id_(lane_id),
   no_stopping_area_reg_elem_(no_stopping_area_reg_elem),
-  planner_param_(planner_param)
+  planner_param_(planner_param),
+  debug_data_()
 {
   velocity_factor_.init(PlanningBehavior::NO_STOPPING_AREA);
   state_machine_.setState(StateMachine::State::GO);
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp
index a32cb4018cda9..5760b6de66a71 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp
@@ -131,7 +131,7 @@ MarkerArray makeDebugInfoMarkers(T & debug_data)
 }
 
 MarkerArray makePolygonMarker(
-  const BasicPolygons & polygons, const std::string ns, const int id, const double z)
+  const BasicPolygons & polygons, const std::string & ns, const int id, const double z)
 {
   MarkerArray debug_markers;
   Marker debug_marker;
@@ -159,7 +159,7 @@ MarkerArray makePolygonMarker(
 }
 
 MarkerArray makeSlicePolygonMarker(
-  const Polygons2d & slices, const std::string ns, const int id, const double z)
+  const Polygons2d & slices, const std::string & ns, const int id, const double z)
 {
   MarkerArray debug_markers;
   Marker debug_marker;
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp
index 911842ddd57ed..6d40c2167961a 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp
@@ -102,7 +102,7 @@ void calcSlowDownPointsForPossibleCollision(
   }
   std::sort(
     possible_collisions.begin(), possible_collisions.end(),
-    [](PossibleCollisionInfo pc1, PossibleCollisionInfo pc2) {
+    [](const PossibleCollisionInfo & pc1, const PossibleCollisionInfo & pc2) {
       return pc1.arc_lane_dist_at_collision.length < pc2.arc_lane_dist_at_collision.length;
     });
 
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp
index 992cc5787bf58..7bff65d59157d 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp
@@ -163,7 +163,14 @@ struct PossibleCollisionInfo
   Pose collision_pose;                                 // only use this for debugging
   Pose intersection_pose;                              // For egp path and hidden obstacle
   lanelet::ArcCoordinates arc_lane_dist_at_collision;  // For ego distance to obstacle in s-d
-  PossibleCollisionInfo() = default;
+  PossibleCollisionInfo()
+  : obstacle_info(),
+    collision_with_margin(),
+    collision_pose(),
+    intersection_pose(),
+    arc_lane_dist_at_collision()
+  {
+  }
   PossibleCollisionInfo(
     const ObstacleInfo & obstacle_info, const PathPoint & collision_with_margin,
     const Pose & intersection_pose, const lanelet::ArcCoordinates & arc_lane_dist_to_occlusion)
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp
index 5900bd3531266..6815ca3ff8cb4 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp
@@ -76,7 +76,7 @@ bool smoothPath(
   TrajectoryPoints traj_smoothed;
   clipped.insert(
     clipped.end(), traj_resampled.begin() + traj_resampled_closest, traj_resampled.end());
-  if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories)) {
+  if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories, false)) {
     std::cerr << "[behavior_velocity][trajectory_utils]: failed to smooth" << std::endl;
     return false;
   }
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp
index 826bdf5a8ed91..983a371234ccf 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp
@@ -202,7 +202,7 @@ autoware::motion_utils::VirtualWalls RunOutDebug::createVirtualWalls()
 }
 
 visualization_msgs::msg::MarkerArray RunOutDebug::createVisualizationMarkerArrayFromDebugData(
-  const builtin_interfaces::msg::Time & current_time)
+  const builtin_interfaces::msg::Time & current_time) const
 {
   visualization_msgs::msg::MarkerArray msg;
 
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.hpp
index b9ea77fc91e8c..65eda86a9a8b8 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.hpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.hpp
@@ -54,7 +54,7 @@ class DebugValues
    * @param [in] type the TYPE enum for which to get the index
    * @return index of the type
    */
-  int getValuesIdx(const TYPE type) const { return static_cast<int>(type); }
+  static int getValuesIdx(const TYPE type) { return static_cast<int>(type); }
   /**
    * @brief get all the debug values as an std::array
    * @return array of all debug values
@@ -127,7 +127,7 @@ class RunOutDebug
 
 private:
   visualization_msgs::msg::MarkerArray createVisualizationMarkerArrayFromDebugData(
-    const builtin_interfaces::msg::Time & current_time);
+    const builtin_interfaces::msg::Time & current_time) const;
   void clearDebugMarker();
 
   rclcpp::Node & node_;
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/state_machine.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/state_machine.cpp
index 484a7cdcd95e3..0a23d2218feb0 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/state_machine.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/state_machine.cpp
@@ -20,7 +20,7 @@ namespace run_out_utils
 {
 using State = StateMachine::State;
 
-std::string StateMachine::toString(const State & state) const
+std::string StateMachine::toString(const State & state)
 {
   switch (state) {
     case State::GO:
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/state_machine.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/state_machine.hpp
index b88aa821548bb..d5b81dd0fc3cb 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/state_machine.hpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/state_machine.hpp
@@ -42,7 +42,7 @@ class StateMachine
   explicit StateMachine(const StateParam & state_param) { state_param_ = state_param; }
   State getCurrentState() const { return state_; }
   std::optional<DynamicObstacle> getTargetObstacle() const { return target_obstacle_; }
-  std::string toString(const State & state) const;
+  static std::string toString(const State & state);
   void updateState(const StateInput & state_input, rclcpp::Clock & clock);
 
 private:
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp
index f0064a52891db..dfe5d4de85300 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp
@@ -35,7 +35,8 @@ SpeedBumpModule::SpeedBumpModule(
   module_id_(module_id),
   lane_id_(lane_id),
   speed_bump_reg_elem_(std::move(speed_bump_reg_elem)),
-  planner_param_(planner_param)
+  planner_param_(planner_param),
+  debug_data_()
 {
   // Read speed bump height [m] from map
   const auto speed_bump_height =
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 c5079452f5ada..50b85613ef08f 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
@@ -33,7 +33,8 @@ StopLineModule::StopLineModule(
 : SceneModuleInterface(module_id, logger, clock),
   lane_id_(lane_id),
   stop_line_(stop_line),
-  state_(State::APPROACH)
+  state_(State::APPROACH),
+  debug_data_()
 {
   velocity_factor_.init(PlanningBehavior::STOP_SIGN);
   planner_param_ = planner_param;
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp
index 7770a480f8401..af16cd85741e5 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp
@@ -170,6 +170,7 @@ TrafficLightModule::TrafficLightModule(
   traffic_light_reg_elem_(traffic_light_reg_elem),
   lane_(lane),
   state_(State::APPROACH),
+  debug_data_(),
   is_prev_state_stop_(false)
 {
   velocity_factor_.init(PlanningBehavior::TRAFFIC_SIGNAL);
diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/parameters.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/parameters.hpp
index c9fe26cc2bb50..86ec8335e6362 100644
--- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/parameters.hpp
+++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/parameters.hpp
@@ -46,9 +46,9 @@ struct ObstacleParameters
   double dynamic_obstacles_buffer{};
   double dynamic_obstacles_min_vel{};
   std::vector<std::string> static_map_tags{};
-  bool filter_envelope;
-  bool ignore_on_path;
-  double ignore_extra_distance;
+  bool filter_envelope{};
+  bool ignore_on_path{};
+  double ignore_extra_distance{};
   size_t rtree_min_points{};
   size_t rtree_min_segments{};
 
diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml
index cc8f6f65b272d..a0cf69ee71027 100644
--- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml
+++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml
@@ -28,7 +28,8 @@
       action:  # action to insert in the trajectory if an object causes a conflict at an overlap
         skip_if_over_max_decel: true  # if true, do not take an action that would cause more deceleration than the maximum allowed
         precision: 0.1  # [m] precision when inserting a stop pose in the trajectory
-        distance_buffer: 1.5  # [m] buffer distance to try to keep between the ego footprint and lane
+        longitudinal_distance_buffer: 1.5  # [m] safety distance buffer to keep in front of the ego vehicle
+        lateral_distance_buffer: 1.0  # [m] safety distance buffer to keep on the side of the ego vehicle
         min_duration: 1.0  # [s] minimum duration needed before a decision can be canceled
         slowdown:
           distance_threshold: 30.0 # [m] insert a slowdown when closer than this distance from an overlap
diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp
index 0c793c9f5798e..8b03fa66eab55 100644
--- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp
+++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp
@@ -70,9 +70,11 @@ std::optional<TrajectoryPoint> calculate_last_in_lane_pose(
 
 std::optional<SlowdownToInsert> calculate_slowdown_point(
   const EgoData & ego_data, const std::vector<Slowdown> & decisions,
-  const std::optional<SlowdownToInsert> prev_slowdown_point, PlannerParam params)
+  const std::optional<SlowdownToInsert> & prev_slowdown_point, PlannerParam params)
 {
-  params.extra_front_offset += params.dist_buffer;
+  params.extra_front_offset += params.lon_dist_buffer;
+  params.extra_right_offset += params.lat_dist_buffer;
+  params.extra_left_offset += params.lat_dist_buffer;
   const auto base_footprint = make_base_footprint(params);
 
   // search for the first slowdown decision for which a stop point can be inserted
diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp
index ee4897de86c5b..145f21c94c0d0 100644
--- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp
+++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp
@@ -53,6 +53,6 @@ std::optional<TrajectoryPoint> calculate_last_in_lane_pose(
 /// @return optional slowdown point to insert in the trajectory
 std::optional<SlowdownToInsert> calculate_slowdown_point(
   const EgoData & ego_data, const std::vector<Slowdown> & decisions,
-  const std::optional<SlowdownToInsert> prev_slowdown_point, PlannerParam params);
+  const std::optional<SlowdownToInsert> & prev_slowdown_point, PlannerParam params);
 }  // namespace autoware::motion_velocity_planner::out_of_lane
 #endif  // CALCULATE_SLOWDOWN_POINTS_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 a7f7cbf5f2242..0afedcd7f9c9a 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
@@ -94,7 +94,9 @@ void OutOfLaneModule::init_parameters(rclcpp::Node & node)
     getOrDeclareParameter<bool>(node, ns_ + ".action.skip_if_over_max_decel");
   pp.precision = getOrDeclareParameter<double>(node, ns_ + ".action.precision");
   pp.min_decision_duration = getOrDeclareParameter<double>(node, ns_ + ".action.min_duration");
-  pp.dist_buffer = getOrDeclareParameter<double>(node, ns_ + ".action.distance_buffer");
+  pp.lon_dist_buffer =
+    getOrDeclareParameter<double>(node, ns_ + ".action.longitudinal_distance_buffer");
+  pp.lat_dist_buffer = getOrDeclareParameter<double>(node, ns_ + ".action.lateral_distance_buffer");
   pp.slow_velocity = getOrDeclareParameter<double>(node, ns_ + ".action.slowdown.velocity");
   pp.slow_dist_threshold =
     getOrDeclareParameter<double>(node, ns_ + ".action.slowdown.distance_threshold");
@@ -140,7 +142,8 @@ void OutOfLaneModule::update_parameters(const std::vector<rclcpp::Parameter> & p
   updateParam(parameters, ns_ + ".action.skip_if_over_max_decel", pp.skip_if_over_max_decel);
   updateParam(parameters, ns_ + ".action.precision", pp.precision);
   updateParam(parameters, ns_ + ".action.min_duration", pp.min_decision_duration);
-  updateParam(parameters, ns_ + ".action.distance_buffer", pp.dist_buffer);
+  updateParam(parameters, ns_ + ".action.longitudinal_distance_buffer", pp.lon_dist_buffer);
+  updateParam(parameters, ns_ + ".action.lateral_distance_buffer", pp.lat_dist_buffer);
   updateParam(parameters, ns_ + ".action.slowdown.velocity", pp.slow_velocity);
   updateParam(parameters, ns_ + ".action.slowdown.distance_threshold", pp.slow_dist_threshold);
   updateParam(parameters, ns_ + ".action.stop.distance_threshold", pp.stop_dist_threshold);
diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp
index 852d3ab2d2bc1..9c68a0bf23a92 100644
--- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp
+++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp
@@ -62,7 +62,8 @@ struct PlannerParam
   double overlap_min_dist;      // [m] min distance inside another lane to consider an overlap
   // action to insert in the trajectory if an object causes a conflict at an overlap
   bool skip_if_over_max_decel;  // if true, skip the action if it causes more than the max decel
-  double dist_buffer;
+  double lon_dist_buffer;       // [m] safety distance buffer to keep in front of the ego vehicle
+  double lat_dist_buffer;       // [m] safety distance buffer to keep on the side of the ego vehicle
   double slow_velocity;
   double slow_dist_threshold;
   double stop_dist_threshold;
diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp
index 7849076af8472..8a1e7a1b6ddb5 100644
--- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp
+++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp
@@ -361,7 +361,7 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s
   autoware::motion_velocity_planner::TrajectoryPoints traj_smoothed;
   clipped.insert(
     clipped.end(), traj_resampled.begin() + traj_resampled_closest, traj_resampled.end());
-  if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories)) {
+  if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories, false)) {
     RCLCPP_ERROR(get_logger(), "failed to smooth");
   }
   traj_smoothed.insert(
diff --git a/sensing/autoware_image_diagnostics/src/image_diagnostics_node.cpp b/sensing/autoware_image_diagnostics/src/image_diagnostics_node.cpp
index 49204f5cbc756..829e0e3ee8d81 100644
--- a/sensing/autoware_image_diagnostics/src/image_diagnostics_node.cpp
+++ b/sensing/autoware_image_diagnostics/src/image_diagnostics_node.cpp
@@ -50,26 +50,19 @@ void ImageDiagNode::onImageDiagChecker(DiagnosticStatusWrapper & stat)
   stat.add("number backlight  regions ", std::to_string(params_.num_of_regions_backlight));
 
   auto level = DiagnosticStatusWrapper::OK;
+  std::string msg = "OK";
+
   if (params_.diagnostic_status < 0) {
     level = DiagnosticStatusWrapper::STALE;
+    msg = "STALE";
   } else if (params_.diagnostic_status == 1) {
     level = DiagnosticStatusWrapper::WARN;
+    msg = "WARNING: abnormal state in image diagnostics";
   } else if (params_.diagnostic_status == 2) {
     level = DiagnosticStatusWrapper::ERROR;
-  } else {
-    level = DiagnosticStatusWrapper::OK;
-  }
-
-  std::string msg;
-  if (level == DiagnosticStatusWrapper::OK) {
-    msg = "OK";
-  } else if (level == DiagnosticStatusWrapper::WARN) {
-    msg = "WARNING: abnormal state in image diagnostics";
-  } else if (level == DiagnosticStatusWrapper::ERROR) {
     msg = "ERROR: abnormal state in image diagnostics";
-  } else if (level == DiagnosticStatusWrapper::STALE) {
-    msg = "STALE";
   }
+
   stat.summary(level, msg);
 }
 
@@ -101,7 +94,6 @@ void ImageDiagNode::ImageChecker(const sensor_msgs::msg::Image::ConstSharedPtr i
   img_gray.convertTo(img_gray_32b, CV_32FC1);
   cv::Mat imgDCT(size, CV_32FC1);
   imgDCT.setTo(0.0);
-  float region_freq_average = 0.0;
   cv::Mat imgDFT(size, CV_32FC1);
   imgDFT.setTo(0.0);
   // calculate the features of each small block in image
@@ -132,7 +124,7 @@ void ImageDiagNode::ImageChecker(const sensor_msgs::msg::Image::ConstSharedPtr i
       cv::Mat rect_tmp = img_gray_32b(roi);
       channelImg[0](original_roi).copyTo(rect_tmp);
       cv::log(rect_tmp, rect_tmp);
-      region_freq_average = cv::mean(rect_tmp)[0];
+      const float region_freq_average = cv::mean(rect_tmp)[0];
 
       region_average_vec.push_back(intensity_average);
       region_blockage_ratio_vec.push_back(roi_blockage_ratio);
diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp
index 2443e45fe4c63..549a60e49d748 100644
--- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp
+++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp
@@ -64,6 +64,7 @@
 #include "autoware_point_types/types.hpp"
 
 #include <autoware/universe_utils/ros/debug_publisher.hpp>
+#include <autoware/universe_utils/ros/static_transform_buffer.hpp>
 #include <autoware/universe_utils/system/stop_watch.hpp>
 #include <diagnostic_updater/diagnostic_updater.hpp>
 #include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>
@@ -149,9 +150,7 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
   // XmlRpc::XmlRpcValue input_topics_;
   std::vector<std::string> input_topics_;
 
-  /** \brief TF listener object. */
-  std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
-  std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
+  std::unique_ptr<autoware::universe_utils::StaticTransformBuffer> static_tf_buffer_{nullptr};
 
   std::deque<geometry_msgs::msg::TwistStamped::ConstSharedPtr> twist_ptr_queue_;
 
@@ -162,7 +161,6 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
   std::vector<double> input_offset_;
   std::map<std::string, double> offset_map_;
 
-  void transformPointCloud(const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out);
   Eigen::Matrix4f computeTransformToAdjustForOldTimestamp(
     const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp);
   std::map<std::string, sensor_msgs::msg::PointCloud2::SharedPtr> combineClouds(
diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp
index 36b1910a4d798..b982553a8c920 100644
--- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp
+++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp
@@ -61,10 +61,10 @@
 #include <vector>
 
 // ROS includes
-#include "autoware_point_types/types.hpp"
-
 #include <autoware/universe_utils/ros/debug_publisher.hpp>
+#include <autoware/universe_utils/ros/static_transform_buffer.hpp>
 #include <autoware/universe_utils/system/stop_watch.hpp>
+#include <autoware_point_types/types.hpp>
 #include <diagnostic_updater/diagnostic_updater.hpp>
 #include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>
 
@@ -139,9 +139,7 @@ class PointCloudConcatenationComponent : public rclcpp::Node
   // XmlRpc::XmlRpcValue input_topics_;
   std::vector<std::string> input_topics_;
 
-  /** \brief TF listener object. */
-  std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
-  std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
+  std::unique_ptr<autoware::universe_utils::StaticTransformBuffer> static_tf_buffer_{nullptr};
 
   std::deque<geometry_msgs::msg::TwistStamped::ConstSharedPtr> twist_ptr_queue_;
 
@@ -152,10 +150,6 @@ class PointCloudConcatenationComponent : public rclcpp::Node
   std::vector<double> input_offset_;
   std::map<std::string, double> offset_map_;
 
-  void transformPointCloud(const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out);
-  void transformPointCloud(
-    const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out,
-    const std::string & target_frame);
   void checkSyncStatus();
   void combineClouds(sensor_msgs::msg::PointCloud2::SharedPtr & concat_cloud_ptr);
   void publish();
diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
index 8226cb6eb774c..7ba419770af17 100644
--- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
+++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
@@ -16,6 +16,7 @@
 #define AUTOWARE__POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_
 
 #include <Eigen/Core>
+#include <autoware/universe_utils/ros/static_transform_buffer.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sophus/se3.hpp>
 
@@ -25,6 +26,7 @@
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <sensor_msgs/point_cloud2_iterator.hpp>
 
+#include <tf2/LinearMath/Transform.h>
 #include <tf2/convert.h>
 #include <tf2/transform_datatypes.h>
 
@@ -72,8 +74,7 @@ class DistortionCorrector : public DistortionCorrectorBase
   bool pointcloud_transform_exists_{false};
   bool imu_transform_exists_{false};
   rclcpp::Node * node_;
-  tf2_ros::Buffer tf_buffer_;
-  tf2_ros::TransformListener tf_listener_;
+  std::unique_ptr<autoware::universe_utils::StaticTransformBuffer> static_tf_buffer_{nullptr};
 
   std::deque<geometry_msgs::msg::TwistStamped> twist_queue_;
   std::deque<geometry_msgs::msg::Vector3Stamped> angular_velocity_queue_;
@@ -95,11 +96,12 @@ class DistortionCorrector : public DistortionCorrectorBase
     static_cast<T *>(this)->undistortPointImplementation(
       it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid);
   };
+  void convertMatrixToTransform(const Eigen::Matrix4f & matrix, tf2::Transform & transform);
 
 public:
-  explicit DistortionCorrector(rclcpp::Node * node)
-  : node_(node), tf_buffer_(node_->get_clock()), tf_listener_(tf_buffer_)
+  explicit DistortionCorrector(rclcpp::Node * node) : node_(node)
   {
+    static_tf_buffer_ = std::make_unique<autoware::universe_utils::StaticTransformBuffer>();
   }
   bool pointcloud_transform_exists();
   bool pointcloud_transform_needed();
diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/filter.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/filter.hpp
index a6e113412231a..789f377e7ec95 100644
--- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/filter.hpp
+++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/filter.hpp
@@ -82,6 +82,7 @@
 // Include tier4 autoware utils
 #include <autoware/universe_utils/ros/debug_publisher.hpp>
 #include <autoware/universe_utils/ros/published_time_publisher.hpp>
+#include <autoware/universe_utils/ros/static_transform_buffer.hpp>
 #include <autoware/universe_utils/system/stop_watch.hpp>
 
 namespace autoware::pointcloud_preprocessor
@@ -239,8 +240,7 @@ class Filter : public rclcpp::Node
    * versus an exact one (false by default). */
   bool approximate_sync_ = false;
 
-  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
-  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+  std::unique_ptr<autoware::universe_utils::StaticTransformBuffer> static_tf_buffer_{nullptr};
 
   inline bool isValid(
     const PointCloud2ConstPtr & cloud, const std::string & /*topic_name*/ = "input")
@@ -289,10 +289,6 @@ class Filter : public rclcpp::Node
     const std::string & target_frame, const sensor_msgs::msg::PointCloud2 & from,
     TransformInfo & transform_info /*output*/);
 
-  bool _calculate_transform_matrix(
-    const std::string & target_frame, const sensor_msgs::msg::PointCloud2 & from,
-    const tf2_ros::Buffer & tf_buffer, Eigen::Matrix4f & eigen_transform /*output*/);
-
   bool convert_output_costly(std::unique_ptr<PointCloud2> & output);
 
   // TODO(sykwer): Temporary Implementation: Remove this interface when all the filter nodes conform
diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp
index 00704b20b85ef..4f08ae3a3ce67 100644
--- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp
+++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp
@@ -61,10 +61,10 @@
 #include <vector>
 
 // ROS includes
-#include "autoware_point_types/types.hpp"
-
 #include <autoware/universe_utils/ros/debug_publisher.hpp>
+#include <autoware/universe_utils/ros/static_transform_buffer.hpp>
 #include <autoware/universe_utils/system/stop_watch.hpp>
+#include <autoware_point_types/types.hpp>
 #include <diagnostic_updater/diagnostic_updater.hpp>
 #include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>
 
@@ -144,9 +144,7 @@ class PointCloudDataSynchronizerComponent : public rclcpp::Node
   // XmlRpc::XmlRpcValue input_topics_;
   std::vector<std::string> input_topics_;
 
-  /** \brief TF listener object. */
-  std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
-  std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
+  std::unique_ptr<autoware::universe_utils::StaticTransformBuffer> static_tf_buffer_{nullptr};
 
   std::deque<geometry_msgs::msg::TwistStamped::ConstSharedPtr> twist_ptr_queue_;
 
@@ -157,10 +155,6 @@ class PointCloudDataSynchronizerComponent : public rclcpp::Node
   std::vector<double> input_offset_;
   std::map<std::string, double> offset_map_;
 
-  void transformPointCloud(const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out);
-  void transformPointCloud(
-    const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out,
-    const std::string & target_frame);
   Eigen::Matrix4f computeTransformToAdjustForOldTimestamp(
     const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp);
   std::map<std::string, sensor_msgs::msg::PointCloud2::SharedPtr> synchronizeClouds();
diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp
index a6b497b40cd77..fef3c8da79714 100644
--- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp
+++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp
@@ -24,6 +24,7 @@
 
 #include <lanelet2_core/geometry/Polygon.h>
 
+#include <memory>
 #include <string>
 
 using autoware::universe_utils::MultiPoint2d;
@@ -46,6 +47,10 @@ class VectorMapInsideAreaFilterComponent : public autoware::pointcloud_preproces
   bool use_z_filter_ = false;
   float z_threshold_;
 
+  // tf2 listener
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
 public:
   PCL_MAKE_ALIGNED_OPERATOR_NEW
   explicit VectorMapInsideAreaFilterComponent(const rclcpp::NodeOptions & options);
diff --git a/sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp
index de570f354393f..9d086e16dd51f 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp
@@ -108,7 +108,7 @@ void BlockageDiagComponent::onBlockageChecker(DiagnosticStatusWrapper & stat)
   // TODO(badai-nguyen): consider sky_blockage_ratio_ for DiagnosticsStatus." [todo]
 
   auto level = DiagnosticStatus::OK;
-  std::string msg;
+  std::string msg = "OK";
   if (ground_blockage_ratio_ < 0) {
     level = DiagnosticStatus::STALE;
     msg = "STALE";
@@ -120,9 +120,6 @@ void BlockageDiagComponent::onBlockageChecker(DiagnosticStatusWrapper & stat)
   } else if (ground_blockage_ratio_ > 0.0f) {
     level = DiagnosticStatus::WARN;
     msg = "WARN";
-  } else {
-    level = DiagnosticStatus::OK;
-    msg = "OK";
   }
 
   if ((ground_blockage_ratio_ > 0.0f) && (sky_blockage_ratio_ > 0.0f)) {
@@ -139,7 +136,7 @@ void BlockageDiagComponent::dustChecker(diagnostic_updater::DiagnosticStatusWrap
 {
   stat.add("ground_dust_ratio", std::to_string(ground_dust_ratio_));
   auto level = DiagnosticStatus::OK;
-  std::string msg;
+  std::string msg = "OK";
   if (ground_dust_ratio_ < 0.0f) {
     level = DiagnosticStatus::STALE;
     msg = "STALE";
@@ -150,9 +147,6 @@ void BlockageDiagComponent::dustChecker(diagnostic_updater::DiagnosticStatusWrap
   } else if (ground_dust_ratio_ > 0.0f) {
     level = DiagnosticStatus::WARN;
     msg = "WARN";
-  } else {
-    level = DiagnosticStatus::OK;
-    msg = "OK";
   }
 
   if (ground_dust_ratio_ > 0.0f) {
diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp
index 95c353212484c..2410a9dccebea 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp
@@ -137,8 +137,7 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro
 
   // tf2 listener
   {
-    tf2_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
-    tf2_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);
+    static_tf_buffer_ = std::make_unique<autoware::universe_utils::StaticTransformBuffer>();
   }
 
   // Output Publishers
@@ -224,25 +223,6 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro
   }
 }
 
-//////////////////////////////////////////////////////////////////////////////////////////////
-void PointCloudConcatenateDataSynchronizerComponent::transformPointCloud(
-  const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out)
-{
-  // Transform the point clouds into the specified output frame
-  if (output_frame_ != in->header.frame_id) {
-    // TODO(YamatoAndo): use TF2
-    if (!pcl_ros::transformPointCloud(output_frame_, *in, *out, *tf2_buffer_)) {
-      RCLCPP_ERROR(
-        this->get_logger(),
-        "[transformPointCloud] Error converting first input dataset from %s to %s.",
-        in->header.frame_id.c_str(), output_frame_.c_str());
-      return;
-    }
-  } else {
-    out = std::make_shared<PointCloud2>(*in);
-  }
-}
-
 std::string PointCloudConcatenateDataSynchronizerComponent::replaceSyncTopicNamePostfix(
   const std::string & original_topic_name, const std::string & postfix)
 {
@@ -381,7 +361,8 @@ PointCloudConcatenateDataSynchronizerComponent::combineClouds(
       }
       sensor_msgs::msg::PointCloud2::SharedPtr transformed_cloud_ptr(
         new sensor_msgs::msg::PointCloud2());
-      transformPointCloud(e.second, transformed_cloud_ptr);
+      static_tf_buffer_->transformPointcloud(
+        this, output_frame_, *e.second, *transformed_cloud_ptr);
 
       // calculate transforms to oldest stamp
       Eigen::Matrix4f adjust_to_old_data_transform = Eigen::Matrix4f::Identity();
@@ -411,9 +392,9 @@ PointCloudConcatenateDataSynchronizerComponent::combineClouds(
       if (keep_input_frame_in_synchronized_pointcloud_ && need_transform_to_sensor_frame) {
         sensor_msgs::msg::PointCloud2::SharedPtr transformed_cloud_ptr_in_sensor_frame(
           new sensor_msgs::msg::PointCloud2());
-        pcl_ros::transformPointCloud(
-          (std::string)e.second->header.frame_id, *transformed_delay_compensated_cloud_ptr,
-          *transformed_cloud_ptr_in_sensor_frame, *tf2_buffer_);
+        static_tf_buffer_->transformPointcloud(
+          this, e.second->header.frame_id, *transformed_delay_compensated_cloud_ptr,
+          *transformed_cloud_ptr_in_sensor_frame);
         transformed_cloud_ptr_in_sensor_frame->header.stamp = oldest_stamp;
         transformed_cloud_ptr_in_sensor_frame->header.frame_id = e.second->header.frame_id;
         transformed_clouds[e.first] = transformed_cloud_ptr_in_sensor_frame;
diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp
index 24c4b572181fe..2c5d7f24cb80c 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp
@@ -93,8 +93,7 @@ PointCloudConcatenationComponent::PointCloudConcatenationComponent(
 
   // tf2 listener
   {
-    tf2_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
-    tf2_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);
+    static_tf_buffer_ = std::make_unique<autoware::universe_utils::StaticTransformBuffer>();
   }
 
   // Output Publishers
@@ -149,30 +148,6 @@ PointCloudConcatenationComponent::PointCloudConcatenationComponent(
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-void PointCloudConcatenationComponent::transformPointCloud(
-  const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out)
-{
-  transformPointCloud(in, out, output_frame_);
-}
-
-void PointCloudConcatenationComponent::transformPointCloud(
-  const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out,
-  const std::string & target_frame)
-{
-  // Transform the point clouds into the specified output frame
-  if (target_frame != in->header.frame_id) {
-    // TODO(YamatoAndo): use TF2
-    if (!pcl_ros::transformPointCloud(target_frame, *in, *out, *tf2_buffer_)) {
-      RCLCPP_ERROR(
-        this->get_logger(),
-        "[transformPointCloud] Error converting first input dataset from %s to %s.",
-        in->header.frame_id.c_str(), target_frame.c_str());
-      return;
-    }
-  } else {
-    out = std::make_shared<PointCloud2>(*in);
-  }
-}
 
 void PointCloudConcatenationComponent::checkSyncStatus()
 {
@@ -262,7 +237,8 @@ void PointCloudConcatenationComponent::combineClouds(
       // transform to output frame
       sensor_msgs::msg::PointCloud2::SharedPtr transformed_cloud_ptr(
         new sensor_msgs::msg::PointCloud2());
-      transformPointCloud(e.second, transformed_cloud_ptr);
+      static_tf_buffer_->transformPointcloud(
+        this, output_frame_, *e.second, *transformed_cloud_ptr);
 
       // concatenate
       if (concat_cloud_ptr == nullptr) {
diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
index 652edd9126358..381c90dda7de5 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
@@ -19,6 +19,8 @@
 #include <autoware/universe_utils/math/trigonometry.hpp>
 #include <tf2_eigen/tf2_eigen.hpp>
 
+#include <cstddef>
+
 namespace autoware::pointcloud_preprocessor
 {
 
@@ -85,29 +87,13 @@ void DistortionCorrector<T>::getIMUTransformation(
     return;
   }
 
-  geometry_imu_to_base_link_ptr_ = std::make_shared<geometry_msgs::msg::TransformStamped>();
-
   tf2::Transform tf2_imu_to_base_link;
-  if (base_frame == imu_frame) {
-    tf2_imu_to_base_link.setOrigin(tf2::Vector3(0.0, 0.0, 0.0));
-    tf2_imu_to_base_link.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0));
-    imu_transform_exists_ = true;
-  } else {
-    try {
-      const auto transform_msg =
-        tf_buffer_.lookupTransform(base_frame, imu_frame, tf2::TimePointZero);
-      tf2::convert(transform_msg.transform, tf2_imu_to_base_link);
-      imu_transform_exists_ = true;
-    } catch (const tf2::TransformException & ex) {
-      RCLCPP_WARN(node_->get_logger(), "%s", ex.what());
-      RCLCPP_ERROR(
-        node_->get_logger(), "Please publish TF %s to %s", base_frame.c_str(), imu_frame.c_str());
-
-      tf2_imu_to_base_link.setOrigin(tf2::Vector3(0.0, 0.0, 0.0));
-      tf2_imu_to_base_link.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0));
-    }
-  }
+  Eigen::Matrix4f eigen_imu_to_base_link;
+  imu_transform_exists_ =
+    static_tf_buffer_->getTransform(node_, base_frame, imu_frame, eigen_imu_to_base_link);
+  convertMatrixToTransform(eigen_imu_to_base_link, tf2_imu_to_base_link);
 
+  geometry_imu_to_base_link_ptr_ = std::make_shared<geometry_msgs::msg::TransformStamped>();
   geometry_imu_to_base_link_ptr_->transform.rotation =
     tf2::toMsg(tf2_imu_to_base_link.getRotation());
 }
@@ -295,6 +281,16 @@ void DistortionCorrector<T>::warnIfTimestampIsTooLate(
   }
 }
 
+template <class T>
+void DistortionCorrector<T>::convertMatrixToTransform(
+  const Eigen::Matrix4f & matrix, tf2::Transform & transform)
+{
+  transform.setOrigin(tf2::Vector3(matrix(0, 3), matrix(1, 3), matrix(2, 3)));
+  transform.setBasis(tf2::Matrix3x3(
+    matrix(0, 0), matrix(0, 1), matrix(0, 2), matrix(1, 0), matrix(1, 1), matrix(1, 2),
+    matrix(2, 0), matrix(2, 1), matrix(2, 2)));
+}
+
 ///////////////////////// Functions for different undistortion strategies /////////////////////////
 
 void DistortionCorrector2D::initialize()
@@ -316,29 +312,12 @@ void DistortionCorrector2D::setPointCloudTransform(
     return;
   }
 
-  if (base_frame == lidar_frame) {
-    tf2_lidar_to_base_link_.setOrigin(tf2::Vector3(0.0, 0.0, 0.0));
-    tf2_lidar_to_base_link_.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0));
-    tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_;
-    pointcloud_transform_exists_ = true;
-  } else {
-    try {
-      const auto transform_msg =
-        tf_buffer_.lookupTransform(base_frame, lidar_frame, tf2::TimePointZero);
-      tf2::convert(transform_msg.transform, tf2_lidar_to_base_link_);
-      tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_.inverse();
-      pointcloud_transform_exists_ = true;
-      pointcloud_transform_needed_ = true;
-    } catch (const tf2::TransformException & ex) {
-      RCLCPP_WARN(node_->get_logger(), "%s", ex.what());
-      RCLCPP_ERROR(
-        node_->get_logger(), "Please publish TF %s to %s", base_frame.c_str(), lidar_frame.c_str());
-
-      tf2_lidar_to_base_link_.setOrigin(tf2::Vector3(0.0, 0.0, 0.0));
-      tf2_lidar_to_base_link_.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0));
-      tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_;
-    }
-  }
+  Eigen::Matrix4f eigen_lidar_to_base_link;
+  pointcloud_transform_exists_ =
+    static_tf_buffer_->getTransform(node_, base_frame, lidar_frame, eigen_lidar_to_base_link);
+  convertMatrixToTransform(eigen_lidar_to_base_link, tf2_lidar_to_base_link_);
+  tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_.inverse();
+  pointcloud_transform_needed_ = base_frame != lidar_frame && pointcloud_transform_exists_;
 }
 
 void DistortionCorrector3D::setPointCloudTransform(
@@ -348,27 +327,10 @@ void DistortionCorrector3D::setPointCloudTransform(
     return;
   }
 
-  if (base_frame == lidar_frame) {
-    eigen_lidar_to_base_link_ = Eigen::Matrix4f::Identity();
-    eigen_base_link_to_lidar_ = Eigen::Matrix4f::Identity();
-    pointcloud_transform_exists_ = true;
-  }
-
-  try {
-    const auto transform_msg =
-      tf_buffer_.lookupTransform(base_frame, lidar_frame, tf2::TimePointZero);
-    eigen_lidar_to_base_link_ =
-      tf2::transformToEigen(transform_msg.transform).matrix().cast<float>();
-    eigen_base_link_to_lidar_ = eigen_lidar_to_base_link_.inverse();
-    pointcloud_transform_exists_ = true;
-    pointcloud_transform_needed_ = true;
-  } catch (const tf2::TransformException & ex) {
-    RCLCPP_WARN(node_->get_logger(), "%s", ex.what());
-    RCLCPP_ERROR(
-      node_->get_logger(), "Please publish TF %s to %s", base_frame.c_str(), lidar_frame.c_str());
-    eigen_lidar_to_base_link_ = Eigen::Matrix4f::Identity();
-    eigen_base_link_to_lidar_ = Eigen::Matrix4f::Identity();
-  }
+  pointcloud_transform_exists_ =
+    static_tf_buffer_->getTransform(node_, base_frame, lidar_frame, eigen_lidar_to_base_link_);
+  eigen_base_link_to_lidar_ = eigen_lidar_to_base_link_.inverse();
+  pointcloud_transform_needed_ = base_frame != lidar_frame && pointcloud_transform_exists_;
 }
 
 inline void DistortionCorrector2D::undistortPointImplementation(
diff --git a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/robin_hood.h b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/robin_hood.h
index f04a84968dbbb..51f40765e9ea1 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/robin_hood.h
+++ b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/robin_hood.h
@@ -567,7 +567,7 @@ template <typename T, size_t MinSize, size_t MaxSize>
 struct NodeAllocator<T, MinSize, MaxSize, true>
 {
   // we are not using the data, so just free it.
-  void addOrFree(void * ptr, size_t ROBIN_HOOD_UNUSED(numBytes) /*unused*/) noexcept
+  static void addOrFree(void * ptr, size_t ROBIN_HOOD_UNUSED(numBytes) /*unused*/) noexcept
   {
     ROBIN_HOOD_LOG("std::free")
     std::free(ptr);
@@ -1051,8 +1051,8 @@ class Table
     }
 
     // doesn't do anything
-    void destroy(M & ROBIN_HOOD_UNUSED(map) /*unused*/) noexcept {}
-    void destroyDoNotDeallocate() noexcept {}
+    static void destroy(M & ROBIN_HOOD_UNUSED(map) /*unused*/) noexcept {}
+    static void destroyDoNotDeallocate() noexcept {}
 
     value_type const * operator->() const noexcept { return &mData; }
     value_type * operator->() noexcept { return &mData; }
@@ -1134,7 +1134,7 @@ class Table
       map.deallocate(mData);
     }
 
-    void destroyDoNotDeallocate() noexcept { mData->~value_type(); }
+    void destroyDoNotDeallocate() const noexcept { mData->~value_type(); }
 
     value_type const * operator->() const noexcept { return mData; }
 
@@ -1262,15 +1262,15 @@ class Table
   template <typename M>
   struct Destroyer<M, true>
   {
-    void nodes(M & m) const noexcept { m.mNumElements = 0; }
+    static void nodes(M & m) noexcept { m.mNumElements = 0; }
 
-    void nodesDoNotDeallocate(M & m) const noexcept { m.mNumElements = 0; }
+    static void nodesDoNotDeallocate(M & m) noexcept { m.mNumElements = 0; }
   };
 
   template <typename M>
   struct Destroyer<M, false>
   {
-    void nodes(M & m) const noexcept
+    static void nodes(M & m) noexcept
     {
       m.mNumElements = 0;
       // clear also resets mInfo to 0, that's sometimes not necessary.
@@ -1285,7 +1285,7 @@ class Table
       }
     }
 
-    void nodesDoNotDeallocate(M & m) const noexcept
+    static void nodesDoNotDeallocate(M & m) noexcept
     {
       m.mNumElements = 0;
       // clear also resets mInfo to 0, that's sometimes not necessary.
@@ -1932,7 +1932,7 @@ class Table
   }
 
   template <typename... Args>
-  iterator emplace_hint(const_iterator position, Args &&... args)
+  iterator emplace_hint(const const_iterator & position, Args &&... args)
   {
     (void)position;
     return emplace(std::forward<Args>(args)...).first;
@@ -1951,14 +1951,14 @@ class Table
   }
 
   template <typename... Args>
-  iterator try_emplace(const_iterator hint, const key_type & key, Args &&... args)
+  iterator try_emplace(const const_iterator & hint, const key_type & key, Args &&... args)
   {
     (void)hint;
     return try_emplace_impl(key, std::forward<Args>(args)...).first;
   }
 
   template <typename... Args>
-  iterator try_emplace(const_iterator hint, key_type && key, Args &&... args)
+  iterator try_emplace(const const_iterator & hint, key_type && key, Args &&... args)
   {
     (void)hint;
     return try_emplace_impl(std::move(key), std::forward<Args>(args)...).first;
@@ -1977,14 +1977,14 @@ class Table
   }
 
   template <typename Mapped>
-  iterator insert_or_assign(const_iterator hint, const key_type & key, Mapped && obj)
+  iterator insert_or_assign(const const_iterator & hint, const key_type & key, Mapped && obj)
   {
     (void)hint;
     return insertOrAssignImpl(key, std::forward<Mapped>(obj)).first;
   }
 
   template <typename Mapped>
-  iterator insert_or_assign(const_iterator hint, key_type && key, Mapped && obj)
+  iterator insert_or_assign(const const_iterator & hint, key_type && key, Mapped && obj)
   {
     (void)hint;
     return insertOrAssignImpl(std::move(key), std::forward<Mapped>(obj)).first;
@@ -1996,7 +1996,7 @@ class Table
     return emplace(keyval);
   }
 
-  iterator insert(const_iterator hint, const value_type & keyval)
+  iterator insert(const const_iterator & hint, const value_type & keyval)
   {
     (void)hint;
     return emplace(keyval).first;
@@ -2004,7 +2004,7 @@ class Table
 
   std::pair<iterator, bool> insert(value_type && keyval) { return emplace(std::move(keyval)); }
 
-  iterator insert(const_iterator hint, value_type && keyval)
+  iterator insert(const const_iterator & hint, value_type && keyval)
   {
     (void)hint;
     return emplace(std::move(keyval)).first;
diff --git a/sensing/autoware_pointcloud_preprocessor/src/filter.cpp b/sensing/autoware_pointcloud_preprocessor/src/filter.cpp
index 95e85f8450840..854e93ed52ebc 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/filter.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/filter.cpp
@@ -114,8 +114,7 @@ autoware::pointcloud_preprocessor::Filter::Filter(
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 void autoware::pointcloud_preprocessor::Filter::setupTF()
 {
-  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
-  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+  static_tf_buffer_ = std::make_unique<autoware::universe_utils::StaticTransformBuffer>();
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -270,20 +269,7 @@ void autoware::pointcloud_preprocessor::Filter::input_indices_callback(
     // Convert the cloud into the different frame
     PointCloud2 cloud_transformed;
 
-    if (!tf_buffer_->canTransform(
-          tf_input_frame_, cloud->header.frame_id, this->now(),
-          rclcpp::Duration::from_seconds(1.0))) {
-      RCLCPP_ERROR_STREAM(
-        this->get_logger(), "[input_indices_callback] timeout tf: " << cloud->header.frame_id
-                                                                    << "->" << tf_input_frame_);
-      return;
-    }
-
-    if (!pcl_ros::transformPointCloud(tf_input_frame_, *cloud, cloud_transformed, *tf_buffer_)) {
-      RCLCPP_ERROR(
-        this->get_logger(),
-        "[input_indices_callback] Error converting input dataset from %s to %s.",
-        cloud->header.frame_id.c_str(), tf_input_frame_.c_str());
+    if (!static_tf_buffer_->transformPointcloud(this, tf_input_frame_, *cloud, cloud_transformed)) {
       return;
     }
     cloud_tf = std::make_shared<PointCloud2>(cloud_transformed);
@@ -299,35 +285,6 @@ void autoware::pointcloud_preprocessor::Filter::input_indices_callback(
   computePublish(cloud_tf, vindices);
 }
 
-// For performance reason, we get only a transformation matrix here.
-// The implementation is based on the one shown in the URL below.
-// https://github.com/ros-perception/perception_pcl/blob/628aaec1dc73ef4adea01e9d28f11eb417b948fd/pcl_ros/src/transforms.cpp#L61-L94
-bool autoware::pointcloud_preprocessor::Filter::_calculate_transform_matrix(
-  const std::string & target_frame, const sensor_msgs::msg::PointCloud2 & from,
-  const tf2_ros::Buffer & tf_buffer, Eigen::Matrix4f & eigen_transform /*output*/)
-{
-  if (from.header.frame_id == target_frame) {
-    eigen_transform = Eigen::Matrix4f::Identity(4, 4);
-    return true;
-  }
-
-  geometry_msgs::msg::TransformStamped transform;
-
-  try {
-    transform = tf_buffer.lookupTransform(
-      target_frame, from.header.frame_id, tf2_ros::fromMsg(from.header.stamp));
-  } catch (tf2::LookupException & e) {
-    RCLCPP_ERROR(this->get_logger(), "%s", e.what());
-    return false;
-  } catch (tf2::ExtrapolationException & e) {
-    RCLCPP_ERROR(this->get_logger(), "%s", e.what());
-    return false;
-  }
-
-  pcl_ros::transformAsMatrix(transform, eigen_transform);
-  return true;
-}
-
 // Returns false in error cases
 bool autoware::pointcloud_preprocessor::Filter::calculate_transform_matrix(
   const std::string & target_frame, const sensor_msgs::msg::PointCloud2 & from,
@@ -341,20 +298,8 @@ bool autoware::pointcloud_preprocessor::Filter::calculate_transform_matrix(
     this->get_logger(), "[get_transform_matrix] Transforming input dataset from %s to %s.",
     from.header.frame_id.c_str(), target_frame.c_str());
 
-  if (!tf_buffer_->canTransform(
-        target_frame, from.header.frame_id, this->now(), rclcpp::Duration::from_seconds(1.0))) {
-    RCLCPP_ERROR_STREAM(
-      this->get_logger(),
-      "[get_transform_matrix] timeout tf: " << from.header.frame_id << "->" << target_frame);
-    return false;
-  }
-
-  if (!_calculate_transform_matrix(
-        target_frame, from, *tf_buffer_, transform_info.eigen_transform /*output*/)) {
-    RCLCPP_ERROR(
-      this->get_logger(),
-      "[calculate_transform_matrix] Error converting input dataset from %s to %s.",
-      from.header.frame_id.c_str(), target_frame.c_str());
+  if (!static_tf_buffer_->getTransform(
+        this, target_frame, from.header.frame_id, transform_info.eigen_transform)) {
     return false;
   }
 
@@ -375,7 +320,9 @@ bool autoware::pointcloud_preprocessor::Filter::convert_output_costly(
 
     // Convert the cloud into the different frame
     auto cloud_transformed = std::make_unique<PointCloud2>();
-    if (!pcl_ros::transformPointCloud(tf_output_frame_, *output, *cloud_transformed, *tf_buffer_)) {
+
+    if (!static_tf_buffer_->transformPointcloud(
+          this, tf_output_frame_, *output, *cloud_transformed)) {
       RCLCPP_ERROR(
         this->get_logger(),
         "[convert_output_costly] Error converting output dataset from %s to %s.",
@@ -394,12 +341,9 @@ bool autoware::pointcloud_preprocessor::Filter::convert_output_costly(
       output->header.frame_id.c_str(), tf_input_orig_frame_.c_str());
 
     auto cloud_transformed = std::make_unique<PointCloud2>();
-    if (!pcl_ros::transformPointCloud(
-          tf_input_orig_frame_, *output, *cloud_transformed, *tf_buffer_)) {
-      RCLCPP_ERROR(
-        this->get_logger(),
-        "[convert_output_costly] Error converting output dataset from %s back to %s.",
-        output->header.frame_id.c_str(), tf_input_orig_frame_.c_str());
+
+    if (!static_tf_buffer_->transformPointcloud(
+          this, tf_input_orig_frame_, *output, *cloud_transformed)) {
       return false;
     }
 
diff --git a/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp
index edbe30f8f0ea7..121e983f152c8 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp
@@ -110,8 +110,7 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent(
 
   // tf2 listener
   {
-    tf2_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
-    tf2_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);
+    static_tf_buffer_ = std::make_unique<autoware::universe_utils::StaticTransformBuffer>();
   }
 
   // Subscribers
@@ -217,30 +216,6 @@ std::string PointCloudDataSynchronizerComponent::replaceSyncTopicNamePostfix(
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 // overloaded functions
-void PointCloudDataSynchronizerComponent::transformPointCloud(
-  const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out)
-{
-  transformPointCloud(in, out, output_frame_);
-}
-
-void PointCloudDataSynchronizerComponent::transformPointCloud(
-  const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out,
-  const std::string & target_frame)
-{
-  // Transform the point clouds into the specified output frame
-  if (target_frame != in->header.frame_id) {
-    // TODO(YamatoAndo): use TF2
-    if (!pcl_ros::transformPointCloud(target_frame, *in, *out, *tf2_buffer_)) {
-      RCLCPP_ERROR(
-        this->get_logger(),
-        "[transformPointCloud] Error converting first input dataset from %s to %s.",
-        in->header.frame_id.c_str(), target_frame.c_str());
-      return;
-    }
-  } else {
-    out = std::make_shared<PointCloud2>(*in);
-  }
-}
 
 /**
  * @brief compute transform to adjust for old timestamp
@@ -346,7 +321,8 @@ PointCloudDataSynchronizerComponent::synchronizeClouds()
         continue;
       }
       // transform pointcloud to output frame
-      transformPointCloud(e.second, transformed_cloud_ptr);
+      static_tf_buffer_->transformPointcloud(
+        this, output_frame_, *e.second, *transformed_cloud_ptr);
 
       // calculate transforms to oldest stamp and transform pointcloud to oldest stamp
       Eigen::Matrix4f adjust_to_old_data_transform = Eigen::Matrix4f::Identity();
@@ -366,9 +342,9 @@ PointCloudDataSynchronizerComponent::synchronizeClouds()
         sensor_msgs::msg::PointCloud2::SharedPtr
           transformed_delay_compensated_cloud_ptr_in_input_frame(
             new sensor_msgs::msg::PointCloud2());
-        transformPointCloud(
-          transformed_delay_compensated_cloud_ptr,
-          transformed_delay_compensated_cloud_ptr_in_input_frame, e.second->header.frame_id);
+        static_tf_buffer_->transformPointcloud(
+          this, e.second->header.frame_id, *transformed_delay_compensated_cloud_ptr,
+          *transformed_delay_compensated_cloud_ptr_in_input_frame);
         transformed_delay_compensated_cloud_ptr =
           transformed_delay_compensated_cloud_ptr_in_input_frame;
       }
diff --git a/sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp b/sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp
index 6272fe30e47c0..81baa173e76ce 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp
@@ -79,6 +79,16 @@ VectorMapInsideAreaFilterComponent::VectorMapInsideAreaFilterComponent(
   // Set parameters
   use_z_filter_ = declare_parameter<bool>("use_z_filter", false);
   z_threshold_ = declare_parameter<float>("z_threshold", 0.0f);  // defined in the base_link frame
+
+  // Set tf
+  {
+    rclcpp::Clock::SharedPtr ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
+    tf_buffer_ = std::make_shared<tf2_ros::Buffer>(ros_clock);
+    auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
+      get_node_base_interface(), get_node_timers_interface());
+    tf_buffer_->setCreateTimerInterface(timer_interface);
+    tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+  }
 }
 
 void VectorMapInsideAreaFilterComponent::filter(
diff --git a/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/model_connections_helpers.hpp b/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/model_connections_helpers.hpp
index 3f21595662fc5..0d46169600b2f 100644
--- a/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/model_connections_helpers.hpp
+++ b/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/model_connections_helpers.hpp
@@ -19,9 +19,10 @@
 #include <vector>
 
 std::vector<double> fillVectorUsingMap(
-  std::vector<double> vector1, std::vector<double> vector2, std::vector<int> map, bool inverse);
+  std::vector<double> vector1, std::vector<double> vector2, const std::vector<int> & map,
+  bool inverse);
 
 std::vector<int> createConnectionsMap(
-  std::vector<char *> connection_names_1, std::vector<char *> connection_names_2);
+  const std::vector<char *> & connection_names_1, const std::vector<char *> & connection_names_2);
 
 #endif  // LEARNING_BASED_VEHICLE_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_
diff --git a/simulator/learning_based_vehicle_model/src/model_connections_helpers.cpp b/simulator/learning_based_vehicle_model/src/model_connections_helpers.cpp
index b13f6497d2dea..c7944116d1758 100644
--- a/simulator/learning_based_vehicle_model/src/model_connections_helpers.cpp
+++ b/simulator/learning_based_vehicle_model/src/model_connections_helpers.cpp
@@ -15,7 +15,8 @@
 #include "learning_based_vehicle_model/model_connections_helpers.hpp"
 
 std::vector<double> fillVectorUsingMap(
-  std::vector<double> vector1, std::vector<double> vector2, std::vector<int> map, bool inverse)
+  std::vector<double> vector1, std::vector<double> vector2, const std::vector<int> & map,
+  bool inverse)
 {
   // index in "map" is index in "vector1" and value in "map" is index in "vector2"
   // inverse = 0 from 1 -> 2; inverse = 1 from 2 -> 1
@@ -27,7 +28,7 @@ std::vector<double> fillVectorUsingMap(
 }
 
 std::vector<int> createConnectionsMap(
-  std::vector<char *> connection_names_1, std::vector<char *> connection_names_2)
+  const std::vector<char *> & connection_names_1, const std::vector<char *> & connection_names_2)
 {
   std::vector<int> connection_map;
   // index in "connection_map" is index in "connection_names_2" and value in "connection_map" is
diff --git a/system/default_ad_api/src/interface.cpp b/system/default_ad_api/src/interface.cpp
index e0009d5bdc382..997f9e01fc6a9 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 = 4;
+    res->minor = 5;
     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 10d00d1dbad30..c91889149cb0e 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 != 4:
+if response.minor != 5:
     exit(1)
 if response.patch != 0:
     exit(1)
diff --git a/system/diagnostic_graph_aggregator/src/common/graph/config.hpp b/system/diagnostic_graph_aggregator/src/common/graph/config.hpp
index a22daf907a27e..935b65203afbe 100644
--- a/system/diagnostic_graph_aggregator/src/common/graph/config.hpp
+++ b/system/diagnostic_graph_aggregator/src/common/graph/config.hpp
@@ -18,6 +18,7 @@
 #include "data.hpp"
 #include "types.hpp"
 
+#include <limits>
 #include <memory>
 #include <string>
 #include <vector>
@@ -54,7 +55,7 @@ struct UnitConfig
   std::string path;
   LinkConfig * item = nullptr;
   std::vector<LinkConfig *> list;
-  size_t index;
+  size_t index = std::numeric_limits<size_t>::max();
 };
 
 struct FileConfig
diff --git a/system/diagnostic_graph_aggregator/src/common/graph/graph.hpp b/system/diagnostic_graph_aggregator/src/common/graph/graph.hpp
index ed7bac0e96ce3..0e64f25860fda 100644
--- a/system/diagnostic_graph_aggregator/src/common/graph/graph.hpp
+++ b/system/diagnostic_graph_aggregator/src/common/graph/graph.hpp
@@ -31,7 +31,7 @@ class Graph
 {
 public:
   void create(const std::string & file, const std::string & id = "");
-  void update(const rclcpp::Time & stamp);
+  void update(const rclcpp::Time & stamp);  // cppcheck-suppress functionConst
   bool update(const rclcpp::Time & stamp, const DiagnosticStatus & status);
   const auto & nodes() const { return nodes_; }
   const auto & diags() const { return diags_; }
diff --git a/system/diagnostic_graph_aggregator/src/common/graph/units.hpp b/system/diagnostic_graph_aggregator/src/common/graph/units.hpp
index 2dbb61eae4367..ced62ed19d800 100644
--- a/system/diagnostic_graph_aggregator/src/common/graph/units.hpp
+++ b/system/diagnostic_graph_aggregator/src/common/graph/units.hpp
@@ -32,7 +32,7 @@ class UnitLink
 public:
   void initialize_object(BaseUnit * parent, BaseUnit * child);
   void initialize_struct();
-  void initialize_status();
+  void initialize_status();  // cppcheck-suppress functionStatic
   DiagLinkStruct create_struct() const { return struct_; }
   DiagLinkStatus create_status() const { return status_; }
   BaseUnit * parent() const { return parent_; }
diff --git a/system/system_diagnostic_monitor/config/autoware-psim.yaml b/system/system_diagnostic_monitor/config/autoware-psim.yaml
index 3d55079229280..499356a3334cc 100644
--- a/system/system_diagnostic_monitor/config/autoware-psim.yaml
+++ b/system/system_diagnostic_monitor/config/autoware-psim.yaml
@@ -8,4 +8,3 @@ edits:
   - { type: remove, path: /autoware/localization/sensor_fusion_status }
   - { type: remove, path: /autoware/localization/topic_rate_check/pose_twist_fusion }
   - { type: remove, path: /autoware/perception/topic_rate_check/pointcloud }
-  - { type: remove, path: /autoware/control/emergency_braking }
diff --git a/vehicle/autoware_accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp b/vehicle/autoware_accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp
index 6a88d5c61a2d8..b361a0d2725c7 100644
--- a/vehicle/autoware_accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp
+++ b/vehicle/autoware_accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp
@@ -1326,15 +1326,12 @@ void AccelBrakeMapCalibrator::check_update_suggest(
   using DiagStatus = diagnostic_msgs::msg::DiagnosticStatus;
   using CalibrationStatus = CalibrationStatus;
   CalibrationStatus accel_brake_map_status;
+  accel_brake_map_status.target = CalibrationStatus::ACCEL_BRAKE_MAP;
+  accel_brake_map_status.status = CalibrationStatus::NORMAL;
   int8_t level = DiagStatus::OK;
-  std::string msg;
+  std::string msg = "OK";
 
-  accel_brake_map_status.target = CalibrationStatus::ACCEL_BRAKE_MAP;
-  if (is_default_map_) {
-    accel_brake_map_status.status = CalibrationStatus::NORMAL;
-    level = DiagStatus::OK;
-    msg = "OK";
-  } else {
+  if (!is_default_map_) {
     accel_brake_map_status.status = CalibrationStatus::UNAVAILABLE;
     level = DiagStatus::ERROR;
     msg = "Default map is not found in " + csv_default_map_dir_;
diff --git a/vehicle/autoware_steer_offset_estimator/launch/steer_offset_estimator.launch.xml b/vehicle/autoware_steer_offset_estimator/launch/steer_offset_estimator.launch.xml
index 9ee05189a35d8..919858950e69c 100644
--- a/vehicle/autoware_steer_offset_estimator/launch/steer_offset_estimator.launch.xml
+++ b/vehicle/autoware_steer_offset_estimator/launch/steer_offset_estimator.launch.xml
@@ -6,7 +6,7 @@
   <include file="$(find-pkg-share global_parameter_loader)/launch/global_params.launch.py"/>
 
   <!-- ndt twist publisher -->
-  <include file="$(find-pkg-share pose2twist)/launch/pose2twist.launch.xml">
+  <include file="$(find-pkg-share autoware_pose2twist)/launch/pose2twist.launch.xml">
     <arg name="input_pose_topic" value="/localization/pose_estimator/pose"/>
     <arg name="output_twist_topic" value="/estimate_twist"/>
   </include>
diff --git a/vehicle/autoware_steer_offset_estimator/package.xml b/vehicle/autoware_steer_offset_estimator/package.xml
index 5e4638f605713..ea35883334e35 100644
--- a/vehicle/autoware_steer_offset_estimator/package.xml
+++ b/vehicle/autoware_steer_offset_estimator/package.xml
@@ -18,8 +18,8 @@
   <depend>rclcpp_components</depend>
   <depend>std_msgs</depend>
   <depend>tier4_debug_msgs</depend>
+  <exec_depend>autoware_pose2twist</exec_depend>
   <exec_depend>global_parameter_loader</exec_depend>
-  <exec_depend>pose2twist</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>