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 index 4a42da9bc5ac3..3a94a18e011b9 100644 --- a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml +++ b/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 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 838cf040c3f8b..54e13de85e8da 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/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,7 @@ 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 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp index 6978a7d494f30..bd4017e5dff7d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp +++ b/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}; 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 index 46db10417d19e..fdc28139aa8f0 100644 --- 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,6 +33,7 @@ StartPlannerModuleManager::StartPlannerModuleManager( std::string ns = "start_planner."; + p.verbose = node->declare_parameter(ns + "verbose"); p.th_arrived_distance = node->declare_parameter(ns + "th_arrived_distance"); p.th_stopped_velocity = node->declare_parameter(ns + "th_stopped_velocity"); p.th_stopped_time = node->declare_parameter(ns + "th_stopped_time"); 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 index a39af7a376b02..fa539e34ab619 100644 --- 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 @@ -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