refactor(start_planner): refactor debug and safety check logic

planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml

@@ -119,8 +119,7 @@
         # EgoPredictedPath
         ego_predicted_path:
           min_velocity: 0.0
-          acceleration: 1.0
-          max_velocity: 1.0
+          min_acceleration: 1.0
           time_horizon_for_front_object: 10.0
           time_horizon_for_rear_object: 10.0
           time_resolution: 0.5

planning/behavior_path_planner/config/start_planner/start_planner.param.yaml

@@ -2,15 +2,13 @@
   ros__parameters:
     start_planner:
 
-      verbose: false
-
       th_arrived_distance: 1.0
       th_stopped_velocity: 0.01
       th_stopped_time: 1.0
       collision_check_margin: 1.0
       collision_check_distance_from_end: 1.0
       th_moving_object_velocity: 1.0
-      th_distance_to_middle_of_the_road: 0.1
+      th_distance_to_middle_of_the_road: 0.5
       center_line_path_interval: 1.0
       # shift pull out
       enable_shift_pull_out: true
@@ -24,7 +22,7 @@
       maximum_curvature: 0.07
       # geometric pull out
       enable_geometric_pull_out: true
-      divide_pull_out_path: false
+      divide_pull_out_path: true
       geometric_pull_out_velocity: 1.0
       arc_path_interval: 1.0
       lane_departure_margin: 0.2
@@ -85,8 +83,7 @@
         # EgoPredictedPath
         ego_predicted_path:
           min_velocity: 0.0
-          acceleration: 1.0
-          max_velocity: 1.0
+          min_acceleration: 1.0
           time_horizon_for_front_object: 10.0
           time_horizon_for_rear_object: 10.0
           time_resolution: 0.5
@@ -140,3 +137,7 @@
           # temporary
           backward_path_length: 30.0
           forward_path_length: 100.0
+
+      # debug
+      debug:
+        print_debug_info: false

planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp

@@ -129,8 +129,14 @@ class StartPlannerModule : public SceneModuleInterface
 
   bool canTransitIdleToRunningState() override;
 
+  /**
+   * @brief init member variables.
+   */
+  void initVariables();
+
   void initializeSafetyCheckParameters();
 
+  bool requiresDynamicObjectsCollisionDetection() const;
   bool receivedNewRoute() const;
 
   bool isModuleRunning() const;
@@ -200,7 +206,8 @@ class StartPlannerModule : public SceneModuleInterface
   PredictedObjects filterStopObjectsInPullOutLanes(
     const lanelet::ConstLanelets & pull_out_lanes, const double velocity_threshold) const;
   bool hasFinishedPullOut() const;
-  bool isBackwardDrivingComplete() const;
+  bool hasFinishedBackwardDriving() const;
+  bool hasCollisionWithDynamicObjects() const;
   bool isStopped();
   bool isStuck();
   bool hasFinishedCurrentPath();

planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp

@@ -46,7 +46,6 @@ struct StartPlannerParameters
   double collision_check_distance_from_end{0.0};
   double th_moving_object_velocity{0.0};
   double center_line_path_interval{0.0};
-  bool verbose{false};
 
   // shift pull out
   bool enable_shift_pull_out{false};
@@ -93,6 +92,8 @@ struct StartPlannerParameters
   utils::path_safety_checker::EgoPredictedPathParams ego_predicted_path_params{};
   utils::path_safety_checker::ObjectsFilteringParams objects_filtering_params{};
   utils::path_safety_checker::SafetyCheckParams safety_check_params{};
+
+  bool print_debug_info{false};
 };
 
 }  // namespace behavior_path_planner

planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp

@@ -255,9 +255,7 @@
     p.ego_predicted_path_params.min_velocity =
       node->declare_parameter<double>(ego_path_ns + "min_velocity");
     p.ego_predicted_path_params.acceleration =
