diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml
index 0c102155d2f00..520637abed775 100644
--- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml
+++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml
@@ -10,7 +10,7 @@
       min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss]
       min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss]
       safe_distance_margin : 6.0 # This is also used as a stop margin [m]
-      terminal_safe_distance_margin : 3.0 # Stop margin at the goal [m]
+      terminal_safe_distance_margin : 3.0 # Stop margin at the goal. This value cannot exceed safe distance margin. [m]
 
       nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index
       nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index
diff --git a/planning/obstacle_cruise_planner/README.md b/planning/obstacle_cruise_planner/README.md
index d4e66d87b5a5a..4d114c47bfaf0 100644
--- a/planning/obstacle_cruise_planner/README.md
+++ b/planning/obstacle_cruise_planner/README.md
@@ -149,15 +149,17 @@ In the `obstacle_filtering` namespace,
 
 ### Stop planning
 
-| Parameter                     | Type   | Description                               |
-| ----------------------------- | ------ | ----------------------------------------- |
-| `common.min_strong_accel`     | double | ego's minimum acceleration to stop [m/ss] |
-| `common.safe_distance_margin` | double | distance with obstacles for stop [m]      |
+| Parameter                              | Type   | Description                                                                                |
+| -------------------------------------- | ------ | ------------------------------------------------------------------------------------------ |
+| `common.min_strong_accel`              | double | ego's minimum acceleration to stop [m/ss]                                                  |
+| `common.safe_distance_margin`          | double | distance with obstacles for stop [m]                                                       |
+| `common.terminal_safe_distance_margin` | double | terminal_distance with obstacles for stop, which cannot be exceed safe distance margin [m] |
 
 The role of the stop planning is keeping a safe distance with static vehicle objects or dynamic/static non vehicle objects.
 
 The stop planning just inserts the stop point in the trajectory to keep a distance with obstacles inside the detection area.
 The safe distance is parameterized as `common.safe_distance_margin`.
+When it stops at the end of the trajectory, and obstacle is on the same point, the safe distance becomes `terminal_safe_distance_margin`.
 
 When inserting the stop point, the required acceleration for the ego to stop in front of the stop point is calculated.
 If the acceleration is less than `common.min_strong_accel`, the stop planning will be cancelled since this package does not assume a strong sudden brake for emergency.
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 0c102155d2f00..520637abed775 100644
--- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml
+++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml
@@ -10,7 +10,7 @@
       min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss]
       min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss]
       safe_distance_margin : 6.0 # This is also used as a stop margin [m]
-      terminal_safe_distance_margin : 3.0 # Stop margin at the goal [m]
+      terminal_safe_distance_margin : 3.0 # Stop margin at the goal. This value cannot exceed safe distance margin. [m]
 
       nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index
       nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index
diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp
index 4ebef793bc733..e73d83e749139 100644
--- a/planning/obstacle_cruise_planner/src/planner_interface.cpp
+++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp
@@ -155,7 +155,7 @@ Trajectory PlannerInterface::generateStopTrajectory(
         abs_ego_offset;
       const double stop_dist_diff =
         closest_behavior_stop_dist_from_ego - closest_obstacle_stop_dist_from_ego;
-      if (stop_dist_diff < 0.5) {
+      if (stop_dist_diff < longitudinal_info_.safe_distance_margin) {
         // Use terminal margin (terminal_safe_distance_margin) for obstacle stop
         return longitudinal_info_.terminal_safe_distance_margin;
       }