From ed21bac339d4d3b9e7805f5d71434ef3cbb1b698 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 7 Mar 2024 17:13:05 +0900 Subject: [PATCH] fix(start_planner): consider backward completion before preparing blinker (#6558) consider backward completion Signed-off-by: Mamoru Sobue --- .../start_planner_module.hpp | 7 ++++--- .../src/start_planner_module.cpp | 18 ++++++++++-------- 2 files changed, 14 insertions(+), 11 deletions(-) diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index 6014e5b8f1fdc..14846a05a0929 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -70,8 +70,9 @@ struct PullOutStatus bool prev_is_safe_dynamic_objects{false}; std::shared_ptr prev_stop_path_after_approval{nullptr}; std::optional stop_pose{std::nullopt}; - //! record the first time when the state changed from !isActivated() to isActivated() - std::optional first_engaged_time{std::nullopt}; + //! record the first time when ego started forward-driving (maybe after backward driving + //! completion) in AUTONOMOUS operation mode + std::optional first_engaged_and_driving_forward_time{std::nullopt}; PullOutStatus() {} }; @@ -279,7 +280,7 @@ class StartPlannerModule : public SceneModuleInterface const lanelet::ConstLanelets & pull_out_lanes, const geometry_msgs::msg::Point & current_pose, const double velocity_threshold, const double object_check_backward_distance, const double object_check_forward_distance) const; - bool needToPrepareBlinkerBeforeStart() const; + bool needToPrepareBlinkerBeforeStartDrivingForward() const; bool hasFinishedPullOut() const; bool hasFinishedBackwardDriving() const; bool hasCollisionWithDynamicObjects() const; 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 88a33d05f4ea7..bb47182d953cc 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 @@ -119,7 +119,7 @@ void StartPlannerModule::onFreespacePlannerTimer() BehaviorModuleOutput StartPlannerModule::run() { updateData(); - if (!isActivated() || needToPrepareBlinkerBeforeStart()) { + if (!isActivated() || needToPrepareBlinkerBeforeStartDrivingForward()) { return planWaitingApproval(); } @@ -179,8 +179,8 @@ void StartPlannerModule::updateData() if ( planner_data_->operation_mode->mode == OperationModeState::AUTONOMOUS && - !status_.first_engaged_time) { - status_.first_engaged_time = clock_->now(); + status_.driving_forward && !status_.first_engaged_and_driving_forward_time) { + status_.first_engaged_and_driving_forward_time = clock_->now(); } status_.backward_driving_complete = hasFinishedBackwardDriving(); @@ -1179,13 +1179,15 @@ bool StartPlannerModule::hasFinishedPullOut() const return has_finished; } -bool StartPlannerModule::needToPrepareBlinkerBeforeStart() const +bool StartPlannerModule::needToPrepareBlinkerBeforeStartDrivingForward() const { - if (!status_.first_engaged_time) { - return true; + if (!status_.first_engaged_and_driving_forward_time) { + return false; } - const auto first_engaged_time = status_.first_engaged_time.value(); - const double elapsed = rclcpp::Duration(clock_->now() - first_engaged_time).seconds(); + const auto first_engaged_and_driving_forward_time = + status_.first_engaged_and_driving_forward_time.value(); + const double elapsed = + rclcpp::Duration(clock_->now() - first_engaged_and_driving_forward_time).seconds(); return elapsed < parameters_->prepare_time_before_start; }