From 62896679daae2f9bda55c09acbc034c006a2e56f Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Fri, 10 Nov 2023 00:28:52 +0900 Subject: [PATCH] Refactor start planner module to update path only when enough time has passed Signed-off-by: kyoichi-sugahara --- .../start_planner/start_planner_module.hpp | 6 +- .../start_planner/start_planner_module.cpp | 74 ++++++++----------- 2 files changed, 34 insertions(+), 46 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 c2778f8b2f9f9..bd11cc5ceef51 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 @@ -128,6 +128,9 @@ class StartPlannerModule : public SceneModuleInterface void initializeSafetyCheckParameters(); + bool receivedNewRoute() const; + void planPathFromStartPose(); + bool isModuleRunning() const; bool isCurrentPoseOnMiddleOfTheRoad() const; bool isCloseToOriginalStartPose() const; @@ -174,7 +177,8 @@ class StartPlannerModule : public SceneModuleInterface PathWithLaneId getFullPath() const; PathWithLaneId calcBackwardPathFromStartPose() const; - std::vector searchPullOutStartPoses(const PathWithLaneId & back_path_from_start_pose) const; + std::vector searchPullOutStartPoseCandidates( + const PathWithLaneId & back_path_from_start_pose) const; std::shared_ptr lane_departure_checker_; 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 b5d48457c4cd0..66b8760cfbf1d 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 @@ -122,38 +122,52 @@ void StartPlannerModule::processOnExit() void StartPlannerModule::updateData() { + bool time_has_passed_from_last_path_update = true; + if (isBackwardDrivingComplete()) { updateStatusAfterBackwardDriving(); } else { 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) { + // skip updating if enough time has not passed for preventing chattering between back and + // start_planner + if (receivedNewRoute()) { status_ = PullOutStatus(); + } else { + if (!last_pull_out_start_update_time_) { + last_pull_out_start_update_time_ = std::make_unique(clock_->now()); + } + const auto elapsed_time = (clock_->now() - *last_pull_out_start_update_time_).seconds(); + if (elapsed_time < parameters_->backward_path_update_duration) { + time_has_passed_from_last_path_update = false; + } } + // TODO(Sugahara): this member variable should be updated when the path is updated + last_pull_out_start_update_time_ = std::make_unique(clock_->now()); - const auto & goal_pose = planner_data_->route_handler->getGoalPose(); + if (time_has_passed_from_last_path_update && isModuleRunning()) { + planPathFromStartPose(); + } +} +void StartPlannerModule::planPathFromStartPose() +{ + const auto & goal_pose = planner_data_->route_handler->getGoalPose(); // refine start pose with pull out lanes. // 1) backward driving is not allowed: use refined pose just as start pose. // 2) backward driving is allowed: use refined pose to check if backward driving is needed. - const PathWithLaneId back_path_from_start_pose = calcBackwardPathFromStartPose(); // start pose along the backward path aligned to the start pose const auto refined_start_pose = calcLongitudinalOffsetPose( back_path_from_start_pose.points, planner_data_->route_handler->getOriginalStartPose().position, 0.0); - if (!refined_start_pose) return; // search pull out start candidates backward const std::vector start_pose_candidates = std::invoke([&]() -> std::vector { if (parameters_->enable_back) { - return searchPullOutStartPoses(back_path_from_start_pose); + return searchPullOutStartPoseCandidates(back_path_from_start_pose); } return {*refined_start_pose}; }); @@ -162,6 +176,12 @@ void StartPlannerModule::updateData() start_pose_candidates, *refined_start_pose, goal_pose, parameters_->search_priority); } +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()) { @@ -713,46 +733,10 @@ std::vector StartPlannerModule::generateDrivableLanes( void StartPlannerModule::updatePullOutStatus() { - const bool has_received_new_route = - !planner_data_->prev_route_id || - *planner_data_->prev_route_id != planner_data_->route_handler->getRouteUuid(); - - // skip updating if enough time has not passed for preventing chattering between back and - // start_planner - if (!has_received_new_route) { - if (!last_pull_out_start_update_time_) { - last_pull_out_start_update_time_ = std::make_unique(clock_->now()); - } - const auto elapsed_time = (clock_->now() - *last_pull_out_start_update_time_).seconds(); - if (elapsed_time < parameters_->backward_path_update_duration) { - return; - } - } - last_pull_out_start_update_time_ = std::make_unique(clock_->now()); - const auto & route_handler = planner_data_->route_handler; const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto & goal_pose = planner_data_->route_handler->getGoalPose(); - // refine start pose with pull out lanes. - // 1) backward driving is not allowed: use refined pose just as start pose. - // 2) backward driving is allowed: use refined pose to check if backward driving is needed. - const PathWithLaneId start_pose_candidates_path = calcBackwardPathFromStartPose(); - const auto refined_start_pose = calcLongitudinalOffsetPose( - start_pose_candidates_path.points, planner_data_->self_odometry->pose.pose.position, 0.0); - if (!refined_start_pose) return; - - // search pull out start candidates backward - const std::vector start_pose_candidates = std::invoke([&]() -> std::vector { - if (parameters_->enable_back) { - return searchPullOutStartPoseCandidates(start_pose_candidates_path); - } - return {*refined_start_pose}; - }); - - planWithPriority( - start_pose_candidates, *refined_start_pose, goal_pose, parameters_->search_priority); - const auto & pull_out_lanes = start_planner_utils::getPullOutLanes( planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance);