From b3315725a20f8aded830e9fab58f7e50c10e1f11 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Tue, 6 Aug 2024 17:28:10 +0900 Subject: [PATCH 1/4] do not cancel if ego vehicle approaching terminal start Signed-off-by: Muhammad Zulfaqar Azmi --- .../scene.hpp | 2 ++ .../utils/base_class.hpp | 2 ++ .../utils/calculation.hpp | 18 +++++++++++++ .../src/interface.cpp | 5 ++++ .../src/scene.cpp | 25 +++++++++++++++++++ .../src/utils/calculation.cpp | 12 +++++++++ 6 files changed, 64 insertions(+) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index cc695f820ee38..3a11b0ed903dc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -92,6 +92,8 @@ class NormalLaneChange : public LaneChangeBase bool isAbleToReturnCurrentLane() const override; + bool is_near_terminal() const final; + bool isEgoOnPreparePhase() const override; bool isAbleToStopSafely() const override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index ccc9258324ffa..65f57fa583817 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp @@ -93,6 +93,8 @@ class LaneChangeBase virtual bool isAbleToReturnCurrentLane() const = 0; + virtual bool is_near_terminal() const = 0; + virtual LaneChangePath getLaneChangePath() const = 0; virtual BehaviorModuleOutput getTerminalLaneChangePath() const = 0; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp index 421b54db9f67a..2e2456faa8760 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp @@ -19,6 +19,7 @@ namespace autoware::behavior_path_planner::utils::lane_change::calculation { using behavior_path_planner::lane_change::CommonDataPtr; +using behavior_path_planner::lane_change::LCParamPtr; /** * @brief Calculates the distance from the ego vehicle to the terminal point. @@ -40,6 +41,23 @@ double calc_ego_dist_to_terminal_end(const CommonDataPtr & common_data_ptr); double calc_dist_from_pose_to_terminal_end( const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes, const Pose & src_pose); + +/** + * @brief Calculates the backward buffer distance required for safe lane changing. + * + * This function computes the minimum required backward buffer distance based on the + * minimum lane changing velocity and the minimum longitudinal acceleration. It then + * compares this calculated distance with a pre-defined backward length buffer parameter + * and returns the larger of the two values to ensure safe lane changing. + * + * @param lc_param_ptr Shared pointer to an LCParam structure, which should include: + * - `minimum_lane_changing_velocity`: The minimum velocity required for lane changing. + * - `min_longitudinal_acc`: The minimum longitudinal acceleration used for deceleration. + * - `backward_length_buffer_for_end_of_lane`: A predefined backward buffer length parameter. + * + * @return The required backward buffer distance in meters. + */ +double calc_backward_buffer(const LCParamPtr & lc_param_ptr); } // namespace autoware::behavior_path_planner::utils::lane_change::calculation #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index afbfc67caa2f7..02262c1744402 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -265,6 +265,11 @@ bool LaneChangeInterface::canTransitFailureState() return true; } + if (module_type_->is_near_terminal()) { + log_debug_throttled("Unsafe, but ego is approaching terminal. Continue lane change"); + return false; + } + if (module_type_->isCancelEnabled() && module_type_->isEgoOnPreparePhase()) { if (module_type_->isStoppedAtRedTrafficLight()) { log_debug_throttled("Stopping at traffic light while in prepare phase. Cancel lane change"); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 11e1c20e57adb..745603f90d8eb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -768,6 +768,31 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const return true; } +bool NormalLaneChange::is_near_terminal() const +{ + const auto & current_lanes = common_data_ptr_->lanes_ptr->current; + + if (current_lanes.empty()) { + return true; + } + + const auto & current_lanes_terminal = current_lanes.back(); + const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr; + const auto direction = common_data_ptr_->direction; + const auto & route_handler_ptr = common_data_ptr_->route_handler_ptr; + const auto min_lane_changing_distance = calcMinimumLaneChangeLength( + route_handler_ptr, current_lanes_terminal, *lc_param_ptr, direction); + + const auto backward_buffer = calculation::calc_backward_buffer(lc_param_ptr); + + const auto min_lc_dist_with_buffer = + backward_buffer + min_lane_changing_distance + lc_param_ptr->lane_change_finish_judge_buffer; + const auto dist_from_ego_to_terminal_end = + calculation::calc_ego_dist_to_terminal_end(common_data_ptr_); + + return dist_from_ego_to_terminal_end < min_lc_dist_with_buffer; +} + bool NormalLaneChange::isEgoOnPreparePhase() const { const auto & start_position = status_.lane_change_path.info.shift_line.start.position; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp index 521c30d406e7a..e20a843f16f34 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -1,3 +1,4 @@ + // Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -42,4 +43,15 @@ double calc_dist_from_pose_to_terminal_end( } return utils::getDistanceToEndOfLane(src_pose, lanes); } + +double calc_backward_buffer(const LCParamPtr & lc_param_ptr) +{ + // v^2 = u^2 + 2ad + const auto min_lc_vel = lc_param_ptr->minimum_lane_changing_velocity; + const auto min_lon_acc = lc_param_ptr->min_longitudinal_acc; + const auto min_back_dist = std::abs((min_lc_vel * min_lc_vel) / (2 * min_lon_acc)); + + const auto param_back_dist = lc_param_ptr->backward_length_buffer_for_end_of_lane; + return std::max(min_back_dist, param_back_dist); +} } // namespace autoware::behavior_path_planner::utils::lane_change::calculation From f5210832ecc5a13876e7c70523ca2588e941c0d0 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Tue, 6 Aug 2024 17:39:58 +0900 Subject: [PATCH 2/4] Insert stop point if object is coming from rear Signed-off-by: Muhammad Zulfaqar Azmi --- .../src/interface.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index 02262c1744402..8d5cde234dea6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -295,6 +295,11 @@ bool LaneChangeInterface::canTransitFailureState() if (post_process_safety_status_.is_safe) { log_debug_throttled("Can't transit to failure state. Ego is on prepare, and it's safe."); + + if (module_type_->isRequiredStop(post_process_safety_status_.is_object_coming_from_rear)) { + log_debug_throttled("Module require stopping"); + } + return false; } From 3ba45e14283f4d371bfc1f5e20914728dfe45268 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Tue, 13 Aug 2024 13:29:51 +0900 Subject: [PATCH 3/4] minor edit to fix conflict Signed-off-by: Muhammad Zulfaqar Azmi --- .../src/interface.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index 8d5cde234dea6..6451d6abadfa2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -267,6 +267,10 @@ bool LaneChangeInterface::canTransitFailureState() if (module_type_->is_near_terminal()) { log_debug_throttled("Unsafe, but ego is approaching terminal. Continue lane change"); + + if (module_type_->isRequiredStop(post_process_safety_status_.is_trailing_object)) { + log_debug_throttled("Module require stopping"); + } return false; } @@ -295,11 +299,6 @@ bool LaneChangeInterface::canTransitFailureState() if (post_process_safety_status_.is_safe) { log_debug_throttled("Can't transit to failure state. Ego is on prepare, and it's safe."); - - if (module_type_->isRequiredStop(post_process_safety_status_.is_object_coming_from_rear)) { - log_debug_throttled("Module require stopping"); - } - return false; } From 9b59c68546df073caf6447acfd6337442129aeab Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Wed, 14 Aug 2024 16:27:40 +0900 Subject: [PATCH 4/4] rename function Signed-off-by: Zulfaqar Azmi --- .../behavior_path_lane_change_module/utils/calculation.hpp | 6 +++--- .../autoware_behavior_path_lane_change_module/src/scene.cpp | 2 +- .../src/utils/calculation.cpp | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp index 2e2456faa8760..3dc5e7ee62a57 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp @@ -43,9 +43,9 @@ double calc_dist_from_pose_to_terminal_end( const Pose & src_pose); /** - * @brief Calculates the backward buffer distance required for safe lane changing. + * @brief Calculates the minimum stopping distance to terminal start. * - * This function computes the minimum required backward buffer distance based on the + * This function computes the minimum stopping distance to terminal start based on the * minimum lane changing velocity and the minimum longitudinal acceleration. It then * compares this calculated distance with a pre-defined backward length buffer parameter * and returns the larger of the two values to ensure safe lane changing. @@ -57,7 +57,7 @@ double calc_dist_from_pose_to_terminal_end( * * @return The required backward buffer distance in meters. */ -double calc_backward_buffer(const LCParamPtr & lc_param_ptr); +double calc_stopping_distance(const LCParamPtr & lc_param_ptr); } // namespace autoware::behavior_path_planner::utils::lane_change::calculation #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 745603f90d8eb..9b097a2308b2d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -783,7 +783,7 @@ bool NormalLaneChange::is_near_terminal() const const auto min_lane_changing_distance = calcMinimumLaneChangeLength( route_handler_ptr, current_lanes_terminal, *lc_param_ptr, direction); - const auto backward_buffer = calculation::calc_backward_buffer(lc_param_ptr); + const auto backward_buffer = calculation::calc_stopping_distance(lc_param_ptr); const auto min_lc_dist_with_buffer = backward_buffer + min_lane_changing_distance + lc_param_ptr->lane_change_finish_judge_buffer; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp index e20a843f16f34..ac205ceedb34b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -44,7 +44,7 @@ double calc_dist_from_pose_to_terminal_end( return utils::getDistanceToEndOfLane(src_pose, lanes); } -double calc_backward_buffer(const LCParamPtr & lc_param_ptr) +double calc_stopping_distance(const LCParamPtr & lc_param_ptr) { // v^2 = u^2 + 2ad const auto min_lc_vel = lc_param_ptr->minimum_lane_changing_velocity;