refactor(start_planner): refactor debug and safety check logic

This commit refactors the start_planner module in several ways: - The verbose parameter is removed from the YAML configuration file and replaced with a print_debug_info parameter under a new debug section. - A new function, initVariables(), is introduced to initialize member variables, which is called during processOnEntry() and processOnExit(). - The function isBackwardDrivingComplete() is renamed to hasFinishedBackwardDriving() to better reflect its purpose. It also includes additional debug prints. - A new function, checkCollisionWithDynamicObjects(), is introduced to encapsulate the logic for checking collisions with dynamic objects. - The function isExecutionReady() is modified to use the new checkCollisionWithDynamicObjects() function. - The function resetStatus() is simplified by directly assigning an empty PullOutStatus object to status_. - The function updatePullOutStatus() now uses the new hasFinishedBackwardDriving() function. - The logging of pull out status is now controlled by the new print_debug_info parameter rather than the removed verbose parameter. These changes improve the clarity of the code and make it easier to control debug output. Signed-off-by: kyoichi-sugahara <[email protected]>
autowarefoundation · Dec 1, 2023 · e8f31b7 · e8f31b7
1 parent ebba4f7
commit e8f31b7
Show file tree

Hide file tree

Showing 5 changed files with 96 additions and 88 deletions.
diff --git a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml
@@ -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
@@ -140,3 +138,7 @@
           # temporary
           backward_path_length: 30.0
           forward_path_length: 100.0
+
+      # debug
+      debug:
+        print_debug_info: false
diff --git a/...planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/...planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp
@@ -129,6 +129,11 @@ class StartPlannerModule : public SceneModuleInterface
 
   bool canTransitIdleToRunningState() override;
 
+  /**
+   * @brief init member variables.
+   */
+  void initVariables();
+
   void initializeSafetyCheckParameters();
 
   bool receivedNewRoute() const;
@@ -200,7 +205,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 checkCollisionWithDynamicObjects() const;
   bool isStopped();
   bool isStuck();
   bool hasFinishedCurrentPath();

diff --git a/...th_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp b/...th_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

diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp
@@ -33,7 +33,6 @@ StartPlannerModuleManager::StartPlannerModuleManager(
 
   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");
@@ -276,6 +275,12 @@ StartPlannerModuleManager::StartPlannerModuleManager(
       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(

diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp
@@ -38,6 +38,12 @@
 
 using motion_utils::calcLongitudinalOffsetPose;
 using tier4_autoware_utils::calcOffsetPose;
+using utils::start_goal_planner_common::initializeCollisionCheckDebugMap;
+
+// 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
 {
@@ -107,38 +113,57 @@ BehaviorModuleOutput StartPlannerModule::run()
 
 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();
-  }
-  // 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;
+  status_.is_safe_dynamic_objects = checkCollisionWithDynamicObjects();
+}
+
+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 +172,12 @@ bool StartPlannerModule::receivedNewRoute() const
          *planner_data_->prev_route_id != planner_data_->route_handler->getRouteUuid();
 }
 
+bool StartPlannerModule::checkCollisionWithDynamicObjects() const
+{
+  return !(parameters_->safety_check_params.enable_safety_check && status_.driving_forward) ||
+         isSafePath();
+}
+
 bool StartPlannerModule::isExecutionRequested() const
 {
   if (isModuleRunning()) {
@@ -211,25 +242,35 @@ bool StartPlannerModule::isMoving() const
          parameters_->th_stopped_velocity;
 }
 
-bool StartPlannerModule::isExecutionReady() const
+bool StartPlannerModule::isStopped()
 {
-  // when found_pull_out_path is false,the path is not generated and approval shouldn't be
-  // allowed
-  if (!status_.found_pull_out_path) {
-    RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Pull over path is not found");
-    return 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;
+  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 true;
+  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
+{
+  // 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
+  const bool is_safe =
+    status_.found_pull_out_path && (!isWaitingApproval() || checkCollisionWithDynamicObjects());
+
+  if (!is_safe) stop_pose_ = planner_data_->self_odometry->pose.pose;
+
+  return is_safe;
 }
 
 bool StartPlannerModule::canTransitSuccessState()
@@ -461,8 +502,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
 
 void StartPlannerModule::resetStatus()
 {
-  PullOutStatus initial_status;
-  status_ = initial_status;
+  status_ = PullOutStatus{};
 }
 
 void StartPlannerModule::incrementPathIndex()
@@ -722,7 +762,7 @@ void StartPlannerModule::updatePullOutStatus()
   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(
@@ -866,48 +906,6 @@ bool StartPlannerModule::hasFinishedPullOut() const
   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()) {
@@ -1262,8 +1260,7 @@ void StartPlannerModule::setDebugData() const
     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,9 +1283,6 @@ void StartPlannerModule::setDebugData() const
     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