From 3b6ee8ba701b35d984f28ee680b1ca644de0f510 Mon Sep 17 00:00:00 2001
From: Dong Li <lidong.main@gmail.com>
Date: Fri, 2 Mar 2018 21:08:28 -0800
Subject: [PATCH] planning: fix reference line drivable bug

---
 .../trajectory/discretized_trajectory.h       |  2 +-
 modules/planning/planner/em/em_planner.cc     |  6 ++++-
 modules/planning/planning.cc                  | 22 +++++++++----------
 3 files changed, 16 insertions(+), 14 deletions(-)

diff --git a/modules/planning/common/trajectory/discretized_trajectory.h b/modules/planning/common/trajectory/discretized_trajectory.h
index edbb2e2a53..3fb5fcea02 100644
--- a/modules/planning/common/trajectory/discretized_trajectory.h
+++ b/modules/planning/common/trajectory/discretized_trajectory.h
@@ -15,7 +15,7 @@
  *****************************************************************************/
 
 /**
- * @file discretized_trajectory.h
+ * @file
  **/
 
 #ifndef MODULES_PLANNING_COMMON_TRAJECTORY_DISCRETIZED_TRAJECTORY_H_
diff --git a/modules/planning/planner/em/em_planner.cc b/modules/planning/planner/em/em_planner.cc
index 85603d9fe9..4295722942 100644
--- a/modules/planning/planner/em/em_planner.cc
+++ b/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);
diff --git a/modules/planning/planning.cc b/modules/planning/planning.cc
index 7d4ec5c03a..9837d4c915 100644
--- a/modules/planning/planning.cc
+++ b/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;
 }
-- 
GitLab