From 89db33569a0b9a5e01c752b69dc3f4014db1be15 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 18 Oct 2023 21:38:00 +0900 Subject: [PATCH] feat(lane_change): disable cancel when ego is out of current lanes (#5348) --- .../src/scene_module/lane_change/interface.cpp | 2 +- .../src/scene_module/lane_change/normal.cpp | 6 ++++++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 51875952d40fb..0a0cc3436bce7 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -147,7 +147,7 @@ ModuleStatus LaneChangeInterface::updateState() return ModuleStatus::RUNNING; } - if (module_type_->isEgoOnPreparePhase()) { + if (module_type_->isEgoOnPreparePhase() && module_type_->isAbleToReturnCurrentLane()) { RCLCPP_WARN_STREAM_THROTTLE( getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000, "Lane change path is unsafe. Cancel lane change."); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index ce07cda882692..de4afc9260da0 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -510,6 +510,12 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const return false; } + if (!utils::isEgoWithinOriginalLane( + status_.current_lanes, getEgoPose(), planner_data_->parameters, + lane_change_parameters_->cancel.overhang_tolerance)) { + return false; + } + const auto nearest_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( status_.lane_change_path.path.points, getEgoPose(), planner_data_->parameters.ego_nearest_dist_threshold,