refactor(start_planner): add verbose parameter for debug print

- Added a new parameter "verbose" to the StartPlannerParameters struct.
- Updated the start_planner.param.yaml file to include the "verbose" parameter.
- Added a new method receivedNewRoute() in the StartPlannerModule class to check if a new route has been received.
- Updated the updateData() method to use the receivedNewRoute() method instead of directly checking if a new route has been received.
- Renamed the method IsGoalBehindOfEgoInSameRouteSegment() to isGoalBehindOfEgoInSameRouteSegment().
- Added a new method logPullOutStatus() to log the pull out status.
- Updated the setDebugData() method to call logPullOutStatus() if the "verbose" parameter is true.

Signed-off-by: kyoichi-sugahara <[email protected]>

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

@@ -1,6 +1,9 @@
 /**:
   ros__parameters:
     start_planner:
+
+      verbose: false
+
       th_arrived_distance: 1.0
       th_stopped_velocity: 0.01
       th_stopped_time: 1.0

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

@@ -129,6 +129,8 @@ class StartPlannerModule : public SceneModuleInterface
 
   void initializeSafetyCheckParameters();
 
+  bool receivedNewRoute() const;
+
   bool isModuleRunning() const;
   bool isCurrentPoseOnMiddleOfTheRoad() const;
   bool isCloseToOriginalStartPose() const;
@@ -207,7 +209,7 @@ class StartPlannerModule : public SceneModuleInterface
   void setDrivableAreaInfo(BehaviorModuleOutput & output) const;
 
   // check if the goal is located behind the ego in the same route segment.
-  bool IsGoalBehindOfEgoInSameRouteSegment() const;
+  bool isGoalBehindOfEgoInSameRouteSegment() const;
 
   // generate BehaviorPathOutput with stopping path and update status
   BehaviorModuleOutput generateStopOutput();
@@ -218,6 +220,8 @@ class StartPlannerModule : public SceneModuleInterface
   bool planFreespacePath();
 
   void setDebugData() const;
+  void logPullOutStatus(
+  rclcpp::Logger::Level log_level = rclcpp::Logger::Level::Info) const;
 };
 }  // namespace behavior_path_planner
 

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

@@ -46,6 +46,7 @@ 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};

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

@@ -33,6 +33,7 @@ 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");

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

@@ -128,11 +128,7 @@ void StartPlannerModule::updateData()
     status_.backward_driving_complete = false;
   }
 
-  const bool has_received_new_route =
-    !planner_data_->prev_route_id ||
-    *planner_data_->prev_route_id != planner_data_->route_handler->getRouteUuid();
-
-  if (has_received_new_route) {
+  if (receivedNewRoute()) {
     status_ = PullOutStatus();
   }
   // check safety status when driving forward
@@ -143,6 +139,12 @@ void StartPlannerModule::updateData()
   }
 }
 
+bool StartPlannerModule::receivedNewRoute() const
+{
+  return !planner_data_->prev_route_id ||
+         *planner_data_->prev_route_id != planner_data_->route_handler->getRouteUuid();
+}
+
 bool StartPlannerModule::isExecutionRequested() const
 {
   if (isModuleRunning()) {
@@ -161,7 +163,7 @@ bool StartPlannerModule::isExecutionRequested() const
   }
 
   // Check if the goal is behind the ego vehicle within the same route segment.
-  if (IsGoalBehindOfEgoInSameRouteSegment()) {
+  if (isGoalBehindOfEgoInSameRouteSegment()) {
     RCLCPP_WARN_THROTTLE(
       getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now");
     return false;
@@ -1118,7 +1120,7 @@ bool StartPlannerModule::isSafePath() const
   return is_safe_dynamic_objects;
 }
 
-bool StartPlannerModule::IsGoalBehindOfEgoInSameRouteSegment() const
+bool StartPlannerModule::isGoalBehindOfEgoInSameRouteSegment() const
 {
   const auto & rh = planner_data_->route_handler;
 
@@ -1306,5 +1308,66 @@ 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
+{
+  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 ========");
+
+  logFunc("[Path Info]");
+  logFunc("  Current Path Index: %zu", status_.current_path_idx);
+
+  logFunc("[Planner Info]");
+  logFunc("  Planner Type: %s", magic_enum::enum_name(status_.planner_type).data());
+
+  logFunc("[Safety and Direction Info]");
+  logFunc("  Found Pull Out Path: %s", status_.found_pull_out_path ? "true" : "false");
+  logFunc(
+    "  Is Safe Against Dynamic Objects: %s", status_.is_safe_dynamic_objects ? "true" : "false");
+  logFunc(
+    "  Previous Is Safe Dynamic Objects: %s",
+    status_.prev_is_safe_dynamic_objects ? "true" : "false");
+  logFunc("  Driving Forward: %s", status_.driving_forward ? "true" : "false");
+  logFunc("  Backward Driving Complete: %s", status_.backward_driving_complete ? "true" : "false");
+  logFunc("  Has Stop Point: %s", status_.has_stop_point ? "true" : "false");
+
+  logFunc("[Module State]");
+  logFunc("  isActivated: %s", isActivated() ? "true" : "false");
+  logFunc("  isWaitingForApproval: %s", isWaitingApproval() ? "true" : "false");
+  logFunc("  ModuleStatus: %s", magic_enum::enum_name(getCurrentStatus()));
+
+  logFunc("=======================================");
 }
 }  // namespace behavior_path_planner