planning: added st boundary for keep clear

modules/planning/proto/dp_st_speed_config.proto

@@ -40,4 +40,6 @@ message DpStSpeedConfig {
   optional double max_deceleration = 22 [ default = -4.5 ];
 
   optional apollo.planning.StBoundaryConfig st_boundary_config = 33;
+
+  optional double keep_clear_cost_factor = 50 [ default = 1000.0 ];
 }

modules/planning/tasks/dp_poly_path/trajectory_cost.cc

@@ -54,7 +54,7 @@ TrajectoryCost::TrajectoryCost(
       continue;
     }
     const auto ptr_obstacle = ptr_path_obstacle->obstacle();
-    if (ptr_obstacle->PerceptionId() < 0) {
+    if (Obstacle::IsVirtualObstacle(ptr_obstacle->Perception())) {
       // Virtual obsatcle
       continue;
     }

modules/planning/tasks/dp_st_speed/BUILD

@@ -25,6 +25,7 @@ cc_library(
         "dp_st_cost.h",
     ],
     deps = [
+        ":st_graph_point",
         "//modules/common/proto:pnc_point_proto",
         "//modules/planning/common:frame",
         "//modules/planning/common/speed:st_boundary",

modules/planning/tasks/dp_st_speed/dp_st_cost.cc

@@ -32,22 +32,32 @@ DpStCost::DpStCost(const DpStSpeedConfig& dp_st_speed_config)
       unit_s_(dp_st_speed_config_.total_path_length() /
               dp_st_speed_config_.matrix_dimension_s()),
       unit_t_(dp_st_speed_config_.total_time() /
-              dp_st_speed_config_.matrix_dimension_t()) {}
+              dp_st_speed_config_.matrix_dimension_t()) {
+  unit_v_ = unit_s_ / unit_t_;
+}
 
 // TODO(all): normalize cost with time
 double DpStCost::GetObstacleCost(
-    const STPoint& point,
+    const StGraphPoint& st_graph_point,
     const std::vector<const StBoundary*>& st_boundaries) const {
   double total_cost = 0.0;
   constexpr double inf = std::numeric_limits<double>::infinity();
-  if (point.s() < 0) {
+  const double unit_v = unit_s_ / unit_t_;
+  const auto& st_point = st_graph_point.point();
+  if (st_point.s() < 0) {
     return inf;
   }
   for (const StBoundary* boundary : st_boundaries) {
-    if (boundary->IsPointInBoundary(point)) {
-      return inf;
+    if (boundary->IsPointInBoundary(st_point)) {
+      if (boundary->boundary_type() == StBoundary::BoundaryType::KEEP_CLEAR) {
+        total_cost += unit_v * ((st_graph_point.index_s() + 1.0) /
+                                (st_graph_point.index_t() + 1.0)) *
+                      dp_st_speed_config_.keep_clear_cost_factor();
+      } else {
+        return inf;
+      }
     } else {
-      const double distance = boundary->DistanceS(point);
+      const double distance = boundary->DistanceS(st_point);
       total_cost += dp_st_speed_config_.default_obstacle_cost() *
                     std::exp(dp_st_speed_config_.obstacle_cost_factor() /
                              boundary->characteristic_length() * distance);

modules/planning/tasks/dp_st_speed/dp_st_cost.h

@@ -28,6 +28,7 @@
 
 #include "modules/planning/common/speed/st_boundary.h"
 #include "modules/planning/common/speed/st_point.h"
+#include "modules/planning/tasks/dp_st_speed/st_graph_point.h"
 
 namespace apollo {
 namespace planning {
@@ -37,7 +38,7 @@ class DpStCost {
   explicit DpStCost(const DpStSpeedConfig& dp_st_speed_config);
 
   double GetObstacleCost(
-      const STPoint& point,
+      const StGraphPoint& point,
       const std::vector<const StBoundary*>& st_boundaries) const;
 
   double GetReferenceCost(const STPoint& point,
@@ -70,6 +71,7 @@ class DpStCost {
   const DpStSpeedConfig& dp_st_speed_config_;
   double unit_s_ = 0.0;
   double unit_t_ = 0.0;
+  double unit_v_ = 0.0;
 };
 
 }  // namespace planning

modules/planning/tasks/dp_st_speed/dp_st_graph.cc

@@ -133,8 +133,7 @@ void DpStGraph::CalculatePointwiseCost(
     for (auto& st_graph_point : cost_table_[i]) {
       double ref_cost = dp_st_cost_.GetReferenceCost(st_graph_point.point(),
                                                      reference_points[i]);
-      double obs_cost =
-          dp_st_cost_.GetObstacleCost(st_graph_point.point(), boundaries);
+      double obs_cost = dp_st_cost_.GetObstacleCost(st_graph_point, boundaries);
       st_graph_point.SetReferenceCost(ref_cost);
       st_graph_point.SetObstacleCost(obs_cost);
       st_graph_point.SetTotalCost(std::numeric_limits<double>::infinity());

modules/planning/tasks/path_decider/path_decider.cc

@@ -96,6 +96,11 @@ bool PathDecider::MakeStaticObstacleDecision(
       continue;
     }
 
+    if (path_obstacle->st_boundary().boundary_type() ==
+        StBoundary::BoundaryType::KEEP_CLEAR) {
+      continue;
+    }
+
     // IGNORE by default
     ObjectDecisionType object_decision;
     object_decision.mutable_ignore();

modules/planning/tasks/speed_decider/speed_decider.h

@@ -35,7 +35,7 @@ class SpeedDecider : public Task {
   SpeedDecider();
   ~SpeedDecider() = default;
 
-  bool Init(const PlanningConfig& config);
+  bool Init(const PlanningConfig& config) override;
 
   apollo::common::Status Execute(
       Frame* frame, ReferenceLineInfo* reference_line_info) override;

modules/planning/tasks/st_graph/st_boundary_mapper.cc

@@ -206,20 +206,15 @@ Status StBoundaryMapper::MapWithoutDecision(PathObstacle* path_obstacle) const {
     return Status::OK();
   }
 
-  if (lower_points.size() != upper_points.size()) {
-    std::string msg = common::util::StrCat(
-        "lower_points.size()[", lower_points.size(),
-        "] and upper_points.size()[", upper_points.size(), "] does NOT match.");
-    return Status(ErrorCode::PLANNING_ERROR, msg);
-  }
-
-  if (lower_points.size() > 1 && upper_points.size() > 1) {
-    auto boundary = StBoundary::GenerateStBoundary(lower_points, upper_points)
-                        .ExpandByS(boundary_s_buffer)
-                        .ExpandByT(boundary_t_buffer);
-    boundary.SetId(path_obstacle->Id());
-    path_obstacle->SetStBoundary(boundary);
+  auto boundary = StBoundary::GenerateStBoundary(lower_points, upper_points)
+                      .ExpandByS(boundary_s_buffer)
+                      .ExpandByT(boundary_t_buffer);
+  boundary.SetId(path_obstacle->Id());
+  const auto& prev_st_boundary = path_obstacle->st_boundary();
+  if (!prev_st_boundary.IsEmpty()) {
+    boundary.SetBoundaryType(prev_st_boundary.boundary_type());
   }
+  path_obstacle->SetStBoundary(boundary);
   return Status::OK();
 }
 
@@ -359,7 +354,7 @@ bool StBoundaryMapper::GetOverlapBoundaryPoints(
     }
   }
   DCHECK_EQ(lower_points->size(), upper_points->size());
-  return (lower_points->size() > 0 && upper_points->size() > 0);
+  return (lower_points->size() > 1 && upper_points->size() > 1);
 }
 
 Status StBoundaryMapper::MapWithPredictionTrajectory(
@@ -376,55 +371,46 @@ Status StBoundaryMapper::MapWithPredictionTrajectory(
   if (!GetOverlapBoundaryPoints(path_data_.discretized_path().path_points(),
                                 *(path_obstacle->obstacle()), &upper_points,
                                 &lower_points)) {
-    return Status(ErrorCode::PLANNING_ERROR, "PLANNING_ERROR");
+    return Status::OK();
   }
 
-  if (lower_points.size() != upper_points.size()) {
-    std::string msg = common::util::StrCat(
-        "lower_points.size()[", lower_points.size(),
-        "] and upper_points.size()[", upper_points.size(), "] does NOT match.");
-    return Status(ErrorCode::PLANNING_ERROR, msg);
+  if (obj_decision.has_follow() && lower_points.back().t() < planning_time_) {
+    const double diff_s = lower_points.back().s() - lower_points.front().s();
+    const double diff_t = lower_points.back().t() - lower_points.front().t();
+    double extend_lower_s =
+        diff_s / diff_t * (planning_time_ - lower_points.front().t()) +
+        lower_points.front().s();
+    const double extend_upper_s =
+        extend_lower_s + (upper_points.back().s() - lower_points.back().s()) +
+        1.0;
+    upper_points.emplace_back(extend_upper_s, planning_time_);
+    lower_points.emplace_back(extend_lower_s, planning_time_);
   }
 
-  if (lower_points.size() > 1 && upper_points.size() > 1) {
-    if (obj_decision.has_follow() && lower_points.back().t() < planning_time_) {
-      const double diff_s = lower_points.back().s() - lower_points.front().s();
-      const double diff_t = lower_points.back().t() - lower_points.front().t();
-      double extend_lower_s =
-          diff_s / diff_t * (planning_time_ - lower_points.front().t()) +
-          lower_points.front().s();
-      const double extend_upper_s =
-          extend_lower_s + (upper_points.back().s() - lower_points.back().s()) +
-          1.0;
-      upper_points.emplace_back(extend_upper_s, planning_time_);
-      lower_points.emplace_back(extend_lower_s, planning_time_);
-    }
-
-    auto boundary = StBoundary::GenerateStBoundary(lower_points, upper_points)
-                        .ExpandByS(boundary_s_buffer)
-                        .ExpandByT(boundary_t_buffer);
-
-    // get characteristic_length and boundary_type.
-    StBoundary::BoundaryType b_type = StBoundary::BoundaryType::UNKNOWN;
-    double characteristic_length = 0.0;
-    if (obj_decision.has_follow()) {
-      characteristic_length = std::fabs(obj_decision.follow().distance_s());
-      b_type = StBoundary::BoundaryType::FOLLOW;
-    } else if (obj_decision.has_yield()) {
-      characteristic_length = std::fabs(obj_decision.yield().distance_s());
-      b_type = StBoundary::BoundaryType::YIELD;
-    } else if (obj_decision.has_overtake()) {
-      characteristic_length = std::fabs(obj_decision.overtake().distance_s());
-      b_type = StBoundary::BoundaryType::OVERTAKE;
-    } else {
-      DCHECK(false) << "Obj decision should be either yield or overtake: "
-                    << obj_decision.DebugString();
-    }
-    boundary.SetBoundaryType(b_type);
-    boundary.SetId(path_obstacle->obstacle()->Id());
-    boundary.SetCharacteristicLength(characteristic_length);
-    path_obstacle->SetStBoundary(boundary);
+  auto boundary = StBoundary::GenerateStBoundary(lower_points, upper_points)
+                      .ExpandByS(boundary_s_buffer)
+                      .ExpandByT(boundary_t_buffer);
+
+  // get characteristic_length and boundary_type.
+  StBoundary::BoundaryType b_type = StBoundary::BoundaryType::UNKNOWN;
+  double characteristic_length = 0.0;
+  if (obj_decision.has_follow()) {
+    characteristic_length = std::fabs(obj_decision.follow().distance_s());
+    b_type = StBoundary::BoundaryType::FOLLOW;
+  } else if (obj_decision.has_yield()) {
+    characteristic_length = std::fabs(obj_decision.yield().distance_s());
+    b_type = StBoundary::BoundaryType::YIELD;
+  } else if (obj_decision.has_overtake()) {
+    characteristic_length = std::fabs(obj_decision.overtake().distance_s());
+    b_type = StBoundary::BoundaryType::OVERTAKE;
+  } else {
+    DCHECK(false) << "Obj decision should be either yield or overtake: "
+                  << obj_decision.DebugString();
   }
+  boundary.SetBoundaryType(b_type);
+  boundary.SetId(path_obstacle->obstacle()->Id());
+  boundary.SetCharacteristicLength(characteristic_length);
+  path_obstacle->SetStBoundary(boundary);
   return Status::OK();
 }
 

modules/planning/tasks/st_graph/st_boundary_mapper.h

@@ -59,6 +59,11 @@ class StBoundaryMapper {
                     const apollo::common::math::Box2d& obs_box,
                     const double buffer) const;
 
+  /**
+   * Creates valid st boundary upper_points and lower_points
+   * If return true, upper_points.size() > 1 and
+   * upper_points.size() = lower_points.size()
+   */
   bool GetOverlapBoundaryPoints(
       const std::vector<apollo::common::PathPoint>& path_points,
       const Obstacle& obstacle, std::vector<STPoint>* upper_points,