diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS
index fb6df15ab7679..03f9f22b56015 100644
--- a/.github/CODEOWNERS
+++ b/.github/CODEOWNERS
@@ -14,6 +14,7 @@ common/component_interface_tools/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp m
 common/component_interface_utils/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp yukihiro.saito@tier4.jp
 common/cuda_utils/** daisuke.nishimatsu@tier4.jp manato.hirabayashi@tier4.jp
 common/fake_test_node/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp
+common/geography_utils/** koji.minoda@tier4.jp
 common/global_parameter_loader/** ryohsuke.mitsudome@tier4.jp
 common/glog_component/** takamasa.horibe@tier4.jp
 common/goal_distance_calculator/** taiki.tanaka@tier4.jp
@@ -117,7 +118,7 @@ perception/lidar_apollo_segmentation_tvm/** ambroise.vincent@arm.com xinyu.wang@
 perception/lidar_apollo_segmentation_tvm_nodes/** ambroise.vincent@arm.com xinyu.wang@tier4.jp
 perception/lidar_centerpoint/** kenzo.lobos@tier4.jp yusuke.muramatsu@tier4.jp
 perception/lidar_centerpoint_tvm/** carl.liu@autocore.ai xinyu.wang@tier4.jp
-perception/map_based_prediction/** shunsuke.miura@tier4.jp tomoya.kimura@tier4.jp yoshi.ri@tier4.jp yutaka.shimizu@tier4.jp
+perception/map_based_prediction/** kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yoshi.ri@tier4.jp yutaka.shimizu@tier4.jp
 perception/multi_object_tracker/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
 perception/object_merger/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
 perception/object_range_splitter/** yukihiro.saito@tier4.jp
@@ -166,7 +167,7 @@ planning/freespace_planning_algorithms/** kosuke.takeuchi@tier4.jp takamasa.hori
 planning/mission_planner/** isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp ryohsuke.mitsudome@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp
 planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp makoto.kurihara@tier4.jp takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp
 planning/obstacle_avoidance_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp
-planning/obstacle_cruise_planner/** takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp
+planning/obstacle_cruise_planner/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp
 planning/obstacle_stop_planner/** berkay@leodrive.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp
 planning/obstacle_velocity_limiter/** maxime.clement@tier4.jp
 planning/path_smoother/** maxime.clement@tier4.jp takayuki.murooka@tier4.jp
diff --git a/common/path_distance_calculator/src/path_distance_calculator.cpp b/common/path_distance_calculator/src/path_distance_calculator.cpp
index e4c9c8e027f7b..85578d3c18dbc 100644
--- a/common/path_distance_calculator/src/path_distance_calculator.cpp
+++ b/common/path_distance_calculator/src/path_distance_calculator.cpp
@@ -14,7 +14,7 @@
 
 #include "path_distance_calculator.hpp"
 
-#include <motion_utils/motion_utils.hpp>
+#include <motion_utils/trajectory/trajectory.hpp>
 #include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 #include <algorithm>
diff --git a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp
index 8859367c25e9b..a8f260795d485 100644
--- a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp
+++ b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp
@@ -28,6 +28,7 @@
 #include <sensor_msgs/msg/imu.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <visualization_msgs/msg/marker.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
 
 #include <boost/optional.hpp>
 
diff --git a/control/control_validator/src/control_validator.cpp b/control/control_validator/src/control_validator.cpp
index 59b798a786722..9fe5121ee3dbd 100644
--- a/control/control_validator/src/control_validator.cpp
+++ b/control/control_validator/src/control_validator.cpp
@@ -204,7 +204,9 @@ void ControlValidator::displayStatus()
 
   const auto & s = validation_status_;
 
-  warn(s.is_valid_max_distance_deviation, "planning trajectory is too far from ego!!");
+  warn(
+    s.is_valid_max_distance_deviation,
+    "predicted trajectory is too far from planning trajectory!!");
 }
 
 }  // namespace control_validator
diff --git a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp
index 4868fa7a6f51d..489709d1f5c4c 100644
--- a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp
+++ b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp
@@ -36,6 +36,7 @@
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav_msgs/msg/odometry.hpp"
 #include "tf2_msgs/msg/tf_message.hpp"
+#include "visualization_msgs/msg/marker_array.hpp"
 
 #include <memory>
 #include <string>
diff --git a/control/trajectory_follower_node/package.xml b/control/trajectory_follower_node/package.xml
index 8673634e24058..34e9fb45424a7 100644
--- a/control/trajectory_follower_node/package.xml
+++ b/control/trajectory_follower_node/package.xml
@@ -32,6 +32,7 @@
   <depend>rclcpp_components</depend>
   <depend>trajectory_follower_base</depend>
   <depend>vehicle_info_util</depend>
+  <depend>visualization_msgs</depend>
 
   <exec_depend>ros2launch</exec_depend>
 
diff --git a/control/trajectory_follower_node/src/controller_node.cpp b/control/trajectory_follower_node/src/controller_node.cpp
index 5ee9f7f4f1e71..675ca9dffe3ae 100644
--- a/control/trajectory_follower_node/src/controller_node.cpp
+++ b/control/trajectory_follower_node/src/controller_node.cpp
@@ -17,6 +17,7 @@
 #include "mpc_lateral_controller/mpc_lateral_controller.hpp"
 #include "pid_longitudinal_controller/pid_longitudinal_controller.hpp"
 #include "pure_pursuit/pure_pursuit_lateral_controller.hpp"
+#include "tier4_autoware_utils/ros/marker_helper.hpp"
 
 #include <algorithm>
 #include <limits>
diff --git a/mkdocs.yaml b/mkdocs.yaml
index a72529fbe959a..2ca8b3c86ad43 100644
--- a/mkdocs.yaml
+++ b/mkdocs.yaml
@@ -55,7 +55,8 @@ plugins:
       regex:
         - ^(?!(.*/)?assets/).*\.(?!(.*\.)?md|(.*\.)?svg|(.*\.)?png|(.*\.)?gif|(.*\.)?jpg).*$
         - ^(.*/)?[^.]*$
-  - macros
+  - macros:
+      module_name: mkdocs_macros
   - mkdocs-video
   - same-dir
   - search
diff --git a/mkdocs_macros.py b/mkdocs_macros.py
new file mode 100644
index 0000000000000..56292a815d80d
--- /dev/null
+++ b/mkdocs_macros.py
@@ -0,0 +1,70 @@
+import json
+
+from tabulate import tabulate
+
+# This file is for defining macros for mkdocs-macros plugin
+# Check https://mkdocs-macros-plugin.readthedocs.io/en/latest/macros/ for the details
+
+
+def format_param_type(param_type):
+    if param_type == "number":
+        return "float"
+    else:
+        return param_type
+
+
+def format_param_range(param):
+    list_of_range = []
+    if "enum" in param.keys():
+        list_of_range.append(param["enum"])
+    if "minimum" in param.keys():
+        list_of_range.append("≥" + str(param["minimum"]))
+    if "exclusiveMinimum" in param.keys():
+        list_of_range.append(">" + str(param["exclusiveMinimum"]))
+    if "maximum" in param.keys():
+        list_of_range.append("≤" + str(param["maximum"]))
+    if "exclusiveMaximum" in param.keys():
+        list_of_range.append("<" + str(param["exclusiveMaximum"]))
+    if "exclusive" in param.keys():
+        list_of_range.append("≠" + str(param["exclusive"]))
+
+    if len(list_of_range) == 0:
+        return "N/A"
+    else:
+        range_in_text = ""
+        for item in list_of_range:
+            if range_in_text != "":
+                range_in_text += "<br/>"
+            range_in_text += str(item)
+        return range_in_text
+
+
+def extract_parameter_info(parameters, namespace=""):
+    params = []
+    for k, v in parameters.items():
+        if v["type"] != "object":
+            param = {}
+            param["Name"] = namespace + k
+            param["Type"] = format_param_type(v["type"])
+            param["Description"] = v["description"]
+            param["Default"] = v["default"]
+            param["Range"] = format_param_range(v)
+            params.append(param)
+        else:  # if the object is namespace, then dive deeper in to json value
+            params.extend(extract_parameter_info(v["properties"], k + "."))
+    return params
+
+
+def format_json(json_data):
+    parameters = list(json_data["definitions"].values())[0]["properties"]
+    # cspell: ignore tablefmt
+    markdown_table = tabulate(extract_parameter_info(parameters), headers="keys", tablefmt="github")
+    return markdown_table
+
+
+def define_env(env):
+    @env.macro
+    def json_to_markdown(json_schema_file_path):
+        with open(json_schema_file_path) as f:
+            data = json.load(f)
+            return format_json(data)
diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/README.md b/perception/lidar_apollo_segmentation_tvm_nodes/README.md
index 626048b2416a0..488eea4f92168 100644
--- a/perception/lidar_apollo_segmentation_tvm_nodes/README.md
+++ b/perception/lidar_apollo_segmentation_tvm_nodes/README.md
@@ -49,6 +49,10 @@ The input are non-ground points as a PointCloud2 message from the sensor_msgs pa
 
 The output is a [DetectedObjectsWithFeature](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_perception_msgs/msg/object_recognition/DetectedObjectsWithFeature.msg).
 
