refactor(start_planner): refactor debug and safety check logic (#5747)

* 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]> --------- Signed-off-by: kyoichi-sugahara <[email protected]>
autowarefoundation · Dec 5, 2023 · 7b11633 · 7b11633
1 parent c6788cf
commit 7b11633
Show file tree

Hide file tree

Showing 9 changed files with 114 additions and 96 deletions.
diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/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

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
@@ -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
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,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();

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/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp
@@ -256,9 +256,7 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node)
     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 =

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 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
 
   const 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");
@@ -172,9 +171,7 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
     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 =
@@ -276,6 +273,12 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
       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
@@ -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 @@ 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();
+  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 @@ bool StartPlannerModule::receivedNewRoute() const
          *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,25 +250,42 @@ bool StartPlannerModule::isMoving() const
          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()
@@ -461,8 +517,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
 
 void StartPlannerModule::resetStatus()
 {
-  PullOutStatus initial_status;
-  status_ = initial_status;
+  status_ = PullOutStatus{};
 }
 
 void StartPlannerModule::incrementPathIndex()
@@ -722,7 +777,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 +921,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 +1275,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 +1298,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