Planning: temporary updates just for running dreamland.

modules/planning/conf/planning_config.pb.txt

@@ -310,6 +310,6 @@ default_task_config: {
 default_task_config: {
   task_type: PATH_BOUNDS_DECIDER
   path_bounds_decider_config {
-    is_lane_borrowing: false
+    is_lane_borrowing: true
   }
 }

modules/planning/conf/scenario/lane_follow_config.pb.txt

@@ -7,7 +7,7 @@ stage_config: {
   task_type: PATH_BOUNDS_DECIDER
   task_type: PIECEWISE_JERK_PATH_OPTIMIZER
   task_type: PATH_ASSESSMENT_DECIDER
-  task_type: PATH_DECIDER
+  #task_type: PATH_DECIDER
   task_type: SPEED_BOUNDS_PRIORI_DECIDER
   task_type: DP_ST_SPEED_OPTIMIZER
   task_type: SPEED_DECIDER
@@ -27,9 +27,9 @@ stage_config: {
   task_config: {
     task_type: PATH_ASSESSMENT_DECIDER
   }
-  task_config: {
-    task_type: PATH_DECIDER
-  }
+  #task_config: {
+  #  task_type: PATH_DECIDER
+  #}
   task_config: {
     task_type: SPEED_BOUNDS_PRIORI_DECIDER
   }

modules/planning/scenarios/side_pass/side_pass_scenario.cc

@@ -89,6 +89,7 @@ std::unique_ptr<Stage> SidePassScenario::CreateStage(
 bool SidePassScenario::IsTransferable(const Scenario& target_scenario,
                                       const Frame& frame) {
   // Sanity checks.
+  return false;
   if (frame.reference_line_info().size() > 1) {
     return false;
   }

modules/planning/tasks/deciders/path_assessment_decider.cc

@@ -25,6 +25,8 @@
 #include "modules/map/hdmap/hdmap_util.h"
 #include "modules/planning/tasks/deciders/path_decider_obstacle_utils.h"
 
+// #define ADEBUG AINFO
+
 namespace apollo {
 namespace planning {
 
@@ -50,6 +52,10 @@ Status PathAssessmentDecider::Process(
   CHECK_NOTNULL(reference_line_info);
   const auto& candidate_path_data = reference_line_info->GetCandidatePathData();
 
+  if (candidate_path_data.empty()) {
+    ADEBUG << "Candidate path data is empty.";
+  }
+
   // Remove invalid path.
   std::vector<PathData> valid_path_data;
   for (const auto& curr_path_data : candidate_path_data) {
@@ -69,6 +75,8 @@ Status PathAssessmentDecider::Process(
     const std::string msg = "Neither regular nor fallback path is valid.";
     AERROR << msg;
     return Status(ErrorCode::PLANNING_ERROR, msg);
+  } else {
+    ADEBUG << "There are " << valid_path_data.size() << " valid path data.";
   }
 
   // Analyze and add important info for speed decider to use.
@@ -76,6 +84,7 @@ Status PathAssessmentDecider::Process(
     if (curr_path_data.path_label() == "fallback") {
       continue;
     }
+    ADEBUG << "Path length = " << curr_path_data.frenet_frame_path().size();
     SetPathInfo(*reference_line_info, &curr_path_data);
   }
 
@@ -89,6 +98,7 @@ Status PathAssessmentDecider::Process(
               // in sorting.
               return lhs.path_label() == "regular";
             });
+  ADEBUG << "Using " << valid_path_data.front().path_label() << " path.";
   *(reference_line_info->mutable_path_data()) = valid_path_data.front();
   reference_line_info->SetBlockingObstacleId(
       valid_path_data.front().blocking_obstacle_id());
@@ -100,14 +110,17 @@ bool PathAssessmentDecider::IsValidRegularPath(
     const ReferenceLineInfo& reference_line_info, const PathData& path_data) {
   // Check if the path is greatly off the reference line.
   if (IsGreatlyOffReferenceLine(path_data)) {
+    ADEBUG << "Regular Path: ADC is greatly off reference line.";
     return false;
   }
   // Check if the path is greatly off the road.
   if (IsGreatlyOffRoad(reference_line_info, path_data)) {
+    ADEBUG << "Regular Path: ADC is greatly off road.";
     return false;
   }
   // Check if there is any collision.
   if (IsCollidingWithStaticObstacles(reference_line_info, path_data)) {
+    ADEBUG << "Regular Path: ADC has collision.";
     return false;
   }
   return true;
@@ -117,10 +130,12 @@ bool PathAssessmentDecider::IsValidFallbackPath(
     const ReferenceLineInfo& reference_line_info, const PathData& path_data) {
   // Check if the path is greatly off the reference line.
   if (IsGreatlyOffReferenceLine(path_data)) {
+    ADEBUG << "Fallback Path: ADC is greatly off reference line.";
     return false;
   }
   // Check if the path is greatly off the road.
   if (IsGreatlyOffRoad(reference_line_info, path_data)) {
+    ADEBUG << "Fallback Path: ADC is greatly off road.";
     return false;
   }
   return true;

modules/planning/tasks/deciders/path_bounds_decider.cc

@@ -31,6 +31,8 @@
 #include "modules/map/hdmap/hdmap_util.h"
 #include "modules/planning/tasks/deciders/path_decider_obstacle_utils.h"
 
+// #define ADEBUG AINFO
+
 namespace apollo {
 namespace planning {
 
@@ -52,7 +54,7 @@ constexpr double kPathBoundsDeciderHorizon = 100.0;
 constexpr double kPathBoundsDeciderResolution = 0.5;
 constexpr double kDefaultLaneWidth = 5.0;
 constexpr double kDefaultRoadWidth = 20.0;
-constexpr double kObstacleSBuffer = 1.0;
+constexpr double kObstacleSBuffer = 2.0;
 constexpr double kObstacleLBuffer = 0.4;
 
 PathBoundsDecider::PathBoundsDecider(const TaskConfig& config)
@@ -101,8 +103,8 @@ Status PathBoundsDecider::Process(
   std::vector<LaneBorrowInfo> lane_borrow_info_list;
   if (config.path_bounds_decider_config().is_lane_borrowing()) {
     // Try borrowing from left and from right neighbor lane.
-    lane_borrow_info_list = {LaneBorrowInfo::LEFT_BORROW,
-                             LaneBorrowInfo::RIGHT_BORROW};
+    lane_borrow_info_list = {LaneBorrowInfo::LEFT_BORROW};//,
+                             //LaneBorrowInfo::RIGHT_BORROW};
   } else {
     // Only use self-lane with no lane borrowing
     lane_borrow_info_list = {LaneBorrowInfo::NO_BORROW};
@@ -206,7 +208,7 @@ std::string PathBoundsDecider::GenerateRegularPathBound(
     AERROR << msg;
     return msg;
   }
-  // PathBoundsDebugString(*path_bound);
+  PathBoundsDebugString(*path_bound);
 
   // 4. Adjust the boundary considering dynamic obstacles
   // TODO(all): may need to implement this in the future.
@@ -318,6 +320,7 @@ bool PathBoundsDecider::GetBoundaryFromLanesAndADC(
       hdmap::LaneInfoConstPtr adjacent_lane = nullptr;
       if (lane_borrow_info == LaneBorrowInfo::LEFT_BORROW) {
         // Borrowing left neighbor lane.
+        ADEBUG << "Borrowing the left lane.";
         if (curr_lane.left_neighbor_forward_lane_id_size() > 0) {
           adjacent_lane = HDMapUtil::BaseMapPtr()->GetLaneById(
               curr_lane.left_neighbor_forward_lane_id(0));
@@ -328,6 +331,7 @@ bool PathBoundsDecider::GetBoundaryFromLanesAndADC(
         }
       } else if (lane_borrow_info == LaneBorrowInfo::RIGHT_BORROW) {
         // Borrowing right neighbor lane.
+        ADEBUG << "Borrowing the right lane.";
         if (curr_lane.right_neighbor_forward_lane_id_size() > 0) {
           adjacent_lane = HDMapUtil::BaseMapPtr()->GetLaneById(
               curr_lane.right_neighbor_forward_lane_id(0));

modules/planning/tasks/optimizers/path_decider/path_decider.cc

@@ -110,6 +110,7 @@ bool PathDecider::MakeStaticObstacleDecision(
 
     if (obstacle->Id() == blocking_obstacle_id) {
       // Add stop decision
+      ADEBUG << "Blocking obstacle = " << blocking_obstacle_id;
       ObjectDecisionType object_decision;
       *object_decision.mutable_stop() = GenerateObjectStopDecision(*obstacle);
       path_decision->AddLongitudinalDecision("PathDecider/blocking_obstacle",

modules/planning/tasks/optimizers/piecewise_jerk_path/piecewise_jerk_path_optimizer.cc

@@ -28,6 +28,8 @@
 #include "modules/planning/common/trajectory1d/piecewise_jerk_trajectory1d.h"
 #include "modules/planning/math/finite_element_qp/fem_1d_qp_problem.h"
 
+// #define ADEBUG AINFO
+
 namespace apollo {
 namespace planning {
 
@@ -54,6 +56,7 @@ common::Status PiecewiseJerkPathOptimizer::Process(
 
   const auto& path_boundaries =
       reference_line_info_->GetCandidatePathBoundaries();
+  ADEBUG << "There are " << path_boundaries.size() << " path boundaries.";
 
   std::vector<PathData> candidate_path_data;
   for (const auto& path_boundary : path_boundaries) {

modules/planning/tasks/task_factory.cc

@@ -29,6 +29,7 @@
 #include "modules/planning/tasks/deciders/open_space_roi_decider.h"
 #include "modules/planning/tasks/deciders/path_assessment_decider.h"
 #include "modules/planning/tasks/deciders/path_bounds_decider.h"
+#include "modules/planning/tasks/deciders/path_assessment_decider.h"
 #include "modules/planning/tasks/deciders/side_pass_path_decider.h"
 #include "modules/planning/tasks/deciders/side_pass_safety.h"
 #include "modules/planning/tasks/deciders/speed_bounds_decider/speed_bounds_decider.h"
@@ -63,6 +64,10 @@ void TaskFactory::Init(const PlanningConfig& config) {
                          [](const TaskConfig& config) -> Task* {
                            return new PathBoundsDecider(config);
                          });
+  task_factory_.Register(TaskConfig::PATH_ASSESSMENT_DECIDER,
+                         [](const TaskConfig& config) -> Task* {
+                           return new PathAssessmentDecider(config);
+                         });
   task_factory_.Register(TaskConfig::PIECEWISE_JERK_PATH_OPTIMIZER,
                          [](const TaskConfig& config) -> Task* {
                            return new PiecewiseJerkPathOptimizer(config);