+#### Parameters
+
+{{ json_to_markdown("perception/lidar_apollo_segmentation_tvm_nodes/schema/lidar_apollo_segmentation_tvm_nodes.schema.json") }}
+
 ### Error detection and handling
 
 Abort and warn when the input frame can't be converted to `base_link`.
diff --git a/planning/behavior_path_planner/config/scene_module_manager.param.yaml b/planning/behavior_path_planner/config/scene_module_manager.param.yaml
index e89c5539a0965..9895d9b5e473f 100644
--- a/planning/behavior_path_planner/config/scene_module_manager.param.yaml
+++ b/planning/behavior_path_planner/config/scene_module_manager.param.yaml
@@ -8,6 +8,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: true
+      keep_last: false
       priority: 6
       max_module_size: 1
 
@@ -16,6 +17,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: true
+      keep_last: false
       priority: 6
       max_module_size: 1
 
@@ -24,6 +26,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: true
       enable_simultaneous_execution_as_candidate_module: true
+      keep_last: false
       priority: 5
       max_module_size: 1
 
@@ -32,6 +35,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: true
       enable_simultaneous_execution_as_candidate_module: true
+      keep_last: false
       priority: 5
       max_module_size: 1
 
@@ -40,6 +44,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: false
+      keep_last: false
       priority: 0
       max_module_size: 1
 
@@ -48,6 +53,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: false
+      keep_last: false
       priority: 2
       max_module_size: 1
 
@@ -56,6 +62,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: false
+      keep_last: true
       priority: 1
       max_module_size: 1
 
@@ -64,6 +71,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: true
       enable_simultaneous_execution_as_candidate_module: false
+      keep_last: false
       priority: 4
       max_module_size: 1
 
@@ -72,6 +80,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: false
+      keep_last: false
       priority: 3
       max_module_size: 1
 
@@ -80,5 +89,6 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: true
       enable_simultaneous_execution_as_candidate_module: true
+      keep_last: false
       priority: 7
       max_module_size: 1
diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md
index 693d610951bbb..da96567ed542a 100644
--- a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md
+++ b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md
@@ -192,7 +192,7 @@ The avoidance target should be limited to stationary objects (you should not avo
 Not only the length from the centerline, but also the length from the road shoulder is calculated and used for the filtering process. It calculates the ratio of _the actual length between the the object's center and the center line_ `shift_length` and _the maximum length the object can shift_ `shiftable_length`.
 
 $$
-l_D = l_a - \frac{width}{2} \\
+l_D = l_a - \frac{width}{2}, \\
 ratio =  \frac{l_d}{l_D}
 $$
 
@@ -201,7 +201,7 @@ $$
 - $l_a$ : distance between centerline and most left boundary.
 - $width$ : object width
 
-The closer the object is to the shoulder, the larger the value of $ratio$ (theoretical max value is 1.0), and it compares the value and `object_check_shiftable_ratio` to determine whether the object is a parked-car.
+The closer the object is to the shoulder, the larger the value of $ratio$ (theoretical max value is 1.0), and it compares the value and `object_check_shiftable_ratio` to determine whether the object is a parked-car. If the road has no road shoulders, it uses `object_check_min_road_shoulder_width` as a road shoulder width virtually.
 
 ![fig2](../image/avoidance/parked-car-detection.svg)
 
@@ -692,7 +692,7 @@ namespace: `avoidance.target_filtering.`
 | object_check_backward_distance                        | [m]  | double | Backward distance to search the avoidance target.                                                                                                                                                                                      | 2.0           |
 | object_check_goal_distance                            | [m]  | double | Backward distance to search the avoidance target.                                                                                                                                                                                      | 20.0          |
 | object_check_shiftable_ratio                          | [m]  | double | Vehicles around the center line within this distance will be excluded from avoidance target.                                                                                                                                           | 0.6           |
-| object_check_min_road_shoulder_width                  | [m]  | double | Vehicles around the center line within this distance will be excluded from avoidance target.                                                                                                                                           | 0.5           |
+| object_check_min_road_shoulder_width                  | [m]  | double | Width considered as a road shoulder if the lane does not have a road shoulder target.                                                                                                                                                  | 0.5           |
 | object_last_seen_threshold                            | [s]  | double | For the compensation of the detection lost. The object is registered once it is observed as an avoidance target. When the detection loses, the timer will start and the object will be un-registered when the time exceeds this limit. | 2.0           |
 
 ### Safety check parameters
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 2a04580aef5b5..2fd1ff1cfd376 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
@@ -163,7 +163,7 @@ First, we divide the target objects into obstacles in the target lane, obstacles
 
 ![object lanes](../image/lane_change/lane_objects.drawio.svg)
 
-Furthermore, to change lanes behind a vehicle waiting at a traffic light, we skip the safety check for the stopping vehicles near the traffic light.
+Furthermore, to change lanes behind a vehicle waiting at a traffic light, we skip the safety check for the stopping vehicles near the traffic light.　The explanation for parked car detection is written in [documentation for avoidance module](../docs/behavior_path_planner_avoidance_design.md).
 
 ##### Collision check in prepare phase
 
@@ -276,7 +276,7 @@ The following parameters are configurable in `lane_change.param.yaml`.
 | `prediction_time_resolution`                | [s]    | double  | Time resolution for object's path interpolation and collision check.                                            | 0.5                |
 | `longitudinal_acceleration_sampling_num`    | [-]    | int     | Number of possible lane-changing trajectories that are being influenced by longitudinal acceleration            | 5                  |
 | `lateral_acceleration_sampling_num`         | [-]    | int     | Number of possible lane-changing trajectories that are being influenced by lateral acceleration                 | 3                  |
-| `object_check_min_road_shoulder_width`      | [m]    | double  | Vehicles around the center line within this distance will be excluded from parking objects                      | 0.5                |
+| `object_check_min_road_shoulder_width`      | [m]    | double  | Width considered as a road shoulder if the lane does not have a road shoulder                                   | 0.5                |
 | `object_shiftable_ratio_threshold`          | [-]    | double  | Vehicles around the center line within this distance ratio will be excluded from parking objects                | 0.6                |
 | `min_length_for_turn_signal_activation`     | [m]    | double  | Turn signal will be activated if the ego vehicle approaches to this length from minimum lane change length      | 10.0               |
 | `length_ratio_for_turn_signal_deactivation` | [-]    | double  | Turn signal will be deactivated if the ego vehicle approaches to this length ratio for lane change finish point | 0.8                |
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 808e76a140761..d337bae17ca6c 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
@@ -26,12 +26,8 @@ namespace marker_utils::lane_change_markers
 {
 using behavior_path_planner::LaneChangePath;
 using visualization_msgs::msg::MarkerArray;
-MarkerArray showObjectInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
 MarkerArray showAllValidLaneChangePath(
   const std::vector<LaneChangePath> & lanes, std::string && ns);
-MarkerArray showLerpedPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
-MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
-MarkerArray showPolygonPose(const CollisionCheckDebugMap & obj_debug_vec, 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);
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 657437615f809..ba8ddd9b19c46 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
@@ -104,6 +104,11 @@ MarkerArray createPredictedPathMarkerArray(
   const PredictedPath & ego_predicted_path, const vehicle_info_util::VehicleInfo & vehicle_info,
   std::string && ns, const int32_t & id, const float & r, const float & g, const float & b);
 
+MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
+
+MarkerArray showPredictedPath(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
+
+MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
 }  // namespace marker_utils
 
 #endif  // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__UTILS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp
index d6b99dca70618..e3a3784c5e928 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp
@@ -28,6 +28,7 @@ struct ModuleConfigParameters
   bool enable_rtc{false};
   bool enable_simultaneous_execution_as_approved_module{false};
   bool enable_simultaneous_execution_as_candidate_module{false};
+  bool keep_last{false};
   uint8_t priority{0};
   uint8_t max_module_size{0};
 };
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 f30bf31d4e010..1a7350d82aa4e 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
@@ -209,6 +209,10 @@ class LaneChangeBase
     return direction_;
   }
 