-      node->declare_parameter<double>(ego_path_ns + "acceleration");
-    p.ego_predicted_path_params.max_velocity =
-      node->declare_parameter<double>(ego_path_ns + "max_velocity");
+      node->declare_parameter<double>(ego_path_ns + "min_acceleration");
     p.ego_predicted_path_params.time_horizon_for_front_object =
       node->declare_parameter<double>(ego_path_ns + "time_horizon_for_front_object");
     p.ego_predicted_path_params.time_horizon_for_rear_object =

planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp

@@ -32,250 +32,253 @@
   StartPlannerParameters p;
 
   std::string ns = "start_planner.";
 
-  p.verbose = node->declare_parameter<bool>(ns + "verbose");
   p.th_arrived_distance = node->declare_parameter<double>(ns + "th_arrived_distance");
   p.th_stopped_velocity = node->declare_parameter<double>(ns + "th_stopped_velocity");
   p.th_stopped_time = node->declare_parameter<double>(ns + "th_stopped_time");
   p.th_turn_signal_on_lateral_offset =
     node->declare_parameter<double>(ns + "th_turn_signal_on_lateral_offset");
   p.th_distance_to_middle_of_the_road =
     node->declare_parameter<double>(ns + "th_distance_to_middle_of_the_road");
   p.intersection_search_length = node->declare_parameter<double>(ns + "intersection_search_length");
   p.length_ratio_for_turn_signal_deactivation_near_intersection = node->declare_parameter<double>(
     ns + "length_ratio_for_turn_signal_deactivation_near_intersection");
   p.collision_check_margin = node->declare_parameter<double>(ns + "collision_check_margin");
   p.collision_check_distance_from_end =
     node->declare_parameter<double>(ns + "collision_check_distance_from_end");
   p.th_moving_object_velocity = node->declare_parameter<double>(ns + "th_moving_object_velocity");
   p.center_line_path_interval = node->declare_parameter<double>(ns + "center_line_path_interval");
   // shift pull out
   p.enable_shift_pull_out = node->declare_parameter<bool>(ns + "enable_shift_pull_out");
   p.check_shift_path_lane_departure =
     node->declare_parameter<bool>(ns + "check_shift_path_lane_departure");
   p.minimum_shift_pull_out_distance =
     node->declare_parameter<double>(ns + "minimum_shift_pull_out_distance");
   p.lateral_acceleration_sampling_num =
     node->declare_parameter<int>(ns + "lateral_acceleration_sampling_num");
   p.lateral_jerk = node->declare_parameter<double>(ns + "lateral_jerk");
   p.maximum_lateral_acc = node->declare_parameter<double>(ns + "maximum_lateral_acc");
   p.minimum_lateral_acc = node->declare_parameter<double>(ns + "minimum_lateral_acc");
   p.maximum_curvature = node->declare_parameter<double>(ns + "maximum_curvature");
   p.deceleration_interval = node->declare_parameter<double>(ns + "deceleration_interval");
   // geometric pull out
   p.enable_geometric_pull_out = node->declare_parameter<bool>(ns + "enable_geometric_pull_out");
   p.divide_pull_out_path = node->declare_parameter<bool>(ns + "divide_pull_out_path");
   p.parallel_parking_parameters.pull_out_velocity =
     node->declare_parameter<double>(ns + "geometric_pull_out_velocity");
   p.parallel_parking_parameters.pull_out_path_interval =
     node->declare_parameter<double>(ns + "arc_path_interval");
   p.parallel_parking_parameters.pull_out_lane_departure_margin =
     node->declare_parameter<double>(ns + "lane_departure_margin");
   p.parallel_parking_parameters.pull_out_max_steer_angle =
     node->declare_parameter<double>(ns + "pull_out_max_steer_angle");  // 15deg
   p.parallel_parking_parameters.center_line_path_interval =
     p.center_line_path_interval;  // for geometric parallel parking
   // search start pose backward
   p.search_priority = node->declare_parameter<std::string>(
     ns + "search_priority");  // "efficient_path" or "short_back_distance"
   p.enable_back = node->declare_parameter<bool>(ns + "enable_back");
   p.backward_velocity = node->declare_parameter<double>(ns + "backward_velocity");
   p.max_back_distance = node->declare_parameter<double>(ns + "max_back_distance");
   p.backward_search_resolution = node->declare_parameter<double>(ns + "backward_search_resolution");
   p.backward_path_update_duration =
     node->declare_parameter<double>(ns + "backward_path_update_duration");
   p.ignore_distance_from_lane_end =
     node->declare_parameter<double>(ns + "ignore_distance_from_lane_end");
   // freespace planner general params
   {
     std::string ns = "start_planner.freespace_planner.";
     p.enable_freespace_planner = node->declare_parameter<bool>(ns + "enable_freespace_planner");
     p.freespace_planner_algorithm =
       node->declare_parameter<std::string>(ns + "freespace_planner_algorithm");
     p.end_pose_search_start_distance =
       node->declare_parameter<double>(ns + "end_pose_search_start_distance");
     p.end_pose_search_end_distance =
       node->declare_parameter<double>(ns + "end_pose_search_end_distance");
     p.end_pose_search_interval = node->declare_parameter<double>(ns + "end_pose_search_interval");
     p.freespace_planner_velocity = node->declare_parameter<double>(ns + "velocity");
     p.vehicle_shape_margin = node->declare_parameter<double>(ns + "vehicle_shape_margin");
     p.freespace_planner_common_parameters.time_limit =
       node->declare_parameter<double>(ns + "time_limit");
     p.freespace_planner_common_parameters.minimum_turning_radius =
       node->declare_parameter<double>(ns + "minimum_turning_radius");
     p.freespace_planner_common_parameters.maximum_turning_radius =
       node->declare_parameter<double>(ns + "maximum_turning_radius");
     p.freespace_planner_common_parameters.turning_radius_size =
       node->declare_parameter<int>(ns + "turning_radius_size");
     p.freespace_planner_common_parameters.maximum_turning_radius = std::max(
       p.freespace_planner_common_parameters.maximum_turning_radius,
       p.freespace_planner_common_parameters.minimum_turning_radius);
     p.freespace_planner_common_parameters.turning_radius_size =
       std::max(p.freespace_planner_common_parameters.turning_radius_size, 1);
   }
   //  freespace planner search config
   {
     std::string ns = "start_planner.freespace_planner.search_configs.";
     p.freespace_planner_common_parameters.theta_size =
       node->declare_parameter<int>(ns + "theta_size");
     p.freespace_planner_common_parameters.angle_goal_range =
       node->declare_parameter<double>(ns + "angle_goal_range");
     p.freespace_planner_common_parameters.curve_weight =
       node->declare_parameter<double>(ns + "curve_weight");
     p.freespace_planner_common_parameters.reverse_weight =
       node->declare_parameter<double>(ns + "reverse_weight");
     p.freespace_planner_common_parameters.lateral_goal_range =
       node->declare_parameter<double>(ns + "lateral_goal_range");
     p.freespace_planner_common_parameters.longitudinal_goal_range =
       node->declare_parameter<double>(ns + "longitudinal_goal_range");
   }
   //  freespace planner costmap configs
   {
     std::string ns = "start_planner.freespace_planner.costmap_configs.";
     p.freespace_planner_common_parameters.obstacle_threshold =
       node->declare_parameter<int>(ns + "obstacle_threshold");
   }
   //  freespace planner astar
   {
     std::string ns = "start_planner.freespace_planner.astar.";
     p.astar_parameters.only_behind_solutions =
       node->declare_parameter<bool>(ns + "only_behind_solutions");
     p.astar_parameters.use_back = node->declare_parameter<bool>(ns + "use_back");
     p.astar_parameters.distance_heuristic_weight =
       node->declare_parameter<double>(ns + "distance_heuristic_weight");
   }
   //   freespace planner rrtstar
   {
     std::string ns = "start_planner.freespace_planner.rrtstar.";
     p.rrt_star_parameters.enable_update = node->declare_parameter<bool>(ns + "enable_update");
     p.rrt_star_parameters.use_informed_sampling =
       node->declare_parameter<bool>(ns + "use_informed_sampling");
     p.rrt_star_parameters.max_planning_time =
       node->declare_parameter<double>(ns + "max_planning_time");
     p.rrt_star_parameters.neighbor_radius = node->declare_parameter<double>(ns + "neighbor_radius");
     p.rrt_star_parameters.margin = node->declare_parameter<double>(ns + "margin");
   }
 
   // stop condition
   {
     p.maximum_deceleration_for_stop =
       node->declare_parameter<double>(ns + "stop_condition.maximum_deceleration_for_stop");
     p.maximum_jerk_for_stop =
       node->declare_parameter<double>(ns + "stop_condition.maximum_jerk_for_stop");
   }
 
   std::string base_ns = "start_planner.path_safety_check.";
 
   // EgoPredictedPath
   std::string ego_path_ns = base_ns + "ego_predicted_path.";
   {
     p.ego_predicted_path_params.min_velocity =
       node->declare_parameter<double>(ego_path_ns + "min_velocity");
     p.ego_predicted_path_params.acceleration =
-      node->declare_parameter<double>(ego_path_ns + "acceleration");
-    p.ego_predicted_path_params.max_velocity =
-      node->declare_parameter<double>(ego_path_ns + "max_velocity");
+      node->declare_parameter<double>(ego_path_ns + "min_acceleration");
     p.ego_predicted_path_params.time_horizon_for_front_object =
       node->declare_parameter<double>(ego_path_ns + "time_horizon_for_front_object");
     p.ego_predicted_path_params.time_horizon_for_rear_object =
       node->declare_parameter<double>(ego_path_ns + "time_horizon_for_rear_object");
     p.ego_predicted_path_params.time_resolution =
       node->declare_parameter<double>(ego_path_ns + "time_resolution");
     p.ego_predicted_path_params.delay_until_departure =
       node->declare_parameter<double>(ego_path_ns + "delay_until_departure");
   }
 
   // ObjectFilteringParams
   std::string obj_filter_ns = base_ns + "target_filtering.";
   {
     p.objects_filtering_params.safety_check_time_horizon =
       node->declare_parameter<double>(obj_filter_ns + "safety_check_time_horizon");
     p.objects_filtering_params.safety_check_time_resolution =
       node->declare_parameter<double>(obj_filter_ns + "safety_check_time_resolution");
     p.objects_filtering_params.object_check_forward_distance =
       node->declare_parameter<double>(obj_filter_ns + "object_check_forward_distance");
     p.objects_filtering_params.object_check_backward_distance =
       node->declare_parameter<double>(obj_filter_ns + "object_check_backward_distance");
     p.objects_filtering_params.ignore_object_velocity_threshold =
       node->declare_parameter<double>(obj_filter_ns + "ignore_object_velocity_threshold");
     p.objects_filtering_params.include_opposite_lane =
       node->declare_parameter<bool>(obj_filter_ns + "include_opposite_lane");
     p.objects_filtering_params.invert_opposite_lane =
       node->declare_parameter<bool>(obj_filter_ns + "invert_opposite_lane");
     p.objects_filtering_params.check_all_predicted_path =
       node->declare_parameter<bool>(obj_filter_ns + "check_all_predicted_path");
     p.objects_filtering_params.use_all_predicted_path =
       node->declare_parameter<bool>(obj_filter_ns + "use_all_predicted_path");
     p.objects_filtering_params.use_predicted_path_outside_lanelet =
       node->declare_parameter<bool>(obj_filter_ns + "use_predicted_path_outside_lanelet");
   }
 
   // ObjectTypesToCheck
   std::string obj_types_ns = obj_filter_ns + "object_types_to_check.";
   {
     p.objects_filtering_params.object_types_to_check.check_car =
       node->declare_parameter<bool>(obj_types_ns + "check_car");
     p.objects_filtering_params.object_types_to_check.check_truck =
       node->declare_parameter<bool>(obj_types_ns + "check_truck");
     p.objects_filtering_params.object_types_to_check.check_bus =
       node->declare_parameter<bool>(obj_types_ns + "check_bus");
     p.objects_filtering_params.object_types_to_check.check_trailer =
       node->declare_parameter<bool>(obj_types_ns + "check_trailer");
     p.objects_filtering_params.object_types_to_check.check_unknown =
       node->declare_parameter<bool>(obj_types_ns + "check_unknown");
     p.objects_filtering_params.object_types_to_check.check_bicycle =
       node->declare_parameter<bool>(obj_types_ns + "check_bicycle");
     p.objects_filtering_params.object_types_to_check.check_motorcycle =
       node->declare_parameter<bool>(obj_types_ns + "check_motorcycle");
     p.objects_filtering_params.object_types_to_check.check_pedestrian =
       node->declare_parameter<bool>(obj_types_ns + "check_pedestrian");
   }
 
   // ObjectLaneConfiguration
   std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration.";
   {
     p.objects_filtering_params.object_lane_configuration.check_current_lane =
       node->declare_parameter<bool>(obj_lane_ns + "check_current_lane");
     p.objects_filtering_params.object_lane_configuration.check_right_lane =
       node->declare_parameter<bool>(obj_lane_ns + "check_right_side_lane");
     p.objects_filtering_params.object_lane_configuration.check_left_lane =
       node->declare_parameter<bool>(obj_lane_ns + "check_left_side_lane");
     p.objects_filtering_params.object_lane_configuration.check_shoulder_lane =
       node->declare_parameter<bool>(obj_lane_ns + "check_shoulder_lane");
     p.objects_filtering_params.object_lane_configuration.check_other_lane =
       node->declare_parameter<bool>(obj_lane_ns + "check_other_lane");
   }
 
   // SafetyCheckParams
   std::string safety_check_ns = base_ns + "safety_check_params.";
   {
     p.safety_check_params.enable_safety_check =
       node->declare_parameter<bool>(safety_check_ns + "enable_safety_check");
     p.safety_check_params.hysteresis_factor_expand_rate =
       node->declare_parameter<double>(safety_check_ns + "hysteresis_factor_expand_rate");
     p.safety_check_params.backward_path_length =
       node->declare_parameter<double>(safety_check_ns + "backward_path_length");
     p.safety_check_params.forward_path_length =
       node->declare_parameter<double>(safety_check_ns + "forward_path_length");
     p.safety_check_params.publish_debug_marker =
       node->declare_parameter<bool>(safety_check_ns + "publish_debug_marker");
   }
 
   // RSSparams
   std::string rss_ns = safety_check_ns + "rss_params.";
   {
     p.safety_check_params.rss_params.rear_vehicle_reaction_time =
       node->declare_parameter<double>(rss_ns + "rear_vehicle_reaction_time");
     p.safety_check_params.rss_params.rear_vehicle_safety_time_margin =
       node->declare_parameter<double>(rss_ns + "rear_vehicle_safety_time_margin");
     p.safety_check_params.rss_params.lateral_distance_max_threshold =
       node->declare_parameter<double>(rss_ns + "lateral_distance_max_threshold");
     p.safety_check_params.rss_params.longitudinal_distance_min_threshold =
       node->declare_parameter<double>(rss_ns + "longitudinal_distance_min_threshold");
     p.safety_check_params.rss_params.longitudinal_velocity_delta_time =
       node->declare_parameter<double>(rss_ns + "longitudinal_velocity_delta_time");
   }
 
