Planning: added unblocking obstacles in poly_st_speed. (#2076)

modules/planning/common/path_obstacle.h

@@ -131,6 +131,9 @@ class PathObstacle {
    **/
   static bool IsLateralDecision(const ObjectDecisionType& decision);
 
+  void SetBlockingObstacle(bool blocking) { is_blocking_obstacle_ = blocking; }
+  bool IsBlockingObstacle() const { return is_blocking_obstacle_; }
+
  private:
   FRIEND_TEST(MergeLongitudinalDecision, AllDecisions);
   static ObjectDecisionType MergeLongitudinalDecision(
@@ -155,6 +158,8 @@ class PathObstacle {
   ObjectDecisionType lateral_decision_;
   ObjectDecisionType longitudinal_decision_;
 
+  bool is_blocking_obstacle_ = false;
+
   struct ObjectTagCaseHash {
     std::size_t operator()(
         const planning::ObjectDecisionType::ObjectTagCase tag) const {

modules/planning/conf/planning_config.pb.txt

@@ -158,6 +158,8 @@ em_planner_config {
         jerk_weight: 1.0
         obstacle_weight: 10.0
 
+        unblocking_obstacle_cost: 1e3
+
         st_boundary_config {
             boundary_buffer: 0.1
             high_speed_centric_acceleration_limit: 1.2

modules/planning/proto/poly_st_speed_config.proto

@@ -16,5 +16,6 @@ message PolyStSpeedConfig {
   optional double speed_weight = 8;
   optional double jerk_weight = 9;
   optional double obstacle_weight = 10;
-  optional apollo.planning.StBoundaryConfig st_boundary_config = 11;
+  optional double unblocking_obstacle_cost = 11;
+  optional apollo.planning.StBoundaryConfig st_boundary_config = 12;
 }

modules/planning/tasks/poly_st_speed/poly_st_speed_optimizer.cc

@@ -87,6 +87,7 @@ Status PolyStSpeedOptimizer::Process(const SLBoundary& adc_sl_boundary,
     DCHECK(path_obstacle->HasLongitudinalDecision());
   }
   // step 1 get boundaries
+  auto path_decision_copy = *path_decision;
   path_decision->EraseStBoundaries();
   if (boundary_mapper.CreateStBoundary(path_decision).code() ==
       ErrorCode::PLANNING_ERROR) {
@@ -94,10 +95,15 @@ Status PolyStSpeedOptimizer::Process(const SLBoundary& adc_sl_boundary,
                   "Mapping obstacle for qp st speed optimizer failed!");
   }
 
-  std::vector<const StBoundary*> boundaries;
   for (const auto* obstacle : path_decision->path_obstacles().Items()) {
+    auto id = obstacle->Id();
+    auto* mutable_obstacle = path_decision->Find(id);
+
     if (!obstacle->st_boundary().IsEmpty()) {
-      boundaries.push_back(&obstacle->st_boundary());
+      mutable_obstacle->SetBlockingObstacle(true);
+    } else {
+      path_decision->SetStBoundary(id,
+                                   path_decision_copy.Find(id)->st_boundary());
     }
   }
 

modules/planning/tasks/poly_st_speed/speed_profile_cost.cc

@@ -85,7 +85,8 @@ double SpeedProfileCost::CalculatePointCost(
     if (t < boundary.min_t() || t > boundary.max_t()) {
       continue;
     }
-    if (boundary.IsPointInBoundary(STPoint(s, t))) {
+    if (obstacle->IsBlockingObstacle() &&
+        boundary.IsPointInBoundary(STPoint(s, t))) {
       return kInfCost;
     }
     double s_upper = 0.0;
@@ -106,6 +107,10 @@ double SpeedProfileCost::CalculatePointCost(
         cost += config_.obstacle_weight() *
                 std::pow((kSafeDistance + s_upper - s), 2);
       }
+    } else {
+      if (!obstacle->IsBlockingObstacle()) {
+        cost += config_.unblocking_obstacle_cost();
+      }
     }
   }
   cost += config_.speed_weight() * std::pow((v - speed_limit), 2);

modules/planning/tasks/poly_st_speed/speed_profile_cost.h

@@ -15,7 +15,7 @@
  *****************************************************************************/
 
 /**
- * @file trajectory_cost.h
+ * @file
  **/
 
 #ifndef MODULES_PLANNING_TASKS_POLY_ST_SPEED_SPEED_PROFILE_COST_H_