+  boost::optional<Pose> getStopPose() const { return lane_change_stop_pose_; }
+
+  void resetStopPose() { lane_change_stop_pose_ = boost::none; }
+
 protected:
   virtual lanelet::ConstLanelets getCurrentLanes() const = 0;
 
@@ -244,6 +248,7 @@ class LaneChangeBase
   PathWithLaneId prev_module_path_{};
   DrivableAreaInfo prev_drivable_area_info_{};
   TurnSignalInfo prev_turn_signal_info_{};
+  boost::optional<Pose> lane_change_stop_pose_{boost::none};
 
   PathWithLaneId prev_approved_path_{};
 
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 d1476abb63e27..ef57c31c28b5e 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
@@ -147,6 +147,8 @@ class NormalLaneChange : public LaneChangeBase
     const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
     const lanelet::ConstLanelets & target_backward_lanes) const;
 
+  void setStopPose(const Pose & stop_pose);
+
   rclcpp::Logger logger_ = rclcpp::get_logger("lane_change").get_child(getModuleTypeStr());
 };
 }  // namespace behavior_path_planner
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp
index 3405ae446615d..6acf5b945308f 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp
@@ -50,12 +50,8 @@ namespace behavior_path_planner
 {
 using autoware_adapi_v1_msgs::msg::SteeringFactor;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
-using motion_utils::createDeadLineVirtualWallMarker;
-using motion_utils::createSlowDownVirtualWallMarker;
-using motion_utils::createStopVirtualWallMarker;
 using rtc_interface::RTCInterface;
 using steering_factor_interface::SteeringFactorInterface;
-using tier4_autoware_utils::appendMarkerArray;
 using tier4_autoware_utils::calcOffsetPose;
 using tier4_autoware_utils::generateUUID;
 using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
@@ -79,16 +75,16 @@ class SceneModuleInterface
 public:
   SceneModuleInterface(
     const std::string & name, rclcpp::Node & node,
-    const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map)
+    std::unordered_map<std::string, std::shared_ptr<RTCInterface>> rtc_interface_ptr_map)
   : name_{name},
     logger_{node.get_logger().get_child(name)},
     clock_{node.get_clock()},
-    rtc_interface_ptr_map_(rtc_interface_ptr_map),
+    rtc_interface_ptr_map_(std::move(rtc_interface_ptr_map)),
     steering_factor_interface_ptr_(
       std::make_unique<SteeringFactorInterface>(&node, utils::convertToSnakeCase(name)))
   {
-    for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) {
-      uuid_map_.emplace(itr->first, generateUUID());
+    for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) {
+      uuid_map_.emplace(module_name, generateUUID());
     }
   }
 
@@ -176,9 +172,9 @@ class SceneModuleInterface
    */
   void publishRTCStatus()
   {
-    for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) {
-      if (itr->second) {
-        itr->second->publishCooperateStatus(clock_->now());
+    for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) {
+      if (ptr) {
+        ptr->publishCooperateStatus(clock_->now());
       }
     }
   }
@@ -193,18 +189,18 @@ class SceneModuleInterface
 
   void lockRTCCommand()
   {
-    for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) {
-      if (itr->second) {
-        itr->second->lockCommandUpdate();
+    for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) {
+      if (ptr) {
+        ptr->lockCommandUpdate();
       }
     }
   }
 
   void unlockRTCCommand()
   {
-    for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) {
-      if (itr->second) {
-        itr->second->unlockCommandUpdate();
+    for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) {
+      if (ptr) {
+        ptr->unlockCommandUpdate();
       }
     }
   }