+  // debug
+  std::string debug_ns = ns + "debug.";
+  {
+    p.print_debug_info = node->declare_parameter<bool>(debug_ns + "print_debug_info");
+  }
+
   // validation of parameters
   if (p.lateral_acceleration_sampling_num < 1) {
     RCLCPP_FATAL_STREAM(

planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp

@@ -1,4 +1,4 @@
 // Copyright 2022 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -36,9 +36,15 @@
 #include <utility>
 #include <vector>
 
+using behavior_path_planner::utils::start_goal_planner_common::initializeCollisionCheckDebugMap;
 using motion_utils::calcLongitudinalOffsetPose;
 using tier4_autoware_utils::calcOffsetPose;
 
+// set as macro so that calling function name will be printed.
+// debug print is heavy. turn on only when debugging.
+#define DEBUG_PRINT(...) \
+  RCLCPP_DEBUG_EXPRESSION(getLogger(), parameters_->print_debug_info, __VA_ARGS__)
+
 namespace behavior_path_planner
 {
 StartPlannerModule::StartPlannerModule(
@@ -107,38 +113,59 @@
 
 void StartPlannerModule::processOnEntry()
 {
-  // Initialize safety checker
-  if (parameters_->safety_check_params.enable_safety_check) {
-    initializeSafetyCheckParameters();
-    utils::start_goal_planner_common::initializeCollisionCheckDebugMap(
-      start_planner_data_.collision_check);
-  }
+  initVariables();
 }
 
 void StartPlannerModule::processOnExit()
+{
+  initVariables();
+}
+
+void StartPlannerModule::initVariables()
 {
   resetPathCandidate();
   resetPathReference();
   debug_marker_.markers.clear();
+  initializeSafetyCheckParameters();
+  initializeCollisionCheckDebugMap(start_planner_data_.collision_check);
 }
 
 void StartPlannerModule::updateData()
 {
-  if (isBackwardDrivingComplete()) {
+  if (receivedNewRoute()) {
+    resetStatus();
+    DEBUG_PRINT("StartPlannerModule::updateData() received new route, reset status");
+  }
+
+  if (hasFinishedBackwardDriving()) {
     updateStatusAfterBackwardDriving();
+    DEBUG_PRINT("StartPlannerModule::updateData() completed backward driving");
   } else {
     status_.backward_driving_complete = false;
   }
 
-  if (receivedNewRoute()) {
-    status_ = PullOutStatus();
+  if (requiresDynamicObjectsCollisionDetection()) {
+    status_.is_safe_dynamic_objects = !hasCollisionWithDynamicObjects();
   }
-  // check safety status when driving forward
-  if (parameters_->safety_check_params.enable_safety_check && status_.driving_forward) {
-    status_.is_safe_dynamic_objects = isSafePath();
-  } else {
-    status_.is_safe_dynamic_objects = true;
+}
+
+bool StartPlannerModule::hasFinishedBackwardDriving() const
+{
+  // check ego car is close enough to pull out start pose and stopped
+  const auto current_pose = planner_data_->self_odometry->pose.pose;
+  const auto distance =
+    tier4_autoware_utils::calcDistance2d(current_pose, status_.pull_out_start_pose);
+
+  const bool is_near = distance < parameters_->th_arrived_distance;
+  const double ego_vel = utils::l2Norm(planner_data_->self_odometry->twist.twist.linear);
+  const bool is_stopped = ego_vel < parameters_->th_stopped_velocity;
+
+  const bool back_finished = !status_.driving_forward && is_near && is_stopped;
+  if (back_finished) {
+    RCLCPP_INFO(getLogger(), "back finished");
   }
+
+  return back_finished;
 }
 
 bool StartPlannerModule::receivedNewRoute() const
@@ -147,6 +174,18 @@
          *planner_data_->prev_route_id != planner_data_->route_handler->getRouteUuid();
 }
 
+bool StartPlannerModule::requiresDynamicObjectsCollisionDetection() const
+{
+  return parameters_->safety_check_params.enable_safety_check && status_.driving_forward;
+}
+
+bool StartPlannerModule::hasCollisionWithDynamicObjects() const
+{
+  // TODO(Sugahara): update path, params for predicted path and so on in this function to avoid
+  // mutable
+  return !isSafePath();
+}
+
 bool StartPlannerModule::isExecutionRequested() const
 {
   if (isModuleRunning()) {
@@ -211,33 +250,50 @@
          parameters_->th_stopped_velocity;
 }
 
+bool StartPlannerModule::isStopped()
+{
+  odometry_buffer_.push_back(planner_data_->self_odometry);
+  // Delete old data in buffer
+  while (rclcpp::ok()) {
+    const auto time_diff = rclcpp::Time(odometry_buffer_.back()->header.stamp) -
+                           rclcpp::Time(odometry_buffer_.front()->header.stamp);
+    if (time_diff.seconds() < parameters_->th_stopped_time) {
+      break;
+    }
+    odometry_buffer_.pop_front();
+  }
+  return !std::any_of(
+    odometry_buffer_.begin(), odometry_buffer_.end(), [this](const auto & odometry) {
+      return utils::l2Norm(odometry->twist.twist.linear) > parameters_->th_stopped_velocity;
+    });
+}
+
 bool StartPlannerModule::isExecutionReady() const
 {
-  // when found_pull_out_path is false,the path is not generated and approval shouldn't be
-  // allowed
+  bool is_safe = true;
+
+  // Evaluate safety. The situation is not safe if any of the following conditions are met:
+  // 1. pull out path has not been found
+  // 2. waiting for approval and there is a collision with dynamic objects
   if (!status_.found_pull_out_path) {
-    RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Pull over path is not found");
-    return false;
+    is_safe = false;
   }
 
-  if (
-    parameters_->safety_check_params.enable_safety_check && status_.driving_forward &&
-    isWaitingApproval()) {
-    if (!isSafePath()) {
-      RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects");
-      stop_pose_ = planner_data_->self_odometry->pose.pose;
-      return false;
-    }
+  if (requiresDynamicObjectsCollisionDetection()) {
+    is_safe = !hasCollisionWithDynamicObjects();
   }
-  return true;
+
+  if (!is_safe) stop_pose_ = planner_data_->self_odometry->pose.pose;
+
+  return is_safe;
 }
 
 bool StartPlannerModule::canTransitSuccessState()
 {
   return hasFinishedPullOut();
 }
 
 bool StartPlannerModule::canTransitIdleToRunningState()
 {
   return isActivated() && !isWaitingApproval();
 }
@@ -461,8 +517,7 @@
 
 void StartPlannerModule::resetStatus()
 {
-  PullOutStatus initial_status;
-  status_ = initial_status;
+  status_ = PullOutStatus{};
 }
 
 void StartPlannerModule::incrementPathIndex()
@@ -722,7 +777,7 @@
   const auto pull_out_lanes = start_planner_utils::getPullOutLanes(
     planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance);
 
-  if (isBackwardDrivingComplete()) {
+  if (hasFinishedBackwardDriving()) {
     updateStatusAfterBackwardDriving();
   } else {
     status_.backward_path = start_planner_utils::getBackwardPath(
@@ -746,7 +801,7 @@
 
 PathWithLaneId StartPlannerModule::calcBackwardPathFromStartPose() const
 {
   const Pose start_pose = planner_data_->route_handler->getOriginalStartPose();
   const auto pull_out_lanes = start_planner_utils::getPullOutLanes(
     planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance);
 
@@ -865,49 +920,7 @@
 
   return has_finished;
 }
 
-bool StartPlannerModule::isBackwardDrivingComplete() const
-{
-  // check ego car is close enough to pull out start pose and stopped
-  const auto current_pose = planner_data_->self_odometry->pose.pose;
-  const auto distance =
-    tier4_autoware_utils::calcDistance2d(current_pose, status_.pull_out_start_pose);
-
-  const bool is_near = distance < parameters_->th_arrived_distance;
-  const double ego_vel = utils::l2Norm(planner_data_->self_odometry->twist.twist.linear);
-  const bool is_stopped = ego_vel < parameters_->th_stopped_velocity;
-
-  const bool back_finished = !status_.driving_forward && is_near && is_stopped;
-  if (back_finished) {
-    RCLCPP_INFO(getLogger(), "back finished");
-  }
-
-  return back_finished;
-}
-
-bool StartPlannerModule::isStopped()
-{
-  odometry_buffer_.push_back(planner_data_->self_odometry);
-  // Delete old data in buffer
-  while (rclcpp::ok()) {
-    const auto time_diff = rclcpp::Time(odometry_buffer_.back()->header.stamp) -
-                           rclcpp::Time(odometry_buffer_.front()->header.stamp);
-    if (time_diff.seconds() < parameters_->th_stopped_time) {
-      break;
-    }
-    odometry_buffer_.pop_front();
-  }
-  bool is_stopped = true;
-  for (const auto & odometry : odometry_buffer_) {
-    const double ego_vel = utils::l2Norm(odometry->twist.twist.linear);
-    if (ego_vel > parameters_->th_stopped_velocity) {
-      is_stopped = false;
-      break;
-    }
-  }
-  return is_stopped;
-}
-
 bool StartPlannerModule::isStuck()
 {
   if (!isStopped()) {
@@ -1091,14 +1104,14 @@
   utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData(
     start_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path);
 
   return behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS(
     pull_out_path, ego_predicted_path, target_objects_on_lane.on_current_lane,
     start_planner_data_.collision_check, planner_data_->parameters,
     safety_check_params_->rss_params, objects_filtering_params_->use_all_predicted_path,
     hysteresis_factor);
 }
 
 bool StartPlannerModule::isGoalBehindOfEgoInSameRouteSegment() const
 {
   const auto & rh = planner_data_->route_handler;
 
@@ -1262,8 +1275,7 @@
     add(showSafetyCheckInfo(start_planner_data_.collision_check, "object_debug_info"));
     add(showPredictedPath(start_planner_data_.collision_check, "ego_predicted_path"));
     add(showPolygon(start_planner_data_.collision_check, "ego_and_target_polygon_relation"));
-    utils::start_goal_planner_common::initializeCollisionCheckDebugMap(
-      start_planner_data_.collision_check);
+    initializeCollisionCheckDebugMap(start_planner_data_.collision_check);
   }
 
   // Visualize planner type text
@@ -1286,41 +1298,38 @@
     planner_type_marker_array.markers.push_back(marker);
     add(planner_type_marker_array);
   }
-  if (parameters_->verbose) {
-    logPullOutStatus();
-  }
 }
 
 void StartPlannerModule::logPullOutStatus(rclcpp::Logger::Level log_level) const
 {
   const auto logger = getLogger();
   auto logFunc = [&logger, log_level](const char * format, ...) {
     char buffer[1024];
     va_list args;
     va_start(args, format);
     vsnprintf(buffer, sizeof(buffer), format, args);
     va_end(args);
     switch (log_level) {
       case rclcpp::Logger::Level::Debug:
         RCLCPP_DEBUG(logger, "%s", buffer);
         break;
       case rclcpp::Logger::Level::Info:
         RCLCPP_INFO(logger, "%s", buffer);
         break;
       case rclcpp::Logger::Level::Warn:
         RCLCPP_WARN(logger, "%s", buffer);
         break;
       case rclcpp::Logger::Level::Error:
         RCLCPP_ERROR(logger, "%s", buffer);
         break;
       case rclcpp::Logger::Level::Fatal:
         RCLCPP_FATAL(logger, "%s", buffer);
         break;
       default:
         RCLCPP_INFO(logger, "%s", buffer);
         break;
     }
   };
 
   logFunc("======== PullOutStatus Report ========");
 
@@ -1348,5 +1357,5 @@
   logFunc("  ModuleStatus: %s", current_status.c_str());
 
   logFunc("=======================================");
 }
 }  // namespace behavior_path_planner