diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 8dc90ba3bfac8..3e2f36dda4c65 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -583,7 +583,12 @@ bool StartPlannerModule::canTransitSuccessState() // - Can transit to success if the goal position is reached. // - Cannot transit to success if the goal position is not reached. if (status_.planner_type == PlannerType::FREESPACE) { - return hasReachedFreespaceEnd(); + if (hasReachedFreespaceEnd()) { + RCLCPP_DEBUG( + getLogger(), "Transit to success: Freespace planner reached the end point of the path."); + return true; + } + return false; } // Other Planners: @@ -597,7 +602,12 @@ bool StartPlannerModule::canTransitSuccessState() return false; } - return hasReachedPullOutEnd(); + if (hasReachedPullOutEnd()) { + RCLCPP_DEBUG(getLogger(), "Transit to success: Reached the end point of the pullout path."); + return true; + } + + return false; } BehaviorModuleOutput StartPlannerModule::plan()