diff --git a/planning/obstacle_cruise_planner/README.md b/planning/obstacle_cruise_planner/README.md
index 5f372a5baed34..084b538f39ddb 100644
--- a/planning/obstacle_cruise_planner/README.md
+++ b/planning/obstacle_cruise_planner/README.md
@@ -4,30 +4,29 @@
 
 The `obstacle_cruise_planner` package has following modules.
 
-- obstacle stop planning
-  - inserting a stop point in the trajectory when there is a static obstacle on the trajectory.
-- adaptive cruise planning
-  - sending an external velocity limit to `motion_velocity_smoother` when there is a dynamic obstacle to cruise on the trajectory
+- Stop planning
+  - stop when there is a static obstacle near the trajectory.
+- Cruise planning
+  - slow down when there is a dynamic obstacle to cruise near the trajectory
 
 ## Interfaces
 
 ### Input topics
 
-| Name                          | Type                                            | Description                       |
-| ----------------------------- | ----------------------------------------------- | --------------------------------- |
-| `~/input/trajectory`          | autoware_auto_planning_msgs::Trajectory         | input trajectory                  |
-| `~/input/smoothed_trajectory` | autoware_auto_planning_msgs::Trajectory         | trajectory with smoothed velocity |
-| `~/input/objects`             | autoware_auto_perception_msgs::PredictedObjects | dynamic objects                   |
-| `~/input/odometry`            | nav_msgs::msg::Odometry                         | ego odometry                      |
+| Name                 | Type                                            | Description      |
+| -------------------- | ----------------------------------------------- | ---------------- |
+| `~/input/trajectory` | autoware_auto_planning_msgs::Trajectory         | input trajectory |
+| `~/input/objects`    | autoware_auto_perception_msgs::PredictedObjects | dynamic objects  |
+| `~/input/odometry`   | nav_msgs::msg::Odometry                         | ego odometry     |
 
 ### Output topics
 
-| Name                           | Type                                           | Description                           |
-| ------------------------------ | ---------------------------------------------- | ------------------------------------- |
-| `~output/trajectory`           | autoware_auto_planning_msgs::Trajectory        | output trajectory                     |
-| `~output/velocity_limit`       | tier4_planning_msgs::VelocityLimit             | velocity limit for cruising           |
-| `~output/clear_velocity_limit` | tier4_planning_msgs::VelocityLimitClearCommand | clear command for velocity limit      |
-| `~output/stop_reasons`         | tier4_planning_msgs::StopReasonArray           | reasons that make the vehicle to stop |
+| Name                            | Type                                           | Description                           |
+| ------------------------------- | ---------------------------------------------- | ------------------------------------- |
+| `~/output/trajectory`           | autoware_auto_planning_msgs::Trajectory        | output trajectory                     |
+| `~/output/velocity_limit`       | tier4_planning_msgs::VelocityLimit             | velocity limit for cruising           |
+| `~/output/clear_velocity_limit` | tier4_planning_msgs::VelocityLimitClearCommand | clear command for velocity limit      |
+| `~/output/stop_reasons`         | tier4_planning_msgs::StopReasonArray           | reasons that make the vehicle to stop |
 
 ## Design
 
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 520637abed775..914e15a997096 100644
--- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml
+++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml
@@ -12,8 +12,6 @@
       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. 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
       min_behavior_stop_margin: 3.0 # [m]
       obstacle_velocity_threshold_from_cruise_to_stop : 3.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]
       obstacle_velocity_threshold_from_stop_to_cruise : 3.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]
diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp
index bdad65c27ae2f..ac9326ea7d16f 100644
--- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp
+++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp
@@ -86,6 +86,50 @@ struct ObstacleCruisePlannerData
 
 struct LongitudinalInfo
 {
+  explicit LongitudinalInfo(rclcpp::Node & node)
+  {
+    max_accel = node.declare_parameter<double>("normal.max_acc");
+    min_accel = node.declare_parameter<double>("normal.min_acc");
+    max_jerk = node.declare_parameter<double>("normal.max_jerk");
+    min_jerk = node.declare_parameter<double>("normal.min_jerk");
+    limit_max_accel = node.declare_parameter<double>("limit.max_acc");
+    limit_min_accel = node.declare_parameter<double>("limit.min_acc");
+    limit_max_jerk = node.declare_parameter<double>("limit.max_jerk");
+    limit_min_jerk = node.declare_parameter<double>("limit.min_jerk");
+
+    idling_time = node.declare_parameter<double>("common.idling_time");
+    min_ego_accel_for_rss = node.declare_parameter<double>("common.min_ego_accel_for_rss");
+    min_object_accel_for_rss = node.declare_parameter<double>("common.min_object_accel_for_rss");
+
+    safe_distance_margin = node.declare_parameter<double>("common.safe_distance_margin");
+    terminal_safe_distance_margin =
+      node.declare_parameter<double>("common.terminal_safe_distance_margin");
+  }
+
+  void onParam(const std::vector<rclcpp::Parameter> & parameters)
+  {
+    tier4_autoware_utils::updateParam<double>(parameters, "normal.max_accel", max_accel);
+    tier4_autoware_utils::updateParam<double>(parameters, "normal.min_accel", min_accel);
+    tier4_autoware_utils::updateParam<double>(parameters, "normal.max_jerk", max_jerk);
+    tier4_autoware_utils::updateParam<double>(parameters, "normal.min_jerk", min_jerk);
+    tier4_autoware_utils::updateParam<double>(parameters, "limit.max_accel", limit_max_accel);
+    tier4_autoware_utils::updateParam<double>(parameters, "limit.min_accel", limit_min_accel);
+    tier4_autoware_utils::updateParam<double>(parameters, "limit.max_jerk", limit_max_jerk);
+    tier4_autoware_utils::updateParam<double>(parameters, "limit.min_jerk", limit_min_jerk);
+
+    tier4_autoware_utils::updateParam<double>(parameters, "common.idling_time", idling_time);
+    tier4_autoware_utils::updateParam<double>(
+      parameters, "common.min_ego_accel_for_rss", min_ego_accel_for_rss);
+    tier4_autoware_utils::updateParam<double>(
+      parameters, "common.min_object_accel_for_rss", min_object_accel_for_rss);
+
+    tier4_autoware_utils::updateParam<double>(
+      parameters, "common.safe_distance_margin", safe_distance_margin);
+    tier4_autoware_utils::updateParam<double>(
+      parameters, "common.terminal_safe_distance_margin", terminal_safe_distance_margin);
+  }
+
+  // common parameter
   double max_accel;
   double min_accel;
   double max_jerk;
@@ -94,9 +138,13 @@ struct LongitudinalInfo
   double limit_min_accel;
   double limit_max_jerk;
   double limit_min_jerk;
+
+  // rss parameter
   double idling_time;
   double min_ego_accel_for_rss;
   double min_object_accel_for_rss;
+
+  // distance margin
   double safe_distance_margin;
   double terminal_safe_distance_margin;
 };
@@ -114,10 +162,13 @@ struct DebugData
 
 struct EgoNearestParam
 {
-  EgoNearestParam(const double arg_dist_threshold, const double arg_yaw_threshold)
-  : dist_threshold(arg_dist_threshold), yaw_threshold(arg_yaw_threshold)
+  EgoNearestParam() = default;
+  explicit EgoNearestParam(rclcpp::Node & node)
   {
+    dist_threshold = node.declare_parameter<double>("ego_nearest_dist_threshold");
+    yaw_threshold = node.declare_parameter<double>("ego_nearest_yaw_threshold");
   }
+
   double dist_threshold;
   double yaw_threshold;
 };
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 91bec46eb69b5..e52c2671fea5e 100644
--- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp
+++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp
@@ -80,8 +80,6 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
 
   bool is_showing_debug_info_;
   double min_behavior_stop_margin_;
-  double nearest_dist_deviation_threshold_;
-  double nearest_yaw_deviation_threshold_;
   double obstacle_velocity_threshold_from_cruise_to_stop_;
   double obstacle_velocity_threshold_from_stop_to_cruise_;
 
@@ -103,15 +101,11 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
   rclcpp::Publisher<Float32Stamped>::SharedPtr debug_calculation_time_pub_;
 
   // subscriber
-  rclcpp::Subscription<Trajectory>::SharedPtr trajectory_sub_;
-  rclcpp::Subscription<Trajectory>::SharedPtr smoothed_trajectory_sub_;
+  rclcpp::Subscription<Trajectory>::SharedPtr traj_sub_;
   rclcpp::Subscription<PredictedObjects>::SharedPtr objects_sub_;
   rclcpp::Subscription<Odometry>::SharedPtr odom_sub_;
   rclcpp::Subscription<AccelWithCovarianceStamped>::SharedPtr acc_sub_;
 
-  // self pose listener
-  tier4_autoware_utils::SelfPoseListener self_pose_listener_;
-
   // data for callback functions
   PredictedObjects::ConstSharedPtr in_objects_ptr_{nullptr};
   Odometry::ConstSharedPtr current_odom_ptr_{nullptr};
@@ -164,13 +158,12 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
   ObstacleFilteringParam obstacle_filtering_param_;
 
   bool need_to_clear_vel_limit_{false};
+  EgoNearestParam ego_nearest_param_;
 
   bool is_driving_forward_{true};
   bool disable_stop_planning_{false};
 
   std::vector<TargetObstacle> prev_target_obstacles_;
-
-  std::shared_ptr<LowpassFilter1d> lpf_acc_ptr_;
 };
 }  // namespace motion_planning
 
diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp
index 5301e7fea6326..bdd1bb5ee276e 100644
--- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp
+++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp
@@ -94,6 +94,11 @@ class OptimizationBasedPlanner : public PlannerInterface
   rclcpp::Publisher<Trajectory>::SharedPtr optimized_st_graph_pub_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_wall_marker_pub_;
 
+  // Subscriber
+  rclcpp::Subscription<Trajectory>::SharedPtr smoothed_traj_sub_;
+
+  Trajectory::ConstSharedPtr smoothed_trajectory_ptr_{nullptr};
+
   // Resampling Parameter
   double dense_resampling_time_interval_;
   double sparse_resampling_time_interval_;
diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp
index 9fea75c1270ee..06d7710290081 100644
--- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp
+++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp
@@ -46,14 +46,10 @@ class PlannerInterface
 
   PlannerInterface() = default;
 
-  void setParams(
-    const bool is_showing_debug_info, const double min_behavior_stop_margin,
-    const double nearest_dist_deviation_threshold, const double nearest_yaw_deviation_threshold)
+  void setParam(const bool is_showing_debug_info, const double min_behavior_stop_margin)
   {
     is_showing_debug_info_ = is_showing_debug_info;
     min_behavior_stop_margin_ = min_behavior_stop_margin;
-    nearest_dist_deviation_threshold_ = nearest_dist_deviation_threshold;
-    nearest_yaw_deviation_threshold_ = nearest_yaw_deviation_threshold;
   }
 
   Trajectory generateStopTrajectory(
@@ -63,31 +59,10 @@ class PlannerInterface
     const ObstacleCruisePlannerData & planner_data, boost::optional<VelocityLimit> & vel_limit,
     DebugData & debug_data) = 0;
 
-  void updateCommonParam(const std::vector<rclcpp::Parameter> & parameters)
+  void onParam(const std::vector<rclcpp::Parameter> & parameters)
   {
-    auto & i = longitudinal_info_;
-
-    tier4_autoware_utils::updateParam<double>(parameters, "common.max_accel", i.max_accel);
-    tier4_autoware_utils::updateParam<double>(parameters, "common.min_accel", i.min_accel);
-    tier4_autoware_utils::updateParam<double>(parameters, "common.max_jerk", i.max_jerk);
-    tier4_autoware_utils::updateParam<double>(parameters, "common.min_jerk", i.min_jerk);
-    tier4_autoware_utils::updateParam<double>(parameters, "limit.max_accel", i.limit_max_accel);
-    tier4_autoware_utils::updateParam<double>(parameters, "limit.min_accel", i.limit_min_accel);
-    tier4_autoware_utils::updateParam<double>(parameters, "limit.max_jerk", i.limit_max_jerk);
-    tier4_autoware_utils::updateParam<double>(parameters, "limit.min_jerk", i.limit_min_jerk);
-    tier4_autoware_utils::updateParam<double>(
-      parameters, "common.min_ego_accel_for_rss", i.min_ego_accel_for_rss);
-    tier4_autoware_utils::updateParam<double>(
-      parameters, "common.min_object_accel_for_rss", i.min_object_accel_for_rss);
-    tier4_autoware_utils::updateParam<double>(parameters, "common.idling_time", i.idling_time);
-  }
-
-  virtual void updateParam([[maybe_unused]] const std::vector<rclcpp::Parameter> & parameters) {}
-
-  // TODO(shimizu) remove this function
-  void setSmoothedTrajectory(const Trajectory::ConstSharedPtr traj)
-  {
-    smoothed_trajectory_ptr_ = traj;
+    updateCommonParam(parameters);
+    updateParam(parameters);
   }
 
   Float32MultiArrayStamped getStopPlanningDebugMessage(const rclcpp::Time & current_time) const
@@ -105,8 +80,6 @@ class PlannerInterface
   bool is_showing_debug_info_{false};
   LongitudinalInfo longitudinal_info_;
   double min_behavior_stop_margin_;
-  double nearest_dist_deviation_threshold_;
-  double nearest_yaw_deviation_threshold_;
 
   // Publishers
   rclcpp::Publisher<StopReasonArray>::SharedPtr stop_reasons_pub_;
@@ -121,9 +94,6 @@ class PlannerInterface
   // debug info
   StopPlanningDebugInfo stop_planning_debug_info_;
 
-  // TODO(shimizu) remove these parameters
-  Trajectory::ConstSharedPtr smoothed_trajectory_ptr_;
-
   double calcDistanceToCollisionPoint(
     const ObstacleCruisePlannerData & planner_data,
     const geometry_msgs::msg::Point & collision_point);
@@ -138,6 +108,13 @@ class PlannerInterface
     return rss_dist_with_margin;
   }
 