@@ -398,10 +394,10 @@ class SceneModuleInterface
 
   virtual void updateRTCStatus(const double start_distance, const double finish_distance)
   {
-    for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) {
-      if (itr->second) {
-        itr->second->updateCooperateStatus(
-          uuid_map_.at(itr->first), isExecutionReady(), start_distance, finish_distance,
+    for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) {
+      if (ptr) {
+        ptr->updateCooperateStatus(
+          uuid_map_.at(module_name), isExecutionReady(), start_distance, finish_distance,
           clock_->now());
       }
     }
@@ -483,9 +479,9 @@ class SceneModuleInterface
 
   void removeRTCStatus()
   {
-    for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) {
-      if (itr->second) {
-        itr->second->clearCooperateStatus();
+    for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) {
+      if (ptr) {
+        ptr->clearCooperateStatus();
       }
     }
   }
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp
index cb47c33b6901c..87b21bf7c2924 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp
@@ -55,6 +55,7 @@ class SceneModuleManagerInterface
     enable_simultaneous_execution_as_candidate_module_(
       config.enable_simultaneous_execution_as_candidate_module),
     enable_rtc_(config.enable_rtc),
+    keep_last_(config.keep_last),
     max_module_num_(config.max_module_size),
     priority_(config.priority)
   {
@@ -224,6 +225,8 @@ class SceneModuleManagerInterface
     return enable_simultaneous_execution_as_candidate_module_;
   }
 
+  bool isKeepLast() const { return keep_last_; }
+
   void setData(const std::shared_ptr<PlannerData> & planner_data) { planner_data_ = planner_data; }
 
   void reset()
@@ -290,6 +293,8 @@ class SceneModuleManagerInterface
 private:
   bool enable_rtc_;
 
+  bool keep_last_;
+
   size_t max_module_num_;
 
   size_t priority_;
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 87c93f49901f0..93eb0457d7cf4 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
@@ -179,10 +179,10 @@ struct SafetyCheckParams
 struct CollisionCheckDebug
 {
   std::string unsafe_reason;                ///< Reason indicating unsafe situation.
-  Pose current_pose{};                      ///< Ego vehicle's current pose.
   Twist current_twist{};                    ///< Ego vehicle's current velocity and rotation.
-  Twist object_twist{};                     ///< Detected object's velocity and rotation.
   Pose expected_ego_pose{};                 ///< Predicted future pose of ego vehicle.
+  Pose current_obj_pose{};                  ///< Detected object's current pose.
+  Twist object_twist{};                     ///< Detected object's velocity and rotation.
   Pose expected_obj_pose{};                 ///< Predicted future pose of object.
   double rss_longitudinal{0.0};             ///< Longitudinal RSS measure.
   double inter_vehicle_distance{0.0};       ///< Distance between ego vehicle and object.
@@ -190,9 +190,10 @@ struct CollisionCheckDebug
   double extended_polygon_lat_offset{0.0};  ///< Lateral offset for extended polygon.
   bool is_front{false};                     ///< True if object is in front of ego vehicle.
   bool is_safe{false};                      ///< True if situation is deemed safe.
-  std::vector<Pose> lerped_path;            ///< Interpolated ego vehicle path.
-  Polygon2d extended_ego_polygon{};         ///< Ego vehicle's extended collision polygon.
-  Polygon2d extended_obj_polygon{};         ///< Detected object's extended collision polygon.
+  std::vector<PoseWithVelocityStamped> ego_predicted_path;  ///< ego vehicle's predicted path.
+  std::vector<PoseWithVelocityAndPolygonStamped> obj_predicted_path;  ///< object's predicted path.
+  Polygon2d extended_ego_polygon{};  ///< Ego vehicle's extended collision polygon.
+  Polygon2d extended_obj_polygon{};  ///< Detected object's extended collision polygon.
 };
 using CollisionCheckDebugPair = std::pair<std::string, CollisionCheckDebug>;
 using CollisionCheckDebugMap =
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 3023357bd0a96..f86b2ff2c9c61 100644
--- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
+++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -284,6 +284,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
       declare_parameter<bool>(ns + "enable_simultaneous_execution_as_approved_module");
     config.enable_simultaneous_execution_as_candidate_module =
       declare_parameter<bool>(ns + "enable_simultaneous_execution_as_candidate_module");
+    config.keep_last = declare_parameter<bool>(ns + "keep_last");
     config.priority = declare_parameter<int>(ns + "priority");
     config.max_module_size = declare_parameter<int>(ns + "max_module_size");
     return config;
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 aab2be6f3054e..7493982db78b1 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
@@ -36,40 +36,6 @@ using geometry_msgs::msg::Point;
 using tier4_autoware_utils::createDefaultMarker;
 using tier4_autoware_utils::createMarkerScale;
 
-MarkerArray showObjectInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns)
-{
-  Marker obj_marker = createDefaultMarker(
-    "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING,
-    createMarkerScale(0.5, 0.5, 0.5), colors::aqua());
-
-  MarkerArray marker_array;
-  int32_t id{0};
-
-  marker_array.markers.reserve(obj_debug_vec.size());
-
-  int idx{0};
-
-  for (const auto & [uuid, info] : obj_debug_vec) {
-    obj_marker.id = ++id;
-    obj_marker.pose = info.current_pose;
-
-    std::ostringstream ss;
-
-    ss << "Idx: " << ++idx << "\nReason: " << info.unsafe_reason
-       << "\nRSS dist: " << std::setprecision(4) << info.rss_longitudinal
-       << "\nEgo to obj: " << info.inter_vehicle_distance
-       << "\nExtended polygon lateral offset: " << info.extended_polygon_lat_offset
-       << "\nExtended polygon longitudinal offset: " << info.extended_polygon_lon_offset
-       << "\nPosition: " << (info.is_front ? "front" : "back")
-       << "\nSafe: " << (info.is_safe ? "Yes" : "No");
-
-    obj_marker.text = ss.str();
-
-    marker_array.markers.push_back(obj_marker);
-  }
-  return marker_array;
-}
-
 MarkerArray showAllValidLaneChangePath(const std::vector<LaneChangePath> & lanes, std::string && ns)
 {
   if (lanes.empty()) {
@@ -103,121 +69,6 @@ MarkerArray showAllValidLaneChangePath(const std::vector<LaneChangePath> & lanes
   return marker_array;
 }
 
-MarkerArray showLerpedPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns)
-{
-  MarkerArray marker_array;
-  int32_t id{0};
-  const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()};
-  marker_array.markers.reserve(obj_debug_vec.size());
-
-  for (const auto & [uuid, info] : obj_debug_vec) {
-    Marker marker = createDefaultMarker(
-      "map", current_time, ns, ++id, Marker::POINTS, createMarkerScale(0.3, 0.3, 0.3),
-      colors::magenta());
-    marker.points.reserve(info.lerped_path.size());
-
-    for (const auto & point : info.lerped_path) {
-      marker.points.push_back(point.position);
-    }
-
-    marker_array.markers.push_back(marker);
-  }
-  return marker_array;
-}
-
-MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns)
-{
-  if (obj_debug_vec.empty()) {
-    return MarkerArray{};
-  }
-
-  constexpr float scale_val{0.2};
-  int32_t id{0};
-  const auto now = rclcpp::Clock{RCL_ROS_TIME}.now();
-  Marker ego_marker = createDefaultMarker(
-    "map", now, ns, id, Marker::LINE_STRIP, createMarkerScale(scale_val, scale_val, scale_val),
-    colors::green());
-  Marker obj_marker = ego_marker;
-
-  auto text_marker = createDefaultMarker(
-    "map", now, ns + "_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING,
-    createMarkerScale(1.5, 1.5, 1.5), colors::white());
-
-  MarkerArray marker_array;
-
-  const auto reserve_size = obj_debug_vec.size();
-
-  marker_array.markers.reserve(reserve_size * 4);
-
-  int32_t idx = {0};
-
-  for (const auto & [uuid, info] : obj_debug_vec) {
-    const auto color = info.is_safe ? colors::green() : colors::red();
-    const auto & ego_polygon = info.extended_ego_polygon.outer();
-    const auto poly_z = info.current_pose.position.z;  // temporally
-    ego_marker.id = ++id;
-    ego_marker.color = color;
-    ego_marker.points.reserve(ego_polygon.size());
-    for (const auto & p : ego_polygon) {
-      ego_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), poly_z));
-    }
-    marker_array.markers.push_back(ego_marker);
-
-    std::ostringstream ss;
-    text_marker.id = ego_marker.id;
-    ss << ++idx;
-    text_marker.text = ss.str();
-    text_marker.pose = info.expected_ego_pose;
-
-    marker_array.markers.push_back(text_marker);
-
-    const auto & obj_polygon = info.extended_obj_polygon.outer();
-    obj_marker.id = ++id;
-    obj_marker.color = color;
-    obj_marker.points.reserve(obj_polygon.size());
-    for (const auto & p : obj_polygon) {
-      obj_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), poly_z));
-    }
-    marker_array.markers.push_back(obj_marker);
-
-    text_marker.id = obj_marker.id;
-    text_marker.pose = info.expected_obj_pose;
-    marker_array.markers.push_back(text_marker);
-
-    ego_marker.points.clear();
-    obj_marker.points.clear();
-  }
-
-  return marker_array;
-}
-
-MarkerArray showPolygonPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns)
-{
-  const auto colors = colors::colors_list();
-  const auto loop_size = std::min(colors.size(), obj_debug_vec.size());
-  MarkerArray marker_array;
-  int32_t id{0};
-  size_t idx{0};
-  const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()};
-  marker_array.markers.reserve(obj_debug_vec.size());
-
-  for (const auto & [uuid, info] : obj_debug_vec) {
-    const auto & color = colors.at(idx);
-    Marker marker = createDefaultMarker(
-      "map", current_time, ns, ++id, Marker::POINTS, createMarkerScale(0.2, 0.2, 0.2), color);
-    marker.points.reserve(2);
-    marker.points.push_back(info.expected_ego_pose.position);
-    marker.points.push_back(info.expected_obj_pose.position);
-    marker_array.markers.push_back(marker);
-    ++idx;
-    if (idx >= loop_size) {
-      break;
-    }
-  }
-
-  return marker_array;
-}
-
 MarkerArray createLaneChangingVirtualWallMarker(
   const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name,
   const rclcpp::Time & now, const std::string & ns)
diff --git a/planning/behavior_path_planner/src/marker_utils/utils.cpp b/planning/behavior_path_planner/src/marker_utils/utils.cpp
index f24e772d198c1..d6a237079448f 100644
--- a/planning/behavior_path_planner/src/marker_utils/utils.cpp
+++ b/planning/behavior_path_planner/src/marker_utils/utils.cpp
@@ -14,6 +14,7 @@
 
 #include "behavior_path_planner/marker_utils/utils.hpp"
 
+#include "behavior_path_planner/marker_utils/colors.hpp"
 #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp"
 #include "behavior_path_planner/utils/path_utils.hpp"
 #include "behavior_path_planner/utils/utils.hpp"
