From 6d467e1033be71c8619fbf678ce906a2bdac4657 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Fri, 17 Nov 2023 19:04:47 +0900 Subject: [PATCH 1/2] 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 --- .../start_planner/start_planner.param.yaml | 3 + .../start_planner/start_planner_module.hpp | 6 +- .../start_planner_parameters.hpp | 1 + .../scene_module/start_planner/manager.cpp | 1 + .../start_planner/start_planner_module.cpp | 77 +++++++++++++++++-- 5 files changed, 80 insertions(+), 8 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 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..92f4d1312dd80 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,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 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 From 09fab36e996c33293ab0ace21bc664523b842a53 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 17 Nov 2023 10:16:28 +0000 Subject: [PATCH 2/2] style(pre-commit): autofix --- .../scene_module/start_planner/start_planner_module.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) 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 92f4d1312dd80..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 @@ -220,8 +220,7 @@ class StartPlannerModule : public SceneModuleInterface bool planFreespacePath(); void setDebugData() const; - void logPullOutStatus( - rclcpp::Logger::Level log_level = rclcpp::Logger::Level::Info) const; + void logPullOutStatus(rclcpp::Logger::Level log_level = rclcpp::Logger::Level::Info) const; }; } // namespace behavior_path_planner