Planning: enable efficient lane-change when surrounding is clear

qxdxx · Nov 20, 2019 · 6402a65 · 6402a65
1 parent 638a217
commit 6402a65
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 0 deletions.
diff --git a/modules/planning/proto/planning_status.proto b/modules/planning/proto/planning_status.proto
@@ -38,6 +38,9 @@ message ChangeLaneStatus {
   // if the current path and speed planning on the lane-change
   // reference-line succeed.
   optional bool is_current_opt_succeed = 7 [default = false];
+  // denotes if the surrounding area is clear for ego vehicle to
+  // change lane at this moment.
+  optional bool is_clear_to_change_lane = 8 [default = false];
 }
 
 message StopTime {

diff --git a/modules/planning/tasks/deciders/lane_change_decider/lane_change_decider.cc b/modules/planning/tasks/deciders/lane_change_decider/lane_change_decider.cc
@@ -57,6 +57,12 @@ Status LaneChangeDecider::Process(
                           ->mutable_change_lane();
   double now = Clock::NowInSeconds();
 
+  prev_status->set_is_clear_to_change_lane(false);
+  if (current_reference_line_info->IsChangeLanePath()) {
+    prev_status->set_is_clear_to_change_lane(
+        IsClearToChangeLane(current_reference_line_info));
+  }
+
   if (!prev_status->has_status()) {
     UpdateStatus(now, ChangeLaneStatus::CHANGE_LANE_FINISHED,
                  GetCurrentPathId(*reference_line_info));

diff --git a/modules/planning/tasks/deciders/path_bounds_decider/path_bounds_decider.cc b/modules/planning/tasks/deciders/path_bounds_decider/path_bounds_decider.cc
@@ -1185,6 +1185,11 @@ void PathBoundsDecider::GetBoundaryFromLaneChangeForbiddenZone(
   auto* lane_change_status = PlanningContext::Instance()
                                  ->mutable_planning_status()
                                  ->mutable_change_lane();
+  if (lane_change_status->is_clear_to_change_lane()) {
+    ADEBUG << "Current position is clear to change lane. No need prep s.";
+    lane_change_status->set_exist_lane_change_start_position(false);
+    return;
+  }
   double lane_change_start_s = 0.0;
   if (lane_change_status->exist_lane_change_start_position()) {
     common::SLPoint point_sl;