@@ -37,7 +38,7 @@ using visualization_msgs::msg::Marker;
 CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj)
 {
   CollisionCheckDebug debug;
-  debug.current_pose = obj.initial_pose.pose;
+  debug.current_obj_pose = obj.initial_pose.pose;
   debug.current_twist = obj.initial_twist.twist;
   return {tier4_autoware_utils::toHexString(obj.uuid), debug};
 }
@@ -486,4 +487,162 @@ MarkerArray createPredictedPathMarkerArray(
   return msg;
 }
 
+MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns)
+{
+  if (obj_debug_vec.empty()) {
+    return MarkerArray{};
+  }
+
+  int32_t id{0};
+  const auto now = rclcpp::Clock{RCL_ROS_TIME}.now();
+
+  constexpr float line_scale_val{0.2};
+  const auto line_marker_scale = createMarkerScale(line_scale_val, line_scale_val, line_scale_val);
+
+  auto default_line_marker = [&](const auto & color = colors::green()) {
+    return createDefaultMarker("map", now, ns, ++id, Marker::LINE_STRIP, line_marker_scale, color);
+  };
+
+  constexpr float text_scale_val{1.5};
+  const auto text_marker_scale = createMarkerScale(text_scale_val, text_scale_val, text_scale_val);
+
+  auto default_text_marker = [&]() {
+    return createDefaultMarker(
+      "map", now, ns + "_text", ++id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING,
+      text_marker_scale, colors::white());
+  };
+
+  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(
+    obj_debug_vec.size() * 5);  // poly ego, text ego, poly obj, text obj, cube obj
+
+  int32_t idx = {0};
+  for (const auto & [uuid, info] : obj_debug_vec) {
+    const auto color = info.is_safe ? colors::green() : colors::red();
+    const auto poly_z = info.current_obj_pose.position.z;  // temporally
+
+    const auto insert_polygon_marker = [&](const auto & polygon) {
+      marker_array.markers.emplace_back();
+      auto & polygon_marker = marker_array.markers.back();
+      polygon_marker = default_line_marker(color);
+      polygon_marker.points.reserve(polygon.outer().size());
+      for (const auto & p : polygon.outer()) {
+        polygon_marker.points.push_back(createPoint(p.x(), p.y(), poly_z));
+      }
+    };
+
+    insert_polygon_marker(info.extended_ego_polygon);
+    insert_polygon_marker(info.extended_obj_polygon);
+
+    const auto str_idx = std::to_string(++idx);
+    const auto insert_text_marker = [&](const auto & pose) {
+      marker_array.markers.emplace_back();
+      auto & text_marker = marker_array.markers.back();
+      text_marker = default_text_marker();
+      text_marker.text = str_idx;
+      text_marker.pose = pose;
+    };
+
+    insert_text_marker(info.expected_ego_pose);
+    insert_text_marker(info.expected_obj_pose);
+
+    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(info.current_obj_pose);
+  }
+  return marker_array;
+}
+
+MarkerArray showPredictedPath(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns)
+{
+  int32_t id{0};
+  const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()};
+  const auto arrow_marker_scale = createMarkerScale(1.0, 0.3, 0.3);
+  const auto default_arrow_marker = [&](const auto & color) {
+    return createDefaultMarker(
+      "map", current_time, ns, ++id, Marker::ARROW, arrow_marker_scale, color);
+  };
+
+  MarkerArray marker_array;
+  marker_array.markers.reserve(std::accumulate(
+    obj_debug_vec.cbegin(), obj_debug_vec.cend(), 0UL,
+    [&](const auto current_sum, const auto & obj_debug) {
+      const auto & [uuid, info] = obj_debug;
+      return current_sum + info.ego_predicted_path.size() + info.obj_predicted_path.size() + 2;
+    }));
+
+  for (const auto & [uuid, info] : obj_debug_vec) {
+    const auto insert_marker = [&](const auto & path, const auto & color) {
+      for (const auto & pose : path) {
+        marker_array.markers.emplace_back();
+        auto & marker = marker_array.markers.back();
+        marker = default_arrow_marker(color);
+        marker.pose = pose.pose;
+      }
+    };
+
+    insert_marker(info.ego_predicted_path, colors::aqua());
+    insert_marker(info.obj_predicted_path, colors::yellow());
+    const auto insert_expected_pose_marker = [&](const auto & pose, const auto & color) {
+      // instead of checking for distance, inserting a new marker might be more efficient
+      marker_array.markers.emplace_back();
+      auto & marker = marker_array.markers.back();
+      marker = default_arrow_marker(color);
+      marker.pose = pose;
+      marker.pose.position.z += 0.05;
+    };
+
+    insert_expected_pose_marker(info.expected_ego_pose, colors::red());
+    insert_expected_pose_marker(info.expected_obj_pose, colors::red());
+  }
+  return marker_array;
+}
+
+MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns)
+{
+  int32_t id{0};
+  auto default_text_marker = [&]() {
+    return createDefaultMarker(
+      "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, ++id, Marker::TEXT_VIEW_FACING,
+      createMarkerScale(0.5, 0.5, 0.5), colors::aqua());
+  };
+
+  MarkerArray marker_array;
+
+  marker_array.markers.reserve(obj_debug_vec.size());
+
+  int idx{0};
+
+  for (const auto & [uuid, info] : obj_debug_vec) {
+    auto safety_check_info_text = default_text_marker();
+    safety_check_info_text.pose = info.current_obj_pose;
+
+    std::ostringstream ss;
+
+    ss << "Idx: " << ++idx << "\nUnsafe reason: " << info.unsafe_reason
+       << "\nRSS dist: " << std::setprecision(4) << info.rss_longitudinal
+       << "\nEgo to obj: " << info.inter_vehicle_distance
+       << "\nExtended polygon: " << (info.is_front ? "ego" : "object")
+       << "\nExtended polygon lateral offset: " << info.extended_polygon_lat_offset
+       << "\nExtended polygon longitudinal offset: " << info.extended_polygon_lon_offset
+       << "\nLast checked position: " << (info.is_front ? "obj in front ego" : "obj at back ego")
+       << "\nSafe: " << (info.is_safe ? "Yes" : "No");
+
+    safety_check_info_text.text = ss.str();
+    marker_array.markers.push_back(safety_check_info_text);
+  }
+  return marker_array;
+}
+
 }  // namespace marker_utils
diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp
index c09df4151ca35..920b44235cba9 100644
--- a/planning/behavior_path_planner/src/planner_manager.cpp
+++ b/planning/behavior_path_planner/src/planner_manager.cpp
@@ -456,11 +456,28 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<Pl
     return output;
   }
 
+  // move modules whose keep last flag is true to end of the approved_module_ptrs_.
+  {
+    std::sort(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [this](auto a, auto b) {
+      return !getManager(a)->isKeepLast() && getManager(b)->isKeepLast();
+    });
+  }
+
   // lock approved modules besides last one
   std::for_each(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [&](const auto & m) {
     m->lockOutputPath();
   });
