planning: fix reference line drivable bug

modules/planning/common/trajectory/discretized_trajectory.h

@@ -15,7 +15,7 @@
  *****************************************************************************/
 
 /**
- * @file discretized_trajectory.h
+ * @file
  **/
 
 #ifndef MODULES_PLANNING_COMMON_TRAJECTORY_DISCRETIZED_TRAJECTORY_H_

modules/planning/planner/em/em_planner.cc

@@ -142,9 +142,13 @@ void EMPlanner::RecordDebugInfo(ReferenceLineInfo* reference_line_info,
 Status EMPlanner::Plan(const TrajectoryPoint& planning_start_point,
                        Frame* frame) {
   bool has_drivable_reference_line = false;
+  bool disable_low_priority_path = false;
   auto status =
       Status(ErrorCode::PLANNING_ERROR, "reference line not drivable");
   for (auto& reference_line_info : frame->reference_line_info()) {
+    if (disable_low_priority_path) {
+      reference_line_info.SetDrivable(false);
+    }
     if (!reference_line_info.IsDrivable()) {
       continue;
     }
@@ -154,7 +158,7 @@ Status EMPlanner::Plan(const TrajectoryPoint& planning_start_point,
       has_drivable_reference_line = true;
       if (FLAGS_prioritize_change_lane &&
           reference_line_info.IsChangeLanePath()) {
-        break;
+        disable_low_priority_path = true;
       }
     } else {
       reference_line_info.SetDrivable(false);

modules/planning/planning.cc

@@ -411,8 +411,8 @@ Status Planning::Plan(const double current_time_stamp,
 
   ExportReferenceLineDebug(ptr_debug);
 
-  const auto* best_reference_line = frame_->FindDriveReferenceLineInfo();
-  if (!best_reference_line) {
+  const auto* best_ref_info = frame_->FindDriveReferenceLineInfo();
+  if (!best_ref_info) {
     std::string msg("planner failed to make a driving plan");
     AERROR << msg;
     if (last_publishable_trajectory_) {
@@ -420,24 +420,23 @@ Status Planning::Plan(const double current_time_stamp,
     }
     return Status(ErrorCode::PLANNING_ERROR, msg);
   }
-  ptr_debug->MergeFrom(best_reference_line->debug());
+  ptr_debug->MergeFrom(best_ref_info->debug());
   trajectory_pb->mutable_latency_stats()->MergeFrom(
-      best_reference_line->latency_stats());
+      best_ref_info->latency_stats());
   // set right of way status
-  trajectory_pb->set_right_of_way_status(
-      best_reference_line->GetRightOfWayStatus());
-  for (const auto& id : best_reference_line->TargetLaneId()) {
+  trajectory_pb->set_right_of_way_status(best_ref_info->GetRightOfWayStatus());
+  for (const auto& id : best_ref_info->TargetLaneId()) {
     trajectory_pb->add_lane_id()->CopyFrom(id);
   }
 
-  best_reference_line->ExportDecision(trajectory_pb->mutable_decision());
+  best_ref_info->ExportDecision(trajectory_pb->mutable_decision());
 
   // Add debug information.
   if (FLAGS_enable_record_debug) {
     auto* reference_line = ptr_debug->mutable_planning_data()->add_path();
     reference_line->set_name("planning_reference_line");
     const auto& reference_points =
-        best_reference_line->reference_line().reference_points();
+        best_ref_info->reference_line().reference_points();
     double s = 0.0;
     double prev_x = 0.0;
     double prev_y = 0.0;
@@ -464,7 +463,7 @@ Status Planning::Plan(const double current_time_stamp,
   }
 
   last_publishable_trajectory_.reset(new PublishableTrajectory(
-      current_time_stamp, best_reference_line->trajectory()));
+      current_time_stamp, best_ref_info->trajectory()));
 
   ADEBUG << "current_time_stamp: " << std::to_string(current_time_stamp);
 
@@ -482,8 +481,7 @@ Status Planning::Plan(const double current_time_stamp,
 
   last_publishable_trajectory_->PopulateTrajectoryProtobuf(trajectory_pb);
 
-  best_reference_line->ExportEngageAdvice(
-      trajectory_pb->mutable_engage_advice());
+  best_ref_info->ExportEngageAdvice(trajectory_pb->mutable_engage_advice());
 
   return status;
 }