Planning: Restructured some code in dp_st_graph.

modules/planning/optimizer/dp_st_speed/dp_st_graph.cc

@@ -100,6 +100,8 @@ Status DpStGraph::InitCostTable() {
 
   unit_t_ = dp_st_speed_config_.total_time() /
             dp_st_speed_config_.matrix_dimension_t();
+  DCHECK_GT(dim_s, 2);
+  DCHECK_GT(dim_t, 2);
   cost_table_ = std::vector<std::vector<StGraphPoint>>(
       dim_t, std::vector<StGraphPoint>(dim_s, StGraphPoint()));
 
@@ -176,8 +178,6 @@ void DpStGraph::GetRowRange(const uint32_t curr_row, const uint32_t curr_col,
                             uint32_t* next_lowest_row) {
   // TODO: finish the implementation of this function.
   const auto& curr_point = cost_table_[curr_col][curr_row];
-  *next_highest_row = cost_table_.back().size() - 1;
-  *next_lowest_row = curr_point.index_s();
 
   double v0 = 0.0;
   if (curr_col == 0) {
@@ -190,16 +190,20 @@ void DpStGraph::GetRowRange(const uint32_t curr_row, const uint32_t curr_col,
   const double delta_s_upper_bound =
       v0 * unit_t_ +
       0.5 * vehicle_param_.max_acceleration() * unit_t_ * unit_t_;
-  *next_highest_row = std::min(
-      static_cast<uint32_t>(cost_table_.back().size() - 1),
-      *next_lowest_row + static_cast<uint32_t>(delta_s_upper_bound / unit_s_));
+  *next_highest_row = curr_point.index_s() +
+                      static_cast<uint32_t>(delta_s_upper_bound / unit_s_);
+  if (*next_highest_row >= cost_table_.back().size()) {
+    *next_highest_row = cost_table_.back().size() - 1;
+  }
 
-  const double delta_s_lower_bound =
-      v0 * unit_t_ +
-      0.5 * vehicle_param_.max_deceleration() * unit_t_ * unit_t_;
+  const double delta_s_lower_bound = std::fmax(
+      0.0, v0 * unit_t_ +
+               0.5 * vehicle_param_.max_deceleration() * unit_t_ * unit_t_);
   *next_lowest_row =
-      *next_lowest_row +
-      std::max(0, static_cast<int32_t>(delta_s_lower_bound / unit_s_));
+      *next_lowest_row + static_cast<int32_t>(delta_s_lower_bound / unit_s_);
+  if (*next_lowest_row >= cost_table_.back().size()) {
+    *next_lowest_row = cost_table_.back().size() - 1;
+  }
 }
 
 void DpStGraph::CalculateCostAt(const StGraphData& st_graph_data,

modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.cc

@@ -326,6 +326,8 @@ Status QpSplineStGraph::GetSConstraintByTime(
         boundary.boundary_type() == StGraphBoundary::BoundaryType::YIELD) {
       *s_upper_bound = std::fmin(*s_upper_bound, s_upper);
     } else {
+      DCHECK(boundary.boundary_type() ==
+             StGraphBoundary::BoundaryType::OVERTAKE);
       *s_lower_bound = std::fmax(*s_lower_bound, s_lower);
     }
   }

modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc

@@ -82,6 +82,7 @@ Status QpSplineStSpeedOptimizer::Process(const PathData& path_data,
     return Status(ErrorCode::PLANNING_ERROR,
                   "Mapping obstacle for qp st speed optimizer failed!");
   }
+
   SpeedLimit speed_limits;
   if (boundary_mapper.GetSpeedLimits(&speed_limits) != Status::OK()) {
     return Status(ErrorCode::PLANNING_ERROR,

modules/planning/optimizer/st_graph/st_boundary_mapper.cc

@@ -112,16 +112,20 @@ Status StBoundaryMapper::GetGraphBoundary(
         AERROR << msg;
         return Status(ErrorCode::PLANNING_ERROR, msg);
       }
+      continue;
     }
     const auto& decision = path_obstacle->LongitutionalDecision();
-    StGraphBoundary boundary(path_obstacle);
     if (decision.has_follow()) {
+      StGraphBoundary boundary(path_obstacle);
       const auto ret = MapFollowDecision(*path_obstacle, decision, &boundary);
-      if (!ret.ok()) {
+      if (ret.code() == ErrorCode::PLANNING_ERROR) {
         AERROR << "Fail to map obstacle " << path_obstacle->Id()
                << " with follow decision: " << decision.DebugString();
         return Status(ErrorCode::PLANNING_ERROR, "Fail to map follow decision");
       }
+      if (ret.ok()) {
+        st_graph_boundaries->push_back(boundary);
+      }
     } else if (decision.has_stop()) {
       // TODO(all) change start_s() to st_boundary.min_s()
       const double stop_s = path_obstacle->perception_sl_boundary().start_s() +
@@ -153,9 +157,9 @@ Status StBoundaryMapper::GetGraphBoundary(
     } else {
       std::string msg = common::util::StrCat("No mapping for decision: ",
                                              decision.DebugString());
-      return Status(ErrorCode::PLANNING_SKIP, msg);
+      ADEBUG << msg;
+      // Status(ErrorCode::PLANNING_SKIP, msg);
     }
-    boundary.set_id(path_obstacle->Id());
   }
 
   if (stop_obstacle) {
@@ -169,7 +173,6 @@ Status StBoundaryMapper::GetGraphBoundary(
     }
     st_graph_boundaries->push_back(stop_boundary);
   }
-
   return Status::OK();
 }