From 68e64acc2809b2970d78210bcce44c28518923a3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Kaan=20=C3=87olak?= <kaancolak95@gmail.com>
Date: Thu, 21 Sep 2023 16:28:36 +0300
Subject: [PATCH 01/35] refactor(traffic_light_classifier): update training
 docs (#4917)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

* refactor(traffic_light_classifier): update training docs

Signed-off-by: Kaan Çolak <kaancolak95@gmail.com>

---------

Signed-off-by: Kaan Çolak <kaancolak95@gmail.com>
---
 perception/traffic_light_classifier/README.md | 261 +++++++++++++++---
 1 file changed, 224 insertions(+), 37 deletions(-)

diff --git a/perception/traffic_light_classifier/README.md b/perception/traffic_light_classifier/README.md
index 53ba0d3be7ba2..3d15af0cf7805 100644
--- a/perception/traffic_light_classifier/README.md
+++ b/perception/traffic_light_classifier/README.md
@@ -92,67 +92,254 @@ These colors and shapes are assigned to the message as follows:
 | `red_max_s`    | int  | the maximum saturation of red color            |
 | `red_max_v`    | int  | the maximum value (brightness) of red color    |
 
-## Customization of CNN model
+## Training Traffic Light Classifier Model
+
+### Overview
+
+This guide provides detailed instructions on training a traffic light classifier model using the **[mmlab/mmpretrain](https://github.com/open-mmlab/mmpretrain)** repository
+and deploying it using **[mmlab/mmdeploy](https://github.com/open-mmlab/mmdeploy)**.
+If you wish to create a custom traffic light classifier model with your own dataset, please follow the steps outlined below.
+
+### Data Preparation
+
+#### Use Sample Dataset
+
+Autoware offers a sample dataset that illustrates the training procedures for
+traffic light classification. This dataset comprises 1045 images categorized
+into red, green, and yellow labels. To utilize this sample dataset,
+please download it from **[link](https://autoware-files.s3.us-west-2.amazonaws.com/dataset/traffic_light_sample_dataset.tar.gz)** and extract it to a designated folder of your choice.
+
+#### Use Your Custom Dataset
+
+To train a traffic light classifier, adopt a structured subfolder format where each
+subfolder represents a distinct class. Below is an illustrative dataset structure example;
+
+```python
+DATASET_ROOT
+    ├── TRAIN
+    │    ├── RED
+    │    │   ├── 001.png
+    │    │   ├── 002.png
+    │    │   └── ...
+    │    │
+    │    ├── GREEN
+    │    │    ├── 001.png
+    │    │    ├── 002.png
+    │    │    └──...
+    │    │
+    │    ├── YELLOW
+    │    │    ├── 001.png
+    │    │    ├── 002.png
+    │    │    └──...
+    │    └── ...
+    │
+    ├── VAL
+    │       └──...
+    │
+    │
+    └── TEST
+           └── ...
 
-Currently, in Autoware, [MobileNetV2](https://arxiv.org/abs/1801.04381v3) and [EfficientNet-b1](https://arxiv.org/abs/1905.11946v5) are provided.
-The corresponding onnx files are `data/traffic_light_classifier_mobilenetv2.onnx` and `data/traffic_light_classifier_efficientNet_b1.onnx` (These files will be downloaded during the build process).
-Also, you can apply the following models shown as below, for example.
 
-- [ResNet](https://openaccess.thecvf.com/content_cvpr_2016/html/He_Deep_Residual_Learning_CVPR_2016_paper.html)
-- [MobileNetV3](https://arxiv.org/abs/1905.02244)
-  ...
+```
+
+### Installation
+
+#### 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
 
-In order to train models and export onnx model, we recommend [open-mmlab/mmclassification](https://github.com/open-mmlab/mmclassification.git).
-Please follow the [official document](https://mmclassification.readthedocs.io/en/latest/) to install and experiment with mmclassification. If you get into troubles, [FAQ page](https://mmclassification.readthedocs.io/en/latest/faq.html) would help you.
+```bash
+conda create --name tl-classifier python=3.8 -y
+conda activate tl-classifier
+```
 
-The following steps are example of a quick-start.
+**Step 3.** Install PyTorch
 
-### step 0. Install [MMCV](https://github.com/open-mmlab/mmcv.git) and [MIM](https://github.com/open-mmlab/mim.git)
+Please ensure you have PyTorch installed, compatible with CUDA 11.6, as it is a requirement for current Autoware
 
-_NOTE_ : First of all, install [PyTorch](https://pytorch.org/) suitable for your CUDA version (CUDA11.6 is supported in Autoware).
+```bash
+conda install pytorch==1.13.1 torchvision==0.14.1 pytorch-cuda=11.6 -c pytorch -c nvidia
+```
 
-In order to install mmcv suitable for your CUDA version, install it specifying a url.
+#### Install mmlab/mmpretrain
 
-```shell
-# Install mim
-$ pip install -U openmim
+**Step 1.** Install mmpretrain from source
 
-# Install mmcv on a machine with CUDA11.6 and PyTorch1.13.0
-$ pip install mmcv-full -f https://download.openmmlab.com/mmcv/dist/cu116/torch1.13/index.html
+```bash
+cd ~/
+git clone https://github.com/open-mmlab/mmpretrain.git
+cd mmpretrain
+pip install -U openmim && mim install -e .
 ```
 
-### step 1. Install MMClassification
+### Training
+
+MMPretrain offers a training script that is controlled through a configuration file.
+Leveraging an inheritance design pattern, you can effortlessly tailor the training script
+using Python files as configuration files.
 
-You can install MMClassification as a Python package or from source.
+In the example, we demonstrate the training steps on the MobileNetV2 model,
+but you have the flexibility to employ alternative classification models such as
+EfficientNetV2, EfficientNetV3, ResNet, and more.
 
-```shell
-# As a Python package
-$ pip install mmcls
+#### Create a config file
 
-# From source
-$ git clone https://github.com/open-mmlab/mmclassification.git
-$ cd mmclassification
-$ pip install -v -e .
+Generate a configuration file for your preferred model within the `configs` folder
+
+```bash
+touch ~/mmpretrain/configs/mobilenet_v2/mobilenet-v2_8xb32_custom.py
 ```
 
-### step 2. Train your model
+Open the configuration file in your preferred text editor and make a copy of
+the provided content. Adjust the **data_root** variable to match the path of your dataset.
+You are welcome to customize the configuration parameters for the model, dataset, and scheduler to
+suit your preferences
+
+```python
+# Inherit model, schedule and default_runtime from base model
+_base_ = [
+    '../_base_/models/mobilenet_v2_1x.py',
+    '../_base_/schedules/imagenet_bs256_epochstep.py',
+    '../_base_/default_runtime.py'
+]
+
+# Set the number of classes to the model
+# You can also change other model parameters here
+# For detailed descriptions of model parameters, please refer to link below
+# (Customize model)[https://mmpretrain.readthedocs.io/en/latest/advanced_guides/modules.html]
+model = dict(head=dict(num_classes=3, topk=(1, 3)))
+
+# Set max epochs and validation interval
+train_cfg = dict(by_epoch=True, max_epochs=50, val_interval=5)
+
+# Set optimizer and lr scheduler
+optim_wrapper = dict(
+    optimizer=dict(type='SGD', lr=0.001, momentum=0.9))
+param_scheduler = dict(type='StepLR', by_epoch=True, step_size=1, gamma=0.98)
+
+dataset_type = 'CustomDataset'
+data_root = "/PATH/OF/YOUR/DATASET"
+
+# Customize data preprocessing and dataloader pipeline for training set
+# These parameters calculated for the sample dataset
+data_preprocessor = dict(
+    mean=[0.2888 * 256, 0.2570 * 256, 0.2329 * 256],
+    std=[0.2106 * 256, 0.2037 * 256, 0.1864 * 256],
+    num_classes=3,
+    to_rgb=True,
+)
+
+# Customize data preprocessing and dataloader pipeline for train set
+# For detailed descriptions of data pipeline, please refer to link below
+# (Customize data pipeline)[https://mmpretrain.readthedocs.io/en/latest/advanced_guides/pipeline.html]
+train_pipeline = [
+    dict(type='LoadImageFromFile'),
+    dict(type='Resize', scale=224),
+    dict(type='RandomFlip', prob=0.5, direction='horizontal'),
+    dict(type='PackInputs'),
+]
+train_dataloader = dict(
+    dataset=dict(
+        type=dataset_type,
+        data_root=data_root,
+        ann_file='',
+        data_prefix='train',
+        with_label=True,
+        pipeline=train_pipeline,
+    ),
+    num_workers=8,
+    batch_size=32,
+    sampler=dict(type='DefaultSampler', shuffle=True)
+)
+
+# Customize data preprocessing and dataloader pipeline for test set
+test_pipeline = [
+    dict(type='LoadImageFromFile'),
+    dict(type='Resize', scale=224),
+    dict(type='PackInputs'),
+]
+
+# Customize data preprocessing and dataloader pipeline for validation set
+val_cfg = dict()
+val_dataloader = dict(
+    dataset=dict(
+        type=dataset_type,
+        data_root=data_root,
+        ann_file='',
+        data_prefix='val',
+        with_label=True,
+        pipeline=test_pipeline,
+    ),
+    num_workers=8,
+    batch_size=32,
+    sampler=dict(type='DefaultSampler', shuffle=True)
+)
+
+val_evaluator = dict(topk=(1, 3,), type='Accuracy')
+
+test_dataloader = val_dataloader
+test_evaluator = val_evaluator
+
+```
 
-Train model with your experiment configuration file. For the details of config file, see [here](https://mmclassification.readthedocs.io/en/latest/tutorials/config.html).
+#### Start training
 
-```shell
-# [] is optional, you can start training from pre-trained checkpoint
-$ mim train mmcls YOUR_CONFIG.py [--resume-from YOUR_CHECKPOINT.pth]
+```bash
+cd ~/mmpretrain
+python tools/train.py configs/mobilenet_v2/mobilenet-v2_8xb32_custom.py
 ```
 
-### step 3. Export onnx model
+Training logs and weights will be saved in the `work_dirs/mobilenet-v2_8xb32_custom` folder.
 
-In exporting onnx, use `mmclassification/tools/deployment/pytorch2onnx.py` or [open-mmlab/mmdeploy](https://github.com/open-mmlab/mmdeploy.git).
+### Convert PyTorh model to ONNX model
 
-```shell
-cd ~/mmclassification/tools/deployment
-python3 pytorch2onnx.py YOUR_CONFIG.py ...
+#### Install mmdeploy
+
+The 'mmdeploy' toolset is designed for deploying your trained model onto various target devices.
+With its capabilities, you can seamlessly convert PyTorch models into the ONNX format.
+
+```bash
+# Activate your conda environment
+conda activate tl-classifier
+
+# Install mmenigne and mmcv
+mim install mmengine
+mim install "mmcv>=2.0.0rc2"
+
+# Install mmdeploy
+pip install mmdeploy==1.2.0
+
+# Support onnxruntime
+pip install mmdeploy-runtime==1.2.0
+pip install mmdeploy-runtime-gpu==1.2.0
+pip install onnxruntime-gpu==1.8.1
+
+#Clone mmdeploy repository
+cd ~/
+git clone -b main https://github.com/open-mmlab/mmdeploy.git
 ```
 
+#### Convert PyTorch model to ONNX model
+
+```bash
+cd ~/mmdeploy
+
+# Run deploy.py script
+# deploy.py script takes 5 main arguments with these order; config file path, train config file path,
+# checkpoint file path, demo image path, and work directory path
+python tools/deploy.py \
+~/mmdeploy/configs/mmpretrain/classification_onnxruntime_static.py\
+~/mmpretrain/configs/mobilenet_v2/train_mobilenet_v2.py \
+~/mmpretrain/work_dirs/train_mobilenet_v2/epoch_300.pth \
+/SAMPLE/IAMGE/DIRECTORY \
+--work-dir mmdeploy_model/mobilenet_v2
+```
+
+Converted ONNX model will be saved in the `mmdeploy/mmdeploy_model/mobilenet_v2` folder.
+
 After obtaining your onnx model, update parameters defined in the launch file (e.g. `model_file_path`, `label_file_path`, `input_h`, `input_w`...).
 Note that, we only support labels defined in [tier4_perception_msgs::msg::TrafficLightElement](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_perception_msgs/msg/traffic_light/TrafficLightElement.msg).
 

From cd2cd521c9b3058e2c677f0f0e2a7ae997feaa7e Mon Sep 17 00:00:00 2001
From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com>
Date: Thu, 21 Sep 2023 22:55:18 +0900
Subject: [PATCH 02/35] refactor(behavior_path_planner): uninitialized values
 in createPredictedPath (#5065)

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
---
 .../path_safety_checker/objects_filtering.cpp      | 14 ++++++--------
 1 file changed, 6 insertions(+), 8 deletions(-)

diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp
index f69aa186dc377..ff54ce2d2dcc4 100644
--- a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp
+++ b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp
@@ -234,14 +234,12 @@ std::vector<PoseWithVelocityStamped> createPredictedPath(
     convertToFrenetPoint(path_points, vehicle_pose.position, ego_seg_idx);
 
   for (double t = 0.0; t < time_horizon; t += time_resolution) {
-    double velocity;
-    double length;
-
-    if (t < delay_until_departure) {
-      // Before the departure, the velocity is 0 and there's no change in position.
-      velocity = 0.0;
-      length = 0.0;
-    } else {
+    double velocity = 0.0;
+    double length = 0.0;
+
+    // If t < delay_until_departure, it means ego have not depart yet, therefore the velocity is
+    // 0 and there's no change in position.
+    if (t >= delay_until_departure) {
       // Adjust time to consider the delay.
       double t_with_delay = t - delay_until_departure;
       velocity =

From 1a5aa69f46c8b40549b92cabce85efd3864768f2 Mon Sep 17 00:00:00 2001
From: Mamoru Sobue <mamoru.sobue@tier4.jp>
Date: Thu, 21 Sep 2023 23:02:37 +0900
Subject: [PATCH 03/35] refactor(behavior_velocity_planner_common): simplify
 header files (#5055)

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
---
 .../geometry/geometry.hpp                     |   7 +-
 .../src/geometry/geometry.cpp                 |   3 +
 .../src/debug.cpp                             |   2 +
 .../src/scene.cpp                             |   1 +
 .../util.hpp                                  |   2 -
 .../src/scene_crosswalk.cpp                   |   1 +
 .../src/util.cpp                              |   1 +
 .../src/debug.cpp                             |   1 +
 .../src/manager.cpp                           |   1 +
 .../src/debug.cpp                             |   8 +
 .../src/scene_intersection.cpp                |   6 +-
 .../src/scene_merge_from_private_road.cpp     |   1 +
 .../src/util.cpp                              |   3 +-
 .../src/manager.cpp                           |   1 +
 .../src/scene.cpp                             |   1 +
 .../src/debug.cpp                             |   2 +-
 .../src/manager.cpp                           |   1 +
 .../src/scene_no_stopping_area.cpp            |   1 +
 .../src/debug.cpp                             |   1 +
 .../src/scene_out_of_lane.cpp                 |   1 +
 .../CMakeLists.txt                            |   5 +
 .../planner_data.hpp                          |   1 -
 .../scene_module_interface.hpp                | 237 ++--------------
 .../utilization/arc_lane_util.hpp             | 114 +-------
 .../utilization/boost_geometry_helper.hpp     |  60 +---
 .../utilization/debug.hpp                     |   6 +-
 .../utilization/path_utilization.hpp          |   1 -
 .../utilization/trajectory_utils.hpp          |  92 +-----
 .../utilization/util.hpp                      |  84 +-----
 .../velocity_factor_interface.hpp             |  19 +-
 .../src/scene_module_interface.cpp            | 264 ++++++++++++++++++
 .../src/utilization/arc_lane_util.cpp         | 138 +++++++++
 .../src/utilization/boost_geometry_helper.cpp |  66 +++++
 .../src/utilization/debug.cpp                 |   2 +-
 .../src/utilization/path_utilization.cpp      |  11 -
 .../src/utilization/trajectory_utils.cpp      | 121 ++++++++
 .../src/utilization/util.cpp                  | 166 ++++++-----
 .../src/velocity_factor_interface.cpp         |  33 +++
 .../CMakeLists.txt                            |   1 +
 .../src/debug.cpp                             |   7 +-
 .../src/debug.hpp                             |   3 -
 .../src/dynamic_obstacle.cpp                  |   4 +
 .../src/dynamic_obstacle.hpp                  |   2 +
 .../src/path_utils.cpp                        |  39 +++
 .../src/path_utils.hpp                        |  23 +-
 .../src/scene.cpp                             |   4 +
 .../src/utils.cpp                             |  13 +
 .../src/utils.hpp                             |   4 +
 .../src/debug.cpp                             |   1 +
 .../src/manager.cpp                           |   2 +
 .../src/debug.cpp                             |   2 +-
 .../src/debug.cpp                             |   2 +-
 .../src/manager.cpp                           |   2 +-
 .../src/scene.cpp                             |   2 +
 .../src/debug.cpp                             |   2 +-
 .../src/manager.cpp                           |   1 +
 .../motion_velocity_smoother/resample.hpp     |   5 -
 .../smoother/smoother_base.hpp                |   7 -
 .../trajectory_utils.hpp                      |   3 -
 .../src/motion_velocity_smoother_node.cpp     |   2 +-
 .../motion_velocity_smoother/src/resample.cpp |   2 +
 .../analytical_jerk_constrained_smoother.cpp  |   1 +
 .../src/smoother/smoother_base.cpp            |   3 +-
 .../src/trajectory_utils.cpp                  |   2 +
 64 files changed, 905 insertions(+), 699 deletions(-)
 create mode 100644 planning/behavior_velocity_planner_common/src/scene_module_interface.cpp
 create mode 100644 planning/behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp
 create mode 100644 planning/behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp
 create mode 100644 planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp
 create mode 100644 planning/behavior_velocity_planner_common/src/velocity_factor_interface.cpp
 create mode 100644 planning/behavior_velocity_run_out_module/src/path_utils.cpp

diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp
index b42bc509db3c1..17ce7e1f4de5a 100644
--- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp
+++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp
@@ -38,15 +38,10 @@
 #include <geometry_msgs/msg/twist.hpp>
 #include <geometry_msgs/msg/twist_with_covariance.hpp>
 #include <geometry_msgs/msg/vector3.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 #include <tf2/utils.h>
 
-#ifdef ROS_DISTRO_GALACTIC
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#else
-#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
-#endif
-
 // TODO(wep21): Remove these apis
 //              after they are implemented in ros2 geometry2.
 namespace tf2
diff --git a/common/tier4_autoware_utils/src/geometry/geometry.cpp b/common/tier4_autoware_utils/src/geometry/geometry.cpp
index 6048527142018..2c2de2d07a3dc 100644
--- a/common/tier4_autoware_utils/src/geometry/geometry.cpp
+++ b/common/tier4_autoware_utils/src/geometry/geometry.cpp
@@ -16,7 +16,10 @@
 
 #include <Eigen/Geometry>
 
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
 #include <tf2/convert.h>
+
 namespace tf2
 {
 void fromMsg(const geometry_msgs::msg::PoseStamped & msg, tf2::Stamped<tf2::Transform> & out)
diff --git a/planning/behavior_velocity_blind_spot_module/src/debug.cpp b/planning/behavior_velocity_blind_spot_module/src/debug.cpp
index 1652d9d56a8a0..f95ed73c2a184 100644
--- a/planning/behavior_velocity_blind_spot_module/src/debug.cpp
+++ b/planning/behavior_velocity_blind_spot_module/src/debug.cpp
@@ -19,6 +19,8 @@
 #include <motion_utils/marker/virtual_wall_marker_creator.hpp>
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
 
+#include <tf2/utils.h>
+
 #include <string>
 
 namespace behavior_velocity_planner
diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp
index 94e11bc144a32..775f3b1c65973 100644
--- a/planning/behavior_velocity_blind_spot_module/src/scene.cpp
+++ b/planning/behavior_velocity_blind_spot_module/src/scene.cpp
@@ -20,6 +20,7 @@
 #include <lanelet2_extension/regulatory_elements/road_marking.hpp>
 #include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
+#include <motion_utils/trajectory/trajectory.hpp>
 
 #include <boost/geometry/algorithms/area.hpp>
 #include <boost/geometry/algorithms/distance.hpp>
diff --git a/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp
index dfe682d0aa0c9..6c70e7a7f6dc0 100644
--- a/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp
+++ b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp
@@ -43,8 +43,6 @@ namespace behavior_velocity_planner
 
 using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
-using tier4_autoware_utils::createPoint;
-using tier4_autoware_utils::Point2d;
 using tier4_planning_msgs::msg::StopFactor;
 
 enum class CollisionState { YIELD, EGO_PASS_FIRST, EGO_PASS_LATER, IGNORE };
diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
index dc37046e1a4d3..9ea03996c88dd 100644
--- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
+++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
@@ -19,6 +19,7 @@
 #include <behavior_velocity_planner_common/utilization/util.hpp>
 #include <motion_utils/distance/distance.hpp>
 #include <motion_utils/resample/resample.hpp>
+#include <motion_utils/trajectory/trajectory.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/geometry/boost_geometry.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
diff --git a/planning/behavior_velocity_crosswalk_module/src/util.cpp b/planning/behavior_velocity_crosswalk_module/src/util.cpp
index 76d59ac895d5a..47bbef7111da0 100644
--- a/planning/behavior_velocity_crosswalk_module/src/util.cpp
+++ b/planning/behavior_velocity_crosswalk_module/src/util.cpp
@@ -17,6 +17,7 @@
 #include <behavior_velocity_planner_common/utilization/util.hpp>
 #include <lanelet2_extension/utility/query.hpp>
 #include <motion_utils/trajectory/path_with_lane_id.hpp>
+#include <motion_utils/trajectory/trajectory.hpp>
 #include <tier4_autoware_utils/geometry/boost_geometry.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 
diff --git a/planning/behavior_velocity_detection_area_module/src/debug.cpp b/planning/behavior_velocity_detection_area_module/src/debug.cpp
index b3d3d5c781ff8..ee2af54e5ea2a 100644
--- a/planning/behavior_velocity_detection_area_module/src/debug.cpp
+++ b/planning/behavior_velocity_detection_area_module/src/debug.cpp
@@ -17,6 +17,7 @@
 #include <behavior_velocity_planner_common/utilization/debug.hpp>
 #include <behavior_velocity_planner_common/utilization/util.hpp>
 #include <motion_utils/marker/virtual_wall_marker_creator.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
 
 #ifdef ROS_DISTRO_GALACTIC
diff --git a/planning/behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_detection_area_module/src/manager.cpp
index 16d32b8a49964..e7ec6b37f7f20 100644
--- a/planning/behavior_velocity_detection_area_module/src/manager.cpp
+++ b/planning/behavior_velocity_detection_area_module/src/manager.cpp
@@ -14,6 +14,7 @@
 
 #include "manager.hpp"
 
+#include <behavior_velocity_planner_common/utilization/util.hpp>
 #include <lanelet2_extension/utility/query.hpp>
 #include <tier4_autoware_utils/ros/parameter.hpp>
 
diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp
index dfaa10ec9d86f..9d8cb46fcf54f 100644
--- a/planning/behavior_velocity_intersection_module/src/debug.cpp
+++ b/planning/behavior_velocity_intersection_module/src/debug.cpp
@@ -20,6 +20,14 @@
 #include <motion_utils/marker/virtual_wall_marker_creator.hpp>
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
 
+#include <tf2/utils.h>
+
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
 #include <string>
 #include <vector>
 
diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
index 5f9d838b7c220..aa1ceca086bc2 100644
--- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
+++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
@@ -24,8 +24,13 @@
 #include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
 #include <magic_enum.hpp>
+#include <motion_utils/trajectory/trajectory.hpp>
 #include <opencv2/imgproc.hpp>
 #include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+#include <tier4_autoware_utils/ros/uuid_helper.hpp>
+
+#include <boost/geometry/algorithms/correct.hpp>
 
 #include <lanelet2_core/geometry/Polygon.h>
 #include <lanelet2_core/primitives/BasicRegulatoryElements.h>
@@ -35,7 +40,6 @@
 #include <memory>
 #include <tuple>
 #include <vector>
-
 namespace behavior_velocity_planner
 {
 namespace bg = boost::geometry;
diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp
index 10ad395953476..9c3a35033ea9b 100644
--- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp
+++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp
@@ -22,6 +22,7 @@
 #include <lanelet2_extension/regulatory_elements/road_marking.hpp>
 #include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
+#include <motion_utils/trajectory/trajectory.hpp>
 
 #include <lanelet2_core/geometry/Polygon.h>
 #include <lanelet2_core/primitives/BasicRegulatoryElements.h>
diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp
index ce2d7252ac93d..24c3454fd7e88 100644
--- a/planning/behavior_velocity_intersection_module/src/util.cpp
+++ b/planning/behavior_velocity_intersection_module/src/util.cpp
@@ -23,9 +23,11 @@
 #include <lanelet2_extension/regulatory_elements/road_marking.hpp>
 #include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
+#include <motion_utils/trajectory/trajectory.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
 
+#include <boost/geometry/algorithms/correct.hpp>
 #include <boost/geometry/algorithms/intersects.hpp>
 
 #include <lanelet2_core/geometry/Polygon.h>
@@ -40,7 +42,6 @@
 #include <tuple>
 #include <unordered_map>
 #include <vector>
-
 namespace behavior_velocity_planner
 {
 namespace bg = boost::geometry;
diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp
index 2c57ff9458833..f1fa4c47e951b 100644
--- a/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp
+++ b/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp
@@ -14,6 +14,7 @@
 
 #include "manager.hpp"
 
+#include <behavior_velocity_planner_common/utilization/util.hpp>
 #include <tier4_autoware_utils/ros/parameter.hpp>
 
 #include <string>
diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/scene.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/scene.cpp
index 8109b99166332..0d0e7cc6cadba 100644
--- a/planning/behavior_velocity_no_drivable_lane_module/src/scene.cpp
+++ b/planning/behavior_velocity_no_drivable_lane_module/src/scene.cpp
@@ -18,6 +18,7 @@
 #include "tier4_autoware_utils/geometry/geometry.hpp"
 #include "util.hpp"
 
+#include <behavior_velocity_planner_common/utilization/util.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 namespace behavior_velocity_planner
diff --git a/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp b/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp
index e8bf83868d122..9e51afca2d475 100644
--- a/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp
+++ b/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp
@@ -17,8 +17,8 @@
 #include <behavior_velocity_planner_common/utilization/debug.hpp>
 #include <behavior_velocity_planner_common/utilization/util.hpp>
 #include <motion_utils/marker/virtual_wall_marker_creator.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
-
 #ifdef ROS_DISTRO_GALACTIC
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #else
diff --git a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp
index 60d659497e55a..d5e9eeddb6377 100644
--- a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp
+++ b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp
@@ -14,6 +14,7 @@
 
 #include "manager.hpp"
 
+#include <behavior_velocity_planner_common/utilization/util.hpp>
 #include <lanelet2_extension/utility/query.hpp>
 #include <tier4_autoware_utils/ros/parameter.hpp>
 
diff --git a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp
index efa2ce60162f6..ceef50094e374 100644
--- a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp
+++ b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp
@@ -18,6 +18,7 @@
 #include <behavior_velocity_planner_common/utilization/path_utilization.hpp>
 #include <behavior_velocity_planner_common/utilization/util.hpp>
 #include <interpolation/spline_interpolation.hpp>
+#include <motion_utils/trajectory/trajectory.hpp>
 #include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 
diff --git a/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp b/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp
index 527f6f02140ea..a6e13d0dfdcdc 100644
--- a/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp
+++ b/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp
@@ -32,6 +32,7 @@ using builtin_interfaces::msg::Time;
 using BasicPolygons = std::vector<lanelet::BasicPolygon2d>;
 using occlusion_spot_utils::PossibleCollisionInfo;
 using tier4_autoware_utils::appendMarkerArray;
+using tier4_autoware_utils::calcOffsetPose;
 using tier4_autoware_utils::createDefaultMarker;
 using tier4_autoware_utils::createMarkerColor;
 using tier4_autoware_utils::createMarkerOrientation;
diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp
index 280a832f6d684..013643b6bd6d4 100644
--- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp
+++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp
@@ -26,6 +26,7 @@
 #include <behavior_velocity_planner_common/utilization/util.hpp>
 #include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
+#include <motion_utils/trajectory/trajectory.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
 #include <tier4_autoware_utils/system/stop_watch.hpp>
diff --git a/planning/behavior_velocity_planner_common/CMakeLists.txt b/planning/behavior_velocity_planner_common/CMakeLists.txt
index 334ee35329ba3..e958df74afe2d 100644
--- a/planning/behavior_velocity_planner_common/CMakeLists.txt
+++ b/planning/behavior_velocity_planner_common/CMakeLists.txt
@@ -5,7 +5,12 @@ find_package(autoware_cmake REQUIRED)
 autoware_package()
 
 ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/scene_module_interface.cpp
+  src/velocity_factor_interface.cpp
   src/utilization/path_utilization.cpp
+  src/utilization/trajectory_utils.cpp
+  src/utilization/arc_lane_util.cpp
+  src/utilization/boost_geometry_helper.cpp
   src/utilization/util.cpp
   src/utilization/debug.cpp
 )
diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp
index 7e48f410f363a..620fc61ec5360 100644
--- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp
+++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp
@@ -39,7 +39,6 @@
 
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
-#include <tf2_ros/transform_listener.h>
 
 #include <algorithm>
 #include <deque>
diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp
index 4735bb34c7b00..90e78215cae2d 100644
--- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp
+++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp
@@ -16,14 +16,11 @@
 #define BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_
 
 #include <behavior_velocity_planner_common/planner_data.hpp>
-#include <behavior_velocity_planner_common/utilization/util.hpp>
 #include <behavior_velocity_planner_common/velocity_factor_interface.hpp>
 #include <builtin_interfaces/msg/time.hpp>
 #include <motion_utils/marker/virtual_wall_marker_creator.hpp>
 #include <rtc_interface/rtc_interface.hpp>
 #include <tier4_autoware_utils/ros/debug_publisher.hpp>
-#include <tier4_autoware_utils/ros/uuid_helper.hpp>
-#include <tier4_autoware_utils/system/stop_watch.hpp>
 
 #include <autoware_adapi_v1_msgs/msg/velocity_factor.hpp>
 #include <autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp>
@@ -35,10 +32,7 @@
 #include <tier4_v2x_msgs/msg/infrastructure_command_array.hpp>
 #include <unique_identifier_msgs/msg/uuid.hpp>
 
-#include <algorithm>
-#include <limits>
 #include <memory>
-#include <random>
 #include <set>
 #include <string>
 #include <unordered_map>
@@ -48,7 +42,6 @@
 #include <rclcpp/rclcpp.hpp>
 
 #include <visualization_msgs/msg/marker_array.hpp>
-
 namespace behavior_velocity_planner
 {
 
@@ -56,7 +49,6 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using builtin_interfaces::msg::Time;
 using rtc_interface::RTCInterface;
 using tier4_autoware_utils::DebugPublisher;
-using tier4_autoware_utils::StopWatch;
 using tier4_debug_msgs::msg::Float64Stamped;
 using tier4_planning_msgs::msg::StopFactor;
 using tier4_planning_msgs::msg::StopReason;
@@ -67,15 +59,7 @@ class SceneModuleInterface
 {
 public:
   explicit SceneModuleInterface(
-    const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock)
-  : module_id_(module_id),
-    activated_(false),
-    safe_(false),
-    distance_(std::numeric_limits<double>::lowest()),
-    logger_(logger),
-    clock_(clock)
-  {
-  }
+    const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock);
   virtual ~SceneModuleInterface() = default;
 
   virtual bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) = 0;
@@ -125,44 +109,14 @@ class SceneModuleInterface
   void setSafe(const bool safe) { safe_ = safe; }
   void setDistance(const double distance) { distance_ = distance; }
 
-  template <class T>
-  size_t findEgoSegmentIndex(const std::vector<T> & points) const
-  {
-    const auto & p = planner_data_;
-    return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
-      points, p->current_odometry->pose, p->ego_nearest_dist_threshold);
-  }
+  size_t findEgoSegmentIndex(
+    const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points) const;
 };
 
 class SceneModuleManagerInterface
 {
 public:
-  SceneModuleManagerInterface(rclcpp::Node & node, [[maybe_unused]] const char * module_name)
-  : node_(node), clock_(node.get_clock()), logger_(node.get_logger())
-  {
-    const auto ns = std::string("~/debug/") + module_name;
-    pub_debug_ = node.create_publisher<visualization_msgs::msg::MarkerArray>(ns, 1);
-    if (!node.has_parameter("is_publish_debug_path")) {
-      is_publish_debug_path_ = node.declare_parameter<bool>("is_publish_debug_path");
-    } else {
-      is_publish_debug_path_ = node.get_parameter("is_publish_debug_path").as_bool();
-    }
-    if (is_publish_debug_path_) {
-      pub_debug_path_ = node.create_publisher<autoware_auto_planning_msgs::msg::PathWithLaneId>(
-        std::string("~/debug/path_with_lane_id/") + module_name, 1);
-    }
-    pub_virtual_wall_ = node.create_publisher<visualization_msgs::msg::MarkerArray>(
-      std::string("~/virtual_wall/") + module_name, 5);
-    pub_velocity_factor_ = node.create_publisher<autoware_adapi_v1_msgs::msg::VelocityFactorArray>(
-      std::string("/planning/velocity_factors/") + module_name, 1);
-    pub_stop_reason_ =
-      node.create_publisher<tier4_planning_msgs::msg::StopReasonArray>("~/output/stop_reasons", 1);
-    pub_infrastructure_commands_ =
-      node.create_publisher<tier4_v2x_msgs::msg::InfrastructureCommandArray>(
-        "~/output/infrastructure_commands", 1);
-
-    processing_time_publisher_ = std::make_shared<DebugPublisher>(&node, "~/debug");
-  }
+  SceneModuleManagerInterface(rclcpp::Node & node, [[maybe_unused]] const char * module_name);
 
   virtual ~SceneModuleManagerInterface() = default;
 
@@ -172,13 +126,7 @@ class SceneModuleManagerInterface
 
   void updateSceneModuleInstances(
     const std::shared_ptr<const PlannerData> & planner_data,
-    const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
-  {
-    planner_data_ = planner_data;
-
-    launchNewModules(path);
-    deleteExpiredModules(path);
-  }
+    const autoware_auto_planning_msgs::msg::PathWithLaneId & path);
 
   virtual void plan(autoware_auto_planning_msgs::msg::PathWithLaneId * path)
   {
@@ -186,120 +134,26 @@ class SceneModuleManagerInterface
   }
 
 protected:
-  virtual void modifyPathVelocity(autoware_auto_planning_msgs::msg::PathWithLaneId * path)
-  {
-    StopWatch<std::chrono::milliseconds> stop_watch;
-    stop_watch.tic("Total");
-    visualization_msgs::msg::MarkerArray debug_marker_array;
-    tier4_planning_msgs::msg::StopReasonArray stop_reason_array;
-    autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array;
-    stop_reason_array.header.frame_id = "map";
-    stop_reason_array.header.stamp = clock_->now();
-    velocity_factor_array.header.frame_id = "map";
-    velocity_factor_array.header.stamp = clock_->now();
-
-    tier4_v2x_msgs::msg::InfrastructureCommandArray infrastructure_command_array;
-    infrastructure_command_array.stamp = clock_->now();
-
-    first_stop_path_point_index_ = static_cast<int>(path->points.size()) - 1;
-    for (const auto & scene_module : scene_modules_) {
-      tier4_planning_msgs::msg::StopReason stop_reason;
-      scene_module->resetVelocityFactor();
-      scene_module->setPlannerData(planner_data_);
-      scene_module->modifyPathVelocity(path, &stop_reason);
-
-      // The velocity factor must be called after modifyPathVelocity.
-      const auto velocity_factor = scene_module->getVelocityFactor();
-      if (velocity_factor.type != VelocityFactor::UNKNOWN) {
-        velocity_factor_array.factors.emplace_back(velocity_factor);
-      }
-      if (stop_reason.reason != "") {
-        stop_reason_array.stop_reasons.emplace_back(stop_reason);
-      }
-
-      if (const auto command = scene_module->getInfrastructureCommand()) {
-        infrastructure_command_array.commands.push_back(*command);
-      }
-
-      if (scene_module->getFirstStopPathPointIndex() < first_stop_path_point_index_) {
-        first_stop_path_point_index_ = scene_module->getFirstStopPathPointIndex();
-      }
-
-      for (const auto & marker : scene_module->createDebugMarkerArray().markers) {
-        debug_marker_array.markers.push_back(marker);
-      }
-
-      virtual_wall_marker_creator_.add_virtual_walls(scene_module->createVirtualWalls());
-    }
-
-    if (!stop_reason_array.stop_reasons.empty()) {
-      pub_stop_reason_->publish(stop_reason_array);
-    }
-    pub_velocity_factor_->publish(velocity_factor_array);
-    pub_infrastructure_commands_->publish(infrastructure_command_array);
-    pub_debug_->publish(debug_marker_array);
-    if (is_publish_debug_path_) {
-      autoware_auto_planning_msgs::msg::PathWithLaneId debug_path;
-      debug_path.header = path->header;
-      debug_path.points = path->points;
-      pub_debug_path_->publish(debug_path);
-    }
-    pub_virtual_wall_->publish(virtual_wall_marker_creator_.create_markers(clock_->now()));
-    processing_time_publisher_->publish<Float64Stamped>(
-      std::string(getModuleName()) + "/processing_time_ms", stop_watch.toc("Total"));
-  }
+  virtual void modifyPathVelocity(autoware_auto_planning_msgs::msg::PathWithLaneId * path);
 
   virtual void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) = 0;
 
   virtual std::function<bool(const std::shared_ptr<SceneModuleInterface> &)>
   getModuleExpiredFunction(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) = 0;
 
-  virtual void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
-  {
-    const auto isModuleExpired = getModuleExpiredFunction(path);
-
-    // Copy container to avoid iterator corruption
-    // due to scene_modules_.erase() in unregisterModule()
-    const auto copied_scene_modules = scene_modules_;
-
-    for (const auto & scene_module : copied_scene_modules) {
-      if (isModuleExpired(scene_module)) {
-        unregisterModule(scene_module);
-      }
-    }
-  }
+  virtual void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path);
 
   bool isModuleRegistered(const int64_t module_id)
   {
     return registered_module_id_set_.count(module_id) != 0;
   }
 
-  void registerModule(const std::shared_ptr<SceneModuleInterface> & scene_module)
-  {
-    RCLCPP_INFO(
-      logger_, "register task: module = %s, id = %lu", getModuleName(),
-      scene_module->getModuleId());
-    registered_module_id_set_.emplace(scene_module->getModuleId());
-    scene_modules_.insert(scene_module);
-  }
+  void registerModule(const std::shared_ptr<SceneModuleInterface> & scene_module);
 
-  void unregisterModule(const std::shared_ptr<SceneModuleInterface> & scene_module)
-  {
-    RCLCPP_INFO(
-      logger_, "unregister task: module = %s, id = %lu", getModuleName(),
-      scene_module->getModuleId());
-    registered_module_id_set_.erase(scene_module->getModuleId());
-    scene_modules_.erase(scene_module);
-  }
+  void unregisterModule(const std::shared_ptr<SceneModuleInterface> & scene_module);
 
-  template <class T>
-  size_t findEgoSegmentIndex(const std::vector<T> & points) const
-  {
-    const auto & p = planner_data_;
-    return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
-      points, p->current_odometry->pose, p->ego_nearest_dist_threshold,
-      p->ego_nearest_yaw_threshold);
-  }
+  size_t findEgoSegmentIndex(
+    const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points) const;
 
   std::set<std::shared_ptr<SceneModuleInterface>> scene_modules_;
   std::set<int64_t> registered_module_id_set_;
@@ -329,38 +183,17 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface
 {
 public:
   SceneModuleManagerInterfaceWithRTC(
-    rclcpp::Node & node, const char * module_name, const bool enable_rtc = true)
-  : SceneModuleManagerInterface(node, module_name), rtc_interface_(&node, module_name, enable_rtc)
-  {
-  }
+    rclcpp::Node & node, const char * module_name, const bool enable_rtc = true);
 
-  void plan(autoware_auto_planning_msgs::msg::PathWithLaneId * path) override
-  {
-    setActivation();
-    modifyPathVelocity(path);
-    sendRTC(path->header.stamp);
-  }
+  void plan(autoware_auto_planning_msgs::msg::PathWithLaneId * path) override;
 
 protected:
   RTCInterface rtc_interface_;
   std::unordered_map<int64_t, UUID> map_uuid_;
 
-  virtual void sendRTC(const Time & stamp)
-  {
-    for (const auto & scene_module : scene_modules_) {
-      const UUID uuid = getUUID(scene_module->getModuleId());
-      updateRTCStatus(uuid, scene_module->isSafe(), scene_module->getDistance(), stamp);
-    }
-    publishRTCStatus(stamp);
-  }
+  virtual void sendRTC(const Time & stamp);
 
-  virtual void setActivation()
-  {
-    for (const auto & scene_module : scene_modules_) {
-      const UUID uuid = getUUID(scene_module->getModuleId());
-      scene_module->setActivation(rtc_interface_.isActivated(uuid));
-    }
-  }
+  virtual void setActivation();
 
   void updateRTCStatus(
     const UUID & uuid, const bool safe, const double distance, const Time & stamp)
@@ -372,45 +205,13 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface
 
   void publishRTCStatus(const Time & stamp) { rtc_interface_.publishCooperateStatus(stamp); }
 
-  UUID getUUID(const int64_t & module_id) const
-  {
-    if (map_uuid_.count(module_id) == 0) {
-      const UUID uuid;
-      return uuid;
-    }
-    return map_uuid_.at(module_id);
-  }
+  UUID getUUID(const int64_t & module_id) const;
 
-  void generateUUID(const int64_t & module_id)
-  {
-    map_uuid_.insert({module_id, tier4_autoware_utils::generateUUID()});
-  }
+  void generateUUID(const int64_t & module_id);
 
-  void removeUUID(const int64_t & module_id)
-  {
-    const auto result = map_uuid_.erase(module_id);
-    if (result == 0) {
-      RCLCPP_WARN_STREAM(
-        logger_, "[removeUUID] module_id = " << module_id << " is not registered.");
-    }
-  }
+  void removeUUID(const int64_t & module_id);
 
-  void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override
-  {
-    const auto isModuleExpired = getModuleExpiredFunction(path);
-
-    // Copy container to avoid iterator corruption
-    // due to scene_modules_.erase() in unregisterModule()
-    const auto copied_scene_modules = scene_modules_;
-
-    for (const auto & scene_module : copied_scene_modules) {
-      if (isModuleExpired(scene_module)) {
-        removeRTCStatus(getUUID(scene_module->getModuleId()));
-        removeUUID(scene_module->getModuleId());
-        unregisterModule(scene_module);
-      }
-    }
-  }
+  void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;
 };
 
 }  // namespace behavior_velocity_planner
diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp
index ffc216e863500..63d7c08b945fd 100644
--- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp
+++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp
@@ -16,22 +16,12 @@
 #define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_
 
 #include <behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp>
-#include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 
 #include <boost/optional.hpp>
 
-#ifdef ROS_DISTRO_GALACTIC
-#include <tf2_eigen/tf2_eigen.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#else
-#include <tf2_eigen/tf2_eigen.hpp>
-
-#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
-#endif
-
 #include <algorithm>
 #include <memory>
 #include <utility>
@@ -46,7 +36,7 @@ namespace bg = boost::geometry;
 
 namespace
 {
-geometry_msgs::msg::Point convertToGeomPoint(const Point2d & p)
+geometry_msgs::msg::Point convertToGeomPoint(const tier4_autoware_utils::Point2d & p)
 {
   geometry_msgs::msg::Point geom_p;
   geom_p.x = p.x();
@@ -55,73 +45,23 @@ geometry_msgs::msg::Point convertToGeomPoint(const Point2d & p)
   return geom_p;
 }
 
-geometry_msgs::msg::Point operator+(
-  const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2)
-{
-  geometry_msgs::msg::Point p;
-  p.x = p1.x + p2.x;
-  p.y = p1.y + p2.y;
-  p.z = p1.z + p2.z;
-
-  return p;
-}
-
-[[maybe_unused]] geometry_msgs::msg::Point operator*(
-  const geometry_msgs::msg::Point & p, const double v)
-{
-  geometry_msgs::msg::Point multiplied_p;
-  multiplied_p.x = p.x * v;
-  multiplied_p.y = p.y * v;
-  multiplied_p.z = p.z * v;
-
-  return multiplied_p;
-}
-
-[[maybe_unused]] geometry_msgs::msg::Point operator*(
-  const double v, const geometry_msgs::msg::Point & p)
-{
-  return p * v;
-}
 }  // namespace
 
 namespace arc_lane_utils
 {
-using PathIndexWithPose = std::pair<size_t, geometry_msgs::msg::Pose>;    // front index, pose
-using PathIndexWithPoint2d = std::pair<size_t, Point2d>;                  // front index, point2d
+using PathIndexWithPose = std::pair<size_t, geometry_msgs::msg::Pose>;  // front index, pose
+using PathIndexWithPoint2d =
+  std::pair<size_t, tier4_autoware_utils::Point2d>;                       // front index, point2d
 using PathIndexWithPoint = std::pair<size_t, geometry_msgs::msg::Point>;  // front index, point2d
 using PathIndexWithOffset = std::pair<size_t, double>;                    // front index, offset
 
-inline double calcSignedDistance(
-  const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Point & p2)
-{
-  Eigen::Affine3d map2p1;
-  tf2::fromMsg(p1, map2p1);
-  const auto basecoords_p2 = map2p1.inverse() * Eigen::Vector3d(p2.x, p2.y, p2.z);
-  return basecoords_p2.x() >= 0 ? basecoords_p2.norm() : -basecoords_p2.norm();
-}
+double calcSignedDistance(
+  const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Point & p2);
 
 // calculate one collision point between the line (from p1 to p2) and the line (from p3 to p4)
-inline boost::optional<geometry_msgs::msg::Point> checkCollision(
+boost::optional<geometry_msgs::msg::Point> checkCollision(
   const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2,
-  const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4)
-{
-  const double det = (p2.x - p1.x) * (p4.y - p3.y) - (p2.y - p1.y) * (p4.x - p3.x);
-
-  if (det == 0.0) {
-    // collision is not one point.
-    return boost::none;
-  }
-
-  const double t1 = ((p4.y - p3.y) * (p4.x - p1.x) - (p4.x - p3.x) * (p4.y - p1.y)) / det;
-  const double t2 = ((p2.x - p1.x) * (p4.y - p1.y) - (p2.y - p1.y) * (p4.x - p1.x)) / det;
-
-  // check collision is outside the segment line
-  if (t1 < 0.0 || 1.0 < t1 || t2 < 0.0 || 1.0 < t2) {
-    return boost::none;
-  }
-
-  return p1 * (1.0 - t1) + p2 * t1;
-}
+  const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4);
 
 template <class T>
 boost::optional<PathIndexWithPoint> findCollisionSegment(
@@ -223,16 +163,9 @@ boost::optional<PathIndexWithOffset> findOffsetSegment(
       tier4_autoware_utils::calcDistance2d(path.points.at(collision_idx + 1), collision_point));
 }
 
-inline boost::optional<PathIndexWithOffset> findOffsetSegment(
+boost::optional<PathIndexWithOffset> findOffsetSegment(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t index,
-  const double offset)
-{
-  if (offset >= 0) {
-    return findForwardOffsetSegment(path, index, offset);
-  }
-
-  return findBackwardOffsetSegment(path, index, -offset);
-}
+  const double offset);
 
 template <class T>
 geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffset & offset_segment)
@@ -263,33 +196,10 @@ geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffse
   return target_pose;
 }
 
-inline boost::optional<PathIndexWithPose> createTargetPoint(
+boost::optional<PathIndexWithPose> createTargetPoint(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line,
-  const size_t lane_id, const double margin, const double vehicle_offset)
-{
-  // Find collision segment
-  const auto collision_segment = findCollisionSegment(path, stop_line, lane_id);
-  if (!collision_segment) {
-    // No collision
-    return {};
-  }
-
-  // Calculate offset length from stop line
-  // Use '-' to make the positive direction is forward
-  const double offset_length = -(margin + vehicle_offset);
+  const size_t lane_id, const double margin, const double vehicle_offset);
 
-  // Find offset segment
-  const auto offset_segment = findOffsetSegment(path, *collision_segment, offset_length);
-  if (!offset_segment) {
-    // No enough path length
-    return {};
-  }
-
-  const auto target_pose = calcTargetPose(path, *offset_segment);
-
-  const auto front_idx = offset_segment->first;
-  return std::make_pair(front_idx, target_pose);
-}
 }  // namespace arc_lane_utils
 }  // namespace behavior_velocity_planner
 
diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp
index 193890ec70ceb..0f3cc7203ef1f 100644
--- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp
+++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp
@@ -24,24 +24,11 @@
 #include <geometry_msgs/msg/polygon.hpp>
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 
-#include <boost/assign/list_of.hpp>
-#include <boost/geometry.hpp>
-#include <boost/geometry/algorithms/area.hpp>
-#include <boost/geometry/algorithms/disjoint.hpp>
-#include <boost/geometry/algorithms/distance.hpp>
-#include <boost/geometry/algorithms/equals.hpp>
-#include <boost/geometry/algorithms/intersection.hpp>
-#include <boost/geometry/geometries/linestring.hpp>
-#include <boost/geometry/geometries/point_xy.hpp>
-#include <boost/geometry/geometries/polygon.hpp>
+#include <boost/geometry/core/cs.hpp>
+#include <boost/geometry/geometries/geometries.hpp>
 #include <boost/geometry/geometries/register/point.hpp>
 
-#include <lanelet2_core/primitives/Polygon.h>
-#include <tf2/utils.h>
-
-#include <algorithm>
 #include <vector>
-
 // cppcheck-suppress unknownMacro
 BOOST_GEOMETRY_REGISTER_POINT_3D(geometry_msgs::msg::Point, double, cs::cartesian, x, y, z)
 BOOST_GEOMETRY_REGISTER_POINT_3D(
@@ -83,48 +70,13 @@ LineString2d to_bg2d(const std::vector<T> & vec)
   return ps;
 }
 
-inline Polygon2d lines2polygon(const LineString2d & left_line, const LineString2d & right_line)
-{
-  Polygon2d polygon;
-
-  polygon.outer().push_back(left_line.front());
+Polygon2d lines2polygon(const LineString2d & left_line, const LineString2d & right_line);
 
-  for (auto itr = right_line.begin(); itr != right_line.end(); ++itr) {
-    polygon.outer().push_back(*itr);
-  }
-
-  for (auto itr = left_line.rbegin(); itr != left_line.rend(); ++itr) {
-    polygon.outer().push_back(*itr);
-  }
+Polygon2d upScalePolygon(
+  const geometry_msgs::msg::Point & position, const Polygon2d & polygon, const double scale);
 
-  bg::correct(polygon);
-  return polygon;
-}
+geometry_msgs::msg::Polygon toGeomPoly(const Polygon2d & polygon);
 
-inline Polygon2d upScalePolygon(
-  const geometry_msgs::msg::Point & position, const Polygon2d & polygon, const double scale)
-{
-  Polygon2d transformed_polygon;
-  // upscale
-  for (size_t i = 0; i < polygon.outer().size(); i++) {
-    const double upscale_x = (polygon.outer().at(i).x() - position.x) * scale + position.x;
-    const double upscale_y = (polygon.outer().at(i).y() - position.y) * scale + position.y;
-    transformed_polygon.outer().emplace_back(Point2d(upscale_x, upscale_y));
-  }
-  return transformed_polygon;
-}
-
-inline geometry_msgs::msg::Polygon toGeomPoly(const Polygon2d & polygon)
-{
-  geometry_msgs::msg::Polygon polygon_msg;
-  geometry_msgs::msg::Point32 point_msg;
-  for (const auto & p : polygon.outer()) {
-    point_msg.x = p.x();
-    point_msg.y = p.y();
-    polygon_msg.points.push_back(point_msg);
-  }
-  return polygon_msg;
-}
 }  // namespace behavior_velocity_planner
 
 #endif  // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_
diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp
index e840b6f8a5995..98df29d1c44c0 100644
--- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp
+++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp
@@ -15,8 +15,12 @@
 #ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_
 #define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_
 
-#include <behavior_velocity_planner_common/utilization/util.hpp>
+#include <rclcpp/time.hpp>
 
+#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
+#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
+#include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/polygon.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
 #include <string>
diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp
index 4156f7df9540b..591ef8667b81b 100644
--- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp
+++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp
@@ -16,7 +16,6 @@
 #define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_
 
 #include <rclcpp/rclcpp.hpp>
-#include <tier4_autoware_utils/geometry/geometry.hpp>
 
 #include <autoware_auto_planning_msgs/msg/path.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp
index 7d5278c3cf972..c86272cff1c2e 100644
--- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp
+++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp
@@ -16,32 +16,18 @@
 #define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_
 
 #include <behavior_velocity_planner_common/planner_data.hpp>
-#include <behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp>
-#include <interpolation/linear_interpolation.hpp>
-#include <motion_utils/trajectory/trajectory.hpp>
-#include <motion_velocity_smoother/trajectory_utils.hpp>
-#include <rclcpp/rclcpp.hpp>
 
-#include <autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <autoware_auto_planning_msgs/msg/trajectory.hpp>
 #include <autoware_auto_planning_msgs/msg/trajectory_point.hpp>
-#include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/quaternion.hpp>
-#include <geometry_msgs/msg/twist_stamped.hpp>
 
-#include <tf2/utils.h>
-
-#include <algorithm>
-#include <limits>
 #include <memory>
-#include <string>
 #include <utility>
 #include <vector>
 
 namespace behavior_velocity_planner
 {
-using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using autoware_auto_planning_msgs::msg::Trajectory;
 using autoware_auto_planning_msgs::msg::TrajectoryPoint;
@@ -49,83 +35,15 @@ using TrajectoryPoints = std::vector<TrajectoryPoint>;
 using geometry_msgs::msg::Quaternion;
 using TrajectoryPointWithIdx = std::pair<TrajectoryPoint, size_t>;
 
-inline TrajectoryPoints convertPathToTrajectoryPoints(const PathWithLaneId & path)
-{
-  TrajectoryPoints tps;
-  for (const auto & p : path.points) {
-    TrajectoryPoint tp;
-    tp.pose = p.point.pose;
-    tp.longitudinal_velocity_mps = p.point.longitudinal_velocity_mps;
-    // since path point doesn't have acc for now
-    tp.acceleration_mps2 = 0;
-    tps.emplace_back(tp);
-  }
-  return tps;
-}
-inline PathWithLaneId convertTrajectoryPointsToPath(const TrajectoryPoints & trajectory)
-{
-  PathWithLaneId path;
-  for (const auto & p : trajectory) {
-    PathPointWithLaneId pp;
-    pp.point.pose = p.pose;
-    pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps;
-    path.points.emplace_back(pp);
-  }
-  return path;
-}
-
-inline Quaternion lerpOrientation(
-  const Quaternion & o_from, const Quaternion & o_to, const double ratio)
-{
-  tf2::Quaternion q_from, q_to;
-  tf2::fromMsg(o_from, q_from);
-  tf2::fromMsg(o_to, q_to);
+TrajectoryPoints convertPathToTrajectoryPoints(const PathWithLaneId & path);
+PathWithLaneId convertTrajectoryPointsToPath(const TrajectoryPoints & trajectory);
 
-  const auto q_interpolated = q_from.slerp(q_to, ratio);
-  return tf2::toMsg(q_interpolated);
-}
+Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio);
 
 //! smooth path point with lane id starts from ego position on path to the path end
-inline bool smoothPath(
+bool smoothPath(
   const PathWithLaneId & in_path, PathWithLaneId & out_path,
-  const std::shared_ptr<const PlannerData> & planner_data)
-{
-  const geometry_msgs::msg::Pose current_pose = planner_data->current_odometry->pose;
-  const double v0 = planner_data->current_velocity->twist.linear.x;
-  const double a0 = planner_data->current_acceleration->accel.accel.linear.x;
-  const auto & external_v_limit = planner_data->external_velocity_limit;
-  const auto & smoother = planner_data->velocity_smoother_;
-
-  auto trajectory = convertPathToTrajectoryPoints(in_path);
-  if (external_v_limit) {
-    motion_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit(
-      0, trajectory.size(), external_v_limit->max_velocity, trajectory);
-  }
-  const auto traj_lateral_acc_filtered = smoother->applyLateralAccelerationFilter(trajectory);
-
-  // Resample trajectory with ego-velocity based interval distances
-  auto traj_resampled = smoother->resampleTrajectory(
-    traj_lateral_acc_filtered, v0, current_pose, planner_data->ego_nearest_dist_threshold,
-    planner_data->ego_nearest_yaw_threshold);
-  const size_t traj_resampled_closest = motion_utils::findFirstNearestIndexWithSoftConstraints(
-    traj_resampled, current_pose, planner_data->ego_nearest_dist_threshold,
-    planner_data->ego_nearest_yaw_threshold);
-  std::vector<TrajectoryPoints> debug_trajectories;
-  // Clip trajectory from closest point
-  TrajectoryPoints clipped;
-  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)) {
-    std::cerr << "[behavior_velocity][trajectory_utils]: failed to smooth" << std::endl;
-    return false;
-  }
-  traj_smoothed.insert(
-    traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest);
-
-  out_path = convertTrajectoryPointsToPath(traj_smoothed);
-  return true;
-}
+  const std::shared_ptr<const PlannerData> & planner_data);
 
 }  // namespace behavior_velocity_planner
 
diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp
index 46798b7f9f15b..ceba86c27f864 100644
--- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp
+++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp
@@ -15,27 +15,17 @@
 #ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_
 #define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_
 
-#include <behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp>
-#include <motion_utils/trajectory/trajectory.hpp>
-#include <tier4_autoware_utils/geometry/geometry.hpp>
+#include <tier4_autoware_utils/geometry/boost_geometry.hpp>
 
-#include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
-#include <autoware_auto_planning_msgs/msg/path.hpp>
-#include <autoware_auto_planning_msgs/msg/path_point.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <autoware_perception_msgs/msg/traffic_signal.hpp>
 #include <geometry_msgs/msg/point.hpp>
-#include <geometry_msgs/msg/pose_stamped.hpp>
-#include <geometry_msgs/msg/quaternion.hpp>
 #include <tier4_planning_msgs/msg/stop_reason.hpp>
-#include <visualization_msgs/msg/marker.hpp>
 
 #include <lanelet2_core/Forward.h>
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_routing/Forward.h>
-#include <pcl/point_types.h>
-#include <tf2/utils.h>
 
 #include <algorithm>
 #include <limits>
@@ -48,11 +38,6 @@
 
 namespace behavior_velocity_planner
 {
-struct SearchRangeIndex
-{
-  size_t min_idx;
-  size_t max_idx;
-};
 struct DetectionRange
 {
   bool use_right = true;
@@ -65,11 +50,6 @@ struct DetectionRange
   double right_overhang;
   double left_overhang;
 };
-struct PointWithSearchRangeIndex
-{
-  geometry_msgs::msg::Point point;
-  SearchRangeIndex index;
-};
 
 struct TrafficSignalStamped
 {
@@ -83,72 +63,24 @@ using LineString2d = tier4_autoware_utils::LineString2d;
 using Polygon2d = tier4_autoware_utils::Polygon2d;
 using BasicPolygons2d = std::vector<lanelet::BasicPolygon2d>;
 using Polygons2d = std::vector<Polygon2d>;
-using autoware_auto_perception_msgs::msg::PredictedObject;
 using autoware_auto_perception_msgs::msg::PredictedObjects;
-using autoware_auto_perception_msgs::msg::Shape;
-using autoware_auto_planning_msgs::msg::Path;
-using autoware_auto_planning_msgs::msg::PathPoint;
 using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
-using autoware_auto_planning_msgs::msg::Trajectory;
-using motion_utils::calcLongitudinalOffsetToSegment;
-using motion_utils::calcSignedArcLength;
-using motion_utils::validateNonEmpty;
-using tier4_autoware_utils::calcAzimuthAngle;
-using tier4_autoware_utils::calcDistance2d;
-using tier4_autoware_utils::calcOffsetPose;
-using tier4_autoware_utils::calcSquaredDistance2d;
-using tier4_autoware_utils::createQuaternionFromYaw;
-using tier4_autoware_utils::getPoint;
 using tier4_planning_msgs::msg::StopFactor;
 using tier4_planning_msgs::msg::StopReason;
 
 namespace planning_utils
 {
-template <class T>
-size_t calcPointIndexFromSegmentIndex(
-  const std::vector<T> & points, const geometry_msgs::msg::Point & point, const size_t seg_idx)
-{
-  const size_t prev_point_idx = seg_idx;
-  const size_t next_point_idx = seg_idx + 1;
-
-  const double prev_dist = tier4_autoware_utils::calcDistance2d(point, points.at(prev_point_idx));
-  const double next_dist = tier4_autoware_utils::calcDistance2d(point, points.at(next_point_idx));
-
-  if (prev_dist < next_dist) {
-    return prev_point_idx;
-  }
-  return next_point_idx;
-}
-
-template <class T>
 size_t calcSegmentIndexFromPointIndex(
-  const std::vector<T> & points, const geometry_msgs::msg::Point & point, const size_t idx)
-{
-  if (idx == 0) {
-    return 0;
-  }
-  if (idx == points.size() - 1) {
-    return idx - 1;
-  }
-  if (points.size() < 3) {
-    return 0;
-  }
-
-  const double offset_to_seg = motion_utils::calcLongitudinalOffsetToSegment(points, idx, point);
-  if (0 < offset_to_seg) {
-    return idx;
-  }
-  return idx - 1;
-}
-
+  const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points,
+  const geometry_msgs::msg::Point & point, const size_t idx);
 // create detection area from given range return false if creation failure
 bool createDetectionAreaPolygons(
   Polygons2d & da_polys, const PathWithLaneId & path, const geometry_msgs::msg::Pose & target_pose,
   const size_t target_seg_idx, const DetectionRange & da_range, const double obstacle_vel_mps,
   const double min_velocity = 1.0);
-PathPoint getLerpPathPointWithLaneId(const PathPoint p0, const PathPoint p1, const double ratio);
-Point2d calculateOffsetPoint2d(const Pose & pose, const double offset_x, const double offset_y);
+Point2d calculateOffsetPoint2d(
+  const geometry_msgs::msg::Pose & pose, const double offset_x, const double offset_y);
 void extractClosePartition(
   const geometry_msgs::msg::Point position, const BasicPolygons2d & all_partitions,
   BasicPolygons2d & close_partition, const double distance_thresh = 30.0);
@@ -162,12 +94,6 @@ inline int64_t bitShift(int64_t original_id)
   return original_id << (sizeof(int32_t) * 8 / 2);
 }
 
-geometry_msgs::msg::Pose transformRelCoordinate2D(
-  const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin);
-geometry_msgs::msg::Pose transformAbsCoordinate2D(
-  const geometry_msgs::msg::Pose & relative, const geometry_msgs::msg::Pose & origin);
-SearchRangeIndex getPathIndexRangeIncludeLaneId(const PathWithLaneId & path, const int64_t lane_id);
-
 bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin);
 geometry_msgs::msg::Pose getAheadPose(
   const size_t start_idx, const double ahead_dist,
diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/velocity_factor_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/velocity_factor_interface.hpp
index 28ac40f0868a0..5804c0cdafcf4 100644
--- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/velocity_factor_interface.hpp
+++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/velocity_factor_interface.hpp
@@ -16,13 +16,11 @@
 #ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__VELOCITY_FACTOR_INTERFACE_HPP_
 #define BEHAVIOR_VELOCITY_PLANNER_COMMON__VELOCITY_FACTOR_INTERFACE_HPP_
 
-#include <rclcpp/rclcpp.hpp>
-
 #include <autoware_adapi_v1_msgs/msg/velocity_factor.hpp>
 #include <autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp>
+#include <autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 
-#include <mutex>
 #include <string>
 #include <vector>
 
@@ -44,19 +42,10 @@ class VelocityFactorInterface
   void init(const VelocityFactorType type) { type_ = type; }
   void reset() { velocity_factor_.type = VelocityFactor::UNKNOWN; }
 
-  template <class T>
   void set(
-    const T & points, const Pose & curr_pose, const Pose & stop_pose,
-    const VelocityFactorStatus status, const std::string detail = "")
-  {
-    const auto & curr_point = curr_pose.position;
-    const auto & stop_point = stop_pose.position;
-    velocity_factor_.type = type_;
-    velocity_factor_.pose = stop_pose;
-    velocity_factor_.distance = motion_utils::calcSignedArcLength(points, curr_point, stop_point);
-    velocity_factor_.status = status;
-    velocity_factor_.detail = detail;
-  }
+    const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points,
+    const Pose & curr_pose, const Pose & stop_pose, const VelocityFactorStatus status,
+    const std::string detail = "");
 
 private:
   VelocityFactorType type_;
diff --git a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp
new file mode 100644
index 0000000000000..6ba982294ccfd
--- /dev/null
+++ b/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp
@@ -0,0 +1,264 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <behavior_velocity_planner_common/scene_module_interface.hpp>
+#include <behavior_velocity_planner_common/utilization/util.hpp>
+#include <motion_utils/trajectory/trajectory.hpp>
+#include <tier4_autoware_utils/ros/uuid_helper.hpp>
+#include <tier4_autoware_utils/system/stop_watch.hpp>
+
+#include <algorithm>
+#include <limits>
+
+namespace behavior_velocity_planner
+{
+
+using tier4_autoware_utils::StopWatch;
+
+SceneModuleInterface::SceneModuleInterface(
+  const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock)
+: module_id_(module_id),
+  activated_(false),
+  safe_(false),
+  distance_(std::numeric_limits<double>::lowest()),
+  logger_(logger),
+  clock_(clock)
+{
+}
+
+size_t SceneModuleInterface::findEgoSegmentIndex(
+  const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points) const
+{
+  const auto & p = planner_data_;
+  return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
+    points, p->current_odometry->pose, p->ego_nearest_dist_threshold);
+}
+
+SceneModuleManagerInterface::SceneModuleManagerInterface(
+  rclcpp::Node & node, [[maybe_unused]] const char * module_name)
+: node_(node), clock_(node.get_clock()), logger_(node.get_logger())
+{
+  const auto ns = std::string("~/debug/") + module_name;
+  pub_debug_ = node.create_publisher<visualization_msgs::msg::MarkerArray>(ns, 1);
+  if (!node.has_parameter("is_publish_debug_path")) {
+    is_publish_debug_path_ = node.declare_parameter<bool>("is_publish_debug_path");
+  } else {
+    is_publish_debug_path_ = node.get_parameter("is_publish_debug_path").as_bool();
+  }
+  if (is_publish_debug_path_) {
+    pub_debug_path_ = node.create_publisher<autoware_auto_planning_msgs::msg::PathWithLaneId>(
+      std::string("~/debug/path_with_lane_id/") + module_name, 1);
+  }
+  pub_virtual_wall_ = node.create_publisher<visualization_msgs::msg::MarkerArray>(
+    std::string("~/virtual_wall/") + module_name, 5);
+  pub_velocity_factor_ = node.create_publisher<autoware_adapi_v1_msgs::msg::VelocityFactorArray>(
+    std::string("/planning/velocity_factors/") + module_name, 1);
+  pub_stop_reason_ =
+    node.create_publisher<tier4_planning_msgs::msg::StopReasonArray>("~/output/stop_reasons", 1);
+  pub_infrastructure_commands_ =
+    node.create_publisher<tier4_v2x_msgs::msg::InfrastructureCommandArray>(
+      "~/output/infrastructure_commands", 1);
+
+  processing_time_publisher_ = std::make_shared<DebugPublisher>(&node, "~/debug");
+}
+
+size_t SceneModuleManagerInterface::findEgoSegmentIndex(
+  const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points) const
+{
+  const auto & p = planner_data_;
+  return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
+    points, p->current_odometry->pose, p->ego_nearest_dist_threshold, p->ego_nearest_yaw_threshold);
+}
+
+void SceneModuleManagerInterface::updateSceneModuleInstances(
+  const std::shared_ptr<const PlannerData> & planner_data,
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
+{
+  planner_data_ = planner_data;
+
+  launchNewModules(path);
+  deleteExpiredModules(path);
+}
+
+void SceneModuleManagerInterface::modifyPathVelocity(
+  autoware_auto_planning_msgs::msg::PathWithLaneId * path)
+{
+  StopWatch<std::chrono::milliseconds> stop_watch;
+  stop_watch.tic("Total");
+  visualization_msgs::msg::MarkerArray debug_marker_array;
+  tier4_planning_msgs::msg::StopReasonArray stop_reason_array;
+  autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array;
+  stop_reason_array.header.frame_id = "map";
+  stop_reason_array.header.stamp = clock_->now();
+  velocity_factor_array.header.frame_id = "map";
+  velocity_factor_array.header.stamp = clock_->now();
+
+  tier4_v2x_msgs::msg::InfrastructureCommandArray infrastructure_command_array;
+  infrastructure_command_array.stamp = clock_->now();
+
+  first_stop_path_point_index_ = static_cast<int>(path->points.size()) - 1;
+  for (const auto & scene_module : scene_modules_) {
+    tier4_planning_msgs::msg::StopReason stop_reason;
+    scene_module->resetVelocityFactor();
+    scene_module->setPlannerData(planner_data_);
+    scene_module->modifyPathVelocity(path, &stop_reason);
+
+    // The velocity factor must be called after modifyPathVelocity.
+    const auto velocity_factor = scene_module->getVelocityFactor();
+    if (velocity_factor.type != VelocityFactor::UNKNOWN) {
+      velocity_factor_array.factors.emplace_back(velocity_factor);
+    }
+    if (stop_reason.reason != "") {
+      stop_reason_array.stop_reasons.emplace_back(stop_reason);
+    }
+
+    if (const auto command = scene_module->getInfrastructureCommand()) {
+      infrastructure_command_array.commands.push_back(*command);
+    }
+
+    if (scene_module->getFirstStopPathPointIndex() < first_stop_path_point_index_) {
+      first_stop_path_point_index_ = scene_module->getFirstStopPathPointIndex();
+    }
+
+    for (const auto & marker : scene_module->createDebugMarkerArray().markers) {
+      debug_marker_array.markers.push_back(marker);
+    }
+
+    virtual_wall_marker_creator_.add_virtual_walls(scene_module->createVirtualWalls());
+  }
+
+  if (!stop_reason_array.stop_reasons.empty()) {
+    pub_stop_reason_->publish(stop_reason_array);
+  }
+  pub_velocity_factor_->publish(velocity_factor_array);
+  pub_infrastructure_commands_->publish(infrastructure_command_array);
+  pub_debug_->publish(debug_marker_array);
+  if (is_publish_debug_path_) {
+    autoware_auto_planning_msgs::msg::PathWithLaneId debug_path;
+    debug_path.header = path->header;
+    debug_path.points = path->points;
+    pub_debug_path_->publish(debug_path);
+  }
+  pub_virtual_wall_->publish(virtual_wall_marker_creator_.create_markers(clock_->now()));
+  processing_time_publisher_->publish<Float64Stamped>(
+    std::string(getModuleName()) + "/processing_time_ms", stop_watch.toc("Total"));
+}
+
+void SceneModuleManagerInterface::deleteExpiredModules(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
+{
+  const auto isModuleExpired = getModuleExpiredFunction(path);
+
+  // Copy container to avoid iterator corruption
+  // due to scene_modules_.erase() in unregisterModule()
+  const auto copied_scene_modules = scene_modules_;
+
+  for (const auto & scene_module : copied_scene_modules) {
+    if (isModuleExpired(scene_module)) {
+      unregisterModule(scene_module);
+    }
+  }
+}
+
+void SceneModuleManagerInterface::registerModule(
+  const std::shared_ptr<SceneModuleInterface> & scene_module)
+{
+  RCLCPP_INFO(
+    logger_, "register task: module = %s, id = %lu", getModuleName(), scene_module->getModuleId());
+  registered_module_id_set_.emplace(scene_module->getModuleId());
+  scene_modules_.insert(scene_module);
+}
+
+void SceneModuleManagerInterface::unregisterModule(
+  const std::shared_ptr<SceneModuleInterface> & scene_module)
+{
+  RCLCPP_INFO(
+    logger_, "unregister task: module = %s, id = %lu", getModuleName(),
+    scene_module->getModuleId());
+  registered_module_id_set_.erase(scene_module->getModuleId());
+  scene_modules_.erase(scene_module);
+}
+
+SceneModuleManagerInterfaceWithRTC::SceneModuleManagerInterfaceWithRTC(
+  rclcpp::Node & node, const char * module_name, const bool enable_rtc)
+: SceneModuleManagerInterface(node, module_name), rtc_interface_(&node, module_name, enable_rtc)
+{
+}
+
+void SceneModuleManagerInterfaceWithRTC::plan(
+  autoware_auto_planning_msgs::msg::PathWithLaneId * path)
+{
+  setActivation();
+  modifyPathVelocity(path);
+  sendRTC(path->header.stamp);
+}
+
+void SceneModuleManagerInterfaceWithRTC::sendRTC(const Time & stamp)
+{
+  for (const auto & scene_module : scene_modules_) {
+    const UUID uuid = getUUID(scene_module->getModuleId());
+    updateRTCStatus(uuid, scene_module->isSafe(), scene_module->getDistance(), stamp);
+  }
+  publishRTCStatus(stamp);
+}
+
+void SceneModuleManagerInterfaceWithRTC::setActivation()
+{
+  for (const auto & scene_module : scene_modules_) {
+    const UUID uuid = getUUID(scene_module->getModuleId());
+    scene_module->setActivation(rtc_interface_.isActivated(uuid));
+  }
+}
+
+UUID SceneModuleManagerInterfaceWithRTC::getUUID(const int64_t & module_id) const
+{
+  if (map_uuid_.count(module_id) == 0) {
+    const UUID uuid;
+    return uuid;
+  }
+  return map_uuid_.at(module_id);
+}
+
+void SceneModuleManagerInterfaceWithRTC::generateUUID(const int64_t & module_id)
+{
+  map_uuid_.insert({module_id, tier4_autoware_utils::generateUUID()});
+}
+
+void SceneModuleManagerInterfaceWithRTC::removeUUID(const int64_t & module_id)
+{
+  const auto result = map_uuid_.erase(module_id);
+  if (result == 0) {
+    RCLCPP_WARN_STREAM(logger_, "[removeUUID] module_id = " << module_id << " is not registered.");
+  }
+}
+
+void SceneModuleManagerInterfaceWithRTC::deleteExpiredModules(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
+{
+  const auto isModuleExpired = getModuleExpiredFunction(path);
+
+  // Copy container to avoid iterator corruption
+  // due to scene_modules_.erase() in unregisterModule()
+  const auto copied_scene_modules = scene_modules_;
+
+  for (const auto & scene_module : copied_scene_modules) {
+    if (isModuleExpired(scene_module)) {
+      removeRTCStatus(getUUID(scene_module->getModuleId()));
+      removeUUID(scene_module->getModuleId());
+      unregisterModule(scene_module);
+    }
+  }
+}
+
+}  // namespace behavior_velocity_planner
diff --git a/planning/behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp b/planning/behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp
new file mode 100644
index 0000000000000..05f634a81c5b7
--- /dev/null
+++ b/planning/behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp
@@ -0,0 +1,138 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <behavior_velocity_planner_common/utilization/arc_lane_util.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+
+#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
+
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_eigen/tf2_eigen.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_eigen/tf2_eigen.hpp>
+
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
+#include <algorithm>
+#include <memory>
+#include <utility>
+#include <vector>
+
+namespace
+{
+geometry_msgs::msg::Point operator+(
+  const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2)
+{
+  geometry_msgs::msg::Point p;
+  p.x = p1.x + p2.x;
+  p.y = p1.y + p2.y;
+  p.z = p1.z + p2.z;
+
+  return p;
+}
+
+geometry_msgs::msg::Point operator*(const geometry_msgs::msg::Point & p, const double v)
+{
+  geometry_msgs::msg::Point multiplied_p;
+  multiplied_p.x = p.x * v;
+  multiplied_p.y = p.y * v;
+  multiplied_p.z = p.z * v;
+
+  return multiplied_p;
+}
+
+/*
+geometry_msgs::msg::Point operator*(const double v, const geometry_msgs::msg::Point & p)
+{
+return p * v;
+}
+*/
+}  // namespace
+
+namespace behavior_velocity_planner::arc_lane_utils
+{
+
+double calcSignedDistance(const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Point & p2)
+{
+  Eigen::Affine3d map2p1;
+  tf2::fromMsg(p1, map2p1);
+  const auto basecoords_p2 = map2p1.inverse() * Eigen::Vector3d(p2.x, p2.y, p2.z);
+  return basecoords_p2.x() >= 0 ? basecoords_p2.norm() : -basecoords_p2.norm();
+}
+
+// calculate one collision point between the line (from p1 to p2) and the line (from p3 to p4)
+
+boost::optional<geometry_msgs::msg::Point> checkCollision(
+  const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2,
+  const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4)
+{
+  const double det = (p2.x - p1.x) * (p4.y - p3.y) - (p2.y - p1.y) * (p4.x - p3.x);
+
+  if (det == 0.0) {
+    // collision is not one point.
+    return boost::none;
+  }
+
+  const double t1 = ((p4.y - p3.y) * (p4.x - p1.x) - (p4.x - p3.x) * (p4.y - p1.y)) / det;
+  const double t2 = ((p2.x - p1.x) * (p4.y - p1.y) - (p2.y - p1.y) * (p4.x - p1.x)) / det;
+
+  // check collision is outside the segment line
+  if (t1 < 0.0 || 1.0 < t1 || t2 < 0.0 || 1.0 < t2) {
+    return boost::none;
+  }
+
+  return p1 * (1.0 - t1) + p2 * t1;
+}
+
+boost::optional<PathIndexWithOffset> findOffsetSegment(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t index,
+  const double offset)
+{
+  if (offset >= 0) {
+    return findForwardOffsetSegment(path, index, offset);
+  }
+
+  return findBackwardOffsetSegment(path, index, -offset);
+}
+
+boost::optional<PathIndexWithPose> createTargetPoint(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line,
+  const size_t lane_id, const double margin, const double vehicle_offset)
+{
+  // Find collision segment
+  const auto collision_segment = findCollisionSegment(path, stop_line, lane_id);
+  if (!collision_segment) {
+    // No collision
+    return {};
+  }
+
+  // Calculate offset length from stop line
+  // Use '-' to make the positive direction is forward
+  const double offset_length = -(margin + vehicle_offset);
+
+  // Find offset segment
+  const auto offset_segment = findOffsetSegment(path, *collision_segment, offset_length);
+  if (!offset_segment) {
+    // No enough path length
+    return {};
+  }
+
+  const auto target_pose = calcTargetPose(path, *offset_segment);
+
+  const auto front_idx = offset_segment->first;
+  return std::make_pair(front_idx, target_pose);
+}
+}  // namespace behavior_velocity_planner::arc_lane_utils
diff --git a/planning/behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp b/planning/behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp
new file mode 100644
index 0000000000000..903cf5aab80e8
--- /dev/null
+++ b/planning/behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp
@@ -0,0 +1,66 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp>
+
+#include <boost/geometry/algorithms/correct.hpp>
+
+#include <algorithm>
+#include <vector>
+namespace behavior_velocity_planner
+{
+
+Polygon2d lines2polygon(const LineString2d & left_line, const LineString2d & right_line)
+{
+  Polygon2d polygon;
+
+  polygon.outer().push_back(left_line.front());
+
+  for (auto itr = right_line.begin(); itr != right_line.end(); ++itr) {
+    polygon.outer().push_back(*itr);
+  }
+
+  for (auto itr = left_line.rbegin(); itr != left_line.rend(); ++itr) {
+    polygon.outer().push_back(*itr);
+  }
+
+  bg::correct(polygon);
+  return polygon;
+}
+
+Polygon2d upScalePolygon(
+  const geometry_msgs::msg::Point & position, const Polygon2d & polygon, const double scale)
+{
+  Polygon2d transformed_polygon;
+  // upscale
+  for (size_t i = 0; i < polygon.outer().size(); i++) {
+    const double upscale_x = (polygon.outer().at(i).x() - position.x) * scale + position.x;
+    const double upscale_y = (polygon.outer().at(i).y() - position.y) * scale + position.y;
+    transformed_polygon.outer().emplace_back(Point2d(upscale_x, upscale_y));
+  }
+  return transformed_polygon;
+}
+
+geometry_msgs::msg::Polygon toGeomPoly(const Polygon2d & polygon)
+{
+  geometry_msgs::msg::Polygon polygon_msg;
+  geometry_msgs::msg::Point32 point_msg;
+  for (const auto & p : polygon.outer()) {
+    point_msg.x = p.x();
+    point_msg.y = p.y();
+    polygon_msg.points.push_back(point_msg);
+  }
+  return polygon_msg;
+}
+}  // namespace behavior_velocity_planner
diff --git a/planning/behavior_velocity_planner_common/src/utilization/debug.cpp b/planning/behavior_velocity_planner_common/src/utilization/debug.cpp
index 845b93576bec8..6ff2ef53176c4 100644
--- a/planning/behavior_velocity_planner_common/src/utilization/debug.cpp
+++ b/planning/behavior_velocity_planner_common/src/utilization/debug.cpp
@@ -13,8 +13,8 @@
 // limitations under the License.
 
 #include <behavior_velocity_planner_common/utilization/debug.hpp>
+#include <behavior_velocity_planner_common/utilization/util.hpp>
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
-
 namespace behavior_velocity_planner
 {
 namespace debug
diff --git a/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp b/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp
index 74bd2f005dd89..59c9e4861fbfd 100644
--- a/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp
+++ b/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp
@@ -13,21 +13,10 @@
 // limitations under the License.
 
 #include <behavior_velocity_planner_common/utilization/path_utilization.hpp>
-#include <interpolation/linear_interpolation.hpp>
-#include <interpolation/spline_interpolation.hpp>
-#include <interpolation/zero_order_hold.hpp>
 #include <motion_utils/resample/resample.hpp>
 #include <motion_utils/trajectory/trajectory.hpp>
 #include <rclcpp/rclcpp.hpp>
 
-#include <tf2/LinearMath/Quaternion.h>
-
-#ifdef ROS_DISTRO_GALACTIC
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#else
-#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
-#endif
-
 #include <algorithm>
 #include <memory>
 #include <vector>
diff --git a/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp b/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp
new file mode 100644
index 0000000000000..71cdd9650a598
--- /dev/null
+++ b/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp
@@ -0,0 +1,121 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// #include <behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp>
+#include <behavior_velocity_planner_common/utilization/trajectory_utils.hpp>
+#include <motion_utils/trajectory/trajectory.hpp>
+#include <motion_velocity_smoother/trajectory_utils.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp>
+#include <geometry_msgs/msg/quaternion.hpp>
+
+#include <tf2/utils.h>
+
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
+#include <memory>
+
+namespace behavior_velocity_planner
+{
+using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
+using autoware_auto_planning_msgs::msg::PathWithLaneId;
+using autoware_auto_planning_msgs::msg::Trajectory;
+using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+using TrajectoryPoints = std::vector<TrajectoryPoint>;
+using geometry_msgs::msg::Quaternion;
+using TrajectoryPointWithIdx = std::pair<TrajectoryPoint, size_t>;
+
+TrajectoryPoints convertPathToTrajectoryPoints(const PathWithLaneId & path)
+{
+  TrajectoryPoints tps;
+  for (const auto & p : path.points) {
+    TrajectoryPoint tp;
+    tp.pose = p.point.pose;
+    tp.longitudinal_velocity_mps = p.point.longitudinal_velocity_mps;
+    // since path point doesn't have acc for now
+    tp.acceleration_mps2 = 0;
+    tps.emplace_back(tp);
+  }
+  return tps;
+}
+PathWithLaneId convertTrajectoryPointsToPath(const TrajectoryPoints & trajectory)
+{
+  PathWithLaneId path;
+  for (const auto & p : trajectory) {
+    PathPointWithLaneId pp;
+    pp.point.pose = p.pose;
+    pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps;
+    path.points.emplace_back(pp);
+  }
+  return path;
+}
+
+Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio)
+{
+  tf2::Quaternion q_from, q_to;
+  tf2::fromMsg(o_from, q_from);
+  tf2::fromMsg(o_to, q_to);
+
+  const auto q_interpolated = q_from.slerp(q_to, ratio);
+  return tf2::toMsg(q_interpolated);
+}
+
+//! smooth path point with lane id starts from ego position on path to the path end
+bool smoothPath(
+  const PathWithLaneId & in_path, PathWithLaneId & out_path,
+  const std::shared_ptr<const PlannerData> & planner_data)
+{
+  const geometry_msgs::msg::Pose current_pose = planner_data->current_odometry->pose;
+  const double v0 = planner_data->current_velocity->twist.linear.x;
+  const double a0 = planner_data->current_acceleration->accel.accel.linear.x;
+  const auto & external_v_limit = planner_data->external_velocity_limit;
+  const auto & smoother = planner_data->velocity_smoother_;
+
+  auto trajectory = convertPathToTrajectoryPoints(in_path);
+  if (external_v_limit) {
+    motion_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit(
+      0, trajectory.size(), external_v_limit->max_velocity, trajectory);
+  }
+  const auto traj_lateral_acc_filtered = smoother->applyLateralAccelerationFilter(trajectory);
+
+  // Resample trajectory with ego-velocity based interval distances
+  auto traj_resampled = smoother->resampleTrajectory(
+    traj_lateral_acc_filtered, v0, current_pose, planner_data->ego_nearest_dist_threshold,
+    planner_data->ego_nearest_yaw_threshold);
+  const size_t traj_resampled_closest = motion_utils::findFirstNearestIndexWithSoftConstraints(
+    traj_resampled, current_pose, planner_data->ego_nearest_dist_threshold,
+    planner_data->ego_nearest_yaw_threshold);
+  std::vector<TrajectoryPoints> debug_trajectories;
+  // Clip trajectory from closest point
+  TrajectoryPoints clipped;
+  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)) {
+    std::cerr << "[behavior_velocity][trajectory_utils]: failed to smooth" << std::endl;
+    return false;
+  }
+  traj_smoothed.insert(
+    traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest);
+
+  out_path = convertTrajectoryPointsToPath(traj_smoothed);
+  return true;
+}
+
+}  // namespace behavior_velocity_planner
diff --git a/planning/behavior_velocity_planner_common/src/utilization/util.cpp b/planning/behavior_velocity_planner_common/src/utilization/util.cpp
index a45331f53d7c2..a3a5a8a169759 100644
--- a/planning/behavior_velocity_planner_common/src/utilization/util.cpp
+++ b/planning/behavior_velocity_planner_common/src/utilization/util.cpp
@@ -12,26 +12,50 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp>
 #include <behavior_velocity_planner_common/utilization/util.hpp>
 #include <lanelet2_extension/utility/query.hpp>
+#include <motion_utils/trajectory/trajectory.hpp>
+
+#include <autoware_auto_planning_msgs/msg/path_point.hpp>
+
+#include <boost/geometry/algorithms/correct.hpp>
 
 #include <lanelet2_core/geometry/Polygon.h>
 #include <lanelet2_routing/RoutingGraph.h>
+#include <tf2/utils.h>
+
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
 
 #include <algorithm>
 #include <limits>
 #include <string>
 #include <vector>
 
-namespace behavior_velocity_planner
-{
-namespace planning_utils
+namespace
 {
-Point2d calculateOffsetPoint2d(const Pose & pose, const double offset_x, const double offset_y)
+size_t calcPointIndexFromSegmentIndex(
+  const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points,
+  const geometry_msgs::msg::Point & point, const size_t seg_idx)
 {
-  return to_bg2d(calcOffsetPose(pose, offset_x, offset_y, 0.0));
+  const size_t prev_point_idx = seg_idx;
+  const size_t next_point_idx = seg_idx + 1;
+
+  const double prev_dist = tier4_autoware_utils::calcDistance2d(point, points.at(prev_point_idx));
+  const double next_dist = tier4_autoware_utils::calcDistance2d(point, points.at(next_point_idx));
+
+  if (prev_dist < next_dist) {
+    return prev_point_idx;
+  }
+  return next_point_idx;
 }
 
+using autoware_auto_planning_msgs::msg::PathPoint;
+
 PathPoint getLerpPathPointWithLaneId(const PathPoint p0, const PathPoint p1, const double ratio)
 {
   auto lerp = [](const double a, const double b, const double t) { return a + t * (b - a); };
@@ -42,6 +66,71 @@ PathPoint getLerpPathPointWithLaneId(const PathPoint p0, const PathPoint p1, con
   return p;
 }
 
+geometry_msgs::msg::Pose transformRelCoordinate2D(
+  const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin)
+{
+  // translation
+  geometry_msgs::msg::Point trans_p;
+  trans_p.x = target.position.x - origin.position.x;
+  trans_p.y = target.position.y - origin.position.y;
+
+  // rotation (use inverse matrix of rotation)
+  double yaw = tf2::getYaw(origin.orientation);
+
+  geometry_msgs::msg::Pose res;
+  res.position.x = (std::cos(yaw) * trans_p.x) + (std::sin(yaw) * trans_p.y);
+  res.position.y = ((-1.0) * std::sin(yaw) * trans_p.x) + (std::cos(yaw) * trans_p.y);
+  res.position.z = target.position.z - origin.position.z;
+  res.orientation =
+    tier4_autoware_utils::createQuaternionFromYaw(tf2::getYaw(target.orientation) - yaw);
+
+  return res;
+}
+
+}  // namespace
+
+namespace behavior_velocity_planner
+{
+namespace planning_utils
+{
+using autoware_auto_planning_msgs::msg::PathPoint;
+using motion_utils::calcLongitudinalOffsetToSegment;
+using motion_utils::calcSignedArcLength;
+using motion_utils::validateNonEmpty;
+using tier4_autoware_utils::calcAzimuthAngle;
+using tier4_autoware_utils::calcDistance2d;
+using tier4_autoware_utils::calcOffsetPose;
+using tier4_autoware_utils::calcSquaredDistance2d;
+using tier4_autoware_utils::createQuaternionFromYaw;
+using tier4_autoware_utils::getPoint;
+
+size_t calcSegmentIndexFromPointIndex(
+  const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points,
+  const geometry_msgs::msg::Point & point, const size_t idx)
+{
+  if (idx == 0) {
+    return 0;
+  }
+  if (idx == points.size() - 1) {
+    return idx - 1;
+  }
+  if (points.size() < 3) {
+    return 0;
+  }
+
+  const double offset_to_seg = motion_utils::calcLongitudinalOffsetToSegment(points, idx, point);
+  if (0 < offset_to_seg) {
+    return idx;
+  }
+  return idx - 1;
+}
+
+Point2d calculateOffsetPoint2d(
+  const geometry_msgs::msg::Pose & pose, const double offset_x, const double offset_y)
+{
+  return to_bg2d(calcOffsetPose(pose, offset_x, offset_y, 0.0));
+}
+
 bool createDetectionAreaPolygons(
   Polygons2d & da_polys, const PathWithLaneId & path, const geometry_msgs::msg::Pose & target_pose,
   const size_t target_seg_idx, const DetectionRange & da_range, const double obstacle_vel_mps,
@@ -171,30 +260,6 @@ void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, BasicPolygons
   }
 }
 
-SearchRangeIndex getPathIndexRangeIncludeLaneId(const PathWithLaneId & path, const int64_t lane_id)
-{
-  /**
-   * @brief find path index range include given lane_id
-   *        |<-min_idx       |<-max_idx
-   *  ------|oooooooooooooooo|-------
-   */
-  SearchRangeIndex search_range = {0, path.points.size() - 1};
-  bool found_first_idx = false;
-  for (size_t i = 0; i < path.points.size(); i++) {
-    const auto & p = path.points.at(i);
-    for (const auto & id : p.lane_ids) {
-      if (id == lane_id) {
-        if (!found_first_idx) {
-          search_range.min_idx = i;
-          found_first_idx = true;
-        }
-        search_range.max_idx = i;
-      }
-    }
-  }
-  return search_range;
-}
-
 void setVelocityFromIndex(const size_t begin_idx, const double vel, PathWithLaneId * input)
 {
   for (size_t i = begin_idx; i < input->points.size(); ++i) {
@@ -232,7 +297,7 @@ void insertVelocity(
 
 bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin)
 {
-  geometry_msgs::msg::Pose p = planning_utils::transformRelCoordinate2D(target, origin);
+  geometry_msgs::msg::Pose p = transformRelCoordinate2D(target, origin);
   const bool is_target_ahead = (p.position.x > 0.0);
   return is_target_ahead;
 }
@@ -317,47 +382,6 @@ lanelet::ConstLanelet generatePathLanelet(
   return lanelet::Lanelet(lanelet::InvalId, left, right);
 }
 
-geometry_msgs::msg::Pose transformRelCoordinate2D(
-  const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin)
-{
-  // translation
-  geometry_msgs::msg::Point trans_p;
-  trans_p.x = target.position.x - origin.position.x;
-  trans_p.y = target.position.y - origin.position.y;
-
-  // rotation (use inverse matrix of rotation)
-  double yaw = tf2::getYaw(origin.orientation);
-
-  geometry_msgs::msg::Pose res;
-  res.position.x = (std::cos(yaw) * trans_p.x) + (std::sin(yaw) * trans_p.y);
-  res.position.y = ((-1.0) * std::sin(yaw) * trans_p.x) + (std::cos(yaw) * trans_p.y);
-  res.position.z = target.position.z - origin.position.z;
-  res.orientation =
-    tier4_autoware_utils::createQuaternionFromYaw(tf2::getYaw(target.orientation) - yaw);
-
-  return res;
-}
-
-geometry_msgs::msg::Pose transformAbsCoordinate2D(
-  const geometry_msgs::msg::Pose & relative, const geometry_msgs::msg::Pose & origin)
-{
-  // rotation
-  geometry_msgs::msg::Point rot_p;
-  double yaw = tf2::getYaw(origin.orientation);
-  rot_p.x = (std::cos(yaw) * relative.position.x) + (-std::sin(yaw) * relative.position.y);
-  rot_p.y = (std::sin(yaw) * relative.position.x) + (std::cos(yaw) * relative.position.y);
-
-  // translation
-  geometry_msgs::msg::Pose absolute;
-  absolute.position.x = rot_p.x + origin.position.x;
-  absolute.position.y = rot_p.y + origin.position.y;
-  absolute.position.z = relative.position.z + origin.position.z;
-  absolute.orientation =
-    tier4_autoware_utils::createQuaternionFromYaw(tf2::getYaw(relative.orientation) + yaw);
-
-  return absolute;
-}
-
 double calcJudgeLineDistWithAccLimit(
   const double velocity, const double max_stop_acceleration, const double delay_response_time)
 {
diff --git a/planning/behavior_velocity_planner_common/src/velocity_factor_interface.cpp b/planning/behavior_velocity_planner_common/src/velocity_factor_interface.cpp
new file mode 100644
index 0000000000000..35c4bc00d8096
--- /dev/null
+++ b/planning/behavior_velocity_planner_common/src/velocity_factor_interface.cpp
@@ -0,0 +1,33 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <behavior_velocity_planner_common/velocity_factor_interface.hpp>
+#include <motion_utils/trajectory/trajectory.hpp>
+namespace behavior_velocity_planner
+{
+void VelocityFactorInterface::set(
+  const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points,
+  const Pose & curr_pose, const Pose & stop_pose, const VelocityFactorStatus status,
+  const std::string detail)
+{
+  const auto & curr_point = curr_pose.position;
+  const auto & stop_point = stop_pose.position;
+  velocity_factor_.type = type_;
+  velocity_factor_.pose = stop_pose;
+  velocity_factor_.distance = motion_utils::calcSignedArcLength(points, curr_point, stop_point);
+  velocity_factor_.status = status;
+  velocity_factor_.detail = detail;
+}
+
+}  // namespace behavior_velocity_planner
diff --git a/planning/behavior_velocity_run_out_module/CMakeLists.txt b/planning/behavior_velocity_run_out_module/CMakeLists.txt
index 89745a18065a3..21758c4d0198c 100644
--- a/planning/behavior_velocity_run_out_module/CMakeLists.txt
+++ b/planning/behavior_velocity_run_out_module/CMakeLists.txt
@@ -12,6 +12,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/scene.cpp
   src/state_machine.cpp
   src/utils.cpp
+  src/path_utils.cpp
 )
 
 ament_auto_package(INSTALL_TO_SHARE config)
diff --git a/planning/behavior_velocity_run_out_module/src/debug.cpp b/planning/behavior_velocity_run_out_module/src/debug.cpp
index 90f69e0faae5e..c0d026b5ffaf4 100644
--- a/planning/behavior_velocity_run_out_module/src/debug.cpp
+++ b/planning/behavior_velocity_run_out_module/src/debug.cpp
@@ -17,6 +17,7 @@
 #include "scene.hpp"
 
 #include <motion_utils/marker/virtual_wall_marker_creator.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
 
 using tier4_autoware_utils::appendMarkerArray;
@@ -75,7 +76,6 @@ RunOutDebug::RunOutDebug(rclcpp::Node & node) : node_(node)
   pub_debug_values_ =
     node.create_publisher<Float32MultiArrayStamped>("~/debug/run_out/debug_values", 1);
   pub_accel_reason_ = node.create_publisher<Int32Stamped>("~/debug/run_out/accel_reason", 1);
-  pub_debug_trajectory_ = node.create_publisher<Trajectory>("~/debug/run_out/trajectory", 1);
   pub_debug_pointcloud_ = node.create_publisher<PointCloud2>(
     "~/debug/run_out/filtered_pointcloud", rclcpp::SensorDataQoS().keep_last(1));
 }
@@ -305,11 +305,6 @@ void RunOutDebug::publishDebugValue()
   pub_accel_reason_->publish(accel_reason);
 }
 
-void RunOutDebug::publishDebugTrajectory(const Trajectory & trajectory)
-{
-  pub_debug_trajectory_->publish(trajectory);
-}
-
 void RunOutDebug::publishFilteredPointCloud(
   const pcl::PointCloud<pcl::PointXYZ> & pointcloud, const std_msgs::msg::Header header)
 {
diff --git a/planning/behavior_velocity_run_out_module/src/debug.hpp b/planning/behavior_velocity_run_out_module/src/debug.hpp
index 0dcd3099b616a..91656d542ea8e 100644
--- a/planning/behavior_velocity_run_out_module/src/debug.hpp
+++ b/planning/behavior_velocity_run_out_module/src/debug.hpp
@@ -26,7 +26,6 @@
 #include <vector>
 namespace behavior_velocity_planner
 {
-using autoware_auto_planning_msgs::msg::Trajectory;
 using sensor_msgs::msg::PointCloud2;
 using tier4_debug_msgs::msg::Float32MultiArrayStamped;
 using tier4_debug_msgs::msg::Int32Stamped;
@@ -113,7 +112,6 @@ class RunOutDebug
     const double travel_time, const geometry_msgs::msg::Pose pose, const float lateral_offset);
   void setAccelReason(const AccelReason & accel_reason);
   void publishDebugValue();
-  void publishDebugTrajectory(const Trajectory & trajectory);
   void publishFilteredPointCloud(const PointCloud2 & pointcloud);
   void publishFilteredPointCloud(
     const pcl::PointCloud<pcl::PointXYZ> & pointcloud, const std_msgs::msg::Header header);
@@ -130,7 +128,6 @@ class RunOutDebug
   rclcpp::Node & node_;
   rclcpp::Publisher<Float32MultiArrayStamped>::SharedPtr pub_debug_values_;
   rclcpp::Publisher<Int32Stamped>::SharedPtr pub_accel_reason_;
-  rclcpp::Publisher<Trajectory>::SharedPtr pub_debug_trajectory_;
   rclcpp::Publisher<PointCloud2>::SharedPtr pub_debug_pointcloud_;
   std::vector<geometry_msgs::msg::Point> collision_points_;
   std::vector<geometry_msgs::msg::Point> nearest_collision_point_;
diff --git a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp
index 728e0fbad92a5..16013efc5fd97 100644
--- a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp
+++ b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp
@@ -14,11 +14,15 @@
 
 #include "dynamic_obstacle.hpp"
 
+#include <motion_utils/trajectory/trajectory.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <tier4_autoware_utils/geometry/pose_deviation.hpp>
 #include <tier4_autoware_utils/math/unit_conversion.hpp>
 #include <tier4_autoware_utils/transform/transforms.hpp>
 
+#include <boost/geometry/algorithms/covered_by.hpp>
+#include <boost/geometry/algorithms/envelope.hpp>
+
 #include <pcl/filters/voxel_grid.h>
 
 #include <algorithm>
diff --git a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp
index 7eece8fac29af..5217193c9673e 100644
--- a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp
+++ b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp
@@ -36,6 +36,8 @@
 #include <tf2_eigen/tf2_eigen.hpp>
 #endif
 
+#include <tf2_ros/transform_listener.h>
+
 #include <memory>
 #include <vector>
 
diff --git a/planning/behavior_velocity_run_out_module/src/path_utils.cpp b/planning/behavior_velocity_run_out_module/src/path_utils.cpp
new file mode 100644
index 0000000000000..1cec20297683c
--- /dev/null
+++ b/planning/behavior_velocity_run_out_module/src/path_utils.cpp
@@ -0,0 +1,39 @@
+// Copyright 2023 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "path_utils.hpp"
+
+#include <motion_utils/trajectory/trajectory.hpp>
+namespace behavior_velocity_planner::run_out_utils
+{
+geometry_msgs::msg::Point findLongitudinalNearestPoint(
+  const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points,
+  const geometry_msgs::msg::Point & src_point,
+  const std::vector<geometry_msgs::msg::Point> & target_points)
+{
+  float min_dist = std::numeric_limits<float>::max();
+  geometry_msgs::msg::Point min_dist_point{};
+
+  for (const auto & p : target_points) {
+    const float dist = motion_utils::calcSignedArcLength(points, src_point, p);
+    if (dist < min_dist) {
+      min_dist = dist;
+      min_dist_point = p;
+    }
+  }
+
+  return min_dist_point;
+}
+
+}  // namespace behavior_velocity_planner::run_out_utils
diff --git a/planning/behavior_velocity_run_out_module/src/path_utils.hpp b/planning/behavior_velocity_run_out_module/src/path_utils.hpp
index 11ed2b7282ab3..92aca946c13ef 100644
--- a/planning/behavior_velocity_run_out_module/src/path_utils.hpp
+++ b/planning/behavior_velocity_run_out_module/src/path_utils.hpp
@@ -15,6 +15,9 @@
 #ifndef PATH_UTILS_HPP_
 #define PATH_UTILS_HPP_
 
+#include <autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp>
+#include <geometry_msgs/msg/point.hpp>
+
 #include <algorithm>
 #include <limits>
 #include <memory>
@@ -27,24 +30,10 @@ namespace behavior_velocity_planner
 namespace run_out_utils
 {
 
-template <class T>
 geometry_msgs::msg::Point findLongitudinalNearestPoint(
-  const T & points, const geometry_msgs::msg::Point & src_point,
-  const std::vector<geometry_msgs::msg::Point> & target_points)
-{
-  float min_dist = std::numeric_limits<float>::max();
-  geometry_msgs::msg::Point min_dist_point{};
-
-  for (const auto & p : target_points) {
-    const float dist = motion_utils::calcSignedArcLength(points, src_point, p);
-    if (dist < min_dist) {
-      min_dist = dist;
-      min_dist_point = p;
-    }
-  }
-
-  return min_dist_point;
-}
+  const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points,
+  const geometry_msgs::msg::Point & src_point,
+  const std::vector<geometry_msgs::msg::Point> & target_points);
 
 }  // namespace run_out_utils
 }  // namespace behavior_velocity_planner
diff --git a/planning/behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_run_out_module/src/scene.cpp
index b062bf9e3175e..b91d1d1fe1ae7 100644
--- a/planning/behavior_velocity_run_out_module/src/scene.cpp
+++ b/planning/behavior_velocity_run_out_module/src/scene.cpp
@@ -19,6 +19,10 @@
 #include <behavior_velocity_planner_common/utilization/trajectory_utils.hpp>
 #include <behavior_velocity_planner_common/utilization/util.hpp>
 #include <motion_utils/distance/distance.hpp>
+#include <motion_utils/trajectory/trajectory.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+
+#include <boost/geometry/algorithms/intersection.hpp>
 
 #include <algorithm>
 #include <limits>
diff --git a/planning/behavior_velocity_run_out_module/src/utils.cpp b/planning/behavior_velocity_run_out_module/src/utils.cpp
index 71ab07922b27d..61396da250c8d 100644
--- a/planning/behavior_velocity_run_out_module/src/utils.cpp
+++ b/planning/behavior_velocity_run_out_module/src/utils.cpp
@@ -17,9 +17,22 @@
 #include <motion_utils/distance/distance.hpp>
 #include <motion_utils/trajectory/path_with_lane_id.hpp>
 
+#include <boost/geometry/algorithms/correct.hpp>
+#include <boost/geometry/algorithms/intersects.hpp>
+#include <boost/geometry/algorithms/make.hpp>
+
+#include <tf2/utils.h>
+
 #include <algorithm>
 #include <limits>
 
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+#include <motion_utils/trajectory/trajectory.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
 namespace behavior_velocity_planner
 {
 namespace run_out_utils
diff --git a/planning/behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_run_out_module/src/utils.hpp
index 74bc2489d7c32..8fc3d48de4858 100644
--- a/planning/behavior_velocity_run_out_module/src/utils.hpp
+++ b/planning/behavior_velocity_run_out_module/src/utils.hpp
@@ -19,6 +19,8 @@
 #include <behavior_velocity_planner_common/utilization/util.hpp>
 #include <vehicle_info_util/vehicle_info_util.hpp>
 
+#include <autoware_auto_perception_msgs/msg/shape.hpp>
+#include <autoware_auto_planning_msgs/msg/path_point.hpp>
 #include <tier4_debug_msgs/msg/float32_stamped.hpp>
 
 #include <string>
@@ -31,6 +33,8 @@ namespace run_out_utils
 namespace bg = boost::geometry;
 using autoware_auto_perception_msgs::msg::ObjectClassification;
 using autoware_auto_perception_msgs::msg::PredictedObjects;
+using autoware_auto_perception_msgs::msg::Shape;
+using autoware_auto_planning_msgs::msg::PathPoint;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using tier4_autoware_utils::Box2d;
 using tier4_autoware_utils::LineString2d;
diff --git a/planning/behavior_velocity_speed_bump_module/src/debug.cpp b/planning/behavior_velocity_speed_bump_module/src/debug.cpp
index a2f78dfe85cd3..7409c24e7e8bf 100644
--- a/planning/behavior_velocity_speed_bump_module/src/debug.cpp
+++ b/planning/behavior_velocity_speed_bump_module/src/debug.cpp
@@ -18,6 +18,7 @@
 #include <lanelet2_extension/regulatory_elements/speed_bump.hpp>
 #include <motion_utils/marker/marker_helper.hpp>
 #include <motion_utils/marker/virtual_wall_marker_creator.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
 
 #include <vector>
diff --git a/planning/behavior_velocity_speed_bump_module/src/manager.cpp b/planning/behavior_velocity_speed_bump_module/src/manager.cpp
index dacc99d55b68e..5949357c90ff3 100644
--- a/planning/behavior_velocity_speed_bump_module/src/manager.cpp
+++ b/planning/behavior_velocity_speed_bump_module/src/manager.cpp
@@ -14,6 +14,8 @@
 
 #include "manager.hpp"
 
+#include <behavior_velocity_planner_common/utilization/util.hpp>
+#include <lanelet2_extension/regulatory_elements/speed_bump.hpp>
 #include <lanelet2_extension/utility/query.hpp>
 #include <tier4_autoware_utils/ros/parameter.hpp>
 
diff --git a/planning/behavior_velocity_stop_line_module/src/debug.cpp b/planning/behavior_velocity_stop_line_module/src/debug.cpp
index 8d7de00635d4f..f63bdca5068a2 100644
--- a/planning/behavior_velocity_stop_line_module/src/debug.cpp
+++ b/planning/behavior_velocity_stop_line_module/src/debug.cpp
@@ -16,8 +16,8 @@
 
 #include <behavior_velocity_planner_common/utilization/util.hpp>
 #include <motion_utils/marker/virtual_wall_marker_creator.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
-
 #ifdef ROS_DISTRO_GALACTIC
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #else
diff --git a/planning/behavior_velocity_traffic_light_module/src/debug.cpp b/planning/behavior_velocity_traffic_light_module/src/debug.cpp
index 6afd5381ce315..9a0ba4f37c3c0 100644
--- a/planning/behavior_velocity_traffic_light_module/src/debug.cpp
+++ b/planning/behavior_velocity_traffic_light_module/src/debug.cpp
@@ -16,8 +16,8 @@
 
 #include <behavior_velocity_planner_common/utilization/util.hpp>
 #include <motion_utils/marker/virtual_wall_marker_creator.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
-
 #ifdef ROS_DISTRO_GALACTIC
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #else
diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp
index f035544b81592..b943feddc103e 100644
--- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp
+++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp
@@ -14,6 +14,7 @@
 
 #include "manager.hpp"
 
+#include <behavior_velocity_planner_common/utilization/util.hpp>
 #include <tier4_autoware_utils/ros/parameter.hpp>
 
 #include <tf2/utils.h>
@@ -24,7 +25,6 @@
 #include <string>
 #include <unordered_map>
 #include <utility>
-
 namespace behavior_velocity_planner
 {
 using lanelet::TrafficLight;
diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp
index e0633926e35df..7c4e947a42e7b 100644
--- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp
+++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp
@@ -17,6 +17,8 @@
 #include <behavior_velocity_planner_common/utilization/util.hpp>
 #include <motion_utils/trajectory/trajectory.hpp>
 
+#include <boost/geometry/algorithms/distance.hpp>
+#include <boost/geometry/algorithms/intersection.hpp>
 #include <boost/optional.hpp>  // To be replaced by std::optional in C++17
 
 #include <tf2/utils.h>
diff --git a/planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp b/planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp
index e5cb4dcbc42e7..83b5e6317aeb0 100644
--- a/planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp
+++ b/planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp
@@ -16,8 +16,8 @@
 
 #include <motion_utils/marker/marker_helper.hpp>
 #include <motion_utils/marker/virtual_wall_marker_creator.hpp>
+#include <tier4_autoware_utils/math/constants.hpp>
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
-
 using motion_utils::createStopVirtualWallMarker;
 using tier4_autoware_utils::appendMarkerArray;
 using tier4_autoware_utils::createDefaultMarker;
diff --git a/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp
index 3fe6c19a5f485..5310fae78c294 100644
--- a/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp
+++ b/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp
@@ -14,6 +14,7 @@
 
 #include "manager.hpp"
 
+#include <behavior_velocity_planner_common/utilization/util.hpp>
 #include <tier4_autoware_utils/math/unit_conversion.hpp>
 #include <tier4_autoware_utils/ros/parameter.hpp>
 
diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp
index 24632c06bad6f..80f691b5f7b40 100644
--- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp
+++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp
@@ -15,14 +15,9 @@
 #ifndef MOTION_VELOCITY_SMOOTHER__RESAMPLE_HPP_
 #define MOTION_VELOCITY_SMOOTHER__RESAMPLE_HPP_
 
-#include "motion_utils/trajectory/trajectory.hpp"
-#include "motion_velocity_smoother/trajectory_utils.hpp"
-
 #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
 #include <geometry_msgs/msg/pose.hpp>
 
-#include "boost/optional.hpp"
-
 #include <vector>
 
 namespace motion_velocity_smoother
diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp
index a7416faa3c652..beb571635f70c 100644
--- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp
+++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp
@@ -15,17 +15,11 @@
 #ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_
 #define MOTION_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_
 
-#include "motion_utils/trajectory/trajectory.hpp"
 #include "motion_velocity_smoother/resample.hpp"
-#include "motion_velocity_smoother/trajectory_utils.hpp"
 #include "rclcpp/rclcpp.hpp"
-#include "tier4_autoware_utils/geometry/geometry.hpp"
-#include "vehicle_info_util/vehicle_info_util.hpp"
 
 #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
 
-#include "boost/optional.hpp"
-
 #include <limits>
 #include <vector>
 
@@ -33,7 +27,6 @@ namespace motion_velocity_smoother
 {
 using autoware_auto_planning_msgs::msg::TrajectoryPoint;
 using TrajectoryPoints = std::vector<TrajectoryPoint>;
-using vehicle_info_util::VehicleInfoUtil;
 
 class SmootherBase
 {
diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp
index 58322fda322bb..ec8a89c5a3e9c 100644
--- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp
+++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp
@@ -15,9 +15,6 @@
 #ifndef MOTION_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_
 #define MOTION_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_
 
-#include "motion_utils/trajectory/trajectory.hpp"
-#include "tier4_autoware_utils/geometry/geometry.hpp"
-
 #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
 #include "geometry_msgs/msg/pose.hpp"
 
diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp
index d3c77aa0faa84..5ed176bd2e215 100644
--- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp
+++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp
@@ -39,7 +39,7 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions
   using std::placeholders::_1;
 
   // set common params
-  const auto vehicle_info = VehicleInfoUtil(*this).getVehicleInfo();
+  const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo();
   wheelbase_ = vehicle_info.wheel_base_m;
   initCommonParam();
   over_stop_velocity_warn_thr_ = declare_parameter<double>("over_stop_velocity_warn_thr");
diff --git a/planning/motion_velocity_smoother/src/resample.cpp b/planning/motion_velocity_smoother/src/resample.cpp
index fbf129f105c7f..37f3ce953bd48 100644
--- a/planning/motion_velocity_smoother/src/resample.cpp
+++ b/planning/motion_velocity_smoother/src/resample.cpp
@@ -16,6 +16,8 @@
 
 #include "motion_utils/resample/resample.hpp"
 #include "motion_utils/trajectory/tmp_conversion.hpp"
+#include "motion_utils/trajectory/trajectory.hpp"
+#include "motion_velocity_smoother/trajectory_utils.hpp"
 
 #include <algorithm>
 #include <vector>
diff --git a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp
index a02e432e1e584..d1694d7420ca2 100644
--- a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp
+++ b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp
@@ -16,6 +16,7 @@
 
 #include "motion_utils/resample/resample.hpp"
 #include "motion_utils/trajectory/tmp_conversion.hpp"
+#include "motion_velocity_smoother/trajectory_utils.hpp"
 
 #include <algorithm>
 #include <string>
diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp
index 2e58961d54c00..7f058d0d138d0 100644
--- a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp
+++ b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp
@@ -16,8 +16,10 @@
 
 #include "motion_utils/resample/resample.hpp"
 #include "motion_utils/trajectory/tmp_conversion.hpp"
+#include "motion_utils/trajectory/trajectory.hpp"
 #include "motion_velocity_smoother/resample.hpp"
 #include "motion_velocity_smoother/trajectory_utils.hpp"
+#include "tier4_autoware_utils/geometry/geometry.hpp"
 #include "tier4_autoware_utils/math/unit_conversion.hpp"
 
 #include <algorithm>
@@ -26,7 +28,6 @@
 
 namespace motion_velocity_smoother
 {
-using vehicle_info_util::VehicleInfoUtil;
 
 SmootherBase::SmootherBase(rclcpp::Node & node)
 {
diff --git a/planning/motion_velocity_smoother/src/trajectory_utils.cpp b/planning/motion_velocity_smoother/src/trajectory_utils.cpp
index ab5db29ad2fc0..dd361696a5dad 100644
--- a/planning/motion_velocity_smoother/src/trajectory_utils.cpp
+++ b/planning/motion_velocity_smoother/src/trajectory_utils.cpp
@@ -16,6 +16,8 @@
 
 #include "interpolation/linear_interpolation.hpp"
 #include "interpolation/spline_interpolation.hpp"
+#include "motion_utils/trajectory/trajectory.hpp"
+#include "tier4_autoware_utils/geometry/geometry.hpp"
 
 #include <algorithm>
 #include <limits>

From 2aa9c7b1db4c0fc7d602feb4984b4d64db5265d4 Mon Sep 17 00:00:00 2001
From: Esteve Fernandez <33620+esteve@users.noreply.github.com>
Date: Thu, 21 Sep 2023 17:04:48 +0200
Subject: [PATCH 04/35] build(traffic_light_classifier): remove download logic
 from CMake (#3144)

Signed-off-by: Esteve Fernandez <esteve.fernandez@tier4.jp>
---
 .../traffic_light_classifier/CMakeLists.txt   | 55 -------------------
 1 file changed, 55 deletions(-)

diff --git a/perception/traffic_light_classifier/CMakeLists.txt b/perception/traffic_light_classifier/CMakeLists.txt
index 72dba86a00d33..22bc7521c6e31 100644
--- a/perception/traffic_light_classifier/CMakeLists.txt
+++ b/perception/traffic_light_classifier/CMakeLists.txt
@@ -65,59 +65,6 @@ else()
   set(CUDNN_AVAIL OFF)
 endif()
 
-# cspell: ignore EFFICIENTNET, MOBILENET
-# Download caffemodel and prototxt
-set(EFFICIENTNET_BATCH_1_HASH 82baba4fcf1abe0c040cd55005e34510)
-set(EFFICIENTNET_BATCH_4_HASH 21b549c2fe4fbb20d32cc019e6d70cd7)
-set(EFFICIENTNET_BATCH_6_HASH 28b408710bcb24f4cdd4d746301d4e78)
-set(MOBILENET_BATCH_1_HASH caa51f2080aa2df943e4f884c41898eb)
-set(MOBILENET_BATCH_4_HASH c2beaf60210f471debfe72b86d076ca0)
-set(MOBILENET_BATCH_6_HASH 47255a11bde479320d703f1f45db1242)
-set(LAMP_LABEL_HASH e9f45efb02f2a9aa8ac27b3d5c164905)
-set(AWS_DIR https://awf.ml.dev.web.auto/perception/models/traffic_light_classifier/v2)
-set(DATA_PATH "${CMAKE_CURRENT_SOURCE_DIR}/data")
-if(NOT EXISTS "${DATA_PATH}")
-  execute_process(COMMAND mkdir -p ${DATA_PATH})
-endif()
-function(download FILE_NAME FILE_HASH)
-  message(STATUS "Checking and downloading ${FILE_NAME}")
-  set(FILE_PATH ${DATA_PATH}/${FILE_NAME})
-  set(STATUS_CODE 0)
-  message(STATUS "start ${FILE_NAME}")
-  if(EXISTS ${FILE_PATH})
-    message(STATUS "found ${FILE_NAME}")
-    file(MD5 ${FILE_PATH} EXISTING_FILE_HASH)
-    if(${FILE_HASH} STREQUAL ${EXISTING_FILE_HASH})
-      message(STATUS "same ${FILE_NAME}")
-      message(STATUS "File already exists.")
-    else()
-      message(STATUS "diff ${FILE_NAME}")
-      message(STATUS "File hash changes. Downloading now ...")
-      file(DOWNLOAD ${AWS_DIR}/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300)
-      list(GET DOWNLOAD_STATUS 0 STATUS_CODE)
-      list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE)
-    endif()
-  else()
-    message(STATUS "not found ${FILE_NAME}")
-    message(STATUS "File doesn't exists. Downloading now ...")
-    file(DOWNLOAD ${AWS_DIR}/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300)
-    list(GET DOWNLOAD_STATUS 0 STATUS_CODE)
-    list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE)
-  endif()
-  if(${STATUS_CODE} EQUAL 0)
-    message(STATUS "Download completed successfully!")
-  else()
-    message(FATAL_ERROR "Error occurred during download: ${ERROR_MESSAGE}")
-  endif()
-endfunction()
-download(traffic_light_classifier_mobilenetv2_batch_1.onnx ${MOBILENET_BATCH_1_HASH})
-download(traffic_light_classifier_mobilenetv2_batch_4.onnx ${MOBILENET_BATCH_4_HASH})
-download(traffic_light_classifier_mobilenetv2_batch_6.onnx ${MOBILENET_BATCH_6_HASH})
-download(traffic_light_classifier_efficientNet_b1_batch_1.onnx ${EFFICIENTNET_BATCH_1_HASH})
-download(traffic_light_classifier_efficientNet_b1_batch_4.onnx ${EFFICIENTNET_BATCH_4_HASH})
-download(traffic_light_classifier_efficientNet_b1_batch_6.onnx ${EFFICIENTNET_BATCH_6_HASH})
-download(lamp_labels.txt 4b2cf910d97d05d464e7c26901af3d4c)
-
 find_package(ament_cmake_auto REQUIRED)
 ament_auto_find_build_dependencies()
 find_package(OpenCV REQUIRED)
@@ -175,7 +122,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL)
   )
 
   ament_auto_package(INSTALL_TO_SHARE
-    data
     launch
   )
 
@@ -200,7 +146,6 @@ else()
   )
 
   ament_auto_package(INSTALL_TO_SHARE
-    data
     launch
   )
 

From df6228617d1348828eaea592b74c10e32a752524 Mon Sep 17 00:00:00 2001
From: Yusuke Muramatsu <yukke42@users.noreply.github.com>
Date: Fri, 22 Sep 2023 10:30:52 +0900
Subject: [PATCH 05/35] fix(image_projection_based_fusion): fix unexpected
 matches between roi and cluster (#4445)

* fix(image_projection_based_fusion): fix error caused by empty cluster

Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp>

* style(pre-commit): autofix

* chore: remove to return

Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp>

* apply suggestion

Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp>

---------

Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
---
 .../src/roi_cluster_fusion/node.cpp                      | 9 ++++++++-
 1 file changed, 8 insertions(+), 1 deletion(-)

diff --git a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp
index e9cb76b050427..316d29dae1301 100644
--- a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp
+++ b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp
@@ -169,7 +169,8 @@ void RoiClusterFusionNode::fuseOnSingleImage(
   }
 
   for (const auto & feature_obj : input_roi_msg.feature_objects) {
-    int index = 0;
+    int index = -1;
+    bool associated = false;
     double max_iou = 0.0;
     bool is_roi_label_known =
       feature_obj.object.classification.front().label != ObjectClassification::UNKNOWN;
@@ -193,8 +194,14 @@ void RoiClusterFusionNode::fuseOnSingleImage(
       if (max_iou < iou + iou_x + iou_y && passed_inside_cluster_gate) {
         index = cluster_map.first;
         max_iou = iou + iou_x + iou_y;
+        associated = true;
       }
     }
+
+    if (!associated) {
+      continue;
+    }
+
     if (!output_cluster_msg.feature_objects.empty()) {
       bool is_roi_existence_prob_higher =
         output_cluster_msg.feature_objects.at(index).object.existence_probability <=

From 4c9146eec95a319ecbed2618e24f6a3e72734b2d Mon Sep 17 00:00:00 2001
From: Vincent Richard <richard-v@macnica.co.jp>
Date: Fri, 22 Sep 2023 10:42:21 +0900
Subject: [PATCH 06/35] fix(euclidean_cluster): print warning on empty cloud,
 prevent log spam (#4029)

solves #4001

Signed-off-by: Vincent Richard <richard-v@macnica.co.jp>
---
 .../src/voxel_grid_based_euclidean_cluster_node.cpp  | 12 ++++++++++--
 1 file changed, 10 insertions(+), 2 deletions(-)

diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp
index 53fb4df8977d8..a28791e5d4057 100644
--- a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp
+++ b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp
@@ -56,11 +56,19 @@ void VoxelGridBasedEuclideanClusterNode::onPointCloud(
 
   // convert ros to pcl
   pcl::PointCloud<pcl::PointXYZ>::Ptr raw_pointcloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
-  pcl::fromROSMsg(*input_msg, *raw_pointcloud_ptr);
+  if (input_msg->data.empty()) {
+    // NOTE: prevent pcl log spam
+    RCLCPP_WARN_STREAM_THROTTLE(
+      this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!");
+  } else {
+    pcl::fromROSMsg(*input_msg, *raw_pointcloud_ptr);
+  }
 
   // clustering
   std::vector<pcl::PointCloud<pcl::PointXYZ>> clusters;
-  cluster_->cluster(raw_pointcloud_ptr, clusters);
+  if (!raw_pointcloud_ptr->empty()) {
+    cluster_->cluster(raw_pointcloud_ptr, clusters);
+  }
 
   // build output msg
   tier4_perception_msgs::msg::DetectedObjectsWithFeature output;

From bf03fe31bdfbc036a661fdcfd8c062a1d90fb306 Mon Sep 17 00:00:00 2001
From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Date: Fri, 22 Sep 2023 11:01:06 +0900
Subject: [PATCH 07/35] fix(simple_planning_simulator): fix build error (#5062)

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
---
 .../simple_planning_simulator_core.hpp               |  2 +-
 .../simple_planning_simulator_core.cpp               | 12 +++++++-----
 .../test/test_simple_planning_simulator.cpp          |  4 ++--
 3 files changed, 10 insertions(+), 8 deletions(-)

diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp
index 8eca49d9ffadf..4b25dba8b16ee 100644
--- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp
+++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp
@@ -269,7 +269,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
    * @brief ControlModeRequest server
    */
   void on_control_mode_request(
-    const ControlModeCommand::Request::SharedPtr request,
+    const ControlModeCommand::Request::ConstSharedPtr request,
     const ControlModeCommand::Response::SharedPtr response);
 
   /**
diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp
index fe28d063eedd3..8c2688928b4c9 100644
--- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp
+++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp
@@ -117,16 +117,18 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt
     "input/initialtwist", QoS{1}, std::bind(&SimplePlanningSimulator::on_initialtwist, this, _1));
   sub_ackermann_cmd_ = create_subscription<AckermannControlCommand>(
     "input/ackermann_control_command", QoS{1},
-    [this](const AckermannControlCommand::SharedPtr msg) { current_ackermann_cmd_ = *msg; });
+    [this](const AckermannControlCommand::ConstSharedPtr msg) { current_ackermann_cmd_ = *msg; });
   sub_manual_ackermann_cmd_ = create_subscription<AckermannControlCommand>(
     "input/manual_ackermann_control_command", QoS{1},
-    [this](const AckermannControlCommand::SharedPtr msg) { current_manual_ackermann_cmd_ = *msg; });
+    [this](const AckermannControlCommand::ConstSharedPtr msg) {
+      current_manual_ackermann_cmd_ = *msg;
+    });
   sub_gear_cmd_ = create_subscription<GearCommand>(
     "input/gear_command", QoS{1},
-    [this](const GearCommand::SharedPtr msg) { current_gear_cmd_ = *msg; });
+    [this](const GearCommand::ConstSharedPtr msg) { current_gear_cmd_ = *msg; });
   sub_manual_gear_cmd_ = create_subscription<GearCommand>(
     "input/manual_gear_command", QoS{1},
-    [this](const GearCommand::SharedPtr msg) { current_manual_gear_cmd_ = *msg; });
+    [this](const GearCommand::ConstSharedPtr msg) { current_manual_gear_cmd_ = *msg; });
   sub_turn_indicators_cmd_ = create_subscription<TurnIndicatorsCommand>(
     "input/turn_indicators_command", QoS{1},
     std::bind(&SimplePlanningSimulator::on_turn_indicators_cmd, this, _1));
@@ -484,7 +486,7 @@ void SimplePlanningSimulator::on_engage(const Engage::ConstSharedPtr msg)
 }
 
 void SimplePlanningSimulator::on_control_mode_request(
-  const ControlModeCommand::Request::SharedPtr request,
+  const ControlModeCommand::Request::ConstSharedPtr request,
   const ControlModeCommand::Response::SharedPtr response)
 {
   const auto m = request->mode;
diff --git a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp
index e57a3683b97a9..cedfec395110e 100644
--- a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp
+++ b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp
@@ -49,7 +49,7 @@ class PubSubNode : public rclcpp::Node
   {
     current_odom_sub_ = create_subscription<Odometry>(
       "output/odometry", rclcpp::QoS{1},
-      [this](const Odometry::SharedPtr msg) { current_odom_ = msg; });
+      [this](const Odometry::ConstSharedPtr msg) { current_odom_ = msg; });
     pub_ackermann_command_ =
       create_publisher<AckermannControlCommand>("input/ackermann_control_command", rclcpp::QoS{1});
     pub_initialpose_ =
@@ -62,7 +62,7 @@ class PubSubNode : public rclcpp::Node
   rclcpp::Publisher<GearCommand>::SharedPtr pub_gear_cmd_;
   rclcpp::Publisher<PoseWithCovarianceStamped>::SharedPtr pub_initialpose_;
 
-  Odometry::SharedPtr current_odom_;
+  Odometry::ConstSharedPtr current_odom_;
 };
 
 /**

From f09e1b75addde381e7bbfb32444a75927a832169 Mon Sep 17 00:00:00 2001
From: TaikiYamada4 <129915538+TaikiYamada4@users.noreply.github.com>
Date: Fri, 22 Sep 2023 13:32:06 +0900
Subject: [PATCH 08/35] refactor(ndt_scan_matcher): rework parameters (#5030)

* Removed all default values from the c++ source code.
Modified the declaration of ndt_base_frame_ so that it works if it is not declared in the param.yaml file

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>

* Added ndt_scan_matcher.schema.json file

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>

* Removed redundant static_cast<int> on declare_paramter

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>

* style(pre-commit): autofix

* Added neighborhood_search_method parameter to ndt_scan_matcher.param.yaml.
This is same to the one in autoware_launch

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>

* Deleted schema.json for now

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>

* Delete ndt_scan_matcher.schema.json for now

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>

* Deleted parameter neighborhood_search_method from ndt_scan_matcher.param.yaml, which was inserted by mistake.

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>

* Added ndt_base_frame to ndt_scan_matcher.param.yaml

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>

---------

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
---
 .../config/ndt_scan_matcher.param.yaml        |  3 ++
 .../src/ndt_scan_matcher_core.cpp             | 42 ++++++++-----------
 2 files changed, 21 insertions(+), 24 deletions(-)

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 b5affd72b2158..c8afa9ea1a916 100644
--- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml
+++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml
@@ -6,6 +6,9 @@
     # Vehicle reference frame
     base_frame: "base_link"
 
+    # NDT reference frame
+    ndt_base_frame: "ndt_base_link"
+
     # Subscriber queue size
     input_sensor_points_queue_size: 1
 
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 580d1e59b65a6..7bb2b79e88f33 100644
--- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
+++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
@@ -97,31 +97,30 @@ NDTScanMatcher::NDTScanMatcher()
   inversion_vector_threshold_(-0.9),
   oscillation_threshold_(10),
   output_pose_covariance_(),
-  regularization_enabled_(declare_parameter("regularization_enabled", false)),
+  regularization_enabled_(declare_parameter<bool>("regularization_enabled")),
   estimate_scores_for_degrounded_scan_(
-    declare_parameter("estimate_scores_for_degrounded_scan", false)),
-  z_margin_for_ground_removal_(declare_parameter("z_margin_for_ground_removal", 0.8))
+    declare_parameter<bool>("estimate_scores_for_degrounded_scan")),
+  z_margin_for_ground_removal_(declare_parameter<double>("z_margin_for_ground_removal"))
 {
   (*state_ptr_)["state"] = "Initializing";
   is_activated_ = false;
 
-  int points_queue_size =
-    static_cast<int>(this->declare_parameter("input_sensor_points_queue_size", 0));
+  int points_queue_size = this->declare_parameter<int>("input_sensor_points_queue_size");
   points_queue_size = std::max(points_queue_size, 0);
   RCLCPP_INFO(get_logger(), "points_queue_size: %d", points_queue_size);
 
-  base_frame_ = this->declare_parameter("base_frame", base_frame_);
+  base_frame_ = this->declare_parameter<std::string>("base_frame");
   RCLCPP_INFO(get_logger(), "base_frame_id: %s", base_frame_.c_str());
 
-  ndt_base_frame_ = this->declare_parameter("ndt_base_frame", ndt_base_frame_);
+  ndt_base_frame_ = this->declare_parameter<std::string>("ndt_base_frame");
   RCLCPP_INFO(get_logger(), "ndt_base_frame_id: %s", ndt_base_frame_.c_str());
 
   pclomp::NdtParams ndt_params{};
   ndt_params.trans_epsilon = this->declare_parameter<double>("trans_epsilon");
   ndt_params.step_size = this->declare_parameter<double>("step_size");
   ndt_params.resolution = this->declare_parameter<double>("resolution");
-  ndt_params.max_iterations = static_cast<int>(this->declare_parameter<int>("max_iterations"));
-  ndt_params.num_threads = static_cast<int>(this->declare_parameter<int>("num_threads"));
+  ndt_params.max_iterations = this->declare_parameter<int>("max_iterations");
+  ndt_params.num_threads = this->declare_parameter<int>("num_threads");
   ndt_params.num_threads = std::max(ndt_params.num_threads, 1);
   ndt_params.regularization_scale_factor =
     static_cast<float>(this->declare_parameter<float>("regularization_scale_factor"));
@@ -132,24 +131,20 @@ NDTScanMatcher::NDTScanMatcher()
     ndt_params.trans_epsilon, ndt_params.step_size, ndt_params.resolution,
     ndt_params.max_iterations);
 
-  int converged_param_type_tmp =
-    static_cast<int>(this->declare_parameter("converged_param_type", 0));
+  int converged_param_type_tmp = this->declare_parameter<int>("converged_param_type");
   converged_param_type_ = static_cast<ConvergedParamType>(converged_param_type_tmp);
 
-  converged_param_transform_probability_ = this->declare_parameter(
-    "converged_param_transform_probability", converged_param_transform_probability_);
-  converged_param_nearest_voxel_transformation_likelihood_ = this->declare_parameter(
-    "converged_param_nearest_voxel_transformation_likelihood",
-    converged_param_nearest_voxel_transformation_likelihood_);
+  converged_param_transform_probability_ =
+    this->declare_parameter<double>("converged_param_transform_probability");
+  converged_param_nearest_voxel_transformation_likelihood_ =
+    this->declare_parameter<double>("converged_param_nearest_voxel_transformation_likelihood");
 
-  lidar_topic_timeout_sec_ =
-    this->declare_parameter("lidar_topic_timeout_sec", lidar_topic_timeout_sec_);
+  lidar_topic_timeout_sec_ = this->declare_parameter<double>("lidar_topic_timeout_sec");
 
-  initial_pose_timeout_sec_ =
-    this->declare_parameter("initial_pose_timeout_sec", initial_pose_timeout_sec_);
+  initial_pose_timeout_sec_ = this->declare_parameter<double>("initial_pose_timeout_sec");
 
-  initial_pose_distance_tolerance_m_ = this->declare_parameter(
-    "initial_pose_distance_tolerance_m", initial_pose_distance_tolerance_m_);
+  initial_pose_distance_tolerance_m_ =
+    this->declare_parameter<double>("initial_pose_distance_tolerance_m");
 
   std::vector<double> output_pose_covariance =
     this->declare_parameter<std::vector<double>>("output_pose_covariance");
@@ -157,8 +152,7 @@ NDTScanMatcher::NDTScanMatcher()
     output_pose_covariance_[i] = output_pose_covariance[i];
   }
 
-  initial_estimate_particles_num_ = static_cast<int>(
-    this->declare_parameter("initial_estimate_particles_num", initial_estimate_particles_num_));
+  initial_estimate_particles_num_ = this->declare_parameter<int>("initial_estimate_particles_num");
 
   rclcpp::CallbackGroup::SharedPtr initial_pose_callback_group;
   initial_pose_callback_group =

From 12d83338a2d66bf23157c0d8f109a217cc0a3a88 Mon Sep 17 00:00:00 2001
From: Fumiya Watanabe <rej55.g@gmail.com>
Date: Fri, 22 Sep 2023 16:39:45 +0900
Subject: [PATCH 09/35] feat(lane_change): regulate lane change on crosswalk
 and intersection (#5068)

* fix(lane_change): fix hasEnoughDistance()

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* feat(lane_change): regulate lane change on crosswalk and intersection

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* fix(lane_change): remove unnecessary value

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* Update planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp

Co-authored-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>

* style(pre-commit): autofix

* Update planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp

Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com>

* style(pre-commit): autofix

* fix(lane_change): separate functions and fix

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

---------

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>
Co-authored-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com>
---
 .../config/lane_change/lane_change.param.yaml |   5 +
 ...ehavior_path_planner_lane_change_design.md |  12 +
 .../scene_module/lane_change/normal.hpp       |   8 +-
 .../lane_change/lane_change_module_data.hpp   |   4 +
 .../src/scene_module/lane_change/manager.cpp  |   5 +
 .../src/scene_module/lane_change/normal.cpp   | 397 +++++++++++-------
 .../behavior_path_planner/src/utils/utils.cpp |   5 +-
 7 files changed, 276 insertions(+), 160 deletions(-)

diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml
index 925e70d9084d6..d7179cd384008 100644
--- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml
+++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml
@@ -55,6 +55,11 @@
         motorcycle: true
         pedestrian: true
 
+      # lane change regulations
+      regulation:
+        crosswalk: false
+        intersection: false
+
       # collision check
       enable_prepare_segment_collision_check: true
       prepare_segment_ignore_object_velocity_thresh: 0.1 # [m/s]
diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md
index 2fd1ff1cfd376..88266a2f4fb2a 100644
--- a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md
+++ b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md
@@ -186,6 +186,11 @@ The following figure illustrates when the lane is blocked in multiple lane chang
 
 ![multiple-lane-changes](../image/lane_change/lane_change-when_cannot_change_lanes.png)
 
+### Lane change regulations
+
+If you want to regulate lane change on crosswalks or intersections, the lane change module finds a lane change path excluding it includes crosswalks or intersections.
+To regulate lane change on crosswalks or intersections, change `regulation.crosswalk` or `regulation.intersection` to `true`.
+
 ### Aborting lane change
 
 The abort process may result in three different outcome; Cancel, Abort and Stop/Cruise.
@@ -294,6 +299,13 @@ The following parameters are configurable in `lane_change.param.yaml`.
 | `target_object.motorcycle`                  | [-]    | boolean | Include motorcycle objects for safety check                                                                     | true               |
 | `target_object.pedestrian`                  | [-]    | boolean | Include pedestrian objects for safety check                                                                     | true               |
 
+### Lane change regulations
+
+| Name                      | Unit | Type    | Description                           | Default value |
+| :------------------------ | ---- | ------- | ------------------------------------- | ------------- |
+| `regulation.crosswalk`    | [-]  | boolean | Regulate lane change on crosswalks    | false         |
+| `regulation.intersection` | [-]  | boolean | Regulate lane change on intersections | false         |
+
 ### Collision checks during lane change
 
 The following parameters are configurable in `behavior_path_planner.param.yaml` and `lane_change.param.yaml`.
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp
index ef57c31c28b5e..747c75babb573 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp
@@ -108,7 +108,7 @@ class NormalLaneChange : public LaneChangeBase
     const lanelet::ConstLanelets & current_lanes,
     const lanelet::ConstLanelets & target_lanes) const;
 
-  double calcPrepareDuration(
+  std::vector<double> calcPrepareDuration(
     const lanelet::ConstLanelets & current_lanes,
     const lanelet::ConstLanelets & target_lanes) const;
 
@@ -129,6 +129,12 @@ class NormalLaneChange : public LaneChangeBase
     const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes,
     const lanelet::ConstLanelets & target_lanes, const Direction direction = Direction::NONE) const;
 
+  bool hasEnoughLengthToCrosswalk(
+    const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const;
+
+  bool hasEnoughLengthToIntersection(
+    const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const;
+
   bool getLaneChangePaths(
     const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
     Direction direction, LaneChangePaths * candidate_paths,
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp
index c78be341b4e16..ce515be800900 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp
@@ -68,6 +68,10 @@ struct LaneChangeParameters
   bool check_objects_on_other_lanes{true};
   bool use_all_predicted_path{false};
 
+  // regulatory elements
+  bool regulate_on_crosswalk{false};
+  bool regulate_on_intersection{false};
+
   // true by default for all objects
   utils::path_safety_checker::ObjectTypesToCheck object_types_to_check;
 
diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp
index e72691ea55391..57986a8f02265 100644
--- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp
+++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp
@@ -78,6 +78,11 @@ LaneChangeModuleManager::LaneChangeModuleManager(
   p.use_all_predicted_path =
     getOrDeclareParameter<bool>(*node, parameter("use_all_predicted_path"));
 
+  // lane change regulations
+  p.regulate_on_crosswalk = getOrDeclareParameter<bool>(*node, parameter("regulation.crosswalk"));
+  p.regulate_on_intersection =
+    getOrDeclareParameter<bool>(*node, parameter("regulation.intersection"));
+
   p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter<double>(
     *node, parameter("safety_check.longitudinal_distance_min_threshold"));
   p.rss_params.longitudinal_velocity_delta_time = getOrDeclareParameter<double>(
diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
index 376bac3ed65fb..36127bbdf20b6 100644
--- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
+++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
@@ -567,19 +567,26 @@ std::vector<double> NormalLaneChange::sampleLongitudinalAccValues(
   return utils::lane_change::getAccelerationValues(min_acc, max_acc, longitudinal_acc_sampling_num);
 }
 
-double NormalLaneChange::calcPrepareDuration(
+std::vector<double> NormalLaneChange::calcPrepareDuration(
   const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const
 {
   const auto & common_parameters = planner_data_->parameters;
-  const auto threshold = lane_change_parameters_->min_length_for_turn_signal_activation;
-  const auto current_vel = getEgoVelocity();
+  const auto base_link2front = planner_data_->parameters.base_link2front;
+  const auto threshold =
+    lane_change_parameters_->min_length_for_turn_signal_activation + base_link2front;
 
-  // if the ego vehicle is close to the end of the lane at a low speed
-  if (isNearEndOfCurrentLanes(current_lanes, target_lanes, threshold) && current_vel < 1.0) {
-    return 0.0;
+  std::vector<double> prepare_durations;
+  constexpr double step = 0.5;
+
+  for (double duration = common_parameters.lane_change_prepare_duration; duration >= 0.0;
+       duration -= step) {
+    prepare_durations.push_back(duration);
+    if (!isNearEndOfCurrentLanes(current_lanes, target_lanes, threshold)) {
+      break;
+    }
   }
 
-  return common_parameters.lane_change_prepare_duration;
+  return prepare_durations;
 }
 
 PathWithLaneId NormalLaneChange::getPrepareSegment(
@@ -800,6 +807,7 @@ bool NormalLaneChange::hasEnoughLength(
 {
   const auto current_pose = getEgoPose();
   const auto & route_handler = *getRouteHandler();
+  const auto overall_graphs_ptr = route_handler.getOverallGraphPtr();
   const auto & common_parameters = planner_data_->parameters;
   const double lane_change_length = path.info.length.sum();
   const auto shift_intervals =
@@ -830,6 +838,68 @@ bool NormalLaneChange::hasEnoughLength(
     return false;
   }
 
+  if (lane_change_parameters_->regulate_on_crosswalk) {
+    const double dist_to_crosswalk_from_lane_change_start_pose =
+      utils::getDistanceToCrosswalk(current_pose, current_lanes, *overall_graphs_ptr) -
+      path.info.length.prepare;
+    // Check lane changing section includes crosswalk
+    if (
+      dist_to_crosswalk_from_lane_change_start_pose > 0.0 &&
+      dist_to_crosswalk_from_lane_change_start_pose < path.info.length.lane_changing) {
+      return false;
+    }
+  }
+
+  if (lane_change_parameters_->regulate_on_intersection) {
+    const double dist_to_intersection_from_lane_change_start_pose =
+      utils::getDistanceToNextIntersection(current_pose, current_lanes) - path.info.length.prepare;
+    // Check lane changing section includes intersection
+    if (
+      dist_to_intersection_from_lane_change_start_pose > 0.0 &&
+      dist_to_intersection_from_lane_change_start_pose < path.info.length.lane_changing) {
+      return false;
+    }
+  }
+
+  return true;
+}
+
+bool NormalLaneChange::hasEnoughLengthToCrosswalk(
+  const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const
+{
+  const auto current_pose = getEgoPose();
+  const auto & route_handler = *getRouteHandler();
+  const auto overall_graphs_ptr = route_handler.getOverallGraphPtr();
+
+  const double dist_to_crosswalk_from_lane_change_start_pose =
+    utils::getDistanceToCrosswalk(current_pose, current_lanes, *overall_graphs_ptr) -
+    path.info.length.prepare;
+  // Check lane changing section includes crosswalk
+  if (
+    dist_to_crosswalk_from_lane_change_start_pose > 0.0 &&
+    dist_to_crosswalk_from_lane_change_start_pose < path.info.length.lane_changing) {
+    return false;
+  }
+
+  return true;
+}
+
+bool NormalLaneChange::hasEnoughLengthToIntersection(
+  const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const
+{
+  const auto current_pose = getEgoPose();
+  const auto & route_handler = *getRouteHandler();
+  const auto overall_graphs_ptr = route_handler.getOverallGraphPtr();
+
+  const double dist_to_intersection_from_lane_change_start_pose =
+    utils::getDistanceToNextIntersection(current_pose, current_lanes) - path.info.length.prepare;
+  // Check lane changing section includes intersection
+  if (
+    dist_to_intersection_from_lane_change_start_pose > 0.0 &&
+    dist_to_intersection_from_lane_change_start_pose < path.info.length.lane_changing) {
+    return false;
+  }
+
   return true;
 }
 
@@ -876,182 +946,199 @@ bool NormalLaneChange::getLaneChangePaths(
 
   const auto target_objects = getTargetObjects(current_lanes, target_lanes);
 
-  candidate_paths->reserve(longitudinal_acc_sampling_values.size() * lateral_acc_sampling_num);
+  const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes);
 
-  const auto prepare_duration = calcPrepareDuration(current_lanes, target_lanes);
+  candidate_paths->reserve(
+    longitudinal_acc_sampling_values.size() * lateral_acc_sampling_num * prepare_durations.size());
 
-  for (const auto & sampled_longitudinal_acc : longitudinal_acc_sampling_values) {
-    // get path on original lanes
-    const auto prepare_velocity = std::max(
-      current_velocity + sampled_longitudinal_acc * prepare_duration,
-      minimum_lane_changing_velocity);
+  for (const auto & prepare_duration : prepare_durations) {
+    for (const auto & sampled_longitudinal_acc : longitudinal_acc_sampling_values) {
+      // get path on original lanes
+      const auto prepare_velocity = std::max(
+        current_velocity + sampled_longitudinal_acc * prepare_duration,
+        minimum_lane_changing_velocity);
 
-    // compute actual longitudinal acceleration
-    const double longitudinal_acc_on_prepare =
-      (prepare_duration < 1e-3) ? 0.0 : ((prepare_velocity - current_velocity) / prepare_duration);
+      // compute actual longitudinal acceleration
+      const double longitudinal_acc_on_prepare =
+        (prepare_duration < 1e-3) ? 0.0
+                                  : ((prepare_velocity - current_velocity) / prepare_duration);
 
-    const double prepare_length = current_velocity * prepare_duration +
-                                  0.5 * longitudinal_acc_on_prepare * std::pow(prepare_duration, 2);
+      const double prepare_length =
+        current_velocity * prepare_duration +
+        0.5 * longitudinal_acc_on_prepare * std::pow(prepare_duration, 2);
 
-    auto prepare_segment = getPrepareSegment(current_lanes, backward_path_length, prepare_length);
+      auto prepare_segment = getPrepareSegment(current_lanes, backward_path_length, prepare_length);
 
-    if (prepare_segment.points.empty()) {
-      RCLCPP_DEBUG(logger_, "prepare segment is empty!!");
-      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(
-      current_lanes, target_lanes.front(), lane_changing_start_pose);
+      if (prepare_segment.points.empty()) {
+        RCLCPP_DEBUG(logger_, "prepare segment is empty!!");
+        continue;
+      }
 
-    // Check if the lane changing start point is not on the lanes next to target lanes,
-    if (target_length_from_lane_change_start_pose > 0.0) {
-      RCLCPP_DEBUG(
-        logger_, "[only new arch] lane change start getEgoPose() is behind target lanelet!!");
-      break;
-    }
+      // 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(
+        current_lanes, target_lanes.front(), lane_changing_start_pose);
 
-    const auto shift_length =
-      lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose);
-
-    const auto initial_lane_changing_velocity = prepare_velocity;
-    const auto & max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps;
-
-    // get lateral acceleration range
-    const auto [min_lateral_acc, max_lateral_acc] =
-      common_parameters.lane_change_lat_acc_map.find(initial_lane_changing_velocity);
-    const auto lateral_acc_resolution =
-      std::abs(max_lateral_acc - min_lateral_acc) / lateral_acc_sampling_num;
-    constexpr double lateral_acc_epsilon = 0.01;
-
-    for (double lateral_acc = min_lateral_acc; lateral_acc < max_lateral_acc + lateral_acc_epsilon;
-         lateral_acc += lateral_acc_resolution) {
-      const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk(
-        shift_length, common_parameters.lane_changing_lateral_jerk, lateral_acc);
-      const double longitudinal_acc_on_lane_changing =
-        utils::lane_change::calcLaneChangingAcceleration(
-          initial_lane_changing_velocity, max_path_velocity, lane_changing_time,
-          sampled_longitudinal_acc);
-      const auto lane_changing_length =
-        initial_lane_changing_velocity * lane_changing_time +
-        0.5 * longitudinal_acc_on_lane_changing * lane_changing_time * lane_changing_time;
-      const auto terminal_lane_changing_velocity =
-        initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time;
-      utils::lane_change::setPrepareVelocity(
-        prepare_segment, current_velocity, terminal_lane_changing_velocity);
-
-      if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) {
-        RCLCPP_DEBUG(logger_, "length of lane changing path is longer than length to goal!!");
-        continue;
+      // Check if the lane changing start point is not on the lanes next to target lanes,
+      if (target_length_from_lane_change_start_pose > 0.0) {
+        RCLCPP_DEBUG(
+          logger_, "[only new arch] lane change start getEgoPose() is behind target lanelet!!");
+        break;
       }
 
-      if (is_goal_in_route) {
-        const double s_start =
-          lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose).length;
-        const double s_goal =
-          lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length;
-        const auto num =
-          std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back(), direction));
-        const double backward_buffer =
-          num == 0 ? 0.0 : common_parameters.backward_length_buffer_for_end_of_lane;
-        const double finish_judge_buffer = common_parameters.lane_change_finish_judge_buffer;
-        if (
-          s_start + lane_changing_length + finish_judge_buffer + backward_buffer +
-            next_lane_change_buffer >
-          s_goal) {
+      const auto shift_length =
+        lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose);
+
+      const auto initial_lane_changing_velocity = prepare_velocity;
+      const auto max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps;
+
+      // get lateral acceleration range
+      const auto [min_lateral_acc, max_lateral_acc] =
+        common_parameters.lane_change_lat_acc_map.find(initial_lane_changing_velocity);
+      const auto lateral_acc_resolution =
+        std::abs(max_lateral_acc - min_lateral_acc) / lateral_acc_sampling_num;
+      constexpr double lateral_acc_epsilon = 0.01;
+
+      for (double lateral_acc = min_lateral_acc;
+           lateral_acc < max_lateral_acc + lateral_acc_epsilon;
+           lateral_acc += lateral_acc_resolution) {
+        const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk(
+          shift_length, common_parameters.lane_changing_lateral_jerk, lateral_acc);
+        const double longitudinal_acc_on_lane_changing =
+          utils::lane_change::calcLaneChangingAcceleration(
+            initial_lane_changing_velocity, max_path_velocity, lane_changing_time,
+            sampled_longitudinal_acc);
+        const auto lane_changing_length =
+          initial_lane_changing_velocity * lane_changing_time +
+          0.5 * longitudinal_acc_on_lane_changing * lane_changing_time * lane_changing_time;
+        const auto terminal_lane_changing_velocity =
+          initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time;
+        utils::lane_change::setPrepareVelocity(
+          prepare_segment, current_velocity, terminal_lane_changing_velocity);
+
+        if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) {
           RCLCPP_DEBUG(logger_, "length of lane changing path is longer than length to goal!!");
           continue;
         }
-      }
 
-      const auto target_segment = getTargetSegment(
-        target_lanes, lane_changing_start_pose, target_lane_length, lane_changing_length,
-        initial_lane_changing_velocity, next_lane_change_buffer);
+        if (is_goal_in_route) {
+          const double s_start =
+            lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose).length;
+          const double s_goal =
+            lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length;
+          const auto num =
+            std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back(), direction));
+          const double backward_buffer =
+            num == 0 ? 0.0 : common_parameters.backward_length_buffer_for_end_of_lane;
+          const double finish_judge_buffer = common_parameters.lane_change_finish_judge_buffer;
+          if (
+            s_start + lane_changing_length + finish_judge_buffer + backward_buffer +
+              next_lane_change_buffer >
+            s_goal) {
+            RCLCPP_DEBUG(logger_, "length of lane changing path is longer than length to goal!!");
+            continue;
+          }
+        }
 
-      if (target_segment.points.empty()) {
-        RCLCPP_DEBUG(logger_, "target segment is empty!! something wrong...");
-        continue;
-      }
+        const auto target_segment = getTargetSegment(
+          target_lanes, lane_changing_start_pose, target_lane_length, lane_changing_length,
+          initial_lane_changing_velocity, next_lane_change_buffer);
 
-      const lanelet::BasicPoint2d lc_start_point(
-        prepare_segment.points.back().point.pose.position.x,
-        prepare_segment.points.back().point.pose.position.y);
+        if (target_segment.points.empty()) {
+          RCLCPP_DEBUG(logger_, "target segment is empty!! something wrong...");
+          continue;
+        }
 
-      const auto target_lane_polygon = lanelet::utils::getPolygonFromArcLength(
-        target_lanes, 0, std::numeric_limits<double>::max());
-      const auto target_lane_poly_2d = lanelet::utils::to2D(target_lane_polygon).basicPolygon();
+        const lanelet::BasicPoint2d lc_start_point(
+          prepare_segment.points.back().point.pose.position.x,
+          prepare_segment.points.back().point.pose.position.y);
 
-      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);
+        const auto target_lane_polygon = lanelet::utils::getPolygonFromArcLength(
+          target_lanes, 0, std::numeric_limits<double>::max());
+        const auto target_lane_poly_2d = lanelet::utils::to2D(target_lane_polygon).basicPolygon();
 
-      if (!is_valid_start_point) {
-        // lane changing points are not inside of the target preferred lanes or its neighbors
-        continue;
-      }
+        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);
 
-      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(
-        route_handler, target_lanes, lane_changing_start_pose, target_lane_length,
-        lane_changing_length, forward_path_length, resample_interval, is_goal_in_route,
-        next_lane_change_buffer);
+        if (!is_valid_start_point) {
+          // lane changing points are not inside of the target preferred lanes or its neighbors
+          continue;
+        }
 
-      if (target_lane_reference_path.points.empty()) {
-        RCLCPP_DEBUG(logger_, "target_lane_reference_path is empty!!");
-        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(
+          route_handler, target_lanes, lane_changing_start_pose, target_lane_length,
+          lane_changing_length, forward_path_length, resample_interval, is_goal_in_route,
+          next_lane_change_buffer);
 
-      LaneChangeInfo lane_change_info;
-      lane_change_info.longitudinal_acceleration =
-        LaneChangePhaseInfo{longitudinal_acc_on_prepare, longitudinal_acc_on_lane_changing};
-      lane_change_info.duration = LaneChangePhaseInfo{prepare_duration, lane_changing_time};
-      lane_change_info.velocity =
-        LaneChangePhaseInfo{prepare_velocity, initial_lane_changing_velocity};
-      lane_change_info.length = LaneChangePhaseInfo{prepare_length, lane_changing_length};
-      lane_change_info.current_lanes = current_lanes;
-      lane_change_info.target_lanes = target_lanes;
-      lane_change_info.lane_changing_start = prepare_segment.points.back().point.pose;
-      lane_change_info.lane_changing_end = target_segment.points.front().point.pose;
-      lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine(
-        prepare_segment, target_segment, target_lane_reference_path, shift_length);
-      lane_change_info.lateral_acceleration = lateral_acc;
-      lane_change_info.terminal_lane_changing_velocity = terminal_lane_changing_velocity;
-
-      const auto candidate_path = utils::lane_change::constructCandidatePath(
-        lane_change_info, prepare_segment, target_segment, target_lane_reference_path,
-        sorted_lane_ids);
-
-      if (!candidate_path) {
-        RCLCPP_DEBUG(logger_, "no candidate path!!");
-        continue;
-      }
+        if (target_lane_reference_path.points.empty()) {
+          RCLCPP_DEBUG(logger_, "target_lane_reference_path is empty!!");
+          continue;
+        }
 
-      const auto is_valid =
-        hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction);
+        LaneChangeInfo lane_change_info;
+        lane_change_info.longitudinal_acceleration =
+          LaneChangePhaseInfo{longitudinal_acc_on_prepare, longitudinal_acc_on_lane_changing};
+        lane_change_info.duration = LaneChangePhaseInfo{prepare_duration, lane_changing_time};
+        lane_change_info.velocity =
+          LaneChangePhaseInfo{prepare_velocity, initial_lane_changing_velocity};
+        lane_change_info.length = LaneChangePhaseInfo{prepare_length, lane_changing_length};
+        lane_change_info.current_lanes = current_lanes;
+        lane_change_info.target_lanes = target_lanes;
+        lane_change_info.lane_changing_start = prepare_segment.points.back().point.pose;
+        lane_change_info.lane_changing_end = target_segment.points.front().point.pose;
+        lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine(
+          prepare_segment, target_segment, target_lane_reference_path, shift_length);
+        lane_change_info.lateral_acceleration = lateral_acc;
+        lane_change_info.terminal_lane_changing_velocity = terminal_lane_changing_velocity;
+
+        const auto candidate_path = utils::lane_change::constructCandidatePath(
+          lane_change_info, prepare_segment, target_segment, target_lane_reference_path,
+          sorted_lane_ids);
+
+        if (!candidate_path) {
+          RCLCPP_DEBUG(logger_, "no candidate path!!");
+          continue;
+        }
 
-      if (!is_valid) {
-        RCLCPP_DEBUG(logger_, "invalid candidate path!!");
-        continue;
-      }
+        if (!hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction)) {
+          RCLCPP_DEBUG(logger_, "invalid candidate path!!");
+          continue;
+        }
 
-      if (utils::lane_change::passParkedObject(
-            route_handler, *candidate_path, target_objects.target_lane, lane_change_buffer,
-            is_goal_in_route, *lane_change_parameters_)) {
-        return false;
-      }
-      candidate_paths->push_back(*candidate_path);
+        if (
+          lane_change_parameters_->regulate_on_crosswalk &&
+          !hasEnoughLengthToCrosswalk(*candidate_path, current_lanes)) {
+          RCLCPP_DEBUG(logger_, "Including crosswalk!!");
+          continue;
+        }
 
-      if (!check_safety) {
-        return false;
-      }
+        if (
+          lane_change_parameters_->regulate_on_intersection &&
+          !hasEnoughLengthToIntersection(*candidate_path, current_lanes)) {
+          RCLCPP_DEBUG(logger_, "Including intersection!!");
+          continue;
+        }
 
-      const auto [is_safe, is_object_coming_from_rear] = isLaneChangePathSafe(
-        *candidate_path, target_objects, lane_change_parameters_->rss_params, object_debug_);
+        if (utils::lane_change::passParkedObject(
+              route_handler, *candidate_path, target_objects.target_lane, lane_change_buffer,
+              is_goal_in_route, *lane_change_parameters_)) {
+          return false;
+        }
+        candidate_paths->push_back(*candidate_path);
+
+        if (!check_safety) {
+          return false;
+        }
 
-      if (is_safe) {
-        return true;
+        const auto [is_safe, is_object_coming_from_rear] = isLaneChangePathSafe(
+          *candidate_path, target_objects, lane_change_parameters_->rss_params, object_debug_);
+
+        if (is_safe) {
+          return true;
+        }
       }
     }
   }
diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp
index 4a2a5cd4af6b3..a4667e1641ca0 100644
--- a/planning/behavior_path_planner/src/utils/utils.cpp
+++ b/planning/behavior_path_planner/src/utils/utils.cpp
@@ -3147,15 +3147,12 @@ double calcMinimumLaneChangeLength(
   const double & max_lateral_acc = lat_acc.second;
   const double & lateral_jerk = common_param.lane_changing_lateral_jerk;
   const double & finish_judge_buffer = common_param.lane_change_finish_judge_buffer;
-  const auto prepare_length = 0.5 * common_param.max_acc *
-                              common_param.lane_change_prepare_duration *
-                              common_param.lane_change_prepare_duration;
 
   double accumulated_length = length_to_intersection;
   for (const auto & shift_interval : shift_intervals) {
     const double t =
       PathShifter::calcShiftTimeFromJerk(shift_interval, lateral_jerk, max_lateral_acc);
-    accumulated_length += vel * t + prepare_length + finish_judge_buffer;
+    accumulated_length += vel * t + finish_judge_buffer;
   }
   accumulated_length +=
     common_param.backward_length_buffer_for_end_of_lane * (shift_intervals.size() - 1.0);

From 44fb0fd3b0da4cf39da19216cd5dada4dbcecbb0 Mon Sep 17 00:00:00 2001
From: SakodaShintaro <shintaro.sakoda@tier4.jp>
Date: Fri, 22 Sep 2023 17:56:06 +0900
Subject: [PATCH 10/35] feat(ar_tag_based_localizer): split the package
 `ar_tag_based_localizer` (#5043)

* Fix package name

Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp>

* Removed utils

Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp>

* Renamed tag_tf_caster to landmark_tf_caster

Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp>

* Updated node_diagram

Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp>

* Fixed documents

Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp>

* style(pre-commit): autofix

* Fixed the directory name

Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp>

* Fixed to split packages

Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp>

* Removed unused package dependency

Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp>

* style(pre-commit): autofix

* Fixed directory structure

Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp>

* style(pre-commit): autofix

* Fixed ArTagDetector to ArTagBasedLocalizer

Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp>

---------

Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
---
 .../ar_tag_based_localizer.launch.xml         |   4 +-
 launch/tier4_localization_launch/package.xml  |   2 +-
 .../include/ar_tag_based_localizer/utils.hpp  |  66 ----------
 .../launch/tag_tf_caster.launch.xml           |  12 --
 .../ar_tag_based_localizer/src/utils.cpp      | 116 -----------------
 .../landmark_based_localizer/README.md        | 123 ++++++++++++++++++
 .../ar_tag_based_localizer/CMakeLists.txt     |  16 ---
 .../ar_tag_based_localizer/README.md          | 109 +---------------
 .../config/ar_tag_based_localizer.param.yaml  |   0
 .../doc_image/ar_tag_image.png                | Bin
 .../doc_image/sample_result.png               | Bin
 .../doc_image/sample_result_in_awsim.png      | Bin
 .../ar_tag_based_localizer_core.hpp           |   1 +
 .../launch/ar_tag_based_localizer.launch.xml  |   0
 .../ar_tag_based_localizer/package.xml        |   2 -
 .../src/ar_tag_based_localizer_core.cpp       |  48 ++++++-
 .../src/ar_tag_based_localizer_node.cpp       |   0
 .../lanelet2_data_structure.drawio.svg        |   6 +-
 .../doc_image/node_diagram.drawio.svg         |  18 +--
 .../doc_image/principle.png                   | Bin
 .../landmark_tf_caster/CMakeLists.txt         |  30 +++++
 .../config/landmark_tf_caster.param.yaml}     |   0
 .../landmark_tf_caster_core.hpp}              |  10 +-
 .../launch/landmark_tf_caster.launch.xml      |  12 ++
 .../landmark_tf_caster/package.xml            |  25 ++++
 .../src/landmark_tf_caster_core.cpp}          |  10 +-
 .../src/landmark_tf_caster_node.cpp}          |   4 +-
 27 files changed, 267 insertions(+), 347 deletions(-)
 delete mode 100644 localization/ar_tag_based_localizer/include/ar_tag_based_localizer/utils.hpp
 delete mode 100644 localization/ar_tag_based_localizer/launch/tag_tf_caster.launch.xml
 delete mode 100644 localization/ar_tag_based_localizer/src/utils.cpp
 create mode 100644 localization/landmark_based_localizer/README.md
 rename localization/{ => landmark_based_localizer}/ar_tag_based_localizer/CMakeLists.txt (74%)
 rename localization/{ => landmark_based_localizer}/ar_tag_based_localizer/README.md (52%)
 rename localization/{ => landmark_based_localizer}/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml (100%)
 rename localization/{ => landmark_based_localizer}/ar_tag_based_localizer/doc_image/ar_tag_image.png (100%)
 rename localization/{ => landmark_based_localizer}/ar_tag_based_localizer/doc_image/sample_result.png (100%)
 rename localization/{ => landmark_based_localizer}/ar_tag_based_localizer/doc_image/sample_result_in_awsim.png (100%)
 rename localization/{ => landmark_based_localizer}/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp (98%)
 rename localization/{ => landmark_based_localizer}/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml (100%)
 rename localization/{ => landmark_based_localizer}/ar_tag_based_localizer/package.xml (92%)
 rename localization/{ => landmark_based_localizer}/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp (89%)
 rename localization/{ => landmark_based_localizer}/ar_tag_based_localizer/src/ar_tag_based_localizer_node.cpp (100%)
 rename localization/{ar_tag_based_localizer => landmark_based_localizer}/doc_image/lanelet2_data_structure.drawio.svg (91%)
 rename localization/{ar_tag_based_localizer => landmark_based_localizer}/doc_image/node_diagram.drawio.svg (85%)
 rename localization/{ar_tag_based_localizer => landmark_based_localizer}/doc_image/principle.png (100%)
 create mode 100644 localization/landmark_based_localizer/landmark_tf_caster/CMakeLists.txt
 rename localization/{ar_tag_based_localizer/config/tag_tf_caster.param.yaml => landmark_based_localizer/landmark_tf_caster/config/landmark_tf_caster.param.yaml} (100%)
 rename localization/{ar_tag_based_localizer/include/ar_tag_based_localizer/tag_tf_caster_core.hpp => landmark_based_localizer/landmark_tf_caster/include/landmark_tf_caster/landmark_tf_caster_core.hpp} (84%)
 create mode 100644 localization/landmark_based_localizer/landmark_tf_caster/launch/landmark_tf_caster.launch.xml
 create mode 100644 localization/landmark_based_localizer/landmark_tf_caster/package.xml
 rename localization/{ar_tag_based_localizer/src/tag_tf_caster_core.cpp => landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_core.cpp} (93%)
 rename localization/{ar_tag_based_localizer/src/tag_tf_caster_node.cpp => landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_node.cpp} (85%)

diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml
index 642edb4f68ab8..a767d2d253cff 100644
--- a/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml
+++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml
@@ -14,9 +14,9 @@
     </group>
 
     <group>
-      <include file="$(find-pkg-share ar_tag_based_localizer)/launch/tag_tf_caster.launch.xml">
+      <include file="$(find-pkg-share landmark_tf_caster)/launch/landmark_tf_caster.launch.xml">
         <arg name="input_lanelet2_map" value="/map/vector_map"/>
-        <arg name="param_file" value="$(find-pkg-share ar_tag_based_localizer)/config/tag_tf_caster.param.yaml"/>
+        <arg name="param_file" value="$(find-pkg-share landmark_tf_caster)/config/landmark_tf_caster.param.yaml"/>
       </include>
     </group>
   </group>
diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml
index f00752d1c14fc..7991ffb0d8adf 100644
--- a/launch/tier4_localization_launch/package.xml
+++ b/launch/tier4_localization_launch/package.xml
@@ -14,13 +14,13 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
-  <exec_depend>ar_tag_based_localizer</exec_depend>
   <exec_depend>automatic_pose_initializer</exec_depend>
   <exec_depend>eagleye_fix2pose</exec_depend>
   <exec_depend>eagleye_gnss_converter</exec_depend>
   <exec_depend>eagleye_rt</exec_depend>
   <exec_depend>ekf_localizer</exec_depend>
   <exec_depend>gyro_odometer</exec_depend>
+  <exec_depend>landmark_based_localizer</exec_depend>
   <exec_depend>ndt_scan_matcher</exec_depend>
   <exec_depend>pointcloud_preprocessor</exec_depend>
   <exec_depend>pose_initializer</exec_depend>
diff --git a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/utils.hpp b/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/utils.hpp
deleted file mode 100644
index 06401dad8fdd1..0000000000000
--- a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/utils.hpp
+++ /dev/null
@@ -1,66 +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.
-
-// This source code is derived from the https://github.com/pal-robotics/aruco_ros.
-// Here is the license statement.
-/*****************************
- Copyright 2011 Rafael Muñoz Salinas. All rights reserved.
-
- Redistribution and use in source and binary forms, with or without modification, are
- permitted provided that the following conditions are met:
-
- 1. Redistributions of source code must retain the above copyright notice, this list of
- conditions and the following disclaimer.
-
- 2. Redistributions in binary form must reproduce the above copyright notice, this list
- of conditions and the following disclaimer in the documentation and/or other materials
- provided with the distribution.
-
- THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED
- WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
- FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR
- CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
- SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
- ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
- NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
- ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-
- The views and conclusions contained in the software and documentation are those of the
- authors and should not be interpreted as representing official policies, either expressed
- or implied, of Rafael Muñoz Salinas.
- ********************************/
-
-#ifndef AR_TAG_BASED_LOCALIZER__UTILS_HPP_
-#define AR_TAG_BASED_LOCALIZER__UTILS_HPP_
-
-#include <sensor_msgs/msg/camera_info.hpp>
-
-#include <aruco/aruco.h>
-#include <tf2/LinearMath/Transform.h>
-
-/**
- * @brief ros_camera_info_to_aruco_cam_params gets the camera intrinsics from a CameraInfo message
- * and copies them to aruco's data structure
- * @param cam_info
- * @param use_rectified_parameters if true, the intrinsics are taken from cam_info.P and the
- * distortion parameters are set to 0. Otherwise, cam_info.K and cam_info.D are taken.
- * @return
- */
-aruco::CameraParameters ros_camera_info_to_aruco_cam_params(
-  const sensor_msgs::msg::CameraInfo & cam_info, bool use_rectified_parameters);
-
-tf2::Transform aruco_marker_to_tf2(const aruco::Marker & marker);
-
-#endif  // AR_TAG_BASED_LOCALIZER__UTILS_HPP_
diff --git a/localization/ar_tag_based_localizer/launch/tag_tf_caster.launch.xml b/localization/ar_tag_based_localizer/launch/tag_tf_caster.launch.xml
deleted file mode 100644
index 6a9c3dcd17e2e..0000000000000
--- a/localization/ar_tag_based_localizer/launch/tag_tf_caster.launch.xml
+++ /dev/null
@@ -1,12 +0,0 @@
-<launch>
-  <arg name="param_file" default="$(find-pkg-share ar_tag_based_localizer)/config/tag_tf_caster.param.yaml"/>
-  <arg name="node_name" default="tag_tf_caster"/>
-
-  <!-- Topic names -->
-  <arg name="input_lanelet2_map" default="~/input/lanelet2_map"/>
-
-  <node pkg="ar_tag_based_localizer" exec="tag_tf_caster" name="$(var node_name)" output="log">
-    <remap from="~/input/lanelet2_map" to="$(var input_lanelet2_map)"/>
-    <param from="$(var param_file)"/>
-  </node>
-</launch>
diff --git a/localization/ar_tag_based_localizer/src/utils.cpp b/localization/ar_tag_based_localizer/src/utils.cpp
deleted file mode 100644
index 9f582830280d9..0000000000000
--- a/localization/ar_tag_based_localizer/src/utils.cpp
+++ /dev/null
@@ -1,116 +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.
-
-// This source code is derived from the https://github.com/pal-robotics/aruco_ros.
-// Here is the license statement.
-/*****************************
- Copyright 2011 Rafael Muñoz Salinas. All rights reserved.
-
- Redistribution and use in source and binary forms, with or without modification, are
- permitted provided that the following conditions are met:
-
- 1. Redistributions of source code must retain the above copyright notice, this list of
- conditions and the following disclaimer.
-
- 2. Redistributions in binary form must reproduce the above copyright notice, this list
- of conditions and the following disclaimer in the documentation and/or other materials
- provided with the distribution.
-
- THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED
- WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
- FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR
- CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
- SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
- ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
- NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
- ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-
- The views and conclusions contained in the software and documentation are those of the
- authors and should not be interpreted as representing official policies, either expressed
- or implied, of Rafael Muñoz Salinas.
- ********************************/
-
-#include "ar_tag_based_localizer/utils.hpp"
-
-#include <opencv4/opencv2/calib3d.hpp>
-#include <rclcpp/logging.hpp>
-
-aruco::CameraParameters ros_camera_info_to_aruco_cam_params(
-  const sensor_msgs::msg::CameraInfo & cam_info, bool use_rectified_parameters)
-{
-  cv::Mat camera_matrix(3, 4, CV_64FC1, 0.0);
-  cv::Mat distortion_coeff(4, 1, CV_64FC1);
-  cv::Size size(static_cast<int>(cam_info.width), static_cast<int>(cam_info.height));
-
-  if (use_rectified_parameters) {
-    camera_matrix.setTo(0);
-    camera_matrix.at<double>(0, 0) = cam_info.p[0];
-    camera_matrix.at<double>(0, 1) = cam_info.p[1];
-    camera_matrix.at<double>(0, 2) = cam_info.p[2];
-    camera_matrix.at<double>(0, 3) = cam_info.p[3];
-    camera_matrix.at<double>(1, 0) = cam_info.p[4];
-    camera_matrix.at<double>(1, 1) = cam_info.p[5];
-    camera_matrix.at<double>(1, 2) = cam_info.p[6];
-    camera_matrix.at<double>(1, 3) = cam_info.p[7];
-    camera_matrix.at<double>(2, 0) = cam_info.p[8];
-    camera_matrix.at<double>(2, 1) = cam_info.p[9];
-    camera_matrix.at<double>(2, 2) = cam_info.p[10];
-    camera_matrix.at<double>(2, 3) = cam_info.p[11];
-
-    for (int i = 0; i < 4; ++i) {
-      distortion_coeff.at<double>(i, 0) = 0;
-    }
-  } else {
-    cv::Mat camera_matrix_from_k(3, 3, CV_64FC1, 0.0);
-    for (int i = 0; i < 9; ++i) {
-      camera_matrix_from_k.at<double>(i % 3, i - (i % 3) * 3) = cam_info.k[i];
-    }
-    camera_matrix_from_k.copyTo(camera_matrix(cv::Rect(0, 0, 3, 3)));
-
-    if (cam_info.d.size() == 4) {
-      for (int i = 0; i < 4; ++i) {
-        distortion_coeff.at<double>(i, 0) = cam_info.d[i];
-      }
-    } else {
-      RCLCPP_WARN(
-        rclcpp::get_logger("ar_tag_based_localizer"),
-        "length of camera_info D vector is not 4, assuming zero distortion...");
-      for (int i = 0; i < 4; ++i) {
-        distortion_coeff.at<double>(i, 0) = 0;
-      }
-    }
-  }
-
-  return {camera_matrix, distortion_coeff, size};
-}
-
-tf2::Transform aruco_marker_to_tf2(const aruco::Marker & marker)
-{
-  cv::Mat rot(3, 3, CV_64FC1);
-  cv::Mat r_vec64;
-  marker.Rvec.convertTo(r_vec64, CV_64FC1);
-  cv::Rodrigues(r_vec64, rot);
-  cv::Mat tran64;
-  marker.Tvec.convertTo(tran64, CV_64FC1);
-
-  tf2::Matrix3x3 tf_rot(
-    rot.at<double>(0, 0), rot.at<double>(0, 1), rot.at<double>(0, 2), rot.at<double>(1, 0),
-    rot.at<double>(1, 1), rot.at<double>(1, 2), rot.at<double>(2, 0), rot.at<double>(2, 1),
-    rot.at<double>(2, 2));
-
-  tf2::Vector3 tf_orig(tran64.at<double>(0, 0), tran64.at<double>(1, 0), tran64.at<double>(2, 0));
-
-  return tf2::Transform(tf_rot, tf_orig);
-}
diff --git a/localization/landmark_based_localizer/README.md b/localization/landmark_based_localizer/README.md
new file mode 100644
index 0000000000000..a8ebdc0c8f1e5
--- /dev/null
+++ b/localization/landmark_based_localizer/README.md
@@ -0,0 +1,123 @@
+# Landmark Based Localizer
+
+This directory contains packages for landmark-based localization.
+
+Landmarks are, for example
+
+- AR tags detected by camera
+- Boards characterized by intensity detected by LiDAR
+
+etc.
+
+Since these landmarks are easy to detect and estimate pose, the ego pose can be calculated from the pose of the detected landmark if the pose of the landmark is written on the map in advance.
+
+Currently, landmarks are assumed to be flat.
+
+The following figure shows the principle of localization in the case of `ar_tag_based_localizer`.
+
+![principle](./doc_image/principle.png)
+
+This calculated ego pose is passed to the EKF, where it is fused with the twist information and used to estimate a more accurate ego pose.
+
+## Node diagram
+
+![node diagram](./doc_image/node_diagram.drawio.svg)
+
+### `landmark_tf_caster` node
+
+The definitions of the landmarks written to the map are introduced in the next section. See `Map Specifications`.
+
+The `landmark_tf_caster` node publishes the TF from the map to the landmark.
+
+- Translation : The center of the four vertices of the landmark
+- Rotation : Let the vertex numbers be 1, 2, 3, 4 counterclockwise as shown in the next section. Direction is defined as the cross product of the vector from 1 to 2 and the vector from 2 to 3.
+
+Users can define landmarks as Lanelet2 4-vertex polygons.
+In this case, it is possible to define an arrangement in which the four vertices cannot be considered to be on the same plane. The direction of the landmark in that case is difficult to calculate.
+So, if the 4 vertices are considered as forming a tetrahedron and its volume exceeds the `volume_threshold` parameter, the landmark will not publish tf_static.
+
+### Landmark based localizer packages
+
+- ar_tag_based_localizer
+- etc.
+
+## Inputs / Outputs
+
+### `landmark_tf_caster` node
+
+#### Input
+
+| Name                   | Type                                         | Description      |
+| :--------------------- | :------------------------------------------- | :--------------- |
+| `~/input/lanelet2_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Data of lanelet2 |
+
+#### Output
+
+| Name        | Type                                   | Description        |
+| :---------- | :------------------------------------- | :----------------- |
+| `tf_static` | `geometry_msgs::msg::TransformStamped` | TF from map to tag |
+
+## Map specifications
+
+For this package to work correctly, the poses of the landmarks must be specified in the Lanelet2 map format that `map_loader` and `landmark_tf_caster` can interpret.
+
+The four vertices of a landmark are defined counterclockwise.
+
+The order of the four vertices is defined as follows. In the coordinate system of a landmark,
+
+- the x-axis is parallel to the vector from the first vertex to the second vertex
+- the y-axis is parallel to the vector from the second vertex to the third vertex
+
+![lanelet2 data structure](./doc_image/lanelet2_data_structure.drawio.svg)
+
+### Example of `lanelet2_map.osm`
+
+The values provided below are placeholders.
+Ensure to input the correct coordinates corresponding to the actual location where the landmark is placed, such as `lat`, `lon`, `mgrs_code`, `local_x`, `local_y`.
+
+For example, when using the AR tag, it would look like this.
+
+```xml
+...
+
+  <node id="1" lat="35.8xxxxx" lon="139.6xxxxx">
+    <tag k="mgrs_code" v="99XXX000000"/>
+    <tag k="local_x" v="22.2356"/>
+    <tag k="local_y" v="87.4506"/>
+    <tag k="ele" v="2.1725"/>
+  </node>
+  <node id="2" lat="35.8xxxxx" lon="139.6xxxxx">
+    <tag k="mgrs_code" v="99XXX000000"/>
+    <tag k="local_x" v="22.639"/>
+    <tag k="local_y" v="87.5886"/>
+    <tag k="ele" v="2.5947"/>
+  </node>
+  <node id="3" lat="35.8xxxxx" lon="139.6xxxxx">
+    <tag k="mgrs_code" v="99XXX000000"/>
+    <tag k="local_x" v="22.2331"/>
+    <tag k="local_y" v="87.4713"/>
+    <tag k="ele" v="3.0208"/>
+  </node>
+  <node id="4" lat="35.8xxxxx" lon="139.6xxxxx">
+    <tag k="mgrs_code" v="99XXX000000"/>
+    <tag k="local_x" v="21.8298"/>
+    <tag k="local_y" v="87.3332"/>
+    <tag k="ele" v="2.5985"/>
+  </node>
+
+...
+
+  <way id="5">
+    <nd ref="1"/>
+    <nd ref="2"/>
+    <nd ref="3"/>
+    <nd ref="4"/>
+    <tag k="type" v="pose_marker"/>
+    <tag k="subtype" v="apriltag_16h5"/>
+    <tag k="area" v="yes"/>
+    <tag k="marker_id" v="0"/>
+  </way>
+
+...
+
+```
diff --git a/localization/ar_tag_based_localizer/CMakeLists.txt b/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt
similarity index 74%
rename from localization/ar_tag_based_localizer/CMakeLists.txt
rename to localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt
index 89337b9bb966c..15aecd1f8ded9 100644
--- a/localization/ar_tag_based_localizer/CMakeLists.txt
+++ b/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt
@@ -14,13 +14,9 @@ ament_auto_find_build_dependencies()
 
 find_package(OpenCV REQUIRED)
 
-#
-# ar_tag_based_localizer
-#
 ament_auto_add_executable(ar_tag_based_localizer
   src/ar_tag_based_localizer_node.cpp
   src/ar_tag_based_localizer_core.cpp
-  src/utils.cpp
 )
 target_include_directories(ar_tag_based_localizer
   SYSTEM PUBLIC
@@ -28,18 +24,6 @@ target_include_directories(ar_tag_based_localizer
 )
 target_link_libraries(ar_tag_based_localizer ${OpenCV_LIBRARIES})
 
-#
-# tag_tf_caster
-#
-ament_auto_add_executable(tag_tf_caster
-  src/tag_tf_caster_node.cpp
-  src/tag_tf_caster_core.cpp
-)
-target_include_directories(tag_tf_caster
-  SYSTEM PUBLIC
-  ${OpenCV_INCLUDE_DIRS}
-)
-
 ament_auto_package(
   INSTALL_TO_SHARE
   launch
diff --git a/localization/ar_tag_based_localizer/README.md b/localization/landmark_based_localizer/ar_tag_based_localizer/README.md
similarity index 52%
rename from localization/ar_tag_based_localizer/README.md
rename to localization/landmark_based_localizer/ar_tag_based_localizer/README.md
index 81bc11a7cbc9b..8c51febc6f60c 100644
--- a/localization/ar_tag_based_localizer/README.md
+++ b/localization/landmark_based_localizer/ar_tag_based_localizer/README.md
@@ -1,17 +1,12 @@
 # AR Tag Based Localizer
 
-**ArTagBasedLocalizer** is a vision-based localization package.
+**ArTagBasedLocalizer** is a vision-based localization node.
 
 <img src="./doc_image/ar_tag_image.png" width="320px">
 
-This package uses [the ArUco library](https://index.ros.org/p/aruco/) to detect AR-Tags from camera images and calculates and publishes the pose of the ego vehicle based on these detections.
+This node uses [the ArUco library](https://index.ros.org/p/aruco/) to detect AR-Tags from camera images and calculates and publishes the pose of the ego vehicle based on these detections.
 The positions and orientations of the AR-Tags are assumed to be written in the Lanelet2 format.
 
-This package includes two nodes.
-
-- `ar_tag_based_localizer` : A node that detects AR-Tags from camera images and publishes the pose of the ego vehicle.
-- `tag_tf_caster` : A node that publishes the pose of the AR-Tags applied in Lanelet2 as `tf_static`.
-
 ## Inputs / Outputs
 
 ### `ar_tag_based_localizer` node
@@ -32,23 +27,9 @@ This package includes two nodes.
 | `~/debug/result`                | `sensor_msgs::msg::Image`                       | [debug topic] Image in which marker detection results are superimposed on the input image |
 | `tf`                            | `geometry_msgs::msg::TransformStamped`          | [debug topic] TF from camera to detected tag                                              |
 
-### `tag_tf_caster` node
-
-#### Input
-
-| Name                   | Type                                         | Description      |
-| :--------------------- | :------------------------------------------- | :--------------- |
-| `~/input/lanelet2_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Data of lanelet2 |
-
-#### Output
-
-| Name        | Type                                   | Description        |
-| :---------- | :------------------------------------- | :----------------- |
-| `tf_static` | `geometry_msgs::msg::TransformStamped` | TF from map to tag |
-
 ## How to launch
 
-When launching Autoware, specify `artag` for `pose_source`.
+When launching Autoware, set `artag` for `pose_source`.
 
 ```bash
 ros2 launch autoware_launch ... \
@@ -87,88 +68,6 @@ The pull request video below should also be helpful.
 
 <https://github.com/autowarefoundation/autoware.universe/pull/4347#issuecomment-1663155248>
 
-## Architecture
-
-![node diagram](./doc_image/node_diagram.drawio.svg)
-
 ## Principle
 
-### `ar_tag_based_localizer` node
-
-![principle](./doc_image/principle.png)
-
-### `tag_tf_caster` node
-
-The definitions of the tags written to the map are introduced in the next section. See `Map Specifications`.
-
-The `tag_tf_caster` node publishes the TF from the map to the tag.
-
-- Translation : The center of the four vertices of the tag
-- Rotation : Let the vertex numbers be 1, 2, 3, 4 counterclockwise as shown in the next section. Direction is defined as the cross product of the vector from 1 to 2 and the vector from 2 to 3.
-
-Users can define tags as Lanelet2 4-vertex polygons.
-In this case, it is possible to define an arrangement in which the four vertices cannot be considered to be on the same plane. The direction of the tag in that case is difficult to calculate.
-So, if the 4 vertices are considered as forming a tetrahedron and its volume exceeds the `volume_threshold` parameter, the tag will not publish tf_static.
-
-## Map specifications
-
-For this package to function correctly, the pose of the AR-Tags must be specified in the Lanelet2 map format that Autoware can interpret.
-
-The four vertices of AR-Tag are defined counterclockwise.
-
-The order of the four vertices is defined as follows. In the coordinate system of AR-Tag,
-
-- the x-axis is parallel to the vector from the first vertex to the second vertex
-- the y-axis is parallel to the vector from the second vertex to the third vertex
-
-![lanelet2 data structure](./doc_image/lanelet2_data_structure.drawio.svg)
-
-### example of `lanelet2_map.osm`
-
-The values provided below are placeholders.
-Ensure to input the correct coordinates corresponding to the actual location where the AR-Tag is placed, such as `lat`, `lon`, `mgrs_code`, `local_x`, `local_y`.
-
-```xml
-...
-
-  <node id="1" lat="35.8xxxxx" lon="139.6xxxxx">
-    <tag k="mgrs_code" v="99XXX000000"/>
-    <tag k="local_x" v="22.2356"/>
-    <tag k="local_y" v="87.4506"/>
-    <tag k="ele" v="2.1725"/>
-  </node>
-  <node id="2" lat="35.8xxxxx" lon="139.6xxxxx">
-    <tag k="mgrs_code" v="99XXX000000"/>
-    <tag k="local_x" v="22.639"/>
-    <tag k="local_y" v="87.5886"/>
-    <tag k="ele" v="2.5947"/>
-  </node>
-  <node id="3" lat="35.8xxxxx" lon="139.6xxxxx">
-    <tag k="mgrs_code" v="99XXX000000"/>
-    <tag k="local_x" v="22.2331"/>
-    <tag k="local_y" v="87.4713"/>
-    <tag k="ele" v="3.0208"/>
-  </node>
-  <node id="4" lat="35.8xxxxx" lon="139.6xxxxx">
-    <tag k="mgrs_code" v="99XXX000000"/>
-    <tag k="local_x" v="21.8298"/>
-    <tag k="local_y" v="87.3332"/>
-    <tag k="ele" v="2.5985"/>
-  </node>
-
-...
-
-  <way id="5">
-    <nd ref="1"/>
-    <nd ref="2"/>
-    <nd ref="3"/>
-    <nd ref="4"/>
-    <tag k="type" v="pose_marker"/>
-    <tag k="subtype" v="apriltag_16h5"/>
-    <tag k="area" v="yes"/>
-    <tag k="marker_id" v="0"/>
-  </way>
-
-...
-
-```
+![principle](../doc_image/principle.png)
diff --git a/localization/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml b/localization/landmark_based_localizer/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml
similarity index 100%
rename from localization/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml
rename to localization/landmark_based_localizer/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml
diff --git a/localization/ar_tag_based_localizer/doc_image/ar_tag_image.png b/localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/ar_tag_image.png
similarity index 100%
rename from localization/ar_tag_based_localizer/doc_image/ar_tag_image.png
rename to localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/ar_tag_image.png
diff --git a/localization/ar_tag_based_localizer/doc_image/sample_result.png b/localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/sample_result.png
similarity index 100%
rename from localization/ar_tag_based_localizer/doc_image/sample_result.png
rename to localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/sample_result.png
diff --git a/localization/ar_tag_based_localizer/doc_image/sample_result_in_awsim.png b/localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/sample_result_in_awsim.png
similarity index 100%
rename from localization/ar_tag_based_localizer/doc_image/sample_result_in_awsim.png
rename to localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/sample_result_in_awsim.png
diff --git a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp b/localization/landmark_based_localizer/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp
similarity index 98%
rename from localization/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp
rename to localization/landmark_based_localizer/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp
index 65c1283a82728..6af9fefae23f7 100644
--- a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp
+++ b/localization/landmark_based_localizer/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp
@@ -72,6 +72,7 @@ class ArTagBasedLocalizer : public rclcpp::Node
   void ekf_pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped & msg);
   void publish_pose_as_base_link(
     const geometry_msgs::msg::PoseStamped & msg, const std::string & camera_frame_id);
+  static tf2::Transform aruco_marker_to_tf2(const aruco::Marker & marker);
 
   // Parameters
   float marker_size_{};
diff --git a/localization/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml
similarity index 100%
rename from localization/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml
rename to localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml
diff --git a/localization/ar_tag_based_localizer/package.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml
similarity index 92%
rename from localization/ar_tag_based_localizer/package.xml
rename to localization/landmark_based_localizer/ar_tag_based_localizer/package.xml
index 4527d2ad9b086..e68fd21ed340a 100644
--- a/localization/ar_tag_based_localizer/package.xml
+++ b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml
@@ -12,11 +12,9 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
   <depend>aruco</depend>
-  <depend>autoware_auto_mapping_msgs</depend>
   <depend>cv_bridge</depend>
   <depend>geometry_msgs</depend>
   <depend>image_transport</depend>
-  <depend>lanelet2_extension</depend>
   <depend>rclcpp</depend>
   <depend>sensor_msgs</depend>
   <depend>tf2_eigen</depend>
diff --git a/localization/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp
similarity index 89%
rename from localization/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp
rename to localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp
index cf0236c1a1e48..f664ef6378a69 100644
--- a/localization/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp
+++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp
@@ -44,12 +44,12 @@
 
 #include "ar_tag_based_localizer/ar_tag_based_localizer_core.hpp"
 
-#include "ar_tag_based_localizer/utils.hpp"
-
 #include <Eigen/Core>
 #include <Eigen/Geometry>
+#include <opencv4/opencv2/calib3d.hpp>
 
 #include <cv_bridge/cv_bridge.h>
+#include <tf2/LinearMath/Transform.h>
 
 #ifdef ROS_DISTRO_GALACTIC
 #include <tf2_eigen/tf2_eigen.h>
@@ -208,7 +208,30 @@ void ArTagBasedLocalizer::cam_info_callback(const sensor_msgs::msg::CameraInfo &
     return;
   }
 
-  cam_param_ = ros_camera_info_to_aruco_cam_params(msg, true);
+  cv::Mat camera_matrix(3, 4, CV_64FC1, 0.0);
+  cv::Mat distortion_coeff(4, 1, CV_64FC1);
+  cv::Size size(static_cast<int>(msg.width), static_cast<int>(msg.height));
+
+  camera_matrix.setTo(0);
+  camera_matrix.at<double>(0, 0) = msg.p[0];
+  camera_matrix.at<double>(0, 1) = msg.p[1];
+  camera_matrix.at<double>(0, 2) = msg.p[2];
+  camera_matrix.at<double>(0, 3) = msg.p[3];
+  camera_matrix.at<double>(1, 0) = msg.p[4];
+  camera_matrix.at<double>(1, 1) = msg.p[5];
+  camera_matrix.at<double>(1, 2) = msg.p[6];
+  camera_matrix.at<double>(1, 3) = msg.p[7];
+  camera_matrix.at<double>(2, 0) = msg.p[8];
+  camera_matrix.at<double>(2, 1) = msg.p[9];
+  camera_matrix.at<double>(2, 2) = msg.p[10];
+  camera_matrix.at<double>(2, 3) = msg.p[11];
+
+  for (int i = 0; i < 4; ++i) {
+    distortion_coeff.at<double>(i, 0) = 0;
+  }
+
+  cam_param_ = aruco::CameraParameters(camera_matrix, distortion_coeff, size);
+
   cam_info_received_ = true;
 }
 
@@ -324,3 +347,22 @@ void ArTagBasedLocalizer::publish_pose_as_base_link(
 
   pose_pub_->publish(pose_with_covariance_stamped);
 }
+
+tf2::Transform ArTagBasedLocalizer::aruco_marker_to_tf2(const aruco::Marker & marker)
+{
+  cv::Mat rot(3, 3, CV_64FC1);
+  cv::Mat r_vec64;
+  marker.Rvec.convertTo(r_vec64, CV_64FC1);
+  cv::Rodrigues(r_vec64, rot);
+  cv::Mat tran64;
+  marker.Tvec.convertTo(tran64, CV_64FC1);
+
+  tf2::Matrix3x3 tf_rot(
+    rot.at<double>(0, 0), rot.at<double>(0, 1), rot.at<double>(0, 2), rot.at<double>(1, 0),
+    rot.at<double>(1, 1), rot.at<double>(1, 2), rot.at<double>(2, 0), rot.at<double>(2, 1),
+    rot.at<double>(2, 2));
+
+  tf2::Vector3 tf_orig(tran64.at<double>(0, 0), tran64.at<double>(1, 0), tran64.at<double>(2, 0));
+
+  return tf2::Transform(tf_rot, tf_orig);
+}
diff --git a/localization/ar_tag_based_localizer/src/ar_tag_based_localizer_node.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_node.cpp
similarity index 100%
rename from localization/ar_tag_based_localizer/src/ar_tag_based_localizer_node.cpp
rename to localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_node.cpp
diff --git a/localization/ar_tag_based_localizer/doc_image/lanelet2_data_structure.drawio.svg b/localization/landmark_based_localizer/doc_image/lanelet2_data_structure.drawio.svg
similarity index 91%
rename from localization/ar_tag_based_localizer/doc_image/lanelet2_data_structure.drawio.svg
rename to localization/landmark_based_localizer/doc_image/lanelet2_data_structure.drawio.svg
index bc8ba297558bb..5895d8ef65073 100644
--- a/localization/ar_tag_based_localizer/doc_image/lanelet2_data_structure.drawio.svg
+++ b/localization/landmark_based_localizer/doc_image/lanelet2_data_structure.drawio.svg
@@ -6,7 +6,7 @@
   width="336px"
   height="336px"
   viewBox="-0.5 -0.5 336 336"
-  content="&lt;mxfile&gt;&lt;diagram id=&quot;z_GJx55S16-dNHi1224P&quot; name=&quot;ページ1&quot;&gt;5ZhNj5swEIZ/DVJ7wzYBcsymu+2lUtVU6tmCCVg1ODLOhuyvr1kMBDsROdBk1eQQ4ddfzDPj8SQeWRf1V0l3+XeRAvewn9Ye+eJhHC1i/d0Ix1Ygy6gVMsnSVkKDsGFvYETfqHuWQjUaqITgiu3GYiLKEhI10qiU4jAethV8vOuOZuAIm4RyV/3NUpW3aoyjQf8GLMu7nVG4bHsK2g02llQ5TcXhRCLPHllLIVT7VNRr4A27jks77+VCb/9iEkp1zYRg0c54pXxvjDMvpo6dtVLsyxSaCb5Hng45U7DZ0aTpPWj3ai1XBdctpB/NciAV1BffCfWW6ggBUYCSRz2kmxAaOCY6UAfrMLDux+SnnDuRGv9m/doDAv1gKFwgEjpEkINET9GxBtM4aLVrA3DL6gbhLHyCMR8cu3zwGTx4DjqRQ8cNmLvSIfiOdGKHDvnYdJB/QzpLh07wsejYJ+uWdBb+dCaGMl0115duJZxWFUvOJV9IndtrEsCJgYszBnaaBE4Vex0vf85qs8MPwfTGPd/YOpq+xa0Se5mAmXR6a1nrBP7EQorKDJSz0LsPequvcwty3FI7ftERp8aeqJQUf2AtuJBaKUXZRPSWcW5JlLOsbNypHQRaf2ril+lKY2U6Cpam/NJxGN/MW1EqUyuhcKZ0EVkHInIPBDkTL2SOA4Ed8sfHId/Xuscu5G8Hnvz/mQhbmZ7YpeO1qcheKPh3mSi4wi33vD2xXVvcsPJauFX7jHBO04enz9j7Zy5okQUtcqCFLrNwBmRuKf/2OPkVLS3uIbldgnV/Jqx+6vYvmn16kdrWz4/jh3jymkNoHjfo5vC/Spt/hz+nyPNf&lt;/diagram&gt;&lt;/mxfile&gt;"
+  content="&lt;mxfile&gt;&lt;diagram id=&quot;z_GJx55S16-dNHi1224P&quot; name=&quot;ページ1&quot;&gt;5ZhNb6MwEIZ/DdLuDWwC9Nhm2+5hV1ophz1beAJWDY6Mk5D++jXFfNmJkgNNqk0OEbz+wPPMeDzg4WVRv0qyyX8LCtxDPq09/MNDCCeJ/m+EQytEKGyFTDLaSsEgrNg7GNE36pZRqCYdlRBcsc1UTEVZQqomGpFS7Kfd1oJPn7ohGTjCKiXcVf8yqvJWTVA86D+BZXn35CB6aFsK0nU2llQ5oWI/kvCzh5dSCNVeFfUSeMOu49KOeznR2i9MQqkuGRAu2hE7wrfGOLMwdeislWJbUmgG+B5+2udMwWpD0qZ1r92rtVwVXN8F+tJdgFnTDqSCeiSZBb2CKEDJg+7StUYGjomOoIO1H1j3ffIx504kxr9ZP/eAQF8YCieIRA6RwEGih+hYg/M4SLVpA3DN6gbhLHzCKR+UuHzQETxoDjqxQ8cNmJvSweiGdBKHDv7adAL/inQeHDrh16Jj76xr0ln45zMxlPSxOb70XcpJVbH0wuQLdHKguQBGBi6OGNhpEjhRbDfNi8esNk/4I5heSc83sbamb3GrxFamYAaNTy1rntA/M5EiMgPlTPThg97qy9wSOG6pHb/o8FJTT1RKijdYCi6kVkpRNhG9ZpxbEuEsKxt3ao+B1p+aYGW60ng0DQWjlJ/aDtOTeS1KZWqlIJopXcTWhojdDYGPxAueY0Mgh/zhfsj3te6hC/nrgcf/fyZCVqbHdul4aSqyJwo/LxOFF7jllqcnsmuLK1ZeC7dqnxHOOH14eo99/OaCFlvQYgda5DKLZkDmlvLv95NfgweLe4Svl2Dd14RfpKQFkW/fXqS29fv9+CE5e8wFwTxu0LfDd5U2/w4fp/DzPw==&lt;/diagram&gt;&lt;/mxfile&gt;"
 >
   <defs/>
   <g>
@@ -138,12 +138,12 @@
           <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 108px; height: 1px; padding-top: 22px; margin-left: 8px;">
             <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;">
-                AR Tag(Front)
+                Landmark(Front)
               </div>
             </div>
           </div>
         </foreignObject>
-        <text x="62" y="27" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="16px" text-anchor="middle">AR Tag(Front)</text>
+        <text x="62" y="27" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="16px" text-anchor="middle">Landmark(Front)</text>
       </switch>
     </g>
   </g>
diff --git a/localization/ar_tag_based_localizer/doc_image/node_diagram.drawio.svg b/localization/landmark_based_localizer/doc_image/node_diagram.drawio.svg
similarity index 85%
rename from localization/ar_tag_based_localizer/doc_image/node_diagram.drawio.svg
rename to localization/landmark_based_localizer/doc_image/node_diagram.drawio.svg
index e41ca0ad019ce..120a1f2aa3391 100644
--- a/localization/ar_tag_based_localizer/doc_image/node_diagram.drawio.svg
+++ b/localization/landmark_based_localizer/doc_image/node_diagram.drawio.svg
@@ -6,20 +6,20 @@
   width="721px"
   height="331px"
   viewBox="-0.5 -0.5 721 331"
-  content="&lt;mxfile&gt;&lt;diagram id=&quot;z_GJx55S16-dNHi1224P&quot; name=&quot;ページ1&quot;&gt;1ZlLc5swEIB/jY/1WAiwfaydOD2k00yTmbYnRgYBmmDECPnVX1/JCDBIjomNkykHBy0SSN+u9qEM4Hy1e2Aoi7/TACcDaxTsBvBuYFnTiSt+pWBfCGygBBEjQSECteCZ/MVKOFLSNQlw3ujIKU04yZpCn6Yp9nlDhhij22a3kCbNr2Yowprg2UeJLv1FAh4X0ok1ruXfMIni8svAnRZPVqjsrFaSxyig2yMRvB/AOaOUF3er3Rwnkl3JpRi3OPG0mhjDKe8ywCoGbFCyVmtT8+L7crGMrtMAy/6jAZyFNOVKIcBS7TlNKDt0hqPDJeT6RNTcNphxvDsSqYk9YLrCnO1FF/XUchQkZSVQNbc18soi4iPcrpIhpeWoenMNQtwoFmYutsbl60/RfkGR+J2hXPCwhM1CINf6SIVpCCbsHDuSJEes5vN7ZyHmM8s5o6/46InrT/AyvC1d6Dbp2jrdqQGu3QNcR4P7g8eS3ujrmtOtWNUR3Az5r2I/5e9lu1jM5ya2E2sJXffGlvt5bOHYsKMXfEty7m0Jjz2fbhAjKPWxRhQHwsupJmU8phFNUXJfS2cCCdv/VsgPjT+yMRw7Zftud/z0bq9aJ6FyxCKsRFDJ5DzexMxwgjjZNL2xiZoa+kSJ+OxJ04f2pPmKnK6Zj9WoFvtqGp3UUfr5pjrISgaYt/CnNH2Lt9ULbzD+GN42dBq8rXHL0ItJaby1F1mjpuIsCG+muIlJcRnN8bu2UanHHeEHNQ4d1ZJ6BOq+1qFs7I8V2qvuCzpN3V+0/5Q6voyGQFzFoGttZNy0EQjfayNlRxqGOb5W/cDoRhHzOIq8pYz/XnJl1D8Vf/Rs4HD1E5ns9g6y9NAEHENscvqITcAEFb+Gl7NUUf5TWLrt/BR8JEujf/LRCjPkkTSk14SXfsL5R4UXLSo43cK5IU41XwQmTicfdIF/saFBfecV1sm1w6muC7v31KqzoU61lT6iFCeY6ytGeVaU6yHZyT0/yzAj4otY7l7xWlHf46daNBM+I0rFI1/gOAiqcloaaYDyuPIcuagjSBq90EwIoBAUOVj1946sRGm3SMhS+nlfIvACwsR0qFzvIkAcSb+fD/NN1I//mLSszZLZQduBmPyHPb1eLbZehMk6IfRyLvTvdzHGii/oapllgXNbL9EZgTE3lwFeYPBRzv+vgr4d2gGYGuzpZgHJ1o9MevNotiG6lNr7BLsxpjGJcmreCmUin0HBxcnM5xxZtK3H6I1uZz2uialMDd9dcp05uQDNVOdcpmOu3KpizVi5GQ3zfNg+mUL1fwAybmWul2ZMbvskxRkP+8qZRLM+By+61/9MgPf/AA==&lt;/diagram&gt;&lt;/mxfile&gt;"
+  content="&lt;mxfile&gt;&lt;diagram id=&quot;z_GJx55S16-dNHi1224P&quot; name=&quot;ページ1&quot;&gt;1VnLjpswFP2aLBsFG0iybF7TxVQdaSq1XSEHDFgDGBnn1a+vHQwE8ExIQiYqm+BjA/Y51/fhDOA83j8xlIbfqYejARh5+wFcDAAwgA3Ej0QOOWIDMwcCRjw1qAJeyV+swJFCN8TDWW0gpzTiJK2DLk0S7PIahhiju/own0b1r6YowC3g1UVRG/1FPB7m6ASMK/wbJkFYfNmwp3lPjIrBaiVZiDy6O4HgcgDnjFKe38X7OY4keQUv+XOrd3rLiTGc8C4PKCG2KNqotal58UOxWEY3iYfl+NEAznyacCWIAVR7TiPKjoPh6HgJvD0RNbctZhzvTyA1sSdMY8zZQQxRvcBSJCkrgaq5qygvsfCEblthSKkclG+uiBA3igs9L2aLl2eUeDFibwKdoUzwAWyxXkOu9ZkK0xCcsHPckSg64Wo+X1orMZ9Zxhl9wyc9tjvBa/++7MJxnV2zza4x0rBr9sCu1WL3Bw8lfaOvG053Ylkn7KbIfRMbKruU3NVqPteROwFraNt3Nl37ceTCsWZPr/iOZNzZER46Lt0iRlDi4hal2BN+TjUp4yENaIKiZYXOBCfs8Ftxfmz8kY3h2Crai/1p7+KgWu+yyhELsIKgwuQ8PuSZ4Qhxsq37Yx1r6tEXSsRnK+Nv6APNSf0VGd0wF6unGtyX0+gkR+Hp63KQWIaYj+hPaPIR36AXvo3x5/BtQqvGNxg3DD2fVIvv1ovAqC4cgPBuwk10wqU0wxdto0LHPeFHGYeWakkdDXVfaSgbh1NBe9U+Z6eu/VX7T8nxZTQ0xJU/dKuNjOs2AuGlNlIMpL6f4VvlN7RuFDGHo8BZywzAiW6M++8FoHY+cLz6CU1mcwcBTWyyNLHJ6iM2GTpS8Zt/PZcqzD+ES7uZoRqfyaXWP7koxgw5JPHpLeGln3D+WeGlFRWsbuFcE6fqLzImVicfdIV/MaFGvvOCdXLtcNrWwuw9tepsqNPWSkUxhSPM2ytGWZoX7D7Zyz0/SzEj4otY7l7xWlHh45cKmgmfESSiyxV0HIGyoJZG6qEsLD1HJgoJkgQ/aSoAKIA8Byt/FyQOxFIispZ+3pUUOB5hYjpUrnflIY6k38+G2Tbox39MGtYGZHbQdCA6/2FOb5fFbFdhsk7wnYwL/d0uxljya3S1zKLAua+X6EyBNjePVKXvCC5clPH/q65vxnfDmGqM6m5RyWyfnPTm1kxNiCkkfIDxaHOZSHk2J0apSGqQd3VG85iDi6b1aF3S/azH1nEq88OL664zxxdGPd85l+7oy7eyYtOWb91C9QPTpuYRILw2bbKbxynWeNhX4iSa1XF4Prz6UwEu/wE=&lt;/diagram&gt;&lt;/mxfile&gt;"
 >
   <defs/>
   <g>
-    <rect x="170" y="0" width="210" height="60" fill="rgb(255, 255, 255)" stroke="rgb(0, 0, 0)" pointer-events="all"/>
-    <rect x="280" y="10" width="90" height="40" fill="#cce5ff" stroke="#6c8ebf" pointer-events="all"/>
+    <rect x="170" y="0" width="230" height="60" fill="rgb(255, 255, 255)" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="290" y="10" width="100" height="40" fill="#cce5ff" stroke="#6c8ebf" pointer-events="all"/>
     <g fill="#000000" font-family="Helvetica" text-anchor="middle" font-size="12px">
-      <text x="324.5" y="27.5">AR Tag Based</text>
-      <text x="324.5" y="41.5">Localizer</text>
+      <text x="339.5" y="27.5">Landmark Based</text>
+      <text x="339.5" y="41.5">Localizer</text>
     </g>
-    <rect x="180" y="10" width="90" height="40" fill="#ccffcc" stroke="#82b366" pointer-events="all"/>
+    <rect x="180" y="10" width="100" height="40" fill="#ccffcc" stroke="#82b366" pointer-events="all"/>
     <g fill="#000000" font-family="Helvetica" text-anchor="middle" font-size="12px">
-      <text x="224.5" y="27.5">Other Autoware</text>
-      <text x="224.5" y="41.5">packages</text>
+      <text x="229.5" y="27.5">Other Autoware</text>
+      <text x="229.5" y="41.5">packages</text>
     </g>
     <path d="M 280 318 L 415 318 Q 425 318 435 317.97 L 563.63 317.52" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
     <path d="M 568.88 317.5 L 561.89 321.03 L 563.63 317.52 L 561.87 314.03 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
@@ -74,7 +74,7 @@
     </g>
     <rect x="320" y="89.5" width="150" height="50" fill="#cce5ff" stroke="#6c8ebf" pointer-events="all"/>
     <g fill="#000000" font-family="Helvetica" text-anchor="middle" font-size="12px">
-      <text x="394.5" y="119">/tag_tf_caster</text>
+      <text x="394.5" y="119">/landmark_tf_caster</text>
     </g>
     <path d="M 270 115.13 L 313.63 114.91" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
     <path d="M 318.88 114.88 L 311.9 118.42 L 313.63 114.91 L 311.86 111.42 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
diff --git a/localization/ar_tag_based_localizer/doc_image/principle.png b/localization/landmark_based_localizer/doc_image/principle.png
similarity index 100%
rename from localization/ar_tag_based_localizer/doc_image/principle.png
rename to localization/landmark_based_localizer/doc_image/principle.png
diff --git a/localization/landmark_based_localizer/landmark_tf_caster/CMakeLists.txt b/localization/landmark_based_localizer/landmark_tf_caster/CMakeLists.txt
new file mode 100644
index 0000000000000..47899d716e411
--- /dev/null
+++ b/localization/landmark_based_localizer/landmark_tf_caster/CMakeLists.txt
@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 3.14)
+project(landmark_tf_caster)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+find_package(OpenCV REQUIRED)
+
+ament_auto_add_executable(landmark_tf_caster
+  src/landmark_tf_caster_node.cpp
+  src/landmark_tf_caster_core.cpp
+)
+target_include_directories(landmark_tf_caster
+  SYSTEM PUBLIC
+  ${OpenCV_INCLUDE_DIRS}
+)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+  config
+)
diff --git a/localization/ar_tag_based_localizer/config/tag_tf_caster.param.yaml b/localization/landmark_based_localizer/landmark_tf_caster/config/landmark_tf_caster.param.yaml
similarity index 100%
rename from localization/ar_tag_based_localizer/config/tag_tf_caster.param.yaml
rename to localization/landmark_based_localizer/landmark_tf_caster/config/landmark_tf_caster.param.yaml
diff --git a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/tag_tf_caster_core.hpp b/localization/landmark_based_localizer/landmark_tf_caster/include/landmark_tf_caster/landmark_tf_caster_core.hpp
similarity index 84%
rename from localization/ar_tag_based_localizer/include/ar_tag_based_localizer/tag_tf_caster_core.hpp
rename to localization/landmark_based_localizer/landmark_tf_caster/include/landmark_tf_caster/landmark_tf_caster_core.hpp
index 5b10fe5b1dc0f..0eb6706a0b5b0 100644
--- a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/tag_tf_caster_core.hpp
+++ b/localization/landmark_based_localizer/landmark_tf_caster/include/landmark_tf_caster/landmark_tf_caster_core.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef AR_TAG_BASED_LOCALIZER__TAG_TF_CASTER_CORE_HPP_
-#define AR_TAG_BASED_LOCALIZER__TAG_TF_CASTER_CORE_HPP_
+#ifndef LANDMARK_TF_CASTER__LANDMARK_TF_CASTER_CORE_HPP_
+#define LANDMARK_TF_CASTER__LANDMARK_TF_CASTER_CORE_HPP_
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -26,10 +26,10 @@
 
 #include <memory>
 
-class TagTfCaster : public rclcpp::Node
+class LandmarkTfCaster : public rclcpp::Node
 {
 public:
-  TagTfCaster();
+  LandmarkTfCaster();
 
 private:
   void map_bin_callback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg);
@@ -46,4 +46,4 @@ class TagTfCaster : public rclcpp::Node
   rclcpp::Subscription<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr map_bin_sub_;
 };
 
-#endif  // AR_TAG_BASED_LOCALIZER__TAG_TF_CASTER_CORE_HPP_
+#endif  // LANDMARK_TF_CASTER__LANDMARK_TF_CASTER_CORE_HPP_
diff --git a/localization/landmark_based_localizer/landmark_tf_caster/launch/landmark_tf_caster.launch.xml b/localization/landmark_based_localizer/landmark_tf_caster/launch/landmark_tf_caster.launch.xml
new file mode 100644
index 0000000000000..eeaba555cb6f5
--- /dev/null
+++ b/localization/landmark_based_localizer/landmark_tf_caster/launch/landmark_tf_caster.launch.xml
@@ -0,0 +1,12 @@
+<launch>
+  <arg name="param_file" default="$(find-pkg-share landmark_tf_caster)/config/landmark_tf_caster.param.yaml"/>
+  <arg name="node_name" default="landmark_tf_caster"/>
+
+  <!-- Topic names -->
+  <arg name="input_lanelet2_map" default="~/input/lanelet2_map"/>
+
+  <node pkg="landmark_tf_caster" exec="landmark_tf_caster" name="$(var node_name)" output="log">
+    <remap from="~/input/lanelet2_map" to="$(var input_lanelet2_map)"/>
+    <param from="$(var param_file)"/>
+  </node>
+</launch>
diff --git a/localization/landmark_based_localizer/landmark_tf_caster/package.xml b/localization/landmark_based_localizer/landmark_tf_caster/package.xml
new file mode 100644
index 0000000000000..9740e26e2d04a
--- /dev/null
+++ b/localization/landmark_based_localizer/landmark_tf_caster/package.xml
@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>landmark_tf_caster</name>
+  <version>0.0.0</version>
+  <description>The landmark_tf_caster package</description>
+  <maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
+  <maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_auto_mapping_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>lanelet2_extension</depend>
+  <depend>rclcpp</depend>
+  <depend>tf2_eigen</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/localization/ar_tag_based_localizer/src/tag_tf_caster_core.cpp b/localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_core.cpp
similarity index 93%
rename from localization/ar_tag_based_localizer/src/tag_tf_caster_core.cpp
rename to localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_core.cpp
index 427bf513058d4..801e036415326 100644
--- a/localization/ar_tag_based_localizer/src/tag_tf_caster_core.cpp
+++ b/localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_core.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "ar_tag_based_localizer/tag_tf_caster_core.hpp"
+#include "landmark_tf_caster/landmark_tf_caster_core.hpp"
 
 #include "lanelet2_extension/utility/message_conversion.hpp"
 
@@ -26,7 +26,7 @@
 #include <tf2_eigen/tf2_eigen.hpp>
 #endif
 
-TagTfCaster::TagTfCaster() : Node("tag_tf_caster")
+LandmarkTfCaster::LandmarkTfCaster() : Node("landmark_tf_caster")
 {
   // Parameters
   volume_threshold_ = this->declare_parameter("volume_threshold", 1e-5);
@@ -38,11 +38,11 @@ TagTfCaster::TagTfCaster() : Node("tag_tf_caster")
   // Subscribers
   map_bin_sub_ = this->create_subscription<autoware_auto_mapping_msgs::msg::HADMapBin>(
     "~/input/lanelet2_map", rclcpp::QoS(10).durability(rclcpp::DurabilityPolicy::TransientLocal),
-    std::bind(&TagTfCaster::map_bin_callback, this, std::placeholders::_1));
+    std::bind(&LandmarkTfCaster::map_bin_callback, this, std::placeholders::_1));
   RCLCPP_INFO(this->get_logger(), "finish setup");
 }
 
-void TagTfCaster::map_bin_callback(
+void LandmarkTfCaster::map_bin_callback(
   const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg)
 {
   RCLCPP_INFO_STREAM(this->get_logger(), "msg->format_version: " << msg->format_version);
@@ -60,7 +60,7 @@ void TagTfCaster::map_bin_callback(
   }
 }
 
-void TagTfCaster::publish_tf(const lanelet::Polygon3d & poly)
+void LandmarkTfCaster::publish_tf(const lanelet::Polygon3d & poly)
 {
   // Get marker_id
   const std::string marker_id = poly.attributeOr("marker_id", "none");
diff --git a/localization/ar_tag_based_localizer/src/tag_tf_caster_node.cpp b/localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_node.cpp
similarity index 85%
rename from localization/ar_tag_based_localizer/src/tag_tf_caster_node.cpp
rename to localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_node.cpp
index ce83c572d6a6e..4c34e593e7552 100644
--- a/localization/ar_tag_based_localizer/src/tag_tf_caster_node.cpp
+++ b/localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_node.cpp
@@ -12,11 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "ar_tag_based_localizer/tag_tf_caster_core.hpp"
+#include "landmark_tf_caster/landmark_tf_caster_core.hpp"
 
 int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<TagTfCaster>());
+  rclcpp::spin(std::make_shared<LandmarkTfCaster>());
   rclcpp::shutdown();
 }

From b58c009ce5875b73cf385884f3eafe75b983c8a9 Mon Sep 17 00:00:00 2001
From: TaikiYamada4 <129915538+TaikiYamada4@users.noreply.github.com>
Date: Fri, 22 Sep 2023 18:02:39 +0900
Subject: [PATCH 11/35] docs(ndt_scan_matcher): update readme (#5074)

Updated README by adding ndt_base_frame in the parameters list

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>
---
 localization/ndt_scan_matcher/README.md | 1 +
 1 file changed, 1 insertion(+)

diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md
index fcbe4804ca7d2..c5a14d94ebed3 100644
--- a/localization/ndt_scan_matcher/README.md
+++ b/localization/ndt_scan_matcher/README.md
@@ -57,6 +57,7 @@ One optional function is regularization. Please see the regularization chapter i
 | Name                                    | Type   | Description                                                                                     |
 | --------------------------------------- | ------ | ----------------------------------------------------------------------------------------------- |
 | `base_frame`                            | string | Vehicle reference frame                                                                         |
+| `ndt_base_frame`                        | string | NDT reference frame                                                                             |
 | `input_sensor_points_queue_size`        | int    | Subscriber queue size                                                                           |
 | `trans_epsilon`                         | double | The maximum difference between two consecutive transformations in order to consider convergence |
 | `step_size`                             | double | The newton line search maximum step length                                                      |

From d8885f92c69dae3a10623675d1bda58eb913b8e9 Mon Sep 17 00:00:00 2001
From: SakodaShintaro <shintaro.sakoda@tier4.jp>
Date: Fri, 22 Sep 2023 18:19:52 +0900
Subject: [PATCH 12/35] fix(tier4_localization_launch):  fixed exec_depend
 (#5075)

* Fixed exec_depend

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* style(pre-commit): autofix

---------

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
---
 launch/tier4_localization_launch/package.xml | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml
index 7991ffb0d8adf..6b3b3d3bbbe01 100644
--- a/launch/tier4_localization_launch/package.xml
+++ b/launch/tier4_localization_launch/package.xml
@@ -14,13 +14,14 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <exec_depend>ar_tag_based_localizer</exec_depend>
   <exec_depend>automatic_pose_initializer</exec_depend>
   <exec_depend>eagleye_fix2pose</exec_depend>
   <exec_depend>eagleye_gnss_converter</exec_depend>
   <exec_depend>eagleye_rt</exec_depend>
   <exec_depend>ekf_localizer</exec_depend>
   <exec_depend>gyro_odometer</exec_depend>
-  <exec_depend>landmark_based_localizer</exec_depend>
+  <exec_depend>landmark_tf_caster</exec_depend>
   <exec_depend>ndt_scan_matcher</exec_depend>
   <exec_depend>pointcloud_preprocessor</exec_depend>
   <exec_depend>pose_initializer</exec_depend>

From fe19de9e13e01bd7400259a86cb40b3e45c9d2c0 Mon Sep 17 00:00:00 2001
From: Esteve Fernandez <33620+esteve@users.noreply.github.com>
Date: Fri, 22 Sep 2023 14:12:46 +0200
Subject: [PATCH 13/35] build(tier4_target_object_type_rviz_plugin): add
 missing cv_bridge dependency (#5000)

Signed-off-by: Esteve Fernandez <esteve.fernandez@tier4.jp>
---
 common/tier4_target_object_type_rviz_plugin/package.xml | 1 +
 1 file changed, 1 insertion(+)

diff --git a/common/tier4_target_object_type_rviz_plugin/package.xml b/common/tier4_target_object_type_rviz_plugin/package.xml
index 6209a5aaaee6b..04d2f28d9ba3e 100644
--- a/common/tier4_target_object_type_rviz_plugin/package.xml
+++ b/common/tier4_target_object_type_rviz_plugin/package.xml
@@ -10,6 +10,7 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <depend>cv_bridge</depend>
   <depend>libqt5-core</depend>
   <depend>libqt5-gui</depend>
   <depend>libqt5-widgets</depend>

From 19114642146db7312a35f7b5841878207776ea6a Mon Sep 17 00:00:00 2001
From: Kosuke Takeuchi <kosuke.tnp@gmail.com>
Date: Sat, 23 Sep 2023 16:59:52 +0900
Subject: [PATCH 14/35] refactor(goal_planner): do not finish goal planner
 (#5037)

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
---
 .../goal_planner/goal_planner_module.hpp             |  1 -
 .../goal_planner/goal_planner_module.cpp             | 12 ------------
 2 files changed, 13 deletions(-)

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp
index dbdfc77f59e38..f6b2fa8666f2e 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp
@@ -242,7 +242,6 @@ class GoalPlannerModule : public SceneModuleInterface
   bool isStopped(
     std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> & odometry_buffer, const double time);
   bool hasFinishedCurrentPath();
-  bool hasFinishedGoalPlanner();
   bool isOnModifiedGoal() const;
   double calcModuleRequestLength() const;
   void resetStatus();
diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp
index 0470b149f2ec3..d6e5c8902084f 100644
--- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp
@@ -451,13 +451,6 @@ Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const
 
 ModuleStatus GoalPlannerModule::updateState()
 {
-  // finish module only when the goal is fixed
-  if (
-    !goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler) &&
-    hasFinishedGoalPlanner()) {
-    return ModuleStatus::SUCCESS;
-  }
-
   // start_planner module will be run when setting new goal, so not need finishing pull_over module.
   // Finishing it causes wrong lane_following path generation.
   return current_state_;
@@ -1227,11 +1220,6 @@ bool GoalPlannerModule::isOnModifiedGoal() const
          parameters_->th_arrived_distance;
 }
 
-bool GoalPlannerModule::hasFinishedGoalPlanner()
-{
-  return isOnModifiedGoal() && isStopped();
-}
-
 TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const
 {
   TurnSignalInfo turn_signal{};  // output

From 499152149e3e01263f93845e947e73f748a7e065 Mon Sep 17 00:00:00 2001
From: Kosuke Takeuchi <kosuke.tnp@gmail.com>
Date: Sun, 24 Sep 2023 17:16:04 +0900
Subject: [PATCH 15/35] chore(geometric_parallel_parking): remove unused params
 and initialize variables (#5084)

chore(geometric_parallel_parking): remove unused parames and initialize variables

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
---
 .../geometric_parallel_parking.hpp            | 48 +++++++++----------
 1 file changed, 22 insertions(+), 26 deletions(-)

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp
index 3b6cca56e774f..ef1301beccf00 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp
@@ -46,29 +46,25 @@ using geometry_msgs::msg::PoseArray;
 
 struct ParallelParkingParameters
 {
-  double th_arrived_distance;
-  double th_stopped_velocity;
-  double maximum_deceleration;
-
   // forward parking
-  double after_forward_parking_straight_distance;
-  double forward_parking_velocity;
-  double forward_parking_lane_departure_margin;
-  double forward_parking_path_interval;
-  double forward_parking_max_steer_angle;
+  double after_forward_parking_straight_distance{0.0};
+  double forward_parking_velocity{0.0};
+  double forward_parking_lane_departure_margin{0.0};
+  double forward_parking_path_interval{0.0};
+  double forward_parking_max_steer_angle{0.0};
 
   // backward parking
-  double after_backward_parking_straight_distance;
-  double backward_parking_velocity;
-  double backward_parking_lane_departure_margin;
-  double backward_parking_path_interval;
-  double backward_parking_max_steer_angle;
+  double after_backward_parking_straight_distance{0.0};
+  double backward_parking_velocity{0.0};
+  double backward_parking_lane_departure_margin{0.0};
+  double backward_parking_path_interval{0.0};
+  double backward_parking_max_steer_angle{0.0};
 
   // pull_out
-  double pull_out_velocity;
-  double pull_out_lane_departure_margin;
-  double pull_out_path_interval;
-  double pull_out_max_steer_angle;
+  double pull_out_velocity{0.0};
+  double pull_out_lane_departure_margin{0.0};
+  double pull_out_path_interval{0.0};
+  double pull_out_max_steer_angle{0.0};
 };
 
 class GeometricParallelParking
@@ -108,11 +104,11 @@ class GeometricParallelParking
   Pose getArcEndPose() const { return arc_end_pose_; }
 
 private:
-  std::shared_ptr<const PlannerData> planner_data_;
-  ParallelParkingParameters parameters_;
+  std::shared_ptr<const PlannerData> planner_data_{nullptr};
+  ParallelParkingParameters parameters_{};
 
-  double R_E_min_;   // base_link
-  double R_Bl_min_;  // front_left
+  double R_E_min_{0.0};   // base_link
+  double R_Bl_min_{0.0};  // front_left
 
   std::vector<PathWithLaneId> arc_paths_;
   std::vector<PathWithLaneId> paths_;
@@ -146,10 +142,10 @@ class GeometricParallelParking
     std::vector<PathWithLaneId> & arc_paths, const double velocity, const bool set_stop_end);
 
   // debug
-  Pose Cr_;
-  Pose Cl_;
-  Pose start_pose_;
-  Pose arc_end_pose_;
+  Pose Cr_{};
+  Pose Cl_{};
+  Pose start_pose_{};
+  Pose arc_end_pose_{};
 };
 }  // namespace behavior_path_planner
 

From 837699ba34d8ae06c2ff436b8b2c48dd840f6dbb Mon Sep 17 00:00:00 2001
From: Kosuke Takeuchi <kosuke.tnp@gmail.com>
Date: Sun, 24 Sep 2023 17:16:40 +0900
Subject: [PATCH 16/35] feat(start_planner): visualize backward driving path
 (#5088)

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
---
 .../src/scene_module/start_planner/start_planner_module.cpp      | 1 +
 1 file changed, 1 insertion(+)

diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp
index 8af4b76f619f3..17068ee392d3b 100644
--- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp
@@ -1191,6 +1191,7 @@ void StartPlannerModule::setDebugData() const
   add(createPoseMarkerArray(status_.pull_out_path.start_pose, "start_pose", 0, 0.3, 0.9, 0.3));
   add(createPoseMarkerArray(status_.pull_out_path.end_pose, "end_pose", 0, 0.9, 0.9, 0.3));
   add(createPathMarkerArray(getFullPath(), "full_path", 0, 0.0, 0.5, 0.9));
+  add(createPathMarkerArray(status_.backward_path, "backward_driving_path", 0, 0.0, 0.9, 0.0));
 
   // safety check
   if (parameters_->safety_check_params.enable_safety_check) {

From a4c94ccc7b02c8aea3a899dd6dbc4cf1ce406945 Mon Sep 17 00:00:00 2001
From: Kosuke Takeuchi <kosuke.tnp@gmail.com>
Date: Sun, 24 Sep 2023 18:06:59 +0900
Subject: [PATCH 17/35] fix(start/goal_planner): resample path and make params 
 (#5085)

* chore(geometric_parallel_parking): remove unused parames and initialize variables

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* fix(start/goal_planner): resample path and make params

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

---------

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
Co-authored-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
---
 .../goal_planner/goal_planner.param.yaml      |  1 +
 .../start_planner/start_planner.param.yaml    |  1 +
 ...havior_path_planner_goal_planner_design.md | 11 +++---
 ...avior_path_planner_start_planner_design.md |  1 +
 .../geometric_parallel_parking.hpp            |  3 ++
 .../goal_planner/goal_planner_parameters.hpp  |  1 +
 .../utils/goal_planner/shift_pull_over.hpp    |  1 -
 .../utils/start_planner/shift_pull_out.hpp    |  4 +--
 .../start_planner_parameters.hpp              |  1 +
 .../src/scene_module/goal_planner/manager.cpp |  9 +++++
 .../scene_module/start_planner/manager.cpp    |  3 ++
 .../geometric_parallel_parking.cpp            | 11 +++---
 .../utils/goal_planner/shift_pull_over.cpp    |  2 +-
 .../start_planner/freespace_pull_out.cpp      |  4 ++-
 .../utils/start_planner/shift_pull_out.cpp    | 34 +++++++++++--------
 15 files changed, 57 insertions(+), 30 deletions(-)

diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml
index 625cd6ab74ff7..f16799d7ea206 100644
--- a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml
+++ b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml
@@ -5,6 +5,7 @@
       th_arrived_distance: 1.0
       th_stopped_velocity: 0.01
       th_stopped_time: 2.0  # It must be greater than the state_machine's.
+      center_line_path_interval: 1.0
 
       # goal search
       goal_search:
diff --git a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml
index a529eefac67af..6760ec787bb03 100644
--- a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml
+++ b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml
@@ -8,6 +8,7 @@
       collision_check_distance_from_end: 1.0
       th_moving_object_velocity: 1.0
       th_distance_to_middle_of_the_road: 0.1
+      center_line_path_interval: 1.0
       # shift pull out
       enable_shift_pull_out: true
       check_shift_path_lane_departure: false
diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md
index 18b77f3faacf0..44abdb4267672 100644
--- a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md
+++ b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md
@@ -118,11 +118,12 @@ Either one is activated when all conditions are met.
 
 ## General parameters for goal_planner
 
-| Name                | Unit  | Type   | Description                                        | Default value |
-| :------------------ | :---- | :----- | :------------------------------------------------- | :------------ |
-| th_arrived_distance | [m]   | double | distance threshold for arrival of path termination | 1.0           |
-| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01          |
-| th_stopped_time     | [s]   | double | time threshold for arrival of path termination     | 2.0           |
+| Name                      | Unit  | Type   | Description                                        | Default value |
+| :------------------------ | :---- | :----- | :------------------------------------------------- | :------------ |
+| th_arrived_distance       | [m]   | double | distance threshold for arrival of path termination | 1.0           |
+| th_stopped_velocity       | [m/s] | double | velocity threshold for arrival of path termination | 0.01          |
+| th_stopped_time           | [s]   | double | time threshold for arrival of path termination     | 2.0           |
+| center_line_path_interval | [m]   | double | reference center line path point interval          | 1.0           |
 
 ## **collision check**
 
diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md
index 0b0e01378b61a..8322952a0be76 100644
--- a/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md
+++ b/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md
@@ -70,6 +70,7 @@ PullOutPath --o PullOutPlannerBase
 | length_ratio_for_turn_signal_deactivation_near_intersection | [m]   | double | deactivate turn signal of this module near intersection                     | 0.5           |
 | collision_check_margin                                      | [m]   | double | Obstacle collision check margin                                             | 1.0           |
 | collision_check_distance_from_end                           | [m]   | double | collision check distance from end point. currently only for pull out        | 15.0          |
+| center_line_path_interval                                   | [m]   | double | reference center line path point interval                                   | 1.0           |
 
 ## Safety check with static obstacles
 
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp
index ef1301beccf00..c2d36fdd6e0d7 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp
@@ -46,6 +46,9 @@ using geometry_msgs::msg::PoseArray;
 
 struct ParallelParkingParameters
 {
+  // common
+  double center_line_path_interval{0.0};
+
   // forward parking
   double after_forward_parking_straight_distance{0.0};
   double forward_parking_velocity{0.0};
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp
index 22c65270c39ea..8ef426d4f2488 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp
@@ -45,6 +45,7 @@ struct GoalPlannerParameters
   double th_stopped_velocity{0.0};
   double th_stopped_time{0.0};
   double th_blinker_on_lateral_offset{0.0};
+  double center_line_path_interval{0.0};
 
   // goal search
   std::string search_priority;   // "efficient_path" or "close_goal"
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp
index 445161f631bb7..dccaed3184e77 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp
@@ -57,7 +57,6 @@ class ShiftPullOver : public PullOverPlannerBase
   LaneDepartureChecker lane_departure_checker_{};
   std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map_{};
 
-  static constexpr double resample_interval_{1.0};
   bool left_side_parking_{true};
 };
 }  // namespace behavior_path_planner
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp
index 0842d0a17bd97..78a64feb2c1ec 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp
@@ -41,9 +41,7 @@ class ShiftPullOut : public PullOutPlannerBase
 
   std::vector<PullOutPath> calcPullOutPaths(
     const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes,
-    const Pose & start_pose, const Pose & goal_pose,
-    const BehaviorPathPlannerParameters & common_parameter,
-    const behavior_path_planner::StartPlannerParameters & parameter);
+    const Pose & start_pose, const Pose & goal_pose);
 
   double calcBeforeShiftedArcLength(
     const PathWithLaneId & path, const double target_after_arc_length, const double dr);
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp
index e2d80759a18d0..6978a7d494f30 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp
@@ -45,6 +45,7 @@ struct StartPlannerParameters
   double collision_check_margin{0.0};
   double collision_check_distance_from_end{0.0};
   double th_moving_object_velocity{0.0};
+  double center_line_path_interval{0.0};
 
   // shift pull out
   bool enable_shift_pull_out{false};
diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp
index da1b8bf5792ee..88fef3a6c6508 100644
--- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp
+++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp
@@ -38,6 +38,8 @@ GoalPlannerModuleManager::GoalPlannerModuleManager(
     p.th_stopped_velocity = node->declare_parameter<double>(base_ns + "th_stopped_velocity");
     p.th_arrived_distance = node->declare_parameter<double>(base_ns + "th_arrived_distance");
     p.th_stopped_time = node->declare_parameter<double>(base_ns + "th_stopped_time");
+    p.center_line_path_interval =
+      node->declare_parameter<double>(base_ns + "center_line_path_interval");
   }
 
   // goal search
@@ -121,6 +123,13 @@ GoalPlannerModuleManager::GoalPlannerModuleManager(
       node->declare_parameter<double>(ns + "after_shift_straight_distance");
   }
 
+  // parallel parking common
+  {
+    const std::string ns = base_ns + "pull_over.parallel_parking.";
+    p.parallel_parking_parameters.center_line_path_interval =
+      p.center_line_path_interval;  // for geometric parallel parking
+  }
+
   // forward parallel parking forward
   {
     const std::string ns = base_ns + "pull_over.parallel_parking.forward.";
diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp
index e00a1041e5316..30810bda252c3 100644
--- a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp
+++ b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp
@@ -47,6 +47,7 @@ StartPlannerModuleManager::StartPlannerModuleManager(
   p.collision_check_distance_from_end =
     node->declare_parameter<double>(ns + "collision_check_distance_from_end");
   p.th_moving_object_velocity = node->declare_parameter<double>(ns + "th_moving_object_velocity");
+  p.center_line_path_interval = node->declare_parameter<double>(ns + "center_line_path_interval");
   // shift pull out
   p.enable_shift_pull_out = node->declare_parameter<bool>(ns + "enable_shift_pull_out");
   p.check_shift_path_lane_departure =
@@ -71,6 +72,8 @@ StartPlannerModuleManager::StartPlannerModuleManager(
     node->declare_parameter<double>(ns + "lane_departure_margin");
   p.parallel_parking_parameters.pull_out_max_steer_angle =
     node->declare_parameter<double>(ns + "pull_out_max_steer_angle");  // 15deg
+  p.parallel_parking_parameters.center_line_path_interval =
+    p.center_line_path_interval;  // for geometric parallel parking
   // search start pose backward
   p.search_priority = node->declare_parameter<std::string>(
     ns + "search_priority");  // "efficient_path" or "short_back_distance"
diff --git a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp
index 6b1a4a82b314e..3d315e7213f96 100644
--- a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp
+++ b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp
@@ -286,8 +286,9 @@ bool GeometricParallelParking::planPullOut(
       s_start, planner_data_->parameters.forward_path_length, road_lanes, goal_pose);
     const double s_end = path_end_info.first;
     const bool path_terminal_is_goal = path_end_info.second;
-    PathWithLaneId road_center_line_path =
-      planner_data_->route_handler->getCenterLinePath(road_lanes, s_start, s_end, true);
+    const PathWithLaneId road_center_line_path = utils::resamplePathWithSpline(
+      planner_data_->route_handler->getCenterLinePath(road_lanes, s_start, s_end, true),
+      parameters_.center_line_path_interval);
 
     if (road_center_line_path.points.empty()) {
       continue;
@@ -362,8 +363,10 @@ PathWithLaneId GeometricParallelParking::generateStraightPath(
   const Pose current_pose = planner_data_->self_odometry->pose.pose;
   const auto current_arc_position = lanelet::utils::getArcCoordinates(road_lanes, current_pose);
 
-  auto path = planner_data_->route_handler->getCenterLinePath(
-    road_lanes, current_arc_position.length, start_arc_position.length, true);
+  auto path = utils::resamplePathWithSpline(
+    planner_data_->route_handler->getCenterLinePath(
+      road_lanes, current_arc_position.length, start_arc_position.length, true),
+    parameters_.center_line_path_interval);
   path.header = planner_data_->route_handler->getRouteHeader();
   if (!path.points.empty()) {
     path.points.back().point.longitudinal_velocity_mps = 0;
diff --git a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp
index d6a3746187b11..1a690362fbe25 100644
--- a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp
+++ b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp
@@ -112,7 +112,7 @@ boost::optional<PullOverPath> ShiftPullOver::generatePullOverPath(
 
   // generate road lane reference path to shift end
   const auto road_lane_reference_path_to_shift_end = utils::resamplePathWithSpline(
-    generateReferencePath(road_lanes, shift_end_pose), resample_interval_);
+    generateReferencePath(road_lanes, shift_end_pose), parameters_.center_line_path_interval);
 
   // calculate shift length
   const Pose & shift_end_pose_road_lane =
diff --git a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp
index 36fb6381ac967..8e5d6b7545c1d 100644
--- a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp
+++ b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp
@@ -96,7 +96,9 @@ boost::optional<PullOutPath> FreespacePullOut::plan(const Pose & start_pose, con
   const bool path_terminal_is_goal = path_end_info.second;
 
   const auto road_center_line_path = route_handler->getCenterLinePath(road_lanes, s_start, s_end);
-  last_path = start_planner_utils::combineReferencePath(last_path, road_center_line_path);
+  last_path = utils::resamplePathWithSpline(
+    start_planner_utils::combineReferencePath(last_path, road_center_line_path),
+    parameters_.center_line_path_interval);
 
   const double original_terminal_velocity = last_path.points.back().point.longitudinal_velocity_mps;
   utils::correctDividedPathVelocity(partial_paths);
diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp
index 6225ce598afe3..a4e4a2378e8f1 100644
--- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp
+++ b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp
@@ -59,8 +59,7 @@ boost::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const P
     planner_data_, backward_path_length, std::numeric_limits<double>::max(),
     /*forward_only_in_route*/ true);
   // find candidate paths
-  auto pull_out_paths = calcPullOutPaths(
-    *route_handler, road_lanes, start_pose, goal_pose, common_parameters, parameters_);
+  auto pull_out_paths = calcPullOutPaths(*route_handler, road_lanes, start_pose, goal_pose);
   if (pull_out_paths.empty()) {
     return boost::none;
   }
@@ -160,8 +159,7 @@ boost::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const P
 
 std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes,
-  const Pose & start_pose, const Pose & goal_pose,
-  const BehaviorPathPlannerParameters & common_parameter, const StartPlannerParameters & parameter)
+  const Pose & start_pose, const Pose & goal_pose)
 {
   std::vector<PullOutPath> candidate_paths{};
 
@@ -170,12 +168,16 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
   }
 
   // rename parameter
-  const double forward_path_length = common_parameter.forward_path_length;
-  const double backward_path_length = common_parameter.backward_path_length;
-  const double lateral_jerk = parameter.lateral_jerk;
-  const double minimum_lateral_acc = parameter.minimum_lateral_acc;
-  const double maximum_lateral_acc = parameter.maximum_lateral_acc;
-  const int lateral_acceleration_sampling_num = parameter.lateral_acceleration_sampling_num;
+  const auto & common_parameters = planner_data_->parameters;
+  const double forward_path_length = common_parameters.forward_path_length;
+  const double backward_path_length = common_parameters.backward_path_length;
+  const double lateral_jerk = parameters_.lateral_jerk;
+  const double minimum_lateral_acc = parameters_.minimum_lateral_acc;
+  const double maximum_lateral_acc = parameters_.maximum_lateral_acc;
+  const double maximum_curvature = parameters_.maximum_curvature;
+  const double minimum_shift_pull_out_distance = parameters_.minimum_shift_pull_out_distance;
+  const int lateral_acceleration_sampling_num = parameters_.lateral_acceleration_sampling_num;
+
   // set minimum acc for breaking loop when sampling num is 1
   const double acc_resolution = std::max(
     std::abs(maximum_lateral_acc - minimum_lateral_acc) / lateral_acceleration_sampling_num,
@@ -189,9 +191,9 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
   const double s_end = path_end_info.first;
   const bool path_terminal_is_goal = path_end_info.second;
 
-  constexpr double RESAMPLE_INTERVAL = 1.0;
   PathWithLaneId road_lane_reference_path = utils::resamplePathWithSpline(
-    route_handler.getCenterLinePath(road_lanes, s_start, s_end), RESAMPLE_INTERVAL);
+    route_handler.getCenterLinePath(road_lanes, s_start, s_end),
+    parameters_.center_line_path_interval);
 
   // non_shifted_path for when shift length or pull out distance is too short
   const PullOutPath non_shifted_path = std::invoke([&]() {
@@ -230,8 +232,8 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
       PathShifter::calcShiftTimeFromJerk(shift_length, lateral_jerk, lateral_acc);
     const double longitudinal_acc = std::clamp(road_velocity / shift_time, 0.0, /* max acc */ 1.0);
     const auto pull_out_distance = calcPullOutLongitudinalDistance(
-      longitudinal_acc, shift_time, shift_length, parameter.maximum_curvature,
-      parameter.minimum_shift_pull_out_distance);
+      longitudinal_acc, shift_time, shift_length, maximum_curvature,
+      minimum_shift_pull_out_distance);
     const double terminal_velocity = longitudinal_acc * shift_time;
 
     // clip from ego pose
@@ -253,7 +255,9 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
       std::max(pull_out_distance, pull_out_distance_converted);
 
     // if before_shifted_pull_out_distance is too short, shifting path fails, so add non shifted
-    if (before_shifted_pull_out_distance < RESAMPLE_INTERVAL && !has_non_shifted_path) {
+    if (
+      before_shifted_pull_out_distance < parameters_.center_line_path_interval &&
+      !has_non_shifted_path) {
       candidate_paths.push_back(non_shifted_path);
       has_non_shifted_path = true;
       continue;

From 1ada5673e3ba561a23cf41cee825b9eec9a76dfd Mon Sep 17 00:00:00 2001
From: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>
Date: Sun, 24 Sep 2023 19:01:52 +0900
Subject: [PATCH 18/35] feat(behavior_path_planner): add public member function
 to get running modules  (#3868)

* add getter function to confirm which module is running

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* delete change for old architecture

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

---------

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
---
 .../behavior_path_planner_node.hpp                   |  3 +++
 .../src/behavior_path_planner_node.cpp               | 12 ++++++++++++
 2 files changed, 15 insertions(+)

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp
index 54cfccc494e8a..0f51144c0b792 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp
@@ -87,6 +87,9 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   // Getter method for waiting approval modules
   std::vector<std::string> getWaitingApprovalModules();
 
+  // Getter method for running modules
+  std::vector<std::string> getRunningModules();
+
 private:
   rclcpp::Subscription<LaneletRoute>::SharedPtr route_subscriber_;
   rclcpp::Subscription<HADMapBin>::SharedPtr vector_map_subscriber_;
diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
index c14daa30b784d..8302779313feb 100644
--- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
+++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -272,6 +272,18 @@ std::vector<std::string> BehaviorPathPlannerNode::getWaitingApprovalModules()
   return waiting_approval_modules;
 }
 
+std::vector<std::string> BehaviorPathPlannerNode::getRunningModules()
+{
+  auto all_scene_module_ptr = planner_manager_->getSceneModuleStatus();
+  std::vector<std::string> running_modules;
+  for (const auto & module : all_scene_module_ptr) {
+    if (module->status == ModuleStatus::RUNNING) {
+      running_modules.push_back(module->module_name);
+    }
+  }
+  return running_modules;
+}
+
 BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
 {
   BehaviorPathPlannerParameters p{};

From ec99b2f2218f6b27a0f9567480df73fe1ed2aea6 Mon Sep 17 00:00:00 2001
From: Kosuke Takeuchi <kosuke.tnp@gmail.com>
Date: Sun, 24 Sep 2023 21:14:58 +0900
Subject: [PATCH 19/35] fix(start_planner): fix backward drving path cutting
 (#5098)

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
---
 .../scene_module/start_planner/start_planner_module.cpp    | 7 +++++--
 planning/behavior_path_planner/src/utils/utils.cpp         | 4 +++-
 2 files changed, 8 insertions(+), 3 deletions(-)

diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp
index 17068ee392d3b..5247a24579212 100644
--- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp
@@ -1160,8 +1160,11 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons
 
     DrivableAreaInfo current_drivable_area_info;
     current_drivable_area_info.drivable_lanes = target_drivable_lanes;
-    output.drivable_area_info = utils::combineDrivableAreaInfo(
-      current_drivable_area_info, getPreviousModuleOutput().drivable_area_info);
+    output.drivable_area_info =
+      status_.back_finished
+        ? utils::combineDrivableAreaInfo(
+            current_drivable_area_info, getPreviousModuleOutput().drivable_area_info)
+        : current_drivable_area_info;
   }
 }
 
diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp
index a4667e1641ca0..9c11469c0f9c9 100644
--- a/planning/behavior_path_planner/src/utils/utils.cpp
+++ b/planning/behavior_path_planner/src/utils/utils.cpp
@@ -1544,7 +1544,9 @@ void generateDrivableArea(
 
   // make bound longitudinally monotonic
   // TODO(Murooka) Fix makeBoundLongitudinallyMonotonic
-  if (enable_expanding_hatched_road_markings || enable_expanding_intersection_areas) {
+  if (
+    is_driving_forward &&
+    (enable_expanding_hatched_road_markings || enable_expanding_intersection_areas)) {
     makeBoundLongitudinallyMonotonic(path, planner_data, true);   // for left bound
     makeBoundLongitudinallyMonotonic(path, planner_data, false);  // for right bound
   }

From 1276da93b382e3f3a64d3154f53b7f9562ad074c Mon Sep 17 00:00:00 2001
From: Takamasa Horibe <horibe.takamasa@gmail.com>
Date: Mon, 25 Sep 2023 09:35:06 +0900
Subject: [PATCH 20/35] refactor(perception_reproducer): improve computational
 efficiency (#5067)

* refactor(perception_reproducer): improve computational efficiency

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* use numpy for nearest search, add stopwatch for verbose

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix order

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* add rosbag ego pose publisher

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

---------

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
---
 .../perception_replayer.py                    |  2 +-
 .../perception_replayer_common.py             | 37 +++++++---
 .../perception_reproducer.py                  | 74 +++++++++++++------
 .../scripts/perception_replayer/utils.py      | 27 +++++++
 4 files changed, 105 insertions(+), 35 deletions(-)

diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py
index 18f903323619f..498f7e458b2e2 100755
--- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py
+++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py
@@ -169,7 +169,7 @@ def publish_recorded_ego_pose(self):
             0.06853892326654787,
         ]
 
-        self.recorded_ego_pub.publish(recorded_ego_pose)
+        self.recorded_ego_pub_as_initialpose.publish(recorded_ego_pose)
         print("Published recorded ego pose as /initialpose")
 
 
diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py
index 8f7cf38772fbb..01079fdc3f8c1 100644
--- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py
+++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py
@@ -68,7 +68,13 @@ def __init__(self, args, name):
         self.pointcloud_pub = self.create_publisher(
             PointCloud2, "/perception/obstacle_segmentation/pointcloud", 1
         )
-        self.recorded_ego_pub = self.create_publisher(PoseWithCovarianceStamped, "/initialpose", 1)
+        self.recorded_ego_pub_as_initialpose = self.create_publisher(
+            PoseWithCovarianceStamped, "/initialpose", 1
+        )
+
+        self.recorded_ego_pub = self.create_publisher(
+            Odometry, "/perception_reproducer/rosbag_ego_odom", 1
+        )
 
         # load rosbag
         print("Stared loading rosbag")
@@ -146,19 +152,26 @@ def kill_online_perception_node(self):
             except CalledProcessError:
                 pass
 
-    def find_topics_by_timestamp(self, timestamp):
-        objects_data = None
-        for data in self.rosbag_objects_data:
-            if timestamp < data[0]:
-                objects_data = data[1]
-                break
+    def binary_search(self, data, timestamp):
+        low, high = 0, len(data) - 1
 
-        traffic_signals_data = None
-        for data in self.rosbag_traffic_signals_data:
-            if timestamp < data[0]:
-                traffic_signals_data = data[1]
-                break
+        while low <= high:
+            mid = (low + high) // 2
+            if data[mid][0] < timestamp:
+                low = mid + 1
+            elif data[mid][0] > timestamp:
+                high = mid - 1
+            else:
+                return data[mid][1]
 
+        # Return the next timestamp's data if available
+        if low < len(data):
+            return data[low][1]
+        return None
+
+    def find_topics_by_timestamp(self, timestamp):
+        objects_data = self.binary_search(self.rosbag_objects_data, timestamp)
+        traffic_signals_data = self.binary_search(self.rosbag_traffic_signals_data, timestamp)
         return objects_data, traffic_signals_data
 
     def find_ego_odom_by_timestamp(self, timestamp):
diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py
index 454ad033082ac..d837abccc0670 100755
--- a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py
+++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py
@@ -15,11 +15,12 @@
 # limitations under the License.
 
 import argparse
-import copy
+import pickle
 
+import numpy as np
 from perception_replayer_common import PerceptionReplayerCommon
 import rclpy
-from utils import calc_squared_distance
+from utils import StopWatch
 from utils import create_empty_pointcloud
 from utils import translate_objects_coordinate
 
@@ -28,18 +29,39 @@ class PerceptionReproducer(PerceptionReplayerCommon):
     def __init__(self, args):
         super().__init__(args, "perception_reproducer")
 
-        self.ego_pose_idx = None
         self.prev_traffic_signals_msg = None
+        self.stopwatch = StopWatch(self.args.verbose)  # for debug
 
-        # start timer callback
+        # to make some data to accelerate computation
+        self.preprocess_data()
+
+        # start main timer callback
         self.timer = self.create_timer(0.1, self.on_timer)
+
+        # kill perception process to avoid a conflict of the perception topics
+        self.timer_check_perception_process = self.create_timer(3.0, self.on_timer_kill_perception)
+
         print("Start timer callback")
 
-    def on_timer(self):
-        timestamp = self.get_clock().now().to_msg()
+    def preprocess_data(self):
+        # closest search with numpy data is much faster than usual
+        self.rosbag_ego_odom_data_numpy = np.array(
+            [
+                [data[1].pose.pose.position.x, data[1].pose.pose.position.y]
+                for data in self.rosbag_ego_odom_data
+            ]
+        )
 
+    def on_timer_kill_perception(self):
         self.kill_online_perception_node()
 
+    def on_timer(self):
+        if self.args.verbose:
+            print("\n-- on_timer start --")
+        self.stopwatch.tic("total on_timer")
+
+        timestamp = self.get_clock().now().to_msg()
+
         if self.args.detected_object:
             pointcloud_msg = create_empty_pointcloud(timestamp)
             self.pointcloud_pub.publish(pointcloud_msg)
@@ -49,15 +71,25 @@ def on_timer(self):
             return
 
         # find nearest ego odom by simulation observation
+        self.stopwatch.tic("find_nearest_ego_odom_by_observation")
         ego_odom = self.find_nearest_ego_odom_by_observation(self.ego_pose)
         pose_timestamp = ego_odom[0]
         log_ego_pose = ego_odom[1].pose.pose
+        self.stopwatch.toc("find_nearest_ego_odom_by_observation")
 
         # extract message by the nearest ego odom timestamp
-        msgs = copy.deepcopy(self.find_topics_by_timestamp(pose_timestamp))
+        self.stopwatch.tic("find_topics_by_timestamp")
+        msgs_orig = self.find_topics_by_timestamp(pose_timestamp)
+        self.stopwatch.toc("find_topics_by_timestamp")
+
+        # copy the messages
+        self.stopwatch.tic("message deepcopy")
+        msgs = pickle.loads(pickle.dumps(msgs_orig))  # this is x5 faster than deepcopy
         objects_msg = msgs[0]
         traffic_signals_msg = msgs[1]
+        self.stopwatch.toc("message deepcopy")
 
+        self.stopwatch.tic("transform and publish")
         # objects
         if objects_msg:
             objects_msg.header.stamp = timestamp
@@ -65,6 +97,9 @@ def on_timer(self):
                 translate_objects_coordinate(self.ego_pose, log_ego_pose, objects_msg)
             self.objects_pub.publish(objects_msg)
 
+        # ego odom
+        self.recorded_ego_pub.publish(ego_odom[1])
+
         # traffic signals
         # temporary support old auto msgs
         if traffic_signals_msg:
@@ -82,23 +117,15 @@ def on_timer(self):
             else:
                 self.prev_traffic_signals_msg.stamp = timestamp
                 self.traffic_signals_pub.publish(self.prev_traffic_signals_msg)
+        self.stopwatch.toc("transform and publish")
+
+        self.stopwatch.toc("total on_timer")
 
     def find_nearest_ego_odom_by_observation(self, ego_pose):
-        if self.ego_pose_idx:
-            start_idx = self.ego_pose_idx - 10
-            end_idx = self.ego_pose_idx + 10
-        else:
-            start_idx = 0
-            end_idx = len(self.rosbag_ego_odom_data) - 1
-
-        nearest_idx = 0
-        nearest_dist = float("inf")
-        for idx in range(start_idx, end_idx + 1):
-            data = self.rosbag_ego_odom_data[idx]
-            dist = calc_squared_distance(data[1].pose.pose.position, ego_pose.position)
-            if dist < nearest_dist:
-                nearest_dist = dist
-                nearest_idx = idx
+        # nearest search with numpy format is much (~ x100) faster than regular for loop
+        self_pose = np.array([ego_pose.position.x, ego_pose.position.y])
+        dists_squared = np.sum((self.rosbag_ego_odom_data_numpy - self_pose) ** 2, axis=1)
+        nearest_idx = np.argmin(dists_squared)
 
         return self.rosbag_ego_odom_data[nearest_idx]
 
@@ -115,6 +142,9 @@ def find_nearest_ego_odom_by_observation(self, ego_pose):
     parser.add_argument(
         "-f", "--rosbag-format", help="rosbag data format (default is db3)", default="db3"
     )
+    parser.add_argument(
+        "-v", "--verbose", help="output debug data", action="store_true", default=False
+    )
     args = parser.parse_args()
 
     rclpy.init()
diff --git a/planning/planning_debug_tools/scripts/perception_replayer/utils.py b/planning/planning_debug_tools/scripts/perception_replayer/utils.py
index 572922d4c7abb..5ded8d2ea409c 100644
--- a/planning/planning_debug_tools/scripts/perception_replayer/utils.py
+++ b/planning/planning_debug_tools/scripts/perception_replayer/utils.py
@@ -15,6 +15,7 @@
 # limitations under the License.
 
 import math
+import time
 
 from geometry_msgs.msg import Quaternion
 import numpy as np
@@ -123,3 +124,29 @@ def translate_objects_coordinate(ego_pose, log_ego_pose, objects_msg):
         object_pose.position.x = object_pos_vec[0]
         object_pose.position.y = object_pos_vec[1]
         object_pose.orientation = get_quaternion_from_yaw(log_object_yaw + log_ego_yaw - ego_yaw)
+
+
+class StopWatch:
+    def __init__(self, verbose):
+        # A dictionary to store the starting times
+        self.start_times = {}
+        self.verbose = verbose
+
+    def tic(self, name):
+        """Store the current time with the given name."""
+        self.start_times[name] = time.perf_counter()
+
+    def toc(self, name):
+        """Print the elapsed time since the last call to tic() with the same name."""
+        if name not in self.start_times:
+            print(f"No start time found for {name}!")
+            return
+
+        elapsed_time = (
+            time.perf_counter() - self.start_times[name]
+        ) * 1000  # Convert to milliseconds
+        if self.verbose:
+            print(f"Time for {name}: {elapsed_time:.2f} ms")
+
+        # Reset the starting time for the name
+        del self.start_times[name]

From 7af40fc9ff9beabae4c8842b21153ba977513cbf Mon Sep 17 00:00:00 2001
From: Takamasa Horibe <horibe.takamasa@gmail.com>
Date: Mon, 25 Sep 2023 09:35:49 +0900
Subject: [PATCH 21/35] chore(motion_velocity_smoother): add enable curve
 filtering param (#5064)

* chore(motion_velocity_smoother): add enable curve filtering param

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* update readme

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

---------

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
---
 planning/motion_velocity_smoother/README.md     | 14 ++++++++------
 .../default_motion_velocity_smoother.param.yaml |  2 ++
 .../motion_velocity_smoother_node.hpp           |  3 +++
 .../src/motion_velocity_smoother_node.cpp       | 17 ++++++++++++++---
 4 files changed, 27 insertions(+), 9 deletions(-)

diff --git a/planning/motion_velocity_smoother/README.md b/planning/motion_velocity_smoother/README.md
index d52a6431c48d8..1ff7f5982b4eb 100644
--- a/planning/motion_velocity_smoother/README.md
+++ b/planning/motion_velocity_smoother/README.md
@@ -140,6 +140,7 @@ After the optimization, a resampling called `post resampling` is performed befor
 
 | Name                                   | Type     | Description                                                                                                                                                                                                  | Default value |
 | :------------------------------------- | :------- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
+| `enable_lateral_acc_limit`             | `bool`   | To toggle the lateral acceleration filter on and off. You can switch it dynamically at runtime.                                                                                                              | true          |
 | `max_lateral_accel`                    | `double` | Max lateral acceleration limit [m/ss]                                                                                                                                                                        | 0.5           |
 | `min_curve_velocity`                   | `double` | Min velocity at lateral acceleration limit [m/ss]                                                                                                                                                            | 2.74          |
 | `decel_distance_before_curve`          | `double` | Distance to slowdown before a curve for lateral acceleration limit [m]                                                                                                                                       | 3.5           |
@@ -197,12 +198,13 @@ After the optimization, a resampling called `post resampling` is performed befor
 
 ### Limit steering angle rate parameters
 
-| Name                             | Type     | Description                                                              | Default value |
-| :------------------------------- | :------- | :----------------------------------------------------------------------- | :------------ |
-| `max_steering_angle_rate`        | `double` | Maximum steering angle rate [degree/s]                                   | 40.0          |
-| `resample_ds`                    | `double` | Distance between trajectory points [m]                                   | 0.1           |
-| `curvature_threshold`            | `double` | If curvature > curvature_threshold, steeringRateLimit is triggered [1/m] | 0.02          |
-| `curvature_calculation_distance` | `double` | Distance of points while curvature is calculating [m]                    | 1.0           |
+| Name                             | Type     | Description                                                                           | Default value |
+| :------------------------------- | :------- | :------------------------------------------------------------------------------------ | :------------ |
+| `enable_steering_rate_limit`     | `bool`   | To toggle the steer rate filter on and off. You can switch it dynamically at runtime. | true          |
+| `max_steering_angle_rate`        | `double` | Maximum steering angle rate [degree/s]                                                | 40.0          |
+| `resample_ds`                    | `double` | Distance between trajectory points [m]                                                | 0.1           |
+| `curvature_threshold`            | `double` | If curvature > curvature_threshold, steeringRateLimit is triggered [1/m]              | 0.02          |
+| `curvature_calculation_distance` | `double` | Distance of points while curvature is calculating [m]                                 | 1.0           |
 
 ### Weights for optimization
 
diff --git a/planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml b/planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml
index bcbf0a5bb6c06..e56845c1c9bd6 100644
--- a/planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml
+++ b/planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml
@@ -8,6 +8,7 @@
     margin_to_insert_external_velocity_limit: 0.3   #  margin distance to insert external velocity limit [m]
 
     # curve parameters
+    enable_lateral_acc_limit: true     # To toggle the lateral acc filter on and off. You can switch it dynamically at runtime.
     max_lateral_accel: 0.8             # max lateral acceleration limit [m/ss]
     min_curve_velocity: 2.74           # min velocity at lateral acceleration limit [m/ss]
     decel_distance_before_curve: 3.5   # slow speed distance before a curve for lateral acceleration limit
@@ -49,6 +50,7 @@
     post_sparse_min_interval_distance: 1.0   # minimum points-interval length for sparse sampling [m]
 
     # steering angle rate limit parameters
+    enable_steering_rate_limit: true         # To toggle the steer rate filter on and off. You can switch it dynamically at runtime.
     max_steering_angle_rate: 40.0            # maximum steering angle rate [degree/s]
     resample_ds: 0.1                         # distance between trajectory points [m]
     curvature_threshold: 0.02                # if curvature > curvature_threshold, steeringRateLimit is triggered [1/m]
diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp
index 503fd4450cdbd..bb49700464715 100644
--- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp
+++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp
@@ -123,6 +123,9 @@ class MotionVelocitySmootherNode : public rclcpp::Node
 
   struct Param
   {
+    bool enable_lateral_acc_limit;
+    bool enable_steering_rate_limit;
+
     double max_velocity;                              // max velocity [m/s]
     double margin_to_insert_external_velocity_limit;  // for external velocity limit [m]
     double replan_vel_deviation;                      // if speed error exceeds this [m/s],
diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp
index 5ed176bd2e215..a4ad118ffe012 100644
--- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp
+++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp
@@ -165,6 +165,9 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter
 
   {
     auto & p = node_param_;
+    update_param_bool("enable_lateral_acc_limit", p.enable_lateral_acc_limit);
+    update_param_bool("enable_steering_rate_limit", p.enable_steering_rate_limit);
+
     update_param("max_velocity", p.max_velocity);
     update_param(
       "margin_to_insert_external_velocity_limit", p.margin_to_insert_external_velocity_limit);
@@ -266,6 +269,9 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter
 void MotionVelocitySmootherNode::initCommonParam()
 {
   auto & p = node_param_;
+  p.enable_lateral_acc_limit = declare_parameter<bool>("enable_lateral_acc_limit");
+  p.enable_steering_rate_limit = declare_parameter<bool>("enable_steering_rate_limit");
+
   p.max_velocity = declare_parameter<double>("max_velocity");  // 72.0 kmph
   p.margin_to_insert_external_velocity_limit =
     declare_parameter<double>("margin_to_insert_external_velocity_limit");
@@ -572,12 +578,17 @@ bool MotionVelocitySmootherNode::smoothVelocity(
   // Lateral acceleration limit
   constexpr bool enable_smooth_limit = true;
   constexpr bool use_resampling = true;
-  const auto traj_lateral_acc_filtered = smoother_->applyLateralAccelerationFilter(
-    input, initial_motion.vel, initial_motion.acc, enable_smooth_limit, use_resampling);
+  const auto traj_lateral_acc_filtered =
+    node_param_.enable_lateral_acc_limit
+      ? smoother_->applyLateralAccelerationFilter(
+          input, initial_motion.vel, initial_motion.acc, enable_smooth_limit, use_resampling)
+      : input;
 
   // Steering angle rate limit (Note: set use_resample = false since it is resampled above)
   const auto traj_steering_rate_limited =
-    smoother_->applySteeringRateLimit(traj_lateral_acc_filtered, false);
+    node_param_.enable_steering_rate_limit
+      ? smoother_->applySteeringRateLimit(traj_lateral_acc_filtered, false)
+      : traj_lateral_acc_filtered;
 
   // Resample trajectory with ego-velocity based interval distance
   auto traj_resampled = smoother_->resampleTrajectory(

From 6006146dcff9cf68265f9f155adcf7dc2fa69528 Mon Sep 17 00:00:00 2001
From: Takayuki Murooka <takayuki5168@gmail.com>
Date: Mon, 25 Sep 2023 10:49:51 +0900
Subject: [PATCH 22/35] feat(mission_planner): add param of
 check_footprint_inside_lanes (#5097)

* feat(mission_planner): add param of check_footprint_inside_lanes

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* update config

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* update

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
---
 planning/mission_planner/config/mission_planner.param.yaml     | 1 +
 .../mission_planner/src/lanelet2_plugins/default_planner.cpp   | 3 +++
 .../mission_planner/src/lanelet2_plugins/default_planner.hpp   | 1 +
 3 files changed, 5 insertions(+)

diff --git a/planning/mission_planner/config/mission_planner.param.yaml b/planning/mission_planner/config/mission_planner.param.yaml
index 98c28344ea25c..9b7dcffbc687b 100644
--- a/planning/mission_planner/config/mission_planner.param.yaml
+++ b/planning/mission_planner/config/mission_planner.param.yaml
@@ -9,3 +9,4 @@
     reroute_time_threshold: 10.0
     minimum_reroute_length: 30.0
     consider_no_drivable_lanes: false # This flag is for considering no_drivable_lanes in planning or not.
+    check_footprint_inside_lanes: true
diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp
index 979800aea0754..673519b6f7a0e 100644
--- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp
+++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp
@@ -152,6 +152,8 @@ void DefaultPlanner::initialize_common(rclcpp::Node * node)
   param_.goal_angle_threshold_deg = node_->declare_parameter<double>("goal_angle_threshold_deg");
   param_.enable_correct_goal_pose = node_->declare_parameter<bool>("enable_correct_goal_pose");
   param_.consider_no_drivable_lanes = node_->declare_parameter<bool>("consider_no_drivable_lanes");
+  param_.check_footprint_inside_lanes =
+    node_->declare_parameter<bool>("check_footprint_inside_lanes");
 }
 
 void DefaultPlanner::initialize(rclcpp::Node * node)
@@ -349,6 +351,7 @@ bool DefaultPlanner::is_goal_valid(
 
   // check if goal footprint exceeds lane when the goal isn't in parking_lot
   if (
+    param_.check_footprint_inside_lanes &&
     !check_goal_footprint(
       closest_lanelet, combined_prev_lanelet, polygon_footprint, next_lane_length) &&
     !is_in_parking_lot(
diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp
index cf5a19443dd82..251d0db533182 100644
--- a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp
+++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp
@@ -38,6 +38,7 @@ struct DefaultPlannerParameters
   double goal_angle_threshold_deg;
   bool enable_correct_goal_pose;
   bool consider_no_drivable_lanes;
+  bool check_footprint_inside_lanes;
 };
 
 class DefaultPlanner : public mission_planner::PlannerPlugin

From 7b1999ab8a7c1ab7fa1402b39d423e11b38cfc91 Mon Sep 17 00:00:00 2001
From: Takayuki Murooka <takayuki5168@gmail.com>
Date: Mon, 25 Sep 2023 10:51:56 +0900
Subject: [PATCH 23/35] refactor(pid_longitudinal_controller): use common
 elevetation angle calculation (#5090)

* refactor(pid_longitudinal_controller): use common elevetation angle calculation

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* update

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
---
 .../longitudinal_controller_utils.hpp         |  5 ----
 .../src/longitudinal_controller_utils.cpp     | 21 +++++----------
 .../src/pid_longitudinal_controller.cpp       |  2 +-
 .../test_longitudinal_controller_utils.cpp    | 27 -------------------
 4 files changed, 7 insertions(+), 48 deletions(-)

diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp
index 8685d6264993b..9c01f7bc26c4b 100644
--- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp
+++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp
@@ -66,11 +66,6 @@ double getPitchByPose(const Quaternion & quaternion);
 double getPitchByTraj(
   const Trajectory & trajectory, const size_t closest_idx, const double wheel_base);
 
-/**
- * @brief calculate elevation angle
- */
-double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint & p_to);
-
 /**
  * @brief calculate vehicle pose after time delay by moving the vehicle at current velocity and
  * acceleration for delayed time
diff --git a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp
index 70cdbcaf17bd2..41bbb48ff5d08 100644
--- a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp
+++ b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp
@@ -96,7 +96,8 @@ double getPitchByTraj(
       trajectory.points.at(nearest_idx), trajectory.points.at(i));
     if (dist > wheel_base) {
       // calculate pitch from trajectory between rear wheel (nearest) and front center (i)
-      return calcElevationAngle(trajectory.points.at(nearest_idx), trajectory.points.at(i));
+      return tier4_autoware_utils::calcElevationAngle(
+        trajectory.points.at(nearest_idx).pose.position, trajectory.points.at(i).pose.position);
     }
   }
 
@@ -108,24 +109,14 @@ double getPitchByTraj(
     if (dist > wheel_base) {
       // calculate pitch from trajectory
       // between wheelbase behind the end of trajectory (i) and the end of trajectory (back)
-      return calcElevationAngle(trajectory.points.at(i), trajectory.points.back());
+      return tier4_autoware_utils::calcElevationAngle(
+        trajectory.points.at(i).pose.position, trajectory.points.back().pose.position);
     }
   }
 
   // calculate pitch from trajectory between the beginning and end of trajectory
-  return calcElevationAngle(trajectory.points.at(0), trajectory.points.back());
-}
-
-double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint & p_to)
-{
-  const double dx = p_from.pose.position.x - p_to.pose.position.x;
-  const double dy = p_from.pose.position.y - p_to.pose.position.y;
-  const double dz = p_from.pose.position.z - p_to.pose.position.z;
-
-  const double dxy = std::max(std::hypot(dx, dy), std::numeric_limits<double>::epsilon());
-  const double pitch = std::atan2(dz, dxy);
-
-  return pitch;
+  return tier4_autoware_utils::calcElevationAngle(
+    trajectory.points.at(0).pose.position, trajectory.points.back().pose.position);
 }
 
 Pose calcPoseAfterTimeDelay(
diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
index 6d3489811b195..985a22d26f9f4 100644
--- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
+++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
@@ -817,7 +817,7 @@ double PidLongitudinalController::applySlopeCompensation(
   const double pitch_limited = std::min(std::max(pitch, m_min_pitch_rad), m_max_pitch_rad);
 
   // Acceleration command is always positive independent of direction (= shift) when car is running
-  double sign = (shift == Shift::Forward) ? -1 : (shift == Shift::Reverse ? 1 : 0);
+  double sign = (shift == Shift::Forward) ? 1.0 : (shift == Shift::Reverse ? -1.0 : 0);
   double compensated_acc = input_acc + sign * 9.81 * std::sin(pitch_limited);
   return compensated_acc;
 }
diff --git a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp
index d9fc404ce6abe..5c7698180f82b 100644
--- a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp
+++ b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp
@@ -158,33 +158,6 @@ TEST(TestLongitudinalControllerUtils, getPitchByTraj)
     std::atan2(0.5, 1));
 }
 
-TEST(TestLongitudinalControllerUtils, calcElevationAngle)
-{
-  using autoware_auto_planning_msgs::msg::TrajectoryPoint;
-  TrajectoryPoint p_from;
-  p_from.pose.position.x = 0.0;
-  p_from.pose.position.y = 0.0;
-  TrajectoryPoint p_to;
-  p_to.pose.position.x = 1.0;
-  p_to.pose.position.y = 0.0;
-  EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), 0.0);
-  p_to.pose.position.x = 1.0;
-  p_to.pose.position.z = 1.0;
-  EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), -M_PI_4);
-  p_to.pose.position.x = -1.0;
-  p_to.pose.position.z = 1.0;
-  EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), -M_PI_4);
-  p_to.pose.position.x = 0.0;
-  p_to.pose.position.z = 1.0;
-  EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), -M_PI_2);
-  p_to.pose.position.x = 1.0;
-  p_to.pose.position.z = -1.0;
-  EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), M_PI_4);
-  p_to.pose.position.x = -1.0;
-  p_to.pose.position.z = -1.0;
-  EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), M_PI_4);
-}
-
 TEST(TestLongitudinalControllerUtils, calcPoseAfterTimeDelay)
 {
   using geometry_msgs::msg::Pose;

From 460fc46c290746e7a04a6148f74b7ef828c668a9 Mon Sep 17 00:00:00 2001
From: Takayuki Murooka <takayuki5168@gmail.com>
Date: Mon, 25 Sep 2023 10:53:39 +0900
Subject: [PATCH 24/35] feat(merge_from_private): make stop_distance_threshold
 tunable (#5086)

* feat(merge_from_private): make stop_distance_threshold tunable

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* update

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
---
 .../config/intersection.param.yaml                | 15 ++++++++-------
 .../src/manager.cpp                               |  1 +
 .../src/scene_merge_from_private_road.cpp         |  5 ++---
 .../src/scene_merge_from_private_road.hpp         |  1 +
 4 files changed, 12 insertions(+), 10 deletions(-)

diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml
index c30c5d70f8c60..def2335e20358 100644
--- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml
+++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml
@@ -2,7 +2,7 @@
   ros__parameters:
     intersection:
       common:
-        attention_area_margin: 0.5 # [m]
+        attention_area_margin: 0.75 # [m]
         attention_area_length: 200.0 # [m]
         attention_area_angle_threshold: 0.785 # [rad]
         stop_line_margin: 3.0
@@ -14,7 +14,7 @@
         consider_wrong_direction_vehicle: false
       stuck_vehicle:
         use_stuck_stopline: true # stopline generated before the first conflicting area
-        stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length.
+        stuck_vehicle_detect_dist: 5.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length.
         stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h
         # enable_front_car_decel_prediction: false # By default this feature is disabled
         # assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning
@@ -45,14 +45,15 @@
         min_vehicle_brake_for_rss: -2.5 # [m/s^2]
         max_vehicle_velocity_for_rss: 16.66 # [m/s] == 60kmph
         denoise_kernel: 1.0 # [m]
-        possible_object_bbox: [1.0, 2.0] # [m x m]
-        ignore_parked_vehicle_speed_threshold: 0.333 # == 1.2km/h
-        stop_release_margin_time: 1.0 # [s]
+        possible_object_bbox: [1.5, 2.5] # [m x m]
+        ignore_parked_vehicle_speed_threshold: 0.8333 # == 3.0km/h
+        stop_release_margin_time: 1.5 # [s]
 
-      enable_rtc: # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval
-        intersection: true
+      enable_rtc:
+        intersection: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval
         intersection_to_occlusion: true
 
     merge_from_private:
       stop_line_margin: 3.0
       stop_duration_sec: 1.0
+      stop_distance_threshold: 1.0
diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp
index bf0970f3640d1..b2cdcf5db79eb 100644
--- a/planning/behavior_velocity_intersection_module/src/manager.cpp
+++ b/planning/behavior_velocity_intersection_module/src/manager.cpp
@@ -273,6 +273,7 @@ MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node
   mp.stop_line_margin = getOrDeclareParameter<double>(node, ns + ".stop_line_margin");
   mp.path_interpolation_ds =
     node.get_parameter("intersection.common.path_interpolation_ds").as_double();
+  mp.stop_distance_threshold = getOrDeclareParameter<double>(node, ns + ".stop_distance_threshold");
 }
 
 void MergeFromPrivateModuleManager::launchNewModules(
diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp
index 9c3a35033ea9b..bb54d829cc477 100644
--- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp
+++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp
@@ -135,12 +135,11 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR
     const double signed_arc_dist_to_stop_point = motion_utils::calcSignedArcLength(
       path->points, current_pose.position, path->points.at(stop_line_idx).point.pose.position);
 
-    constexpr double distance_threshold = 2.0;
     if (
-      signed_arc_dist_to_stop_point < distance_threshold &&
+      signed_arc_dist_to_stop_point < planner_param_.stop_distance_threshold &&
       planner_data_->isVehicleStopped(planner_param_.stop_duration_sec)) {
       state_machine_.setState(StateMachine::State::GO);
-      if (signed_arc_dist_to_stop_point < -distance_threshold) {
+      if (signed_arc_dist_to_stop_point < -planner_param_.stop_distance_threshold) {
         RCLCPP_ERROR(logger_, "Failed to stop near stop line but ego stopped. Change state to GO");
       }
     }
diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp
index e00b2a8b886d1..19e7b9201093f 100644
--- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp
+++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp
@@ -58,6 +58,7 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface
     double attention_area_length;
     double stop_line_margin;
     double stop_duration_sec;
+    double stop_distance_threshold;
     double path_interpolation_ds;
     double occlusion_attention_area_length;
     bool consider_wrong_direction_vehicle;

From 617a6273be90e2130618b08eeae5f97121137c72 Mon Sep 17 00:00:00 2001
From: Takayuki Murooka <takayuki5168@gmail.com>
Date: Mon, 25 Sep 2023 11:07:08 +0900
Subject: [PATCH 25/35] fix(pid_longitudinal_controller): use
 is_autoware_control_enabled for manual driving (#5092)

* fix(pid_longitudinal_controller): use is_autoware_control_enabled for manual driving

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* update

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
---
 .../src/pid_longitudinal_controller.cpp                        | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
index 985a22d26f9f4..fc10611e74287 100644
--- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
+++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
@@ -923,7 +923,8 @@ double PidLongitudinalController::applyVelocityFeedback(
 {
   const double current_vel_abs = std::fabs(current_vel);
   const double target_vel_abs = std::fabs(target_motion.vel);
-  const bool is_under_control = m_current_operation_mode.mode == OperationModeState::AUTONOMOUS;
+  const bool is_under_control = m_current_operation_mode.is_autoware_control_enabled &&
+                                m_current_operation_mode.mode == OperationModeState::AUTONOMOUS;
   const bool enable_integration =
     (current_vel_abs > m_current_vel_threshold_pid_integrate) && is_under_control;
   const double error_vel_filtered = m_lpf_vel_error->filter(target_vel_abs - current_vel_abs);

From a73bbf97c9d833a05bb8533e7bebb8f9b8d51fb6 Mon Sep 17 00:00:00 2001
From: Takayuki Murooka <takayuki5168@gmail.com>
Date: Mon, 25 Sep 2023 11:08:10 +0900
Subject: [PATCH 26/35] feat(pid_longitudinal_controller): transit state from
 emergency under the looser condition on manual driving (#5093)

* feat(pid_longitudinal_controller): transit from emergency under the looser condition on manual driving

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* update

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* add NOTE

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
---
 .../src/pid_longitudinal_controller.cpp       | 22 ++++++++++++++-----
 1 file changed, 16 insertions(+), 6 deletions(-)

diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
index fc10611e74287..8636448515061 100644
--- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
+++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
@@ -531,10 +531,8 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
     RCLCPP_INFO_SKIPFIRST_THROTTLE(logger_, *clock_, 5000, "%s", s);
   };
 
-  // if current operation mode is not autonomous mode, then change state to stopped
-  if (m_current_operation_mode.mode != OperationModeState::AUTONOMOUS) {
-    return changeState(ControlState::STOPPED);
-  }
+  const bool is_under_control = m_current_operation_mode.is_autoware_control_enabled &&
+                                m_current_operation_mode.mode == OperationModeState::AUTONOMOUS;
 
   // transit state
   // in DRIVE state
@@ -543,6 +541,12 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
       return changeState(ControlState::EMERGENCY);
     }
 
+    if (!is_under_control && stopped_condition && keep_stopped_condition) {
+      // NOTE: When the ego is stopped on manual driving, since the driving state may transit to
+      //       autonomous, keep_stopped_condition should be checked.
+      return changeState(ControlState::STOPPED);
+    }
+
     if (m_enable_smooth_stop) {
       if (stopping_condition) {
         // predictions after input time delay
@@ -609,8 +613,14 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
 
   // in EMERGENCY state
   if (m_control_state == ControlState::EMERGENCY) {
-    if (stopped_condition && !emergency_condition) {
-      return changeState(ControlState::STOPPED);
+    if (!emergency_condition) {
+      if (stopped_condition) {
+        return changeState(ControlState::STOPPED);
+      }
+      if (!is_under_control) {
+        // NOTE: On manual driving, no need stopping to exit the emergency.
+        return changeState(ControlState::DRIVE);
+      }
     }
     return;
   }

From b6446b0e91d5230d27a8bbc2c64ec19b082e40de Mon Sep 17 00:00:00 2001
From: Takamasa Horibe <horibe.takamasa@gmail.com>
Date: Mon, 25 Sep 2023 11:34:18 +0900
Subject: [PATCH 27/35] fix(motion_velocity_smoother): change curvature
 calculation distance parameter (#4924)

* fix(motion_velocity_smoother): use curvature calculation distance parameter

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix param format

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

---------

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
---
 ...efault_motion_velocity_smoother.param.yaml | 19 ++++++++++---------
 .../src/smoother/smoother_base.cpp            |  6 ++----
 2 files changed, 12 insertions(+), 13 deletions(-)

diff --git a/planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml b/planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml
index e56845c1c9bd6..21ad85137ef36 100644
--- a/planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml
+++ b/planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml
@@ -7,13 +7,21 @@
     # external velocity limit parameter
     margin_to_insert_external_velocity_limit: 0.3   #  margin distance to insert external velocity limit [m]
 
-    # curve parameters
+    # ---- curve parameters  ----
+    # - common parameters -
+    curvature_calculation_distance: 5.0         # distance of points while curvature is calculating for the steer rate and lateral acceleration limit [m]
+    # - lateral acceleration limit parameters -
     enable_lateral_acc_limit: true     # To toggle the lateral acc filter on and off. You can switch it dynamically at runtime.
     max_lateral_accel: 0.8             # max lateral acceleration limit [m/ss]
-    min_curve_velocity: 2.74           # min velocity at lateral acceleration limit [m/ss]
+    min_curve_velocity: 2.74           # min velocity at lateral acceleration limit and steering angle rate limit [m/s]
     decel_distance_before_curve: 3.5   # slow speed distance before a curve for lateral acceleration limit
     decel_distance_after_curve: 2.0    # slow speed distance after a curve for lateral acceleration limit
     min_decel_for_lateral_acc_lim_filter: -2.5  # deceleration limit applied in the lateral acceleration filter to avoid sudden braking [m/ss]
+    # - steering angle rate limit parameters -
+    enable_steering_rate_limit: true         # To toggle the steer rate filter on and off. You can switch it dynamically at runtime.
+    max_steering_angle_rate: 40.0            # maximum steering angle rate [degree/s]
+    resample_ds: 0.1                         # distance between trajectory points [m]
+    curvature_threshold: 0.02                # if curvature > curvature_threshold, steeringRateLimit is triggered [1/m]
 
     # engage & replan parameters
     replan_vel_deviation: 5.53         # velocity deviation to replan initial velocity [m/s]
@@ -49,13 +57,6 @@
     post_sparse_resample_dt: 0.1             # resample time interval for sparse sampling [s]
     post_sparse_min_interval_distance: 1.0   # minimum points-interval length for sparse sampling [m]
 
-    # steering angle rate limit parameters
-    enable_steering_rate_limit: true         # To toggle the steer rate filter on and off. You can switch it dynamically at runtime.
-    max_steering_angle_rate: 40.0            # maximum steering angle rate [degree/s]
-    resample_ds: 0.1                         # distance between trajectory points [m]
-    curvature_threshold: 0.02                # if curvature > curvature_threshold, steeringRateLimit is triggered [1/m]
-    curvature_calculation_distance: 1.0      # distance of points while curvature is calculating [m]
-
     # system
     over_stop_velocity_warn_thr: 1.389  # used to check if the optimization exceeds the input velocity on the stop point
 
diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp
index 7f058d0d138d0..4e5efdbfc32e4 100644
--- a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp
+++ b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp
@@ -103,8 +103,6 @@ TrajectoryPoints SmootherBase::applyLateralAccelerationFilter(
     return input;  // cannot calculate lateral acc. do nothing.
   }
 
-  constexpr double curvature_calc_dist = 5.0;  // [m] calc curvature with 5m away points
-
   // Interpolate with constant interval distance for lateral acceleration calculation.
   TrajectoryPoints output;
   const double points_interval =
@@ -124,8 +122,8 @@ TrajectoryPoints SmootherBase::applyLateralAccelerationFilter(
     output = input;
   }
 
-  const size_t idx_dist =
-    static_cast<size_t>(std::max(static_cast<int>((curvature_calc_dist) / points_interval), 1));
+  const size_t idx_dist = static_cast<size_t>(
+    std::max(static_cast<int>((base_param_.curvature_calculation_distance) / points_interval), 1));
 
   // Calculate curvature assuming the trajectory points interval is constant
   const auto curvature_v = trajectory_utils::calcTrajectoryCurvatureFrom3Points(output, idx_dist);

From e2123a2146f0ba44be65b3604a26b5d3f2c60699 Mon Sep 17 00:00:00 2001
From: kminoda <44218668+kminoda@users.noreply.github.com>
Date: Mon, 25 Sep 2023 12:12:21 +0900
Subject: [PATCH 28/35] feat(imu_corrector): add gyro_bias estimation in diag
 (#5054)

* feat(imu_corrector): add gyro_bias estimation in diag

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* update

Signed-off-by: kminoda <koji.minoda@tier4.jp>

---------

Signed-off-by: kminoda <koji.minoda@tier4.jp>
---
 sensing/imu_corrector/src/gyro_bias_estimator.cpp | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp
index 581eccb0097d4..5f41a9089b6ee 100644
--- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp
+++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp
@@ -83,6 +83,14 @@ void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusW
     stat.add("gyro_bias", "Bias estimation is not yet ready because of insufficient data.");
     stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Not initialized");
   } else {
+    stat.add("gyro_bias_x_for_imu_corrector", gyro_bias_.value().x);
+    stat.add("gyro_bias_y_for_imu_corrector", gyro_bias_.value().y);
+    stat.add("gyro_bias_z_for_imu_corrector", gyro_bias_.value().z);
+
+    stat.add("estimated_gyro_bias_x", gyro_bias_.value().x - angular_velocity_offset_x_);
+    stat.add("estimated_gyro_bias_y", gyro_bias_.value().y - angular_velocity_offset_y_);
+    stat.add("estimated_gyro_bias_z", gyro_bias_.value().z - angular_velocity_offset_z_);
+
     // Validation
     const bool is_bias_small_enough =
       std::abs(gyro_bias_.value().x - angular_velocity_offset_x_) < gyro_bias_threshold_ &&

From 5b92c71ba22aea5c0564d21fe6ea2f1e92cf5640 Mon Sep 17 00:00:00 2001
From: Takayuki Murooka <takayuki5168@gmail.com>
Date: Mon, 25 Sep 2023 14:04:56 +0900
Subject: [PATCH 29/35] feat(intersection): consider amber traffic signal for
 collision detection level (#5096)

* feat(intersection): consider amber traffic signal for collision detection level

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* update config

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* protected -> prioritized

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
---
 .../config/intersection.param.yaml            | 11 ++++--
 .../src/manager.cpp                           | 25 ++++++++----
 .../src/scene_intersection.cpp                | 39 ++++++++++++-------
 .../src/scene_intersection.hpp                | 19 +++++----
 .../src/util.cpp                              | 31 ++++++++-------
 .../src/util.hpp                              |  3 +-
 .../src/util_type.hpp                         | 18 ++++++---
 7 files changed, 93 insertions(+), 53 deletions(-)

diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml
index def2335e20358..5b626ec19d917 100644
--- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml
+++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml
@@ -24,12 +24,15 @@
         state_transit_margin_time: 1.0
         min_predicted_path_confidence: 0.05
         minimum_ego_predicted_velocity: 1.388 # [m/s]
-        normal:
-          collision_start_margin_time: 4.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object
-          collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object
-        relaxed:
+        fully_prioritized:
           collision_start_margin_time: 2.0
           collision_end_margin_time: 0.0
+        partially_prioritized:
+          collision_start_margin_time: 2.0
+          collision_end_margin_time: 2.0
+        not_prioritized:
+          collision_start_margin_time: 4.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object
+          collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object
         keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr
 
       occlusion:
diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp
index b2cdcf5db79eb..633ab3115b94c 100644
--- a/planning/behavior_velocity_intersection_module/src/manager.cpp
+++ b/planning/behavior_velocity_intersection_module/src/manager.cpp
@@ -84,14 +84,23 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
     getOrDeclareParameter<double>(node, ns + ".collision_detection.minimum_ego_predicted_velocity");
   ip.collision_detection.state_transit_margin_time =
     getOrDeclareParameter<double>(node, ns + ".collision_detection.state_transit_margin_time");
-  ip.collision_detection.normal.collision_start_margin_time = getOrDeclareParameter<double>(
-    node, ns + ".collision_detection.normal.collision_start_margin_time");
-  ip.collision_detection.normal.collision_end_margin_time = getOrDeclareParameter<double>(
-    node, ns + ".collision_detection.normal.collision_end_margin_time");
-  ip.collision_detection.relaxed.collision_start_margin_time = getOrDeclareParameter<double>(
-    node, ns + ".collision_detection.relaxed.collision_start_margin_time");
-  ip.collision_detection.relaxed.collision_end_margin_time = getOrDeclareParameter<double>(
-    node, ns + ".collision_detection.relaxed.collision_end_margin_time");
+  ip.collision_detection.fully_prioritized.collision_start_margin_time =
+    getOrDeclareParameter<double>(
+      node, ns + ".collision_detection.fully_prioritized.collision_start_margin_time");
+  ip.collision_detection.fully_prioritized.collision_end_margin_time =
+    getOrDeclareParameter<double>(
+      node, ns + ".collision_detection.fully_prioritized.collision_end_margin_time");
+  ip.collision_detection.partially_prioritized.collision_start_margin_time =
+    getOrDeclareParameter<double>(
+      node, ns + ".collision_detection.partially_prioritized.collision_start_margin_time");
+  ip.collision_detection.partially_prioritized.collision_end_margin_time =
+    getOrDeclareParameter<double>(
+      node, ns + ".collision_detection.partially_prioritized.collision_end_margin_time");
+  ip.collision_detection.not_prioritized.collision_start_margin_time =
+    getOrDeclareParameter<double>(
+      node, ns + ".collision_detection.not_prioritized.collision_start_margin_time");
+  ip.collision_detection.not_prioritized.collision_end_margin_time = getOrDeclareParameter<double>(
+    node, ns + ".collision_detection.not_prioritized.collision_end_margin_time");
   ip.collision_detection.keep_detection_vel_thr =
     getOrDeclareParameter<double>(node, ns + ".collision_detection.keep_detection_vel_thr");
 
diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
index aa1ceca086bc2..f335970f4ffda 100644
--- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
+++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
@@ -757,9 +757,11 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
       planner_param_.occlusion.occlusion_attention_area_length,
       planner_param_.common.consider_wrong_direction_vehicle);
   }
-  const bool tl_arrow_solid_on =
-    util::isTrafficLightArrowActivated(assigned_lanelet, planner_data_->traffic_light_id_map);
-  intersection_lanelets_.value().update(tl_arrow_solid_on, interpolated_path_info);
+  const auto traffic_prioritized_level =
+    util::getTrafficPrioritizedLevel(assigned_lanelet, planner_data_->traffic_light_id_map);
+  const bool is_prioritized =
+    traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED;
+  intersection_lanelets_.value().update(is_prioritized, interpolated_path_info);
 
   const auto & conflicting_lanelets = intersection_lanelets_.value().conflicting();
   const auto & first_conflicting_area_opt = intersection_lanelets_.value().first_conflicting_area();
@@ -890,7 +892,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
   }
 
   // calculate dynamic collision around detection area
-  const double time_delay = (is_go_out_ || tl_arrow_solid_on)
+  const double time_delay = (is_go_out_ || is_prioritized)
                               ? 0.0
                               : (planner_param_.collision_detection.state_transit_margin_time -
                                  collision_state_machine_.getDuration());
@@ -898,14 +900,14 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
     filterTargetObjects(attention_lanelets, adjacent_lanelets, intersection_area);
 
   const bool has_collision = checkCollision(
-    *path, target_objects, path_lanelets, closest_idx, time_delay, tl_arrow_solid_on);
+    *path, target_objects, path_lanelets, closest_idx, time_delay, traffic_prioritized_level);
   collision_state_machine_.setStateWithMarginTime(
     has_collision ? StateMachine::State::STOP : StateMachine::State::GO,
     logger_.get_child("collision state_machine"), *clock_);
   const bool has_collision_with_margin =
     collision_state_machine_.getState() == StateMachine::State::STOP;
 
-  if (tl_arrow_solid_on) {
+  if (is_prioritized) {
     return TrafficLightArrowSolidOn{
       has_collision_with_margin, closest_idx, collision_stop_line_idx, occlusion_stop_line_idx};
   }
@@ -931,7 +933,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
                object.kinematics.initial_twist_with_covariance.twist.linear.y) <= thresh;
     });
   const bool is_occlusion_cleared =
-    (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !tl_arrow_solid_on)
+    (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !is_prioritized)
       ? isOcclusionCleared(
           *planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets,
           first_attention_area, interpolated_path_info, occlusion_attention_divisions,
@@ -1058,7 +1060,7 @@ bool IntersectionModule::checkCollision(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   const autoware_auto_perception_msgs::msg::PredictedObjects & objects,
   const util::PathLanelets & path_lanelets, const int closest_idx, const double time_delay,
-  const bool tl_arrow_solid_on)
+  const util::TrafficPrioritizedLevel & traffic_prioritized_level)
 {
   using lanelet::utils::getArcCoordinates;
   using lanelet::utils::getPolygonFromArcLength;
@@ -1081,12 +1083,21 @@ bool IntersectionModule::checkCollision(
 
   const auto ego_poly = ego_lane.polygon2d().basicPolygon();
   // check collision between predicted_path and ego_area
-  const double collision_start_margin_time =
-    tl_arrow_solid_on ? planner_param_.collision_detection.relaxed.collision_start_margin_time
-                      : planner_param_.collision_detection.normal.collision_start_margin_time;
-  const double collision_end_margin_time =
-    tl_arrow_solid_on ? planner_param_.collision_detection.relaxed.collision_end_margin_time
-                      : planner_param_.collision_detection.normal.collision_end_margin_time;
+  const auto [collision_start_margin_time, collision_end_margin_time] = [&]() {
+    if (traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED) {
+      return std::make_pair(
+        planner_param_.collision_detection.fully_prioritized.collision_start_margin_time,
+        planner_param_.collision_detection.fully_prioritized.collision_end_margin_time);
+    }
+    if (traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) {
+      return std::make_pair(
+        planner_param_.collision_detection.partially_prioritized.collision_start_margin_time,
+        planner_param_.collision_detection.partially_prioritized.collision_end_margin_time);
+    }
+    return std::make_pair(
+      planner_param_.collision_detection.not_prioritized.collision_start_margin_time,
+      planner_param_.collision_detection.not_prioritized.collision_end_margin_time);
+  }();
   bool collision_detected = false;
   for (const auto & object : target_objects.objects) {
     for (const auto & predicted_path : object.kinematics.predicted_paths) {
diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
index 8063300b9b787..268bdb5fe1f72 100644
--- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
+++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
@@ -79,16 +79,21 @@ class IntersectionModule : public SceneModuleInterface
       //! minimum confidence value of predicted path to use for collision detection
       double minimum_ego_predicted_velocity;  //! used to calculate ego's future velocity profile
       double state_transit_margin_time;
-      struct Normal
+      struct FullyPrioritized
       {
         double collision_start_margin_time;  //! start margin time to check collision
         double collision_end_margin_time;    //! end margin time to check collision
-      } normal;
-      struct Relaxed
+      } fully_prioritized;
+      struct PartiallyPrioritized
       {
-        double collision_start_margin_time;
-        double collision_end_margin_time;
-      } relaxed;
+        double collision_start_margin_time;  //! start margin time to check collision
+        double collision_end_margin_time;    //! end margin time to check collision
+      } partially_prioritized;
+      struct NotPrioritized
+      {
+        double collision_start_margin_time;  //! start margin time to check collision
+        double collision_end_margin_time;    //! end margin time to check collision
+      } not_prioritized;
       double keep_detection_vel_thr;  //! keep detection if ego is ego.vel < keep_detection_vel_thr
     } collision_detection;
     struct Occlusion
@@ -250,7 +255,7 @@ class IntersectionModule : public SceneModuleInterface
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
     const autoware_auto_perception_msgs::msg::PredictedObjects & target_objects,
     const util::PathLanelets & path_lanelets, const int closest_idx, const double time_delay,
-    const bool tl_arrow_solid_on);
+    const util::TrafficPrioritizedLevel & traffic_prioritized_level);
 
   bool isOcclusionCleared(
     const nav_msgs::msg::OccupancyGrid & occ_grid,
diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp
index 24c3454fd7e88..a59c11deb55d1 100644
--- a/planning/behavior_velocity_intersection_module/src/util.cpp
+++ b/planning/behavior_velocity_intersection_module/src/util.cpp
@@ -765,12 +765,11 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane)
   return tl_id.has_value();
 }
 
-bool isTrafficLightArrowActivated(
+TrafficPrioritizedLevel getTrafficPrioritizedLevel(
   lanelet::ConstLanelet lane, const std::map<int, TrafficSignalStamped> & tl_infos)
 {
   using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement;
 
-  const auto & turn_direction = lane.attributeOr("turn_direction", "else");
   std::optional<int> tl_id = std::nullopt;
   for (auto && tl_reg_elem : lane.regulatoryElementsAs<lanelet::TrafficLight>()) {
     tl_id = tl_reg_elem->id();
@@ -778,24 +777,28 @@ bool isTrafficLightArrowActivated(
   }
   if (!tl_id) {
     // this lane has no traffic light
-    return false;
+    return TrafficPrioritizedLevel::NOT_PRIORITIZED;
   }
   const auto tl_info_it = tl_infos.find(tl_id.value());
   if (tl_info_it == tl_infos.end()) {
     // the info of this traffic light is not available
-    return false;
+    return TrafficPrioritizedLevel::NOT_PRIORITIZED;
   }
   const auto & tl_info = tl_info_it->second;
+  bool has_amber_signal{false};
   for (auto && tl_light : tl_info.signal.elements) {
-    if (tl_light.color != TrafficSignalElement::GREEN) continue;
-    if (tl_light.status != TrafficSignalElement::SOLID_ON) continue;
-    if (turn_direction == std::string("left") && tl_light.shape == TrafficSignalElement::LEFT_ARROW)
-      return true;
-    if (
-      turn_direction == std::string("right") && tl_light.shape == TrafficSignalElement::RIGHT_ARROW)
-      return true;
+    if (tl_light.color == TrafficSignalElement::AMBER) {
+      has_amber_signal = true;
+    }
+    if (tl_light.color == TrafficSignalElement::RED) {
+      // NOTE: Return here since the red signal has the highest priority.
+      return TrafficPrioritizedLevel::FULLY_PRIORITIZED;
+    }
   }
-  return false;
+  if (has_amber_signal) {
+    return TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED;
+  }
+  return TrafficPrioritizedLevel::NOT_PRIORITIZED;
 }
 
 std::vector<DiscretizedLane> generateDetectionLaneDivisions(
@@ -1178,9 +1181,9 @@ double calcDistanceUntilIntersectionLanelet(
 }
 
 void IntersectionLanelets::update(
-  const bool tl_arrow_solid_on, const InterpolatedPathInfo & interpolated_path_info)
+  const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info)
 {
-  tl_arrow_solid_on_ = tl_arrow_solid_on;
+  is_prioritized_ = is_prioritized;
   // find the first conflicting/detection area polygon intersecting the path
   const auto & path = interpolated_path_info.path;
   const auto & lane_interval = interpolated_path_info.lane_id_interval.value();
diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp
index e766da6651417..72e96876f02c9 100644
--- a/planning/behavior_velocity_intersection_module/src/util.hpp
+++ b/planning/behavior_velocity_intersection_module/src/util.hpp
@@ -111,7 +111,8 @@ std::optional<Polygon2d> getIntersectionArea(
   lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr);
 
 bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane);
-bool isTrafficLightArrowActivated(
+
+TrafficPrioritizedLevel getTrafficPrioritizedLevel(
   lanelet::ConstLanelet lane, const std::map<int, TrafficSignalStamped> & tl_infos);
 
 std::vector<DiscretizedLane> generateDetectionLaneDivisions(
diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp
index 64be330a99232..643ac884d9361 100644
--- a/planning/behavior_velocity_intersection_module/src/util_type.hpp
+++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp
@@ -64,20 +64,20 @@ struct InterpolatedPathInfo
 struct IntersectionLanelets
 {
 public:
-  void update(const bool tl_arrow_solid_on, const InterpolatedPathInfo & interpolated_path_info);
+  void update(const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info);
   const lanelet::ConstLanelets & attention() const
   {
-    return tl_arrow_solid_on_ ? attention_non_preceding_ : attention_;
+    return is_prioritized_ ? attention_non_preceding_ : attention_;
   }
   const lanelet::ConstLanelets & conflicting() const { return conflicting_; }
   const lanelet::ConstLanelets & adjacent() const { return adjacent_; }
   const lanelet::ConstLanelets & occlusion_attention() const
   {
-    return tl_arrow_solid_on_ ? attention_non_preceding_ : occlusion_attention_;
+    return is_prioritized_ ? attention_non_preceding_ : occlusion_attention_;
   }
   const std::vector<lanelet::CompoundPolygon3d> & attention_area() const
   {
-    return tl_arrow_solid_on_ ? attention_non_preceding_area_ : attention_area_;
+    return is_prioritized_ ? attention_non_preceding_area_ : attention_area_;
   }
   const std::vector<lanelet::CompoundPolygon3d> & conflicting_area() const
   {
@@ -110,7 +110,7 @@ struct IntersectionLanelets
   // the first area intersecting with the path
   // even if lane change/re-routing happened on the intersection, these areas area are supposed to
   // be invariant under the 'associative' lanes.
-  bool tl_arrow_solid_on_ = false;
+  bool is_prioritized_ = false;
   std::optional<lanelet::CompoundPolygon3d> first_conflicting_area_ = std::nullopt;
   std::optional<lanelet::CompoundPolygon3d> first_attention_area_ = std::nullopt;
 };
@@ -151,6 +151,14 @@ struct PathLanelets
                                          // conflicting lanelets plus the next lane part of the path
 };
 
+enum class TrafficPrioritizedLevel {
+  // The target lane's traffic signal is red or the ego's traffic signal has an arrow.
+  FULLY_PRIORITIZED = 0,
+  // The target lane's traffic signal is amber
+  PARTIALLY_PRIORITIZED,
+  // The target lane's traffic signal is green
+  NOT_PRIORITIZED
+};
 }  // namespace behavior_velocity_planner::util
 
 #endif  // UTIL_TYPE_HPP_

From 367686bc8c8454bcc47f7962408ffd643f4fa7d1 Mon Sep 17 00:00:00 2001
From: Takayuki Murooka <takayuki5168@gmail.com>
Date: Mon, 25 Sep 2023 14:15:46 +0900
Subject: [PATCH 30/35] fix(pid_longitudinal_controller): fix the sign of
 velocity (#5094)

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
---
 .../pid_longitudinal_controller.hpp            |  2 +-
 .../src/pid_longitudinal_controller.cpp        | 18 ++++++++++--------
 2 files changed, 11 insertions(+), 9 deletions(-)

diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp
index 41d98a4d15541..921cd3995f6a9 100644
--- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp
+++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp
@@ -365,7 +365,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
    * @param [in] current_vel current velocity of the vehicle
    */
   double applyVelocityFeedback(
-    const Motion target_motion, const double dt, const double current_vel);
+    const Motion target_motion, const double dt, const double current_vel, const Shift & shift);
 
   /**
    * @brief update variables for debugging about pitch
diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
index 8636448515061..a943aa05be5ff 100644
--- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
+++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
@@ -654,7 +654,8 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd(
     m_debug_values.setValues(DebugValues::TYPE::PREDICTED_VEL, pred_vel_in_target);
 
     raw_ctrl_cmd.vel = target_motion.vel;
-    raw_ctrl_cmd.acc = applyVelocityFeedback(target_motion, control_data.dt, pred_vel_in_target);
+    raw_ctrl_cmd.acc =
+      applyVelocityFeedback(target_motion, control_data.dt, pred_vel_in_target, control_data.shift);
     RCLCPP_DEBUG(
       logger_,
       "[feedback control]  vel: %3.3f, acc: %3.3f, dt: %3.3f, v_curr: %3.3f, v_ref: %3.3f "
@@ -929,15 +930,16 @@ double PidLongitudinalController::predictedVelocityInTargetPoint(
 }
 
 double PidLongitudinalController::applyVelocityFeedback(
-  const Motion target_motion, const double dt, const double current_vel)
+  const Motion target_motion, const double dt, const double current_vel, const Shift & shift)
 {
-  const double current_vel_abs = std::fabs(current_vel);
-  const double target_vel_abs = std::fabs(target_motion.vel);
+  // NOTE: Acceleration command is always positive even if the ego drives backward.
+  const double vel_sign = (shift == Shift::Forward) ? 1.0 : (shift == Shift::Reverse ? -1.0 : 0.0);
+  const double diff_vel = (target_motion.vel - current_vel) * vel_sign;
   const bool is_under_control = m_current_operation_mode.is_autoware_control_enabled &&
                                 m_current_operation_mode.mode == OperationModeState::AUTONOMOUS;
   const bool enable_integration =
-    (current_vel_abs > m_current_vel_threshold_pid_integrate) && is_under_control;
-  const double error_vel_filtered = m_lpf_vel_error->filter(target_vel_abs - current_vel_abs);
+    (std::abs(current_vel) > m_current_vel_threshold_pid_integrate) && is_under_control;
+  const double error_vel_filtered = m_lpf_vel_error->filter(diff_vel);
 
   std::vector<double> pid_contributions(3);
   const double pid_acc =
@@ -950,8 +952,8 @@ double PidLongitudinalController::applyVelocityFeedback(
   // deviation will be bigger.
   constexpr double ff_scale_max = 2.0;  // for safety
   constexpr double ff_scale_min = 0.5;  // for safety
-  const double ff_scale =
-    std::clamp(current_vel_abs / std::max(target_vel_abs, 0.1), ff_scale_min, ff_scale_max);
+  const double ff_scale = std::clamp(
+    std::abs(current_vel) / std::max(std::abs(target_motion.vel), 0.1), ff_scale_min, ff_scale_max);
   const double ff_acc = target_motion.acc * ff_scale;
 
   const double feedback_acc = ff_acc + pid_acc;

From 7f98ecfd9e6a12f4a506fe52f1bdd1a6302d17cc Mon Sep 17 00:00:00 2001
From: Takayuki Murooka <takayuki5168@gmail.com>
Date: Mon, 25 Sep 2023 14:16:22 +0900
Subject: [PATCH 31/35] fix(pid_longitudinal_controller): better slope
 compensation (#5095)

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
---
 .../src/longitudinal_controller_utils.cpp     | 36 +++++++------------
 1 file changed, 13 insertions(+), 23 deletions(-)

diff --git a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp
index 41bbb48ff5d08..9791a1f0b5e3e 100644
--- a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp
+++ b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp
@@ -91,32 +91,22 @@ double getPitchByTraj(
     return 0.0;
   }
 
-  for (size_t i = nearest_idx + 1; i < trajectory.points.size(); ++i) {
-    const double dist = tier4_autoware_utils::calcDistance2d(
-      trajectory.points.at(nearest_idx), trajectory.points.at(i));
-    if (dist > wheel_base) {
-      // calculate pitch from trajectory between rear wheel (nearest) and front center (i)
-      return tier4_autoware_utils::calcElevationAngle(
-        trajectory.points.at(nearest_idx).pose.position, trajectory.points.at(i).pose.position);
+  const auto [prev_idx, next_idx] = [&]() {
+    for (size_t i = nearest_idx + 1; i < trajectory.points.size(); ++i) {
+      const double dist = tier4_autoware_utils::calcDistance2d(
+        trajectory.points.at(nearest_idx), trajectory.points.at(i));
+      if (dist > wheel_base) {
+        // calculate pitch from trajectory between rear wheel (nearest) and front center (i)
+        return std::make_pair(nearest_idx, i);
+      }
     }
-  }
-
-  // close to goal
-  for (size_t i = trajectory.points.size() - 1; i > 0; --i) {
-    const double dist =
-      tier4_autoware_utils::calcDistance2d(trajectory.points.back(), trajectory.points.at(i));
-
-    if (dist > wheel_base) {
-      // calculate pitch from trajectory
-      // between wheelbase behind the end of trajectory (i) and the end of trajectory (back)
-      return tier4_autoware_utils::calcElevationAngle(
-        trajectory.points.at(i).pose.position, trajectory.points.back().pose.position);
-    }
-  }
+    // NOTE: The ego pose is close to the goal.
+    return std::make_pair(
+      std::min(nearest_idx, trajectory.points.size() - 2), trajectory.points.size() - 1);
+  }();
 
-  // calculate pitch from trajectory between the beginning and end of trajectory
   return tier4_autoware_utils::calcElevationAngle(
-    trajectory.points.at(0).pose.position, trajectory.points.back().pose.position);
+    trajectory.points.at(prev_idx).pose.position, trajectory.points.at(next_idx).pose.position);
 }
 
 Pose calcPoseAfterTimeDelay(

From 092ecea44c3e38242ca4b1d5b27a9fc4c86a8a26 Mon Sep 17 00:00:00 2001
From: Takayuki Murooka <takayuki5168@gmail.com>
Date: Mon, 25 Sep 2023 14:16:47 +0900
Subject: [PATCH 32/35] feat(tier4_planning_rviz_plugin): visualize text of
 slope angle (#5091)

---
 .../include/path/display_base.hpp             | 82 ++++++++++++++++++-
 1 file changed, 81 insertions(+), 1 deletion(-)

diff --git a/common/tier4_planning_rviz_plugin/include/path/display_base.hpp b/common/tier4_planning_rviz_plugin/include/path/display_base.hpp
index ccedcceefaaf9..74c2a60df3f32 100644
--- a/common/tier4_planning_rviz_plugin/include/path/display_base.hpp
+++ b/common/tier4_planning_rviz_plugin/include/path/display_base.hpp
@@ -133,7 +133,10 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay<T>
     property_point_alpha_{"Alpha", 1.0, "", &property_point_view_},
     property_point_color_{"Color", QColor(0, 60, 255), "", &property_point_view_},
     property_point_radius_{"Radius", 0.1, "", &property_point_view_},
-    property_point_offset_{"Offset", 0.0, "", &property_point_view_}
+    property_point_offset_{"Offset", 0.0, "", &property_point_view_},
+    // slope
+    property_slope_text_view_{"View Text Slope", false, "", this},
+    property_slope_text_scale_{"Scale", 0.3, "", &property_slope_text_view_}
   {
     // path
     property_path_width_.setMin(0.0);
@@ -171,6 +174,12 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay<T>
         node->detachAllObjects();
         this->scene_manager_->destroySceneNode(node);
       }
+      for (size_t i = 0; i < slope_text_nodes_.size(); i++) {
+        Ogre::SceneNode * node = slope_text_nodes_.at(i);
+        node->removeAndDestroyAllChildren();
+        node->detachAllObjects();
+        this->scene_manager_->destroySceneNode(node);
+      }
       this->scene_manager_->destroyManualObject(footprint_manual_object_);
       this->scene_manager_->destroyManualObject(point_manual_object_);
     }
@@ -198,6 +207,7 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay<T>
     rviz_common::MessageFilterDisplay<T>::MFDClass::reset();
     path_manual_object_->clear();
     velocity_manual_object_->clear();
+
     for (size_t i = 0; i < velocity_texts_.size(); i++) {
       Ogre::SceneNode * node = velocity_text_nodes_.at(i);
       node->detachAllObjects();
@@ -206,6 +216,16 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay<T>
     }
     velocity_text_nodes_.clear();
     velocity_texts_.clear();
+
+    for (size_t i = 0; i < slope_texts_.size(); i++) {
+      Ogre::SceneNode * node = slope_text_nodes_.at(i);
+      node->detachAllObjects();
+      node->removeAndDestroyAllChildren();
+      this->scene_manager_->destroySceneNode(node);
+    }
+    slope_text_nodes_.clear();
+    slope_texts_.clear();
+
     footprint_manual_object_->clear();
     point_manual_object_->clear();
   }
@@ -300,6 +320,29 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay<T>
       velocity_text_nodes_.resize(msg_ptr->points.size());
     }
 
+    if (msg_ptr->points.size() > slope_texts_.size()) {
+      for (size_t i = slope_texts_.size(); i < msg_ptr->points.size(); i++) {
+        Ogre::SceneNode * node = this->scene_node_->createChildSceneNode();
+        rviz_rendering::MovableText * text =
+          new rviz_rendering::MovableText("not initialized", "Liberation Sans", 0.1);
+        text->setVisible(false);
+        text->setTextAlignment(
+          rviz_rendering::MovableText::H_CENTER, rviz_rendering::MovableText::V_ABOVE);
+        node->attachObject(text);
+        slope_texts_.push_back(text);
+        slope_text_nodes_.push_back(node);
+      }
+    } else if (msg_ptr->points.size() < slope_texts_.size()) {
+      for (size_t i = slope_texts_.size() - 1; i >= msg_ptr->points.size(); i--) {
+        Ogre::SceneNode * node = slope_text_nodes_.at(i);
+        node->detachAllObjects();
+        node->removeAndDestroyAllChildren();
+        this->scene_manager_->destroySceneNode(node);
+      }
+      slope_texts_.resize(msg_ptr->points.size());
+      slope_text_nodes_.resize(msg_ptr->points.size());
+    }
+
     const auto info = vehicle_footprint_info_;
     const float left = property_path_width_view_.getBool() ? -property_path_width_.getFloat() / 2.0
                                                            : -info->width / 2.0;
@@ -395,6 +438,38 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay<T>
         rviz_rendering::MovableText * text = velocity_texts_.at(point_idx);
         text->setVisible(false);
       }
+
+      // slope text
+      if (property_slope_text_view_.getBool() && 1 < msg_ptr->points.size()) {
+        const size_t prev_idx =
+          (point_idx != msg_ptr->points.size() - 1) ? point_idx : point_idx - 1;
+        const size_t next_idx =
+          (point_idx != msg_ptr->points.size() - 1) ? point_idx + 1 : point_idx;
+
+        const auto & prev_path_pos =
+          tier4_autoware_utils::getPose(msg_ptr->points.at(prev_idx)).position;
+        const auto & next_path_pos =
+          tier4_autoware_utils::getPose(msg_ptr->points.at(next_idx)).position;
+
+        Ogre::Vector3 position;
+        position.x = pose.position.x;
+        position.y = pose.position.y;
+        position.z = pose.position.z;
+        Ogre::SceneNode * node = slope_text_nodes_.at(point_idx);
+        node->setPosition(position);
+
+        rviz_rendering::MovableText * text = slope_texts_.at(point_idx);
+        const double slope = tier4_autoware_utils::calcElevationAngle(prev_path_pos, next_path_pos);
+
+        std::stringstream ss;
+        ss << std::fixed << std::setprecision(2) << slope;
+        text->setCaption(ss.str());
+        text->setCharacterHeight(property_slope_text_scale_.getFloat());
+        text->setVisible(true);
+      } else {
+        rviz_rendering::MovableText * text = slope_texts_.at(point_idx);
+        text->setVisible(false);
+      }
     }
 
     path_manual_object_->end();
@@ -526,6 +601,8 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay<T>
   Ogre::ManualObject * point_manual_object_{nullptr};
   std::vector<rviz_rendering::MovableText *> velocity_texts_;
   std::vector<Ogre::SceneNode *> velocity_text_nodes_;
+  std::vector<rviz_rendering::MovableText *> slope_texts_;
+  std::vector<Ogre::SceneNode *> slope_text_nodes_;
 
   rviz_common::properties::BoolProperty property_path_view_;
   rviz_common::properties::BoolProperty property_path_width_view_;
@@ -556,6 +633,9 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay<T>
   rviz_common::properties::FloatProperty property_point_radius_;
   rviz_common::properties::FloatProperty property_point_offset_;
 
+  rviz_common::properties::BoolProperty property_slope_text_view_;
+  rviz_common::properties::FloatProperty property_slope_text_scale_;
+
   std::shared_ptr<VehicleInfo> vehicle_info_;
 
 private:

From eeab6c09cdda6bc5321397fb1ee125c3b62ff248 Mon Sep 17 00:00:00 2001
From: Kosuke Takeuchi <kosuke.tnp@gmail.com>
Date: Mon, 25 Sep 2023 15:35:35 +0900
Subject: [PATCH 33/35] fix(start_planner): remove inverse orientation points
 (#5100)

* fix(start_planner): remove inverse orientation points

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* use loop

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

---------

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
---
 .../motion_utils/trajectory/trajectory.hpp        | 15 ++++++++++-----
 .../utils/start_goal_planner_common/utils.hpp     | 15 ---------------
 .../src/utils/goal_planner/shift_pull_over.cpp    |  3 ---
 .../src/utils/path_shifter/path_shifter.cpp       | 14 ++++++++++++--
 .../src/utils/start_goal_planner_common/utils.cpp | 15 ---------------
 .../src/utils/start_planner/shift_pull_out.cpp    |  3 ---
 6 files changed, 22 insertions(+), 43 deletions(-)

diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp
index 83221f4d2f0ae..94cf48f29c57a 100644
--- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp
+++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp
@@ -1850,7 +1850,8 @@ insertOrientation<std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>
 
 /**
  * @brief Remove points with invalid orientation differences from a given points container
- * (trajectory, path, ...)
+ * (trajectory, path, ...). Check the difference between the angles of two points and the difference
+ * between the azimuth angle between the two points and the angle of the next point.
  * @param points Points of trajectory, path, or other point container (input / output)
  * @param max_yaw_diff Maximum acceptable yaw angle difference between two consecutive points in
  * radians (default: M_PI_2)
@@ -1859,10 +1860,14 @@ template <class T>
 void removeInvalidOrientationPoints(T & points, const double max_yaw_diff = M_PI_2)
 {
   for (size_t i = 1; i < points.size();) {
-    const double yaw1 = tf2::getYaw(tier4_autoware_utils::getPose(points.at(i - 1)).orientation);
-    const double yaw2 = tf2::getYaw(tier4_autoware_utils::getPose(points.at(i)).orientation);
-
-    if (max_yaw_diff < std::abs(tier4_autoware_utils::normalizeRadian(yaw1 - yaw2))) {
+    const auto p1 = tier4_autoware_utils::getPose(points.at(i - 1));
+    const auto p2 = tier4_autoware_utils::getPose(points.at(i));
+    const double yaw1 = tf2::getYaw(p1.orientation);
+    const double yaw2 = tf2::getYaw(p2.orientation);
+
+    if (
+      max_yaw_diff < std::abs(tier4_autoware_utils::normalizeRadian(yaw1 - yaw2)) ||
+      !tier4_autoware_utils::isDrivingForward(p1, p2)) {
       points.erase(points.begin() + i);
     } else {
       ++i;
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp
index 913018d25f8f8..37206c79542ff 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp
@@ -98,21 +98,6 @@ std::pair<double, double> getPairsTerminalVelocityAndAccel(
   const std::vector<std::pair<double, double>> & pairs_terminal_velocity_and_accel,
   const size_t current_path_idx);
 
-/**
- * @brief removeInverseOrderPathPoints function
- *
- * This function is designed to handle a situation that can arise when shifting paths on a curve,
- * where the positions of the path points may become inverted (i.e., a point further along the path
- * comes to be located before an earlier point). It corrects for this by using the function
- * tier4_autoware_utils::isDrivingForward(p1, p2) to check if each pair of adjacent points is in
- * the correct order (with the second point being 'forward' of the first). Any points which fail
- * this test are omitted from the returned PathWithLaneId.
- *
- * @param path A path with points possibly in incorrect order.
- * @return Returns a new PathWithLaneId that has all points in the correct order.
- */
-PathWithLaneId removeInverseOrderPathPoints(const PathWithLaneId & path);
-
 std::optional<PathWithLaneId> generateFeasibleStopPath(
   PathWithLaneId & current_path, std::shared_ptr<const PlannerData> planner_data,
   geometry_msgs::msg::Pose & stop_pose, const double maximum_deceleration,
diff --git a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp
index 1a690362fbe25..e1bdaf977dad8 100644
--- a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp
+++ b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp
@@ -172,9 +172,6 @@ boost::optional<PullOverPath> ShiftPullOver::generatePullOverPath(
     shifted_path.path.points.push_back(p);
   }
 
-  shifted_path.path =
-    utils::start_goal_planner_common::removeInverseOrderPathPoints(shifted_path.path);
-
   // set the same z as the goal
   for (auto & p : shifted_path.path.points) {
     p.point.pose.position.z = goal_pose.position.z;
diff --git a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp
index 44ba6bd364bc4..21d4ff666e28a 100644
--- a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp
+++ b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp
@@ -56,6 +56,7 @@ namespace behavior_path_planner
 using motion_utils::findNearestIndex;
 using motion_utils::insertOrientation;
 using motion_utils::removeInvalidOrientationPoints;
+using motion_utils::removeOverlapPoints;
 
 void PathShifter::setPath(const PathWithLaneId & path)
 {
@@ -147,9 +148,18 @@ bool PathShifter::generate(
   type == SHIFT_TYPE::SPLINE ? applySplineShifter(shifted_path, offset_back)
                              : applyLinearShifter(shifted_path);
 
-  const bool is_driving_forward = true;
-  insertOrientation(shifted_path->path.points, is_driving_forward);
+  shifted_path->path.points = removeOverlapPoints(shifted_path->path.points);
+  // Use orientation before shift to remove points in reverse order
+  // before setting wrong azimuth orientation
   removeInvalidOrientationPoints(shifted_path->path.points);
+  size_t previous_size{shifted_path->path.points.size()};
+  do {
+    previous_size = shifted_path->path.points.size();
+    // Set the azimuth orientation to the next point at each point
+    insertOrientation(shifted_path->path.points, true);
+    // Use azimuth orientation to remove points in reverse order
+    removeInvalidOrientationPoints(shifted_path->path.points);
+  } while (previous_size != shifted_path->path.points.size());
 
   // DEBUG
   RCLCPP_DEBUG_STREAM_THROTTLE(
diff --git a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp
index d52cebc6ce5eb..1eaea06c023f9 100644
--- a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp
+++ b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp
@@ -167,21 +167,6 @@ std::pair<double, double> getPairsTerminalVelocityAndAccel(
   return pairs_terminal_velocity_and_accel.at(current_path_idx);
 }
 
-PathWithLaneId removeInverseOrderPathPoints(const PathWithLaneId & path)
-{
-  PathWithLaneId fixed_path;
-  fixed_path.header = path.header;
-  fixed_path.points.push_back(path.points.at(0));
-  for (size_t i = 1; i < path.points.size(); i++) {
-    const auto p1 = path.points.at(i - 1).point.pose;
-    const auto p2 = path.points.at(i).point.pose;
-    if (tier4_autoware_utils::isDrivingForward(p1, p2)) {
-      fixed_path.points.push_back(path.points.at(i));
-    }
-  }
-  return fixed_path;
-}
-
 std::optional<PathWithLaneId> generateFeasibleStopPath(
   PathWithLaneId & current_path, std::shared_ptr<const PlannerData> planner_data,
   geometry_msgs::msg::Pose & stop_pose, const double maximum_deceleration,
diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp
index a4e4a2378e8f1..2de72d781994a 100644
--- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp
+++ b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp
@@ -303,9 +303,6 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
       continue;
     }
 
-    shifted_path.path =
-      utils::start_goal_planner_common::removeInverseOrderPathPoints(shifted_path.path);
-
     // set velocity
     const size_t pull_out_end_idx =
       findNearestIndex(shifted_path.path.points, shift_end_pose_ptr->position);

From f7a1b16d2c61c4aba6e1c63b9ef0fa848e640352 Mon Sep 17 00:00:00 2001
From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com>
Date: Mon, 25 Sep 2023 16:24:33 +0900
Subject: [PATCH 34/35] feat(obstacle_cruise_planner)!: add ego pose
 consideration (#5036)

* feat: add the feature to consider the current ego position

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

* feat: add ego pose consideration. Both the lateral error and the yaw error are assumed to decrease to zero by a specified constant time (e.g 1.5 second).

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

* feat: (continue) implement parameter settings for "cosider-current-ego-pose"

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

* cleaned up the code

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

* change description of the parameters

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

* align format

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

* Update planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp

Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>

* Update planning/obstacle_cruise_planner/src/node.cpp

Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>

---------

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>
---
 .../config/obstacle_cruise_planner.param.yaml |  7 ++
 .../include/obstacle_cruise_planner/node.hpp  |  7 ++
 .../obstacle_cruise_planner/polygon_utils.hpp |  8 +-
 planning/obstacle_cruise_planner/src/node.cpp | 71 ++++++++++++++--
 .../src/polygon_utils.cpp                     | 83 +++++++------------
 5 files changed, 117 insertions(+), 59 deletions(-)

diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml
index f3f1932c44c43..73bc4a83249a4 100644
--- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml
+++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml
@@ -106,6 +106,13 @@
         successive_num_to_entry_slow_down_condition: 5
         successive_num_to_exit_slow_down_condition: 5
 
+      # consider the current ego pose (it is not the nearest pose on the reference trajectory)
+      # Both the lateral error and the yaw error are assumed to decrease to zero by the time duration "time_to_convergence"
+      # The both errors decrease with constant rates against the time.
+      consider_current_pose:
+        enable_to_consider_current_pose: false
+        time_to_convergence: 1.5 #[s]
+
     cruise:
       pid_based_planner:
         use_velocity_limit_based_planner: true
diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp
index 00afc11985d72..5786a96f72c41 100644
--- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp
+++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp
@@ -48,6 +48,10 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
   void onSmoothedTrajectory(const Trajectory::ConstSharedPtr msg);
 
   // main functions
+  std::vector<Polygon2d> createOneStepPolygons(
+    const std::vector<TrajectoryPoint> & traj_points,
+    const vehicle_info_util::VehicleInfo & vehicle_info,
+    const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin = 0.0) const;
   std::vector<Obstacle> convertToObstacles(const std::vector<TrajectoryPoint> & traj_points) const;
   std::tuple<std::vector<StopObstacle>, std::vector<CruiseObstacle>, std::vector<SlowDownObstacle>>
   determineEgoBehaviorAgainstObstacles(
@@ -193,6 +197,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
     double lat_hysteresis_margin_for_slow_down;
     int successive_num_to_entry_slow_down_condition;
     int successive_num_to_exit_slow_down_condition;
+    // consideration for the current ego pose
+    bool enable_to_consider_current_pose{false};
+    double time_to_convergence{1.5};
   };
   BehaviorDeterminationParam behavior_determination_param_;
 
diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp
index 5178597bc4588..c2b14cc2e875f 100644
--- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp
+++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp
@@ -33,6 +33,11 @@ namespace bg = boost::geometry;
 using tier4_autoware_utils::Point2d;
 using tier4_autoware_utils::Polygon2d;
 
+Polygon2d createOneStepPolygon(
+  const std::vector<geometry_msgs::msg::Pose> & last_poses,
+  const std::vector<geometry_msgs::msg::Pose> & current_poses,
+  const vehicle_info_util::VehicleInfo & vehicle_info, const double lat_margin);
+
 std::optional<geometry_msgs::msg::Point> getCollisionPoint(
   const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polygons,
   const Obstacle & obstacle, const bool is_driving_forward);
@@ -45,9 +50,6 @@ std::vector<PointWithStamp> getCollisionPoints(
   const double max_lat_dist = std::numeric_limits<double>::max(),
   const double max_prediction_time_for_collision_check = std::numeric_limits<double>::max());
 
-std::vector<Polygon2d> createOneStepPolygons(
-  const std::vector<TrajectoryPoint> & traj_points,
-  const vehicle_info_util::VehicleInfo & vehicle_info, const double lat_margin = 0.0);
 }  // namespace polygon_utils
 
 #endif  // OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_
diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp
index d0c240a862a74..8ef610cc9df10 100644
--- a/planning/obstacle_cruise_planner/src/node.cpp
+++ b/planning/obstacle_cruise_planner/src/node.cpp
@@ -274,6 +274,10 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara
     "behavior_determination.slow_down.successive_num_to_entry_slow_down_condition");
   successive_num_to_exit_slow_down_condition = node.declare_parameter<int>(
     "behavior_determination.slow_down.successive_num_to_exit_slow_down_condition");
+  enable_to_consider_current_pose = node.declare_parameter<bool>(
+    "behavior_determination.consider_current_pose.enable_to_consider_current_pose");
+  time_to_convergence = node.declare_parameter<double>(
+    "behavior_determination.consider_current_pose.time_to_convergence");
 }
 
 void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam(
@@ -332,6 +336,12 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam(
   tier4_autoware_utils::updateParam<int>(
     parameters, "behavior_determination.slow_down.successive_num_to_exit_slow_down_condition",
     successive_num_to_exit_slow_down_condition);
+  tier4_autoware_utils::updateParam<bool>(
+    parameters, "behavior_determination.consider_current_pose.enable_to_consider_current_pose",
+    enable_to_consider_current_pose);
+  tier4_autoware_utils::updateParam<double>(
+    parameters, "behavior_determination.consider_current_pose.time_to_convergence",
+    time_to_convergence);
 }
 
 ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & node_options)
@@ -534,6 +544,57 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms
     get_logger(), enable_calculation_time_info_, "%s := %f [ms]", __func__, calculation_time);
 }
 
+std::vector<Polygon2d> ObstacleCruisePlannerNode::createOneStepPolygons(
+  const std::vector<TrajectoryPoint> & traj_points,
+  const vehicle_info_util::VehicleInfo & vehicle_info,
+  const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin) const
+{
+  const auto & p = behavior_determination_param_;
+  const bool is_enable_current_pose_consideration = p.enable_to_consider_current_pose;
+  const double step_length = p.decimate_trajectory_step_length;
+  const double time_to_convergence = p.time_to_convergence;
+
+  std::vector<Polygon2d> polygons;
+  const double current_ego_lat_error =
+    motion_utils::calcLateralOffset(traj_points, current_ego_pose.position);
+  const double current_ego_yaw_error =
+    motion_utils::calcYawDeviation(traj_points, current_ego_pose);
+  double time_elapsed = 0.0;
+
+  std::vector<geometry_msgs::msg::Pose> last_poses = {traj_points.at(0).pose};
+  if (is_enable_current_pose_consideration) {
+    last_poses.push_back(current_ego_pose);
+  }
+
+  for (size_t i = 0; i < traj_points.size(); ++i) {
+    std::vector<geometry_msgs::msg::Pose> current_poses = {traj_points.at(i).pose};
+
+    // estimate the future ego pose with assuming that the pose error against the reference path
+    // will decrease to zero by the time_to_convergence
+    if (is_enable_current_pose_consideration && time_elapsed < time_to_convergence) {
+      const double rem_ratio = (time_to_convergence - time_elapsed) / time_to_convergence;
+      geometry_msgs::msg::Pose indexed_pose_err;
+      indexed_pose_err.set__orientation(
+        tier4_autoware_utils::createQuaternionFromYaw(current_ego_yaw_error * rem_ratio));
+      indexed_pose_err.set__position(
+        tier4_autoware_utils::createPoint(0.0, current_ego_lat_error * rem_ratio, 0.0));
+
+      current_poses.push_back(
+        tier4_autoware_utils::transformPose(indexed_pose_err, traj_points.at(i).pose));
+
+      if (traj_points.at(i).longitudinal_velocity_mps != 0.0) {
+        time_elapsed += step_length / traj_points.at(i).longitudinal_velocity_mps;
+      } else {
+        time_elapsed = std::numeric_limits<double>::max();
+      }
+    }
+    polygons.push_back(
+      polygon_utils::createOneStepPolygon(last_poses, current_poses, vehicle_info, lat_margin));
+    last_poses = current_poses;
+  }
+  return polygons;
+}
+
 std::vector<Obstacle> ObstacleCruisePlannerNode::convertToObstacles(
   const std::vector<TrajectoryPoint> & traj_points) const
 {
@@ -653,7 +714,7 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles(
   // calculated decimated trajectory points and trajectory polygon
   const auto decimated_traj_points = decimateTrajectoryPoints(traj_points);
   const auto decimated_traj_polys =
-    polygon_utils::createOneStepPolygons(decimated_traj_points, vehicle_info_);
+    createOneStepPolygons(decimated_traj_points, vehicle_info_, ego_odom_ptr_->pose.pose);
   debug_data_ptr_->detection_polygons = decimated_traj_polys;
 
   // determine ego's behavior from stop, cruise and slow down
@@ -985,8 +1046,8 @@ ObstacleCruisePlannerNode::createCollisionPointForStopObstacle(
   }
 
   // calculate collision points with trajectory with lateral stop margin
-  const auto traj_polys_with_lat_margin =
-    polygon_utils::createOneStepPolygons(traj_points, vehicle_info_, p.max_lat_margin_for_stop);
+  const auto traj_polys_with_lat_margin = createOneStepPolygons(
+    traj_points, vehicle_info_, ego_odom_ptr_->pose.pose, p.max_lat_margin_for_stop);
   const auto collision_point = polygon_utils::getCollisionPoint(
     traj_points, traj_polys_with_lat_margin, obstacle, is_driving_forward_);
   return collision_point;
@@ -1047,8 +1108,8 @@ std::optional<SlowDownObstacle> ObstacleCruisePlannerNode::createSlowDownObstacl
 
   // calculate collision points with trajectory with lateral stop margin
   // NOTE: For additional margin, hysteresis is not divided by two.
-  const auto traj_polys_with_lat_margin = polygon_utils::createOneStepPolygons(
-    traj_points, vehicle_info_,
+  const auto traj_polys_with_lat_margin = createOneStepPolygons(
+    traj_points, vehicle_info_, ego_odom_ptr_->pose.pose,
     p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down);
 
   std::vector<Polygon2d> front_collision_polygons;
diff --git a/planning/obstacle_cruise_planner/src/polygon_utils.cpp b/planning/obstacle_cruise_planner/src/polygon_utils.cpp
index 077b783f0e65e..e6b4ed89ca460 100644
--- a/planning/obstacle_cruise_planner/src/polygon_utils.cpp
+++ b/planning/obstacle_cruise_planner/src/polygon_utils.cpp
@@ -31,36 +31,6 @@ geometry_msgs::msg::Point calcOffsetPosition(
   return tier4_autoware_utils::calcOffsetPose(pose, offset_x, offset_y, 0.0).position;
 }
 
-Polygon2d createOneStepPolygon(
-  const geometry_msgs::msg::Pose & base_step_pose, const geometry_msgs::msg::Pose & next_step_pose,
-  const vehicle_info_util::VehicleInfo & vehicle_info, const double lat_margin)
-{
-  Polygon2d polygon;
-
-  const double base_to_front = vehicle_info.max_longitudinal_offset_m;
-  const double width = vehicle_info.vehicle_width_m / 2.0 + lat_margin;
-  const double base_to_rear = vehicle_info.rear_overhang_m;
-
-  // base step
-  appendPointToPolygon(polygon, calcOffsetPosition(base_step_pose, base_to_front, width));
-  appendPointToPolygon(polygon, calcOffsetPosition(base_step_pose, base_to_front, -width));
-  appendPointToPolygon(polygon, calcOffsetPosition(base_step_pose, -base_to_rear, -width));
-  appendPointToPolygon(polygon, calcOffsetPosition(base_step_pose, -base_to_rear, width));
-
-  // next step
-  appendPointToPolygon(polygon, calcOffsetPosition(next_step_pose, base_to_front, width));
-  appendPointToPolygon(polygon, calcOffsetPosition(next_step_pose, base_to_front, -width));
-  appendPointToPolygon(polygon, calcOffsetPosition(next_step_pose, -base_to_rear, -width));
-  appendPointToPolygon(polygon, calcOffsetPosition(next_step_pose, -base_to_rear, width));
-
-  bg::correct(polygon);
-
-  Polygon2d hull_polygon;
-  bg::convex_hull(polygon, hull_polygon);
-
-  return hull_polygon;
-}
-
 PointWithStamp calcNearestCollisionPoint(
   const size_t first_within_idx, const std::vector<PointWithStamp> & collision_points,
   const std::vector<TrajectoryPoint> & decimated_traj_points, const bool is_driving_forward)
@@ -132,6 +102,38 @@ std::optional<std::pair<size_t, std::vector<PointWithStamp>>> getCollisionIndex(
 
 namespace polygon_utils
 {
+Polygon2d createOneStepPolygon(
+  const std::vector<geometry_msgs::msg::Pose> & last_poses,
+  const std::vector<geometry_msgs::msg::Pose> & current_poses,
+  const vehicle_info_util::VehicleInfo & vehicle_info, const double lat_margin)
+{
+  Polygon2d polygon;
+
+  const double base_to_front = vehicle_info.max_longitudinal_offset_m;
+  const double width = vehicle_info.vehicle_width_m / 2.0 + lat_margin;
+  const double base_to_rear = vehicle_info.rear_overhang_m;
+
+  for (auto & pose : last_poses) {
+    appendPointToPolygon(polygon, calcOffsetPosition(pose, base_to_front, width));
+    appendPointToPolygon(polygon, calcOffsetPosition(pose, base_to_front, -width));
+    appendPointToPolygon(polygon, calcOffsetPosition(pose, -base_to_rear, -width));
+    appendPointToPolygon(polygon, calcOffsetPosition(pose, -base_to_rear, width));
+  }
+  for (auto & pose : current_poses) {
+    appendPointToPolygon(polygon, calcOffsetPosition(pose, base_to_front, width));
+    appendPointToPolygon(polygon, calcOffsetPosition(pose, base_to_front, -width));
+    appendPointToPolygon(polygon, calcOffsetPosition(pose, -base_to_rear, -width));
+    appendPointToPolygon(polygon, calcOffsetPosition(pose, -base_to_rear, width));
+  }
+
+  bg::correct(polygon);
+
+  Polygon2d hull_polygon;
+  bg::convex_hull(polygon, hull_polygon);
+
+  return hull_polygon;
+}
+
 std::optional<geometry_msgs::msg::Point> getCollisionPoint(
   const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polygons,
   const Obstacle & obstacle, const bool is_driving_forward)
@@ -184,25 +186,4 @@ std::vector<PointWithStamp> getCollisionPoints(
   return collision_points;
 }
 
-std::vector<Polygon2d> createOneStepPolygons(
-  const std::vector<TrajectoryPoint> & traj_points,
-  const vehicle_info_util::VehicleInfo & vehicle_info, const double lat_margin)
-{
-  std::vector<Polygon2d> polygons;
-
-  for (size_t i = 0; i < traj_points.size(); ++i) {
-    const auto polygon = [&]() {
-      if (i == 0) {
-        return createOneStepPolygon(
-          traj_points.at(i).pose, traj_points.at(i).pose, vehicle_info, lat_margin);
-      }
-      return createOneStepPolygon(
-        traj_points.at(i - 1).pose, traj_points.at(i).pose, vehicle_info, lat_margin);
-    }();
-
-    polygons.push_back(polygon);
-  }
-  return polygons;
-}
-
 }  // namespace polygon_utils

From 71da68ce1dc1a8c32a9ce5103908df9b65850af5 Mon Sep 17 00:00:00 2001
From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com>
Date: Mon, 25 Sep 2023 16:41:52 +0900
Subject: [PATCH 35/35] feat(lane_change): object filter visualization (#5082)

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
---
 .../marker_utils/lane_change/debug.hpp        |  6 +++
 .../marker_utils/utils.hpp                    |  6 +++
 .../scene_module/lane_change/base_class.hpp   |  6 +++
 .../path_safety_checker_parameters.hpp        |  1 +
 .../src/marker_utils/lane_change/debug.cpp    | 24 ++++++++++++
 .../src/marker_utils/utils.cpp                | 38 +++++++++++++++++++
 .../scene_module/lane_change/interface.cpp    |  6 +++
 .../src/scene_module/lane_change/normal.cpp   |  2 +
 8 files changed, 89 insertions(+)

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp
index d337bae17ca6c..1d0c9f5c2ce65 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp
@@ -17,6 +17,7 @@
 
 #include "behavior_path_planner/marker_utils/utils.hpp"
 #include "behavior_path_planner/utils/lane_change/lane_change_path.hpp"
+#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp"
 
 #include <string>
 #include <unordered_map>
@@ -25,11 +26,16 @@
 namespace marker_utils::lane_change_markers
 {
 using behavior_path_planner::LaneChangePath;
+using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects;
 using visualization_msgs::msg::MarkerArray;
 MarkerArray showAllValidLaneChangePath(
   const std::vector<LaneChangePath> & lanes, std::string && ns);
 MarkerArray createLaneChangingVirtualWallMarker(
   const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name,
   const rclcpp::Time & now, const std::string & ns);
+MarkerArray showFilteredObjects(
+  const ExtendedPredictedObjects & current_lane_objects,
+  const ExtendedPredictedObjects & target_lane_objects,
+  const ExtendedPredictedObjects & other_lane_objects, const std::string & ns);
 }  // namespace marker_utils::lane_change_markers
 #endif  // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__LANE_CHANGE__DEBUG_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp
index 244e1a8a71616..86f1a390261c6 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp
@@ -26,6 +26,7 @@
 
 #include <lanelet2_core/Forward.h>
 
+#include <cstdint>
 #include <string>
 #include <vector>
 
@@ -39,6 +40,7 @@ using behavior_path_planner::ShiftLineArray;
 using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap;
 using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugPair;
 using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject;
+using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Polygon;
 using geometry_msgs::msg::Pose;
@@ -106,6 +108,10 @@ MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::strin
 MarkerArray showPredictedPath(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
 
 MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
+
+MarkerArray showFilteredObjects(
+  const ExtendedPredictedObjects & predicted_objects, const std::string & ns,
+  const ColorRGBA & color, int32_t id);
 }  // namespace marker_utils
 
 #endif  // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__UTILS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp
index 1a7350d82aa4e..a3469dee2909b 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp
@@ -154,6 +154,11 @@ class LaneChangeBase
     return object_debug_after_approval_;
   }
 
+  const LaneChangeTargetObjects & getDebugFilteredObjects() const
+  {
+    return debug_filtered_objects_;
+  }
+
   const Pose & getEgoPose() const { return planner_data_->self_odometry->pose.pose; }
 
   const Point & getEgoPosition() const { return getEgoPose().position; }
@@ -262,6 +267,7 @@ class LaneChangeBase
   mutable LaneChangePaths debug_valid_path_{};
   mutable CollisionCheckDebugMap object_debug_{};
   mutable CollisionCheckDebugMap object_debug_after_approval_{};
+  mutable LaneChangeTargetObjects debug_filtered_objects_{};
   mutable double object_debug_lifetime_{0.0};
   mutable StopWatch<std::chrono::milliseconds> stop_watch_;
 };
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp
index aa470eb2dd80c..889f016e7182f 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp
@@ -77,6 +77,7 @@ struct ExtendedPredictedObject
   autoware_auto_perception_msgs::msg::Shape shape;
   std::vector<PredictedPathWithPolygon> predicted_paths;
 };
+using ExtendedPredictedObjects = std::vector<ExtendedPredictedObject>;
 
 /**
  * @brief Specifies which object class should be checked.
diff --git a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp
index 7493982db78b1..a1b77b1802999 100644
--- a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp
+++ b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp
@@ -98,4 +98,28 @@ MarkerArray createLaneChangingVirtualWallMarker(
   return marker_array;
 }
 
+MarkerArray showFilteredObjects(
+  const ExtendedPredictedObjects & current_lane_objects,
+  const ExtendedPredictedObjects & target_lane_objects,
+  const ExtendedPredictedObjects & other_lane_objects, const std::string & ns)
+{
+  int32_t update_id = 0;
+  auto current_marker =
+    marker_utils::showFilteredObjects(current_lane_objects, ns, colors::yellow(), update_id);
+  update_id += static_cast<int32_t>(current_marker.markers.size());
+  auto target_marker =
+    marker_utils::showFilteredObjects(target_lane_objects, ns, colors::aqua(), update_id);
+  update_id += static_cast<int32_t>(target_marker.markers.size());
+  auto other_marker =
+    marker_utils::showFilteredObjects(other_lane_objects, ns, colors::medium_orchid(), update_id);
+
+  MarkerArray marker_array;
+  marker_array.markers.insert(
+    marker_array.markers.end(), current_marker.markers.begin(), current_marker.markers.end());
+  marker_array.markers.insert(
+    marker_array.markers.end(), target_marker.markers.begin(), target_marker.markers.end());
+  marker_array.markers.insert(
+    marker_array.markers.end(), other_marker.markers.begin(), other_marker.markers.end());
+  return marker_array;
+}
 }  // namespace marker_utils::lane_change_markers
diff --git a/planning/behavior_path_planner/src/marker_utils/utils.cpp b/planning/behavior_path_planner/src/marker_utils/utils.cpp
index 4cec9b958f904..a2883403e1ccd 100644
--- a/planning/behavior_path_planner/src/marker_utils/utils.cpp
+++ b/planning/behavior_path_planner/src/marker_utils/utils.cpp
@@ -26,6 +26,8 @@
 #include <lanelet2_core/primitives/Lanelet.h>
 #include <lanelet2_core/primitives/LineString.h>
 
+#include <cstdint>
+
 namespace marker_utils
 {
 using behavior_path_planner::ShiftLine;
@@ -650,4 +652,40 @@ MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, st
   return marker_array;
 }
 
+MarkerArray showFilteredObjects(
+  const ExtendedPredictedObjects & predicted_objects, const std::string & ns,
+  const ColorRGBA & color, int32_t id)
+{
+  using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject;
+  if (predicted_objects.empty()) {
+    return MarkerArray{};
+  }
+
+  const auto now = rclcpp::Clock{RCL_ROS_TIME}.now();
+
+  auto default_cube_marker =
+    [&](const auto & width, const auto & depth, const auto & color = colors::green()) {
+      return createDefaultMarker(
+        "map", now, ns + "_cube", ++id, visualization_msgs::msg::Marker::CUBE,
+        createMarkerScale(width, depth, 1.0), color);
+    };
+
+  MarkerArray marker_array;
+  marker_array.markers.reserve(
+    predicted_objects.size());  // poly ego, text ego, poly obj, text obj, cube obj
+
+  std::for_each(
+    predicted_objects.begin(), predicted_objects.end(), [&](const ExtendedPredictedObject & obj) {
+      const auto insert_cube_marker = [&](const auto & pose) {
+        marker_array.markers.emplace_back();
+        auto & cube_marker = marker_array.markers.back();
+        cube_marker = default_cube_marker(1.0, 1.0, color);
+        cube_marker.pose = pose;
+      };
+      insert_cube_marker(obj.initial_pose.pose);
+    });
+
+  return marker_array;
+}
+
 }  // namespace marker_utils
diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp
index 74af83c2db2a1..51875952d40fb 100644
--- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp
+++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp
@@ -15,6 +15,7 @@
 #include "behavior_path_planner/scene_module/lane_change/interface.hpp"
 
 #include "behavior_path_planner/marker_utils/lane_change/debug.hpp"
+#include "behavior_path_planner/marker_utils/utils.hpp"
 #include "behavior_path_planner/scene_module/scene_module_interface.hpp"
 #include "behavior_path_planner/scene_module/scene_module_visitor.hpp"
 #include "behavior_path_planner/utils/lane_change/utils.hpp"
@@ -293,10 +294,12 @@ void LaneChangeInterface::setObjectDebugVisualization() const
   using marker_utils::showPredictedPath;
   using marker_utils::showSafetyCheckInfo;
   using marker_utils::lane_change_markers::showAllValidLaneChangePath;
+  using marker_utils::lane_change_markers::showFilteredObjects;
 
   const auto debug_data = module_type_->getDebugData();
   const auto debug_after_approval = module_type_->getAfterApprovalDebugData();
   const auto debug_valid_path = module_type_->getDebugValidPath();
+  const auto debug_filtered_objects = module_type_->getDebugFilteredObjects();
 
   debug_marker_.markers.clear();
   const auto add = [this](const MarkerArray & added) {
@@ -304,6 +307,9 @@ void LaneChangeInterface::setObjectDebugVisualization() const
   };
 
   add(showAllValidLaneChangePath(debug_valid_path, "lane_change_valid_paths"));
+  add(showFilteredObjects(
+    debug_filtered_objects.current_lane, debug_filtered_objects.target_lane,
+    debug_filtered_objects.other_lane, "object_filtered"));
   if (!debug_data.empty()) {
     add(showSafetyCheckInfo(debug_data, "object_debug_info"));
     add(showPredictedPath(debug_data, "ego_predicted_path"));
diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
index 36127bbdf20b6..568538967f2ac 100644
--- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
+++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
@@ -945,6 +945,7 @@ bool NormalLaneChange::getLaneChangePaths(
     utils::lane_change::getTargetNeighborLanesPolygon(route_handler, current_lanes, type_);
 
   const auto target_objects = getTargetObjects(current_lanes, target_lanes);
+  debug_filtered_objects_ = target_objects;
 
   const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes);
 
@@ -1153,6 +1154,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const
   const auto & target_lanes = status_.target_lanes;
 
   const auto target_objects = getTargetObjects(current_lanes, target_lanes);
+  debug_filtered_objects_ = target_objects;
 
   CollisionCheckDebugMap debug_data;
   const auto safety_status = isLaneChangePathSafe(