From e6fbf9ea81125109fa39a374b9d7b74612e63673 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 20 Feb 2024 16:24:35 +0900 Subject: [PATCH 1/3] feat(strat_planner): add a prepare time for blinker before taking action for approval (#6438) Signed-off-by: Mamoru Sobue --- .../config/start_planner.param.yaml | 1 + .../start_planner_module.hpp | 4 ++++ .../start_planner_parameters.hpp | 1 + .../src/manager.cpp | 1 + .../src/start_planner_module.cpp | 16 +++++++++++++++- 5 files changed, 22 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml index 514d61e225ecd..4904be536ee95 100644 --- a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml @@ -36,6 +36,7 @@ backward_path_update_duration: 3.0 ignore_distance_from_lane_end: 15.0 # turns signal + prepare_time_before_start: 0.0 th_turn_signal_on_lateral_offset: 1.0 intersection_search_length: 30.0 length_ratio_for_turn_signal_deactivation_near_intersection: 0.5 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 a143f34ead649..641907bbb5ed9 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 @@ -69,6 +69,9 @@ struct PullOutStatus bool prev_is_safe_dynamic_objects{false}; std::shared_ptr prev_stop_path_after_approval{nullptr}; bool has_stop_point{false}; + std::optional stop_pose{std::nullopt}; + //! record the first time when the state changed from !isActivated() to isActivated() + std::optional first_approved_time{std::nullopt}; PullOutStatus() {} }; @@ -228,6 +231,7 @@ class StartPlannerModule : public SceneModuleInterface void updateStatusAfterBackwardDriving(); PredictedObjects filterStopObjectsInPullOutLanes( const lanelet::ConstLanelets & pull_out_lanes, const double velocity_threshold) const; + bool needToPrepareBlinkerBeforeStart() const; bool hasFinishedPullOut() const; bool hasFinishedBackwardDriving() const; bool hasCollisionWithDynamicObjects() const; diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp index 783aace0983ca..c50648ecd3731 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp @@ -38,6 +38,7 @@ struct StartPlannerParameters double th_arrived_distance{0.0}; double th_stopped_velocity{0.0}; double th_stopped_time{0.0}; + double prepare_time_before_start{0.0}; double th_turn_signal_on_lateral_offset{0.0}; double th_distance_to_middle_of_the_road{0.0}; double intersection_search_length{0.0}; diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index 58d0fbee7921b..c8c12363921b1 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -36,6 +36,7 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) 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"); + p.prepare_time_before_start = node->declare_parameter(ns + "prepare_time_before_start"); p.th_turn_signal_on_lateral_offset = node->declare_parameter(ns + "th_turn_signal_on_lateral_offset"); p.th_distance_to_middle_of_the_road = 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 c9fa15d855268..b6da9acee05de 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 @@ -104,7 +104,7 @@ void StartPlannerModule::onFreespacePlannerTimer() BehaviorModuleOutput StartPlannerModule::run() { updateData(); - if (!isActivated()) { + if (!isActivated() || needToPrepareBlinkerBeforeStart()) { return planWaitingApproval(); } @@ -161,6 +161,10 @@ void StartPlannerModule::updateData() DEBUG_PRINT("StartPlannerModule::updateData() received new route, reset status"); } + if (!status_.first_approved_time && isActivated()) { + status_.first_approved_time = clock_->now(); + } + if (hasFinishedBackwardDriving()) { updateStatusAfterBackwardDriving(); DEBUG_PRINT("StartPlannerModule::updateData() completed backward driving"); @@ -962,6 +966,16 @@ bool StartPlannerModule::hasFinishedPullOut() const return has_finished; } +bool StartPlannerModule::needToPrepareBlinkerBeforeStart() const +{ + if (!status_.first_approved_time) { + return true; + } + const auto first_approved_time = status_.first_approved_time.value(); + const double elapsed = rclcpp::Duration(clock_->now() - first_approved_time).seconds(); + return elapsed < parameters_->prepare_time_before_start; +} + bool StartPlannerModule::isStuck() { if (!isStopped()) { From df33eb0273d134891540f1f25deeeb7bc1b7cd13 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 26 Feb 2024 22:12:33 +0900 Subject: [PATCH 2/3] fix(start_planner): start prepare blinker when autonomous mode is available (#6470) Signed-off-by: Mamoru Sobue --- .../start_planner_module.hpp | 2 +- .../src/start_planner_module.cpp | 12 +++++++----- 2 files changed, 8 insertions(+), 6 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 641907bbb5ed9..eafe8d02617f7 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 @@ -71,7 +71,7 @@ struct PullOutStatus bool has_stop_point{false}; std::optional stop_pose{std::nullopt}; //! record the first time when the state changed from !isActivated() to isActivated() - std::optional first_approved_time{std::nullopt}; + std::optional first_engaged_time{std::nullopt}; PullOutStatus() {} }; 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 b6da9acee05de..338082e7d3c01 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 @@ -161,8 +161,10 @@ void StartPlannerModule::updateData() DEBUG_PRINT("StartPlannerModule::updateData() received new route, reset status"); } - if (!status_.first_approved_time && isActivated()) { - status_.first_approved_time = clock_->now(); + if ( + planner_data_->operation_mode->mode == OperationModeState::AUTONOMOUS && + !status_.first_engaged_time) { + status_.first_engaged_time = clock_->now(); } if (hasFinishedBackwardDriving()) { @@ -968,11 +970,11 @@ bool StartPlannerModule::hasFinishedPullOut() const bool StartPlannerModule::needToPrepareBlinkerBeforeStart() const { - if (!status_.first_approved_time) { + if (!status_.first_engaged_time) { return true; } - const auto first_approved_time = status_.first_approved_time.value(); - const double elapsed = rclcpp::Duration(clock_->now() - first_approved_time).seconds(); + const auto first_engaged_time = status_.first_engaged_time.value(); + const double elapsed = rclcpp::Duration(clock_->now() - first_engaged_time).seconds(); return elapsed < parameters_->prepare_time_before_start; } From 45dbbbdf5ffd058830490f9a4e623c66f6ca96da Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 7 Mar 2024 17:13:05 +0900 Subject: [PATCH 3/3] 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 eafe8d02617f7..bf5a64b4d0973 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 std::shared_ptr prev_stop_path_after_approval{nullptr}; bool has_stop_point{false}; 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() {} }; @@ -231,7 +232,7 @@ class StartPlannerModule : public SceneModuleInterface void updateStatusAfterBackwardDriving(); PredictedObjects filterStopObjectsInPullOutLanes( const lanelet::ConstLanelets & pull_out_lanes, const double velocity_threshold) 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 338082e7d3c01..1eb5aa9191a5c 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 @@ -104,7 +104,7 @@ void StartPlannerModule::onFreespacePlannerTimer() BehaviorModuleOutput StartPlannerModule::run() { updateData(); - if (!isActivated() || needToPrepareBlinkerBeforeStart()) { + if (!isActivated() || needToPrepareBlinkerBeforeStartDrivingForward()) { return planWaitingApproval(); } @@ -163,8 +163,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(); } if (hasFinishedBackwardDriving()) { @@ -968,13 +968,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; }