+  void updateCommonParam(const std::vector<rclcpp::Parameter> & parameters)
+  {
+    longitudinal_info_.onParam(parameters);
+  }
+
+  virtual void updateParam([[maybe_unused]] const std::vector<rclcpp::Parameter> & parameters) {}
+
   size_t findEgoIndex(const Trajectory & traj, const geometry_msgs::msg::Pose & ego_pose) const
   {
     const auto traj_points = motion_utils::convertToTrajectoryPointArray(traj);
diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp
index 17fe73a10ef8d..01d3fc979c2cc 100644
--- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp
+++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp
@@ -30,8 +30,6 @@
 
 namespace obstacle_cruise_utils
 {
-bool isVehicle(const uint8_t label);
-
 Marker getObjectMarker(
   const geometry_msgs::msg::Pose & obstacle_pose, size_t idx, const std::string & ns,
   const double r, const double g, const double b);
diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp
index c6e6c0f0a0ece..d91c30a4854d6 100644
--- a/planning/obstacle_cruise_planner/src/node.cpp
+++ b/planning/obstacle_cruise_planner/src/node.cpp
@@ -38,23 +38,11 @@ VelocityLimitClearCommand createVelocityLimitClearCommandMessage(const rclcpp::T
   return msg;
 }
 
-// TODO(murooka) make this function common
-size_t findExtendedNearestIndex(
-  const Trajectory traj, const geometry_msgs::msg::Pose & pose, const double max_dist,
-  const double max_yaw)
-{
-  const auto nearest_idx = motion_utils::findNearestIndex(traj.points, pose, max_dist, max_yaw);
-  if (nearest_idx) {
-    return nearest_idx.get();
-  }
-  return motion_utils::findNearestIndex(traj.points, pose.position);
-}
-
-Trajectory trimTrajectoryFrom(const Trajectory & input, const double nearest_idx)
+Trajectory trimTrajectoryFrom(const Trajectory & input, const double start_idx)
 {
   Trajectory output{};
 
-  for (size_t i = nearest_idx; i < input.points.size(); ++i) {
+  for (size_t i = start_idx; i < input.points.size(); ++i) {
     output.points.push_back(input.points.at(i));
   }
 
@@ -194,18 +182,14 @@ namespace motion_planning
 {
 ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & node_options)
 : Node("obstacle_cruise_planner", node_options),
-  self_pose_listener_(this),
   vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo())
 {
   using std::placeholders::_1;
 
   // subscriber
-  trajectory_sub_ = create_subscription<Trajectory>(
+  traj_sub_ = create_subscription<Trajectory>(
     "~/input/trajectory", rclcpp::QoS{1},
     std::bind(&ObstacleCruisePlannerNode::onTrajectory, this, _1));
-  smoothed_trajectory_sub_ = create_subscription<Trajectory>(
-    "/planning/scenario_planning/trajectory", rclcpp::QoS{1},
-    std::bind(&ObstacleCruisePlannerNode::onSmoothedTrajectory, this, _1));
   objects_sub_ = create_subscription<PredictedObjects>(
     "~/input/objects", rclcpp::QoS{1},
     [this](const PredictedObjects::ConstSharedPtr msg) { in_objects_ptr_ = msg; });
@@ -222,6 +206,8 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions &
     create_publisher<VelocityLimit>("~/output/velocity_limit", rclcpp::QoS{1}.transient_local());
   clear_vel_limit_pub_ = create_publisher<VelocityLimitClearCommand>(
     "~/output/clear_velocity_limit", rclcpp::QoS{1}.transient_local());
+
+  // debug publisher
   debug_calculation_time_pub_ = create_publisher<Float32Stamped>("~/debug/calculation_time", 1);
   debug_cruise_wall_marker_pub_ = create_publisher<MarkerArray>("~/debug/cruise/virtual_wall", 1);
   debug_stop_wall_marker_pub_ = create_publisher<MarkerArray>("~/virtual_wall", 1);
@@ -231,50 +217,9 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions &
   debug_cruise_planning_info_pub_ =
     create_publisher<Float32MultiArrayStamped>("~/debug/cruise_planning_info", 1);
 
-  // longitudinal_info
-  const auto longitudinal_info = [&]() {
-    const double max_accel = declare_parameter<double>("normal.max_acc");
-    const double min_accel = declare_parameter<double>("normal.min_acc");
-    const double max_jerk = declare_parameter<double>("normal.max_jerk");
-    const double min_jerk = declare_parameter<double>("normal.min_jerk");
-    const double limit_max_accel = declare_parameter<double>("limit.max_acc");
-    const double limit_min_accel = declare_parameter<double>("limit.min_acc");
-    const double limit_max_jerk = declare_parameter<double>("limit.max_jerk");
-    const double limit_min_jerk = declare_parameter<double>("limit.min_jerk");
-
-    const double min_ego_accel_for_rss = declare_parameter<double>("common.min_ego_accel_for_rss");
-    const double min_object_accel_for_rss =
-      declare_parameter<double>("common.min_object_accel_for_rss");
-    const double idling_time = declare_parameter<double>("common.idling_time");
-    const double safe_distance_margin = declare_parameter<double>("common.safe_distance_margin");
-    const double terminal_safe_distance_margin =
-      declare_parameter<double>("common.terminal_safe_distance_margin");
-
-    lpf_acc_ptr_ = std::make_shared<LowpassFilter1d>(0.2);
-
-    return LongitudinalInfo{
-      max_accel,
-      min_accel,
-      max_jerk,
-      min_jerk,
-      limit_max_accel,
-      limit_min_accel,
-      limit_max_jerk,
-      limit_min_jerk,
-      idling_time,
-      min_ego_accel_for_rss,
-      min_object_accel_for_rss,
-      safe_distance_margin,
-      terminal_safe_distance_margin};
-  }();
+  const auto longitudinal_info = LongitudinalInfo(*this);
 
-  const auto ego_nearest_param = [&]() {
-    const double ego_nearest_dist_threshold =
-      declare_parameter<double>("ego_nearest_dist_threshold");
-    const double ego_nearest_yaw_threshold = declare_parameter<double>("ego_nearest_yaw_threshold");
-
-    return EgoNearestParam(ego_nearest_dist_threshold, ego_nearest_yaw_threshold);
-  }();
+  ego_nearest_param_ = EgoNearestParam(*this);
 
   is_showing_debug_info_ = declare_parameter<bool>("common.is_showing_debug_info");
   disable_stop_planning_ = declare_parameter<bool>("common.disable_stop_planning");
@@ -354,26 +299,20 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions &
 
     if (planning_algorithm_ == PlanningAlgorithm::OPTIMIZATION_BASE) {
       planner_ptr_ = std::make_unique<OptimizationBasedPlanner>(
-        *this, longitudinal_info, vehicle_info_, ego_nearest_param);
+        *this, longitudinal_info, vehicle_info_, ego_nearest_param_);
     } else if (planning_algorithm_ == PlanningAlgorithm::PID_BASE) {
       planner_ptr_ = std::make_unique<PIDBasedPlanner>(
-        *this, longitudinal_info, vehicle_info_, ego_nearest_param);
+        *this, longitudinal_info, vehicle_info_, ego_nearest_param_);
     } else {
       std::logic_error("Designated algorithm is not supported.");
     }
 
     min_behavior_stop_margin_ = declare_parameter<double>("common.min_behavior_stop_margin");
-    nearest_dist_deviation_threshold_ =
-      declare_parameter<double>("common.nearest_dist_deviation_threshold");
-    nearest_yaw_deviation_threshold_ =
-      declare_parameter<double>("common.nearest_yaw_deviation_threshold");
     obstacle_velocity_threshold_from_cruise_to_stop_ =
       declare_parameter<double>("common.obstacle_velocity_threshold_from_cruise_to_stop");
     obstacle_velocity_threshold_from_stop_to_cruise_ =
       declare_parameter<double>("common.obstacle_velocity_threshold_from_stop_to_cruise");
-    planner_ptr_->setParams(
-      is_showing_debug_info_, min_behavior_stop_margin_, nearest_dist_deviation_threshold_,
-      nearest_yaw_deviation_threshold_);
+    planner_ptr_->setParam(is_showing_debug_info_, min_behavior_stop_margin_);
   }
 
   {  // cruise obstacle type
@@ -429,8 +368,6 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions &
       stop_obstacle_types_.push_back(ObjectClassification::PEDESTRIAN);
     }
   }
-  // wait for first self pose
-  self_pose_listener_.waitForFirstPose();
 
   // set parameter callback
   set_param_res_ = this->add_on_set_parameters_callback(
@@ -451,14 +388,11 @@ ObstacleCruisePlannerNode::PlanningAlgorithm ObstacleCruisePlannerNode::getPlann
 rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam(
   const std::vector<rclcpp::Parameter> & parameters)
 {
-  planner_ptr_->updateCommonParam(parameters);
-  planner_ptr_->updateParam(parameters);
+  planner_ptr_->onParam(parameters);
 
   tier4_autoware_utils::updateParam<bool>(
     parameters, "common.is_showing_debug_info", is_showing_debug_info_);
-  planner_ptr_->setParams(
-    is_showing_debug_info_, min_behavior_stop_margin_, nearest_dist_deviation_threshold_,
-    nearest_yaw_deviation_threshold_);
+  planner_ptr_->setParam(is_showing_debug_info_, min_behavior_stop_margin_);
 
   tier4_autoware_utils::updateParam<bool>(
     parameters, "common.disable_stop_planning", disable_stop_planning_);
@@ -513,40 +447,34 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam(
   return result;
 }
 
-void ObstacleCruisePlannerNode::onSmoothedTrajectory(const Trajectory::ConstSharedPtr msg)
-{
-  planner_ptr_->setSmoothedTrajectory(msg);
-}
-
 void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr msg)
 {
-  const auto current_pose_ptr = self_pose_listener_.getCurrentPose();
-
   // check if subscribed variables are ready
-  if (msg->points.empty() || !current_odom_ptr_ || !in_objects_ptr_ || !current_pose_ptr) {
+  if (msg->points.empty() || !current_odom_ptr_ || !in_objects_ptr_) {
     return;
   }
-  const double current_vel = current_odom_ptr_->twist.twist.linear.x;
 
   stop_watch_.tic(__func__);
 
+  const auto current_pose = current_odom_ptr_->pose.pose;
+  const double current_vel = current_odom_ptr_->twist.twist.linear.x;
+
   // Get Target Obstacles
   DebugData debug_data;
   const auto is_driving_forward = motion_utils::isDrivingForwardWithTwist(msg->points);
   is_driving_forward_ = is_driving_forward ? is_driving_forward.get() : is_driving_forward_;
   const auto target_obstacles =
-    getTargetObstacles(*msg, current_pose_ptr->pose, current_vel, is_driving_forward_, debug_data);
+    getTargetObstacles(*msg, current_pose, current_vel, is_driving_forward_, debug_data);
 
   // create data for stop
-  const auto stop_data =
-    createStopData(*msg, current_pose_ptr->pose, target_obstacles, is_driving_forward_);
+  const auto stop_data = createStopData(*msg, current_pose, target_obstacles, is_driving_forward_);
 
   // stop planning
   const auto stop_traj = planner_ptr_->generateStopTrajectory(stop_data, debug_data);
 
   // create data for cruise
   const auto cruise_data =
-    createCruiseData(stop_traj, current_pose_ptr->pose, target_obstacles, is_driving_forward_);
+    createCruiseData(stop_traj, current_pose, target_obstacles, is_driving_forward_);
 
   // cruise planning
   boost::optional<VelocityLimit> vel_limit;
@@ -675,8 +603,8 @@ std::vector<TargetObstacle> ObstacleCruisePlannerNode::filterObstacles(
   const auto current_time = now();
   const auto time_stamp = rclcpp::Time(predicted_objects.header.stamp);
 
-  const size_t ego_idx = findExtendedNearestIndex(
-    traj, current_pose, nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_);
+  const size_t ego_idx = motion_utils::findFirstNearestIndexWithSoftConstraints(
+    traj.points, current_pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold);
 
   // calculate decimated trajectory
   const auto trimmed_traj = trimTrajectoryFrom(traj, ego_idx);
@@ -1003,23 +931,28 @@ void ObstacleCruisePlannerNode::publishVelocityLimit(
   const boost::optional<VelocityLimit> & vel_limit)
 {
   if (vel_limit) {
+    // publish velocity limit
     vel_limit_pub_->publish(vel_limit.get());
     need_to_clear_vel_limit_ = true;
-  } else {
-    if (need_to_clear_vel_limit_) {
-      const auto clear_vel_limit_msg = createVelocityLimitClearCommandMessage(now());
-      clear_vel_limit_pub_->publish(clear_vel_limit_msg);
-      need_to_clear_vel_limit_ = false;
-    }
+    return;
   }
+
+  if (!need_to_clear_vel_limit_) {
+    return;
+  }
+
+  // clear velocity limit
+  const auto clear_vel_limit_msg = createVelocityLimitClearCommandMessage(now());
+  clear_vel_limit_pub_->publish(clear_vel_limit_msg);
+  need_to_clear_vel_limit_ = false;
 }
 
 void ObstacleCruisePlannerNode::publishDebugMarker(const DebugData & debug_data) const
 {
   stop_watch_.tic(__func__);
 
+  // 1. publish debug marker
   MarkerArray debug_marker;
-  const auto current_time = now();
 
   // obstacles to cruise
   for (size_t i = 0; i < debug_data.obstacles_to_cruise.size(); ++i) {
@@ -1045,7 +978,7 @@ void ObstacleCruisePlannerNode::publishDebugMarker(const DebugData & debug_data)
 
   {  // footprint polygons
     auto marker = tier4_autoware_utils::createDefaultMarker(
-      "map", current_time, "detection_polygons", 0, Marker::LINE_LIST,
+      "map", now(), "detection_polygons", 0, Marker::LINE_LIST,
       tier4_autoware_utils::createMarkerScale(0.01, 0.0, 0.0),
       tier4_autoware_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999));
 
@@ -1067,7 +1000,7 @@ void ObstacleCruisePlannerNode::publishDebugMarker(const DebugData & debug_data)
   {  // collision points
     for (size_t i = 0; i < debug_data.collision_points.size(); ++i) {
       auto marker = tier4_autoware_utils::createDefaultMarker(
-        "map", current_time, "collision_points", i, Marker::SPHERE,
+        "map", now(), "collision_points", i, Marker::SPHERE,
         tier4_autoware_utils::createMarkerScale(0.25, 0.25, 0.25),
         tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999));
       marker.pose.position = debug_data.collision_points.at(i);
@@ -1077,11 +1010,11 @@ void ObstacleCruisePlannerNode::publishDebugMarker(const DebugData & debug_data)
 
   debug_marker_pub_->publish(debug_marker);
 
-  // virtual wall for cruise and stop
+  // 2. publish virtual wall for cruise and stop
   debug_cruise_wall_marker_pub_->publish(debug_data.cruise_wall_marker);
   debug_stop_wall_marker_pub_->publish(debug_data.stop_wall_marker);
 
-  // print calculation time
+  // 3. print calculation time
   const double calculation_time = stop_watch_.toc(__func__);
   RCLCPP_INFO_EXPRESSION(
     rclcpp::get_logger("ObstacleCruisePlanner"), is_showing_debug_info_, "  %s := %f [ms]",
@@ -1090,14 +1023,12 @@ void ObstacleCruisePlannerNode::publishDebugMarker(const DebugData & debug_data)
 
 void ObstacleCruisePlannerNode::publishDebugInfo() const
 {
-  const auto current_time = now();
-
   // stop
-  const auto stop_debug_msg = planner_ptr_->getStopPlanningDebugMessage(current_time);
+  const auto stop_debug_msg = planner_ptr_->getStopPlanningDebugMessage(now());
   debug_stop_planning_info_pub_->publish(stop_debug_msg);
 
   // cruise
-  const auto cruise_debug_msg = planner_ptr_->getCruisePlanningDebugMessage(current_time);
+  const auto cruise_debug_msg = planner_ptr_->getCruisePlanningDebugMessage(now());
   debug_cruise_planning_info_pub_->publish(cruise_debug_msg);
 }
 
diff --git a/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp
index 1d63c5af02c8a..262e2b619d36e 100644
--- a/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp
+++ b/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp
@@ -39,6 +39,10 @@ OptimizationBasedPlanner::OptimizationBasedPlanner(
   const vehicle_info_util::VehicleInfo & vehicle_info, const EgoNearestParam & ego_nearest_param)
 : PlannerInterface(node, longitudinal_info, vehicle_info, ego_nearest_param)
 {
+  smoothed_traj_sub_ = node.create_subscription<Trajectory>(
+    "/planning/scenario_planning/trajectory", rclcpp::QoS{1},
+    [this](const Trajectory::ConstSharedPtr msg) { smoothed_trajectory_ptr_ = msg; });
+
   // parameter
   dense_resampling_time_interval_ =
     node.declare_parameter<double>("optimization_based_planner.dense_resampling_time_interval");
@@ -283,8 +287,8 @@ std::tuple<double, double> OptimizationBasedPlanner::calcInitialMotion(
   const auto & input_traj = planner_data.traj;
   const double vehicle_speed{std::abs(current_vel)};
   const auto current_closest_point = motion_utils::calcInterpolatedPoint(
-    input_traj, planner_data.current_pose, nearest_dist_deviation_threshold_,
-    nearest_yaw_deviation_threshold_);
+    input_traj, planner_data.current_pose, ego_nearest_param_.dist_threshold,
+    ego_nearest_param_.yaw_threshold);
   const double target_vel{std::abs(current_closest_point.longitudinal_velocity_mps)};
 
   double initial_vel{};
@@ -300,11 +304,11 @@ std::tuple<double, double> OptimizationBasedPlanner::calcInitialMotion(
   TrajectoryPoint prev_output_closest_point;
   if (smoothed_trajectory_ptr_) {
     prev_output_closest_point = motion_utils::calcInterpolatedPoint(
-      *smoothed_trajectory_ptr_, current_pose, nearest_dist_deviation_threshold_,
-      nearest_yaw_deviation_threshold_);
+      *smoothed_trajectory_ptr_, current_pose, ego_nearest_param_.dist_threshold,
+      ego_nearest_param_.yaw_threshold);
   } else {
     prev_output_closest_point = motion_utils::calcInterpolatedPoint(
-      prev_traj, current_pose, nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_);
+      prev_traj, current_pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold);
   }
 
   // when velocity tracking deviation is large
@@ -327,8 +331,8 @@ std::tuple<double, double> OptimizationBasedPlanner::calcInitialMotion(
   if (vehicle_speed < engage_vel_thr) {
     if (target_vel >= engage_velocity_) {
       const auto stop_dist = motion_utils::calcDistanceToForwardStopPoint(
-        input_traj.points, current_pose, nearest_dist_deviation_threshold_,
-        nearest_yaw_deviation_threshold_);
+        input_traj.points, current_pose, ego_nearest_param_.dist_threshold,
+        ego_nearest_param_.yaw_threshold);
       if ((stop_dist && *stop_dist > stop_dist_to_prohibit_engage_) || !stop_dist) {
         initial_vel = engage_velocity_;
         initial_acc = engage_acceleration_;
@@ -362,8 +366,8 @@ bool OptimizationBasedPlanner::checkHasReachedGoal(const ObstacleCruisePlannerDa
 {
   // If goal is close and current velocity is low, we don't optimize trajectory
   const auto closest_stop_dist = motion_utils::calcDistanceToForwardStopPoint(
-    planner_data.traj.points, planner_data.current_pose, nearest_dist_deviation_threshold_,
-    nearest_yaw_deviation_threshold_);
+    planner_data.traj.points, planner_data.current_pose, ego_nearest_param_.dist_threshold,
+    ego_nearest_param_.yaw_threshold);
   if (closest_stop_dist && *closest_stop_dist < 0.5 && planner_data.current_vel < 0.6) {
     return true;
   }
@@ -592,15 +596,15 @@ boost::optional<double> OptimizationBasedPlanner::calcTrajectoryLengthFromCurren
   const Trajectory & traj, const geometry_msgs::msg::Pose & current_pose)
 {
   const auto traj_length = motion_utils::calcSignedArcLength(
-    traj.points, current_pose, traj.points.size() - 1, nearest_dist_deviation_threshold_,
-    nearest_yaw_deviation_threshold_);
+    traj.points, current_pose, traj.points.size() - 1, ego_nearest_param_.dist_threshold,
+    ego_nearest_param_.yaw_threshold);
 
   if (!traj_length) {
     return {};
   }
 
   const auto dist_to_closest_stop_point = motion_utils::calcDistanceToForwardStopPoint(
-    traj.points, current_pose, nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_);
+    traj.points, current_pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold);
   if (dist_to_closest_stop_point) {
     return std::min(traj_length.get(), dist_to_closest_stop_point.get());
   }
diff --git a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp
index cfd9dd24c543b..19bf69bc2b235 100644
--- a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp
+++ b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp
@@ -462,14 +462,14 @@ Trajectory PIDBasedPlanner::doCruiseWithTrajectory(
 
   // set target longitudinal motion
   const auto prev_traj_closest_point = [&]() -> TrajectoryPoint {
-    if (smoothed_trajectory_ptr_) {
-      return motion_utils::calcInterpolatedPoint(
-        *smoothed_trajectory_ptr_, planner_data.current_pose, nearest_dist_deviation_threshold_,
-        nearest_yaw_deviation_threshold_);
-    }
+    // if (smoothed_trajectory_ptr_) {
+    //   return motion_utils::calcInterpolatedPoint(
+    //     *smoothed_trajectory_ptr_, planner_data.current_pose, nearest_dist_deviation_threshold_,
+    //     nearest_yaw_deviation_threshold_);
+    // }
     return motion_utils::calcInterpolatedPoint(
-      prev_traj_, planner_data.current_pose, nearest_dist_deviation_threshold_,
-      nearest_yaw_deviation_threshold_);
+      prev_traj_, planner_data.current_pose, ego_nearest_param_.dist_threshold,
+      ego_nearest_param_.yaw_threshold);
   }();
   const double v0 = prev_traj_closest_point.longitudinal_velocity_mps;
   const double a0 = prev_traj_closest_point.acceleration_mps2;
@@ -580,9 +580,7 @@ Trajectory PIDBasedPlanner::getAccelerationLimitedTrajectory(
   }
 
   auto acc_limited_traj = traj;
-  const size_t ego_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
-    acc_limited_traj.points, start_pose, nearest_dist_deviation_threshold_,
-    nearest_yaw_deviation_threshold_);
+  const size_t ego_seg_idx = findEgoIndex(acc_limited_traj, start_pose);
   double sum_dist = 0.0;
   for (size_t i = ego_seg_idx; i < acc_limited_traj.points.size(); ++i) {
     if (i != ego_seg_idx) {
diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp
index b93242e1aeae1..420b72d2bf0aa 100644
--- a/planning/obstacle_cruise_planner/src/planner_interface.cpp
+++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp
@@ -144,8 +144,8 @@ Trajectory PlannerInterface::generateStopTrajectory(
     planner_data.traj.points, 0, closest_stop_obstacle->collision_points.front().point);
 
   const auto negative_dist_to_ego = motion_utils::calcSignedArcLength(
-    planner_data.traj.points, planner_data.current_pose, 0, nearest_dist_deviation_threshold_,
-    nearest_yaw_deviation_threshold_);
+    planner_data.traj.points, planner_data.current_pose, 0, ego_nearest_param_.dist_threshold,
+    ego_nearest_param_.yaw_threshold);
   if (!negative_dist_to_ego) {
     // delete marker
     const auto markers =
@@ -160,9 +160,7 @@ Trajectory PlannerInterface::generateStopTrajectory(
   // we set closest_obstacle_stop_distance to closest_behavior_stop_distance
   const double margin_from_obstacle = [&]() {
     const size_t nearest_segment_idx =
-      motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
-        planner_data.traj.points, planner_data.current_pose, nearest_dist_deviation_threshold_,
-        nearest_yaw_deviation_threshold_);
+      findEgoSegmentIndex(planner_data.traj, planner_data.current_pose);
     const auto closest_behavior_stop_idx =
       motion_utils::searchZeroVelocityIndex(planner_data.traj.points, nearest_segment_idx + 1);
 
@@ -264,7 +262,7 @@ double PlannerInterface::calcDistanceToCollisionPoint(
 
   const auto dist_to_collision_point = motion_utils::calcSignedArcLength(
     planner_data.traj.points, planner_data.current_pose, collision_point,
-    nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_);
+    ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold);
 
   if (dist_to_collision_point) {
     return dist_to_collision_point.get() - offset;
diff --git a/planning/obstacle_cruise_planner/src/utils.cpp b/planning/obstacle_cruise_planner/src/utils.cpp
index 219a691b0fbc3..51a02aa776a40 100644
--- a/planning/obstacle_cruise_planner/src/utils.cpp
+++ b/planning/obstacle_cruise_planner/src/utils.cpp
@@ -18,12 +18,6 @@
 
 namespace obstacle_cruise_utils
 {
-bool isVehicle(const uint8_t label)
-{
-  return label == ObjectClassification::CAR || label == ObjectClassification::TRUCK ||
-         label == ObjectClassification::BUS || label == ObjectClassification::MOTORCYCLE;
-}
-
 visualization_msgs::msg::Marker getObjectMarker(
   const geometry_msgs::msg::Pose & obstacle_pose, size_t idx, const std::string & ns,
   const double r, const double g, const double b)