feat(lane_change): disable cancel when ego is out of current lanes (a…

…utowarefoundation#5348)
zulfaqar-azmi-t4 · Oct 18, 2023 · 89db335 · 89db335
1 parent 8530b6b
commit 89db335
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 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
@@ -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
@@ -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,