-  approved_module_ptrs_.back()->unlockOutputPath();
+
+  // unlock only last approved module except keep last module.
+  {
+    const auto not_keep_last_modules = std::find_if(
+      approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(),
+      [this](const auto & m) { return !getManager(m)->isKeepLast(); });
+
+    if (not_keep_last_modules != approved_module_ptrs_.rend()) {
+      (*not_keep_last_modules)->unlockOutputPath();
+    }
+  }
 
   /**
    * execute all approved modules.
@@ -478,21 +495,31 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<Pl
    * candidate_module_ptrs_.
    */
   {
-    const auto waiting_approval_modules = [](const auto & m) { return m->isWaitingApproval(); };
+    const auto not_keep_last_module = std::find_if(
+      approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(),
+      [this](const auto & m) { return !getManager(m)->isKeepLast(); });
 
-    const auto itr = std::find_if(
-      approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(), waiting_approval_modules);
+    // convert reverse iterator -> iterator
+    const auto begin_itr = not_keep_last_module != approved_module_ptrs_.rend()
+                             ? std::next(not_keep_last_module).base()
+                             : approved_module_ptrs_.begin();
 
-    if (itr != approved_module_ptrs_.rend()) {
-      const auto is_last_module = std::distance(approved_module_ptrs_.rbegin(), itr) == 0;
-      if (is_last_module) {
-        clearCandidateModules();
-        candidate_module_ptrs_.push_back(*itr);
+    const auto waiting_approval_modules_itr = std::find_if(
+      begin_itr, approved_module_ptrs_.end(),
+      [](const auto & m) { return m->isWaitingApproval(); });
 
-        debug_info_.emplace_back(*itr, Action::MOVE, "Back To Waiting Approval");
+    if (waiting_approval_modules_itr != approved_module_ptrs_.end()) {
+      clearCandidateModules();
+      candidate_module_ptrs_.push_back(*waiting_approval_modules_itr);
 
-        approved_module_ptrs_.pop_back();
-      }
+      debug_info_.emplace_back(
+        *waiting_approval_modules_itr, Action::MOVE, "Back To Waiting Approval");
+
+      std::for_each(
+        waiting_approval_modules_itr, approved_module_ptrs_.end(),
+        [&results](const auto & m) { results.erase(m->name()); });
+
+      approved_module_ptrs_.erase(waiting_approval_modules_itr);
 
       std::for_each(
         manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); });
@@ -529,12 +556,15 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<Pl
   /**
    * use the last module's output as approved modules planning result.
    */
-  const auto output_module_name = approved_module_ptrs_.back()->name();
-  const auto approved_modules_output = [&output_module_name, &results]() {
-    if (results.count(output_module_name) == 0) {
-      return results.at("root");
+  const auto approved_modules_output = [&results, this]() {
+    const auto itr = std::find_if(
+      approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(),
+      [&results](const auto & m) { return results.count(m->name()) != 0; });
+
+    if (itr != approved_module_ptrs_.rend()) {
+      return results.at((*itr)->name());
     }
-    return results.at(output_module_name);
+    return results.at("root");
   }();
 
   /**
diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
index 543f02abe730f..c47aa66fdac90 100644
--- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
@@ -50,6 +50,7 @@ using motion_utils::calcLongitudinalOffsetPose;
 using motion_utils::calcSignedArcLength;
 using motion_utils::findNearestIndex;
 using motion_utils::findNearestSegmentIndex;
+using tier4_autoware_utils::appendMarkerArray;
 using tier4_autoware_utils::calcDistance2d;
 using tier4_autoware_utils::calcInterpolatedPose;
 using tier4_autoware_utils::calcLateralDeviation;
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 072c1c62ae3d2..64a7e926cc2df 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
@@ -31,6 +31,7 @@
 
 namespace behavior_path_planner
 {
+using tier4_autoware_utils::appendMarkerArray;
 using utils::lane_change::assignToCandidate;
 
 LaneChangeInterface::LaneChangeInterface(
@@ -203,6 +204,7 @@ void LaneChangeInterface::updateData()
   module_type_->setPreviousModulePaths(
     getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path);
   module_type_->updateSpecialData();
+  module_type_->resetStopPose();
 }
 
 BehaviorModuleOutput LaneChangeInterface::plan()
@@ -221,6 +223,8 @@ BehaviorModuleOutput LaneChangeInterface::plan()
   *prev_approved_path_ = *getPreviousModuleOutput().path;
   module_type_->insertStopPoint(module_type_->getLaneChangeStatus().target_lanes, *output.path);
 
+  stop_pose_ = module_type_->getStopPose();
+
   updateSteeringFactorPtr(output);
   clearWaitingApproval();
 
@@ -249,6 +253,8 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
 
   path_reference_ = getPreviousModuleOutput().reference_path;
 
+  stop_pose_ = module_type_->getStopPose();
+
   if (!module_type_->isValidPath()) {
     removeRTCStatus();
     path_candidate_ = std::make_shared<PathWithLaneId>();
@@ -286,6 +292,7 @@ void LaneChangeInterface::updateModuleParams(const std::any & parameters)
 
 void LaneChangeInterface::setData(const std::shared_ptr<const PlannerData> & data)
 {
+  planner_data_ = data;
   module_type_->setData(data);
 }
 
@@ -295,11 +302,10 @@ void LaneChangeInterface::setObjectDebugVisualization() const
   if (!parameters_->publish_debug_marker) {
     return;
   }
+  using marker_utils::showPolygon;
+  using marker_utils::showPredictedPath;
+  using marker_utils::showSafetyCheckInfo;
   using marker_utils::lane_change_markers::showAllValidLaneChangePath;
-  using marker_utils::lane_change_markers::showLerpedPose;
-  using marker_utils::lane_change_markers::showObjectInfo;
-  using marker_utils::lane_change_markers::showPolygon;
-  using marker_utils::lane_change_markers::showPolygonPose;
 
   const auto debug_data = module_type_->getDebugData();
   const auto debug_after_approval = module_type_->getAfterApprovalDebugData();
@@ -312,14 +318,14 @@ void LaneChangeInterface::setObjectDebugVisualization() const
 
   add(showAllValidLaneChangePath(debug_valid_path, "lane_change_valid_paths"));
   if (!debug_data.empty()) {
-    add(showObjectInfo(debug_data, "object_debug_info"));
-    add(showLerpedPose(debug_data, "ego_predicted_path"));
+    add(showSafetyCheckInfo(debug_data, "object_debug_info"));
+    add(showPredictedPath(debug_data, "ego_predicted_path"));
     add(showPolygon(debug_data, "ego_and_target_polygon_relation"));
   }
 
   if (!debug_after_approval.empty()) {
-    add(showObjectInfo(debug_after_approval, "object_debug_info_after_approval"));
-    add(showLerpedPose(debug_after_approval, "ego_predicted_path_after_approval"));
+    add(showSafetyCheckInfo(debug_after_approval, "object_debug_info_after_approval"));
+    add(showPredictedPath(debug_after_approval, "ego_predicted_path_after_approval"));
     add(showPolygon(debug_after_approval, "ego_and_target_polygon_relation_after_approval"));
   }
 }
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 bf0175ccd92e4..28ceb63d88352 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
@@ -138,6 +138,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput()
     const auto stop_dist =
       -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc));
     const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, *output.path);
+    setStopPose(stop_point.point.pose);
   }
 
   const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path->points);
@@ -198,6 +199,7 @@ void NormalLaneChange::insertStopPoint(
   const double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer;
   if (stopping_distance > 0.0) {
     const auto stop_point = utils::insertStopPoint(stopping_distance, path);
+    setStopPose(stop_point.point.pose);
   }
 }
 
@@ -1379,4 +1381,10 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
 
   return path_safety_status;
 }
+
+void NormalLaneChange::setStopPose(const Pose & stop_pose)
+{
+  lane_change_stop_pose_ = stop_pose;
+}
+
 }  // namespace behavior_path_planner
diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp
index 035cc579d2a82..5304985b1cbf2 100644
--- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp
+++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp
@@ -25,10 +25,15 @@ double calculateDistanceLimit(
   const multi_linestring_t & limit_lines)
 {
   auto dist_limit = std::numeric_limits<double>::max();
-  multi_point_t intersections;
-  boost::geometry::intersection(expansion_polygon, limit_lines, intersections);
-  for (const auto & p : intersections)
-    dist_limit = std::min(dist_limit, boost::geometry::distance(p, base_ls));
+  for (const auto & line : limit_lines) {
+    multi_point_t intersections;
+    boost::geometry::intersection(expansion_polygon, limit_lines, intersections);
+    for (const auto & p : intersections)
+      dist_limit = std::min(dist_limit, boost::geometry::distance(p, base_ls));
+    for (const auto & p : line)
+      if (boost::geometry::within(p, expansion_polygon))
+        dist_limit = std::min(dist_limit, boost::geometry::distance(p, base_ls));
+  }
   return dist_limit;
 }
 
diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp
index 0c31093a83c0e..45e128288578a 100644
--- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp
+++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp
@@ -16,6 +16,9 @@
 
 #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp"
 
+#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 #include <boost/assign.hpp>
diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp
index 2939ca29c0a40..1838dc6bdc07b 100644
--- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp
+++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp
@@ -253,7 +253,11 @@ bool checkCollision(
   const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
   double hysteresis_factor, CollisionCheckDebug & debug)
 {
-  debug.lerped_path.reserve(target_object_path.path.size());
+  {
+    debug.ego_predicted_path = predicted_ego_path;
+    debug.obj_predicted_path = target_object_path.path;
+    debug.current_obj_pose = target_object.initial_pose.pose;
+  }
 
   for (const auto & obj_pose_with_poly : target_object_path.path) {
     const auto & current_time = obj_pose_with_poly.time;
@@ -277,7 +281,6 @@ bool checkCollision(
     const auto & ego_velocity = interpolated_data->velocity;
 
     {
-      debug.lerped_path.push_back(ego_pose);
       debug.expected_ego_pose = ego_pose;
       debug.expected_obj_pose = obj_pose;
       debug.extended_ego_polygon = ego_polygon;
diff --git a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp
index 605b3b96ee9b9..fe33d48f74fe5 100644
--- a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp
+++ b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp
@@ -304,7 +304,7 @@ TEST(DrivableAreaExpansion, calculateDistanceLimit)
     const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}};
     const multi_linestring_t uncrossable_lines = {};
     const polygon_t expansion_polygon = {
-      {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {10.0, -4.0}}, {}};
+      {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {0.0, -4.0}}, {}};
     const auto limit_distance =
       calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines);
     EXPECT_NEAR(limit_distance, std::numeric_limits<double>::max(), 1e-9);
@@ -313,7 +313,7 @@ TEST(DrivableAreaExpansion, calculateDistanceLimit)
     const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}};
     const linestring_t uncrossable_line = {{0.0, 2.0}, {10.0, 2.0}};
     const polygon_t expansion_polygon = {
-      {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {10.0, -4.0}}, {}};
+      {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {0.0, -4.0}}, {}};
     const auto limit_distance =
       calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_line});
     EXPECT_NEAR(limit_distance, 2.0, 1e-9);
@@ -323,9 +323,44 @@ TEST(DrivableAreaExpansion, calculateDistanceLimit)
     const multi_linestring_t uncrossable_lines = {
       {{0.0, 2.0}, {10.0, 2.0}}, {{0.0, 1.5}, {10.0, 1.0}}};
     const polygon_t expansion_polygon = {
-      {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {10.0, -4.0}}, {}};
+      {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {0.0, -4.0}}, {}};
     const auto limit_distance =
       calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines);
     EXPECT_NEAR(limit_distance, 1.0, 1e-9);
   }
 }
+
+TEST(DrivableAreaExpansion, calculateDistanceLimitEdgeCases)
+{
+  using drivable_area_expansion::calculateDistanceLimit;
+  using drivable_area_expansion::linestring_t;
+  using drivable_area_expansion::polygon_t;
+
+  const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}};
+  const polygon_t expansion_polygon = {
+    {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {0.0, -4.0}}, {}};
+  {  // intersection points further than the line point inside the expansion polygon
+    const linestring_t uncrossable_lines = {{4.0, 5.0}, {6.0, 3.0}};
+    const auto limit_distance =
+      calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_lines});
+    EXPECT_NEAR(limit_distance, 3.0, 1e-9);
+  }
+  {  // intersection points further than the line point inside the expansion polygon
+    const linestring_t uncrossable_lines = {{4.0, 5.0}, {5.0, 2.0}, {6.0, 4.5}};
+    const auto limit_distance =
+      calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_lines});
+    EXPECT_NEAR(limit_distance, 2.0, 1e-9);
+  }
+  {  // line completely inside the expansion polygon
+    const linestring_t uncrossable_lines = {{4.0, 2.0}, {6.0, 3.0}};
+    const auto limit_distance =
+      calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_lines});
+    EXPECT_NEAR(limit_distance, 2.0, 1e-9);
+  }
+  {  // line completely inside the expansion polygon
+    const linestring_t uncrossable_lines = {{4.0, 3.5}, {6.0, 3.0}};
+    const auto limit_distance =
+      calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_lines});
+    EXPECT_NEAR(limit_distance, 3.0, 1e-9);
+  }
+}
diff --git a/planning/behavior_velocity_walkway_module/src/manager.cpp b/planning/behavior_velocity_walkway_module/src/manager.cpp
index d10ff52850752..f9c222f662e82 100644
--- a/planning/behavior_velocity_walkway_module/src/manager.cpp
+++ b/planning/behavior_velocity_walkway_module/src/manager.cpp
@@ -44,11 +44,8 @@ WalkwayModuleManager::WalkwayModuleManager(rclcpp::Node & node)
 void WalkwayModuleManager::launchNewModules(const PathWithLaneId & path)
 {
   const auto rh = planner_data_->route_handler_;
-  if (!opt_use_regulatory_element_) {
-    opt_use_regulatory_element_ = checkRegulatoryElementExistence(rh->getLaneletMapPtr());
-  }
 
-  const auto launch = [this, &path](const auto & lanelet) {
+  const auto launch = [this, &path](const auto & lanelet, const auto use_regulatory_element) {
     const auto attribute =
       lanelet.attributeOr(lanelet::AttributeNamesString::Subtype, std::string(""));
     if (attribute != lanelet::AttributeValueString::Walkway) {
@@ -64,24 +61,21 @@ void WalkwayModuleManager::launchNewModules(const PathWithLaneId & path)
     const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr();
 
     registerModule(std::make_shared<WalkwayModule>(
-      lanelet.id(), lanelet_map_ptr, p, *opt_use_regulatory_element_, logger, clock_));
+      lanelet.id(), lanelet_map_ptr, p, use_regulatory_element, logger, clock_));
   };
 
-  if (*opt_use_regulatory_element_) {
-    const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath<Crosswalk>(
-      path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose);
+  const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath<Crosswalk>(
+    path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose);
 
-    for (const auto & crosswalk : crosswalk_leg_elem_map) {
-      launch(crosswalk.first->crosswalkLanelet());
-    }
-  } else {
-    const auto crosswalk_lanelets = getCrosswalksOnPath(
-      planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(),
-      rh->getOverallGraphPtr());
+  for (const auto & crosswalk : crosswalk_leg_elem_map) {
+    launch(crosswalk.first->crosswalkLanelet(), true);
+  }
 
-    for (const auto & crosswalk : crosswalk_lanelets) {
-      launch(crosswalk);
-    }
+  const auto crosswalk_lanelets = getCrosswalksOnPath(
+    planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr());
+
+  for (const auto & crosswalk : crosswalk_lanelets) {
+    launch(crosswalk, false);
   }
 }
 
@@ -92,17 +86,14 @@ WalkwayModuleManager::getModuleExpiredFunction(const PathWithLaneId & path)
 
   std::set<int64_t> walkway_id_set;
 
-  if (*opt_use_regulatory_element_) {
-    const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath<Crosswalk>(
-      path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose);
+  walkway_id_set = getCrosswalkIdSetOnPath(
+    planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr());
 
-    for (const auto & crosswalk : crosswalk_leg_elem_map) {
-      walkway_id_set.insert(crosswalk.first->id());
-    }
-  } else {
-    walkway_id_set = getCrosswalkIdSetOnPath(
-      planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(),
-      rh->getOverallGraphPtr());
+  const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath<Crosswalk>(
+    path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose);
+
+  for (const auto & crosswalk : crosswalk_leg_elem_map) {
+    walkway_id_set.insert(crosswalk.first->id());
   }
 
   return [walkway_id_set](const std::shared_ptr<SceneModuleInterface> & scene_module) {
diff --git a/vehicle/vehicle_info_util/CMakeLists.txt b/vehicle/vehicle_info_util/CMakeLists.txt
index f490b0680a19a..ca72b0ed2db65 100644
--- a/vehicle/vehicle_info_util/CMakeLists.txt
+++ b/vehicle/vehicle_info_util/CMakeLists.txt
@@ -5,6 +5,7 @@ find_package(autoware_cmake REQUIRED)
 autoware_package()
 
 ament_auto_add_library(vehicle_info_util SHARED
+  src/vehicle_info.cpp
   src/vehicle_info_util.cpp
 )
 
diff --git a/vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info.hpp b/vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info.hpp
index 967e71933a209..cdbbe0427b145 100644
--- a/vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info.hpp
+++ b/vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info.hpp
@@ -15,7 +15,7 @@
 #ifndef VEHICLE_INFO_UTIL__VEHICLE_INFO_HPP_
 #define VEHICLE_INFO_UTIL__VEHICLE_INFO_HPP_
 
-#include "tier4_autoware_utils/tier4_autoware_utils.hpp"
+#include "tier4_autoware_utils/geometry/boost_geometry.hpp"
 
 namespace vehicle_info_util
 {
@@ -47,76 +47,18 @@ struct VehicleInfo
   double min_height_offset_m;
   double max_height_offset_m;
 
-  tier4_autoware_utils::LinearRing2d createFootprint(const double margin = 0.0) const
-  {
-    return createFootprint(margin, margin);
-  }
-
+  tier4_autoware_utils::LinearRing2d createFootprint(const double margin = 0.0) const;
   tier4_autoware_utils::LinearRing2d createFootprint(
-    const double lat_margin, const double lon_margin) const
-  {
-    using tier4_autoware_utils::LinearRing2d;
-    using tier4_autoware_utils::Point2d;
-
-    const double x_front = front_overhang_m + wheel_base_m + lon_margin;
-    const double x_center = wheel_base_m / 2.0;
-    const double x_rear = -(rear_overhang_m + lon_margin);
-    const double y_left = wheel_tread_m / 2.0 + left_overhang_m + lat_margin;
-    const double y_right = -(wheel_tread_m / 2.0 + right_overhang_m + lat_margin);
-
-    LinearRing2d footprint;
-    footprint.push_back(Point2d{x_front, y_left});
-    footprint.push_back(Point2d{x_front, y_right});
-    footprint.push_back(Point2d{x_center, y_right});
-    footprint.push_back(Point2d{x_rear, y_right});
-    footprint.push_back(Point2d{x_rear, y_left});
-    footprint.push_back(Point2d{x_center, y_left});
-    footprint.push_back(Point2d{x_front, y_left});
-
-    return footprint;
-  }
+    const double lat_margin, const double lon_margin) const;
 };
 
 /// Create vehicle info from base parameters
-inline VehicleInfo createVehicleInfo(
+VehicleInfo createVehicleInfo(
   const double wheel_radius_m, const double wheel_width_m, const double wheel_base_m,
   const double wheel_tread_m, const double front_overhang_m, const double rear_overhang_m,
   const double left_overhang_m, const double right_overhang_m, const double vehicle_height_m,
-  const double max_steer_angle_rad)
-{
-  // Calculate derived parameters
-  const double vehicle_length_m_ = front_overhang_m + wheel_base_m + rear_overhang_m;
-  const double vehicle_width_m_ = wheel_tread_m + left_overhang_m + right_overhang_m;
-  const double min_longitudinal_offset_m_ = -rear_overhang_m;
-  const double max_longitudinal_offset_m_ = front_overhang_m + wheel_base_m;
-  const double min_lateral_offset_m_ = -(wheel_tread_m / 2.0 + right_overhang_m);
-  const double max_lateral_offset_m_ = wheel_tread_m / 2.0 + left_overhang_m;
-  const double min_height_offset_m_ = 0.0;
-  const double max_height_offset_m_ = vehicle_height_m;
+  const double max_steer_angle_rad);
 
-  return VehicleInfo{
-    // Base parameters
-    wheel_radius_m,
-    wheel_width_m,
-    wheel_base_m,
-    wheel_tread_m,
-    front_overhang_m,
-    rear_overhang_m,
-    left_overhang_m,
-    right_overhang_m,
-    vehicle_height_m,
-    max_steer_angle_rad,
-    // Derived parameters
-    vehicle_length_m_,
-    vehicle_width_m_,
-    min_longitudinal_offset_m_,
-    max_longitudinal_offset_m_,
-    min_lateral_offset_m_,
-    max_lateral_offset_m_,
-    min_height_offset_m_,
-    max_height_offset_m_,
-  };
-}
 }  // namespace vehicle_info_util
 
 #endif  // VEHICLE_INFO_UTIL__VEHICLE_INFO_HPP_
diff --git a/vehicle/vehicle_info_util/src/vehicle_info.cpp b/vehicle/vehicle_info_util/src/vehicle_info.cpp
new file mode 100644
index 0000000000000..1a47252f82d04
--- /dev/null
+++ b/vehicle/vehicle_info_util/src/vehicle_info.cpp
@@ -0,0 +1,88 @@
+// Copyright 2015-2021 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "vehicle_info_util/vehicle_info.hpp"
+
+namespace vehicle_info_util
+{
+tier4_autoware_utils::LinearRing2d VehicleInfo::createFootprint(const double margin) const
+{
+  return createFootprint(margin, margin);
+}
+
+tier4_autoware_utils::LinearRing2d VehicleInfo::createFootprint(
+  const double lat_margin, const double lon_margin) const
+{
+  using tier4_autoware_utils::LinearRing2d;
+  using tier4_autoware_utils::Point2d;
+
+  const double x_front = front_overhang_m + wheel_base_m + lon_margin;
+  const double x_center = wheel_base_m / 2.0;
+  const double x_rear = -(rear_overhang_m + lon_margin);
+  const double y_left = wheel_tread_m / 2.0 + left_overhang_m + lat_margin;
+  const double y_right = -(wheel_tread_m / 2.0 + right_overhang_m + lat_margin);
+
+  LinearRing2d footprint;
+  footprint.push_back(Point2d{x_front, y_left});
+  footprint.push_back(Point2d{x_front, y_right});
+  footprint.push_back(Point2d{x_center, y_right});
+  footprint.push_back(Point2d{x_rear, y_right});
+  footprint.push_back(Point2d{x_rear, y_left});
+  footprint.push_back(Point2d{x_center, y_left});
+  footprint.push_back(Point2d{x_front, y_left});
+
+  return footprint;
+}
+
+VehicleInfo createVehicleInfo(
+  const double wheel_radius_m, const double wheel_width_m, const double wheel_base_m,
+  const double wheel_tread_m, const double front_overhang_m, const double rear_overhang_m,
+  const double left_overhang_m, const double right_overhang_m, const double vehicle_height_m,
+  const double max_steer_angle_rad)
+{
+  // Calculate derived parameters
+  const double vehicle_length_m_ = front_overhang_m + wheel_base_m + rear_overhang_m;
+  const double vehicle_width_m_ = wheel_tread_m + left_overhang_m + right_overhang_m;
+  const double min_longitudinal_offset_m_ = -rear_overhang_m;
+  const double max_longitudinal_offset_m_ = front_overhang_m + wheel_base_m;
+  const double min_lateral_offset_m_ = -(wheel_tread_m / 2.0 + right_overhang_m);
+  const double max_lateral_offset_m_ = wheel_tread_m / 2.0 + left_overhang_m;
+  const double min_height_offset_m_ = 0.0;
+  const double max_height_offset_m_ = vehicle_height_m;
+
+  return VehicleInfo{
+    // Base parameters
+    wheel_radius_m,
+    wheel_width_m,
+    wheel_base_m,
+    wheel_tread_m,
+    front_overhang_m,
+    rear_overhang_m,
+    left_overhang_m,
+    right_overhang_m,
+    vehicle_height_m,
+    max_steer_angle_rad,
+    // Derived parameters
+    vehicle_length_m_,
+    vehicle_width_m_,
+    min_longitudinal_offset_m_,
+    max_longitudinal_offset_m_,
+    min_lateral_offset_m_,
+    max_lateral_offset_m_,
+    min_height_offset_m_,
+    max_height_offset_m_,
+  };
+}
+
+}  // namespace vehicle_info_util