planning: keep_clear improvements (#2803)

modules/planning/common/frame.cc

@@ -210,7 +210,7 @@ const Obstacle *Frame::AddVirtualStopObstacle(
   double lane_left_width = 0.0;
   double lane_right_width = 0.0;
   reference_line.GetLaneWidth(object_s, &lane_left_width, &lane_right_width);
-  Box2d stop_wall_box{box_center, heading, FLAGS_virtual_stop_wall_height,
+  Box2d stop_wall_box{box_center, heading, FLAGS_virtual_stop_wall_length,
                       lane_left_width + lane_right_width};
 
   return AddStaticVirtualObstacle(object_id, stop_wall_box);

modules/planning/common/planning_gflags.cc

@@ -257,6 +257,9 @@ DEFINE_double(creep_stop_distance, 0.5,
 DEFINE_bool(enable_keep_clear, false, "enable keep clear zone");
 DEFINE_string(keep_clear_virtual_object_id_prefix, "KC_",
               "prefix for converting keep_clear id to virtual object id");
+DEFINE_string(keep_clear_junction_virtual_object_id_prefix, "KC_JC_",
+              "prefix for converting keep_clear(junction) id "
+              "to virtual object id");
 DEFINE_double(keep_clear_min_pass_distance, 2.0,
               "valid min distance(m) for vehicles to be considered as "
               "have passed keep_clear zone (stop_line_end_s)");

modules/planning/common/planning_gflags.h

@@ -155,6 +155,7 @@ DECLARE_double(creep_stop_distance);
 /// keep_clear
 DECLARE_bool(enable_keep_clear);
 DECLARE_string(keep_clear_virtual_object_id_prefix);
+DECLARE_string(keep_clear_junction_virtual_object_id_prefix);
 DECLARE_double(keep_clear_min_pass_distance);
 /// traffic light
 DECLARE_bool(enable_traffic_light);

modules/planning/tasks/dp_st_speed/dp_st_cost.cc

@@ -64,6 +64,7 @@ void DpStCost::AddToKeepClearRange(
         StBoundary::BoundaryType::KEEP_CLEAR) {
       continue;
     }
+
     double start_s = obstacle->st_boundary().min_s();
     double end_s = obstacle->st_boundary().max_s();
     keep_clear_range_.emplace_back(start_s, end_s);
@@ -110,6 +111,10 @@ double DpStCost::GetObstacleCost(const StGraphPoint& st_graph_point) {
 
   double cost = 0.0;
   for (const auto* obstacle : obstacles_) {
+    if (!obstacle->IsBlockingObstacle()) {
+      continue;
+    }
+
     auto boundary = obstacle->st_boundary();
     const double kIgnoreDistance = 200.0;
     if (boundary.min_s() > kIgnoreDistance) {
@@ -169,11 +174,13 @@ double DpStCost::GetSpeedCost(const STPoint& first, const STPoint& second,
   if (speed < 0) {
     return kInf;
   }
+
   if (speed < FLAGS_max_stop_speed && InKeepClearRange(second.s())) {
     // first.s in range
     cost += config_.keep_clear_low_speed_penalty() * unit_t_ *
             config_.default_speed_cost();
   }
+
   double det_speed = (speed - speed_limit) / speed_limit;
   if (det_speed > 0) {
     cost += config_.exceed_speed_penalty() * config_.default_speed_cost() *

modules/planning/tasks/dp_st_speed/dp_st_graph.cc

@@ -47,6 +47,9 @@ bool CheckOverlapOnDpStGraph(const std::vector<const StBoundary*>& boundaries,
                              const StGraphPoint& p1, const StGraphPoint& p2) {
   const common::math::LineSegment2d seg(p1.point(), p2.point());
   for (const auto* boundary : boundaries) {
+    if (boundary->boundary_type() == StBoundary::BoundaryType::KEEP_CLEAR) {
+      continue;
+    }
     if (boundary->HasOverlap(seg)) {
       return true;
     }

modules/planning/tasks/dp_st_speed/dp_st_speed_optimizer.cc

@@ -63,8 +63,6 @@ bool DpStSpeedOptimizer::SearchStGraph(const StBoundaryMapper& boundary_mapper,
     auto id = obstacle->Id();
     if (!obstacle->st_boundary().IsEmpty()) {
       if (obstacle->st_boundary().boundary_type() ==
-              StBoundary::BoundaryType::KEEP_CLEAR ||
-          obstacle->reference_line_st_boundary().boundary_type() ==
               StBoundary::BoundaryType::KEEP_CLEAR) {
         path_decision->Find(id)->SetBlockingObstacle(false);
       } else {

modules/planning/tasks/st_graph/st_boundary_mapper.cc

@@ -190,6 +190,7 @@ Status StBoundaryMapper::CreateStBoundaryWithHistory(
     } else {
       decision = iter->second;
     }
+
     if (!path_obstacle->HasLongitudinalDecision()) {
       if (!MapWithoutDecision(path_obstacle).ok()) {
         std::string msg = StrCat("Fail to map obstacle ", path_obstacle->Id(),
@@ -352,10 +353,19 @@ bool StBoundaryMapper::GetOverlapBoundaryPoints(
 
       if (CheckOverlap(curr_point_on_path, obs_box,
                        st_boundary_config_.boundary_buffer())) {
-        lower_points->emplace_back(curr_point_on_path.s(), 0.0);
-        lower_points->emplace_back(curr_point_on_path.s(), planning_time_);
-        upper_points->emplace_back(planning_distance_, 0.0);
-        upper_points->emplace_back(planning_distance_, planning_time_);
+        const double backward_distance =
+            -vehicle_param_.front_edge_to_center();
+        const double forward_distance =
+            vehicle_param_.length() + vehicle_param_.width() +
+            obs_box.length() + obs_box.width();
+        double low_s =  std::fmax(0.0,
+                                  curr_point_on_path.s() + backward_distance);
+        double high_s = std::fmin(planning_distance_,
+                                  curr_point_on_path.s() + forward_distance);
+        lower_points->emplace_back(low_s, 0.0);
+        lower_points->emplace_back(low_s, planning_time_);
+        upper_points->emplace_back(high_s, 0.0);
+        upper_points->emplace_back(high_s, planning_time_);
         break;
       }
     }

modules/planning/tasks/traffic_decider/keep_clear.cc

@@ -46,8 +46,12 @@ bool KeepClear::ApplyRule(Frame* const frame,
   const std::vector<PathOverlap>& keep_clear_overlaps =
       reference_line_info->reference_line().map_path().clear_area_overlaps();
   for (const auto& keep_clear_overlap : keep_clear_overlaps) {
-    if (BuildKeepClearObstacle(frame, reference_line_info,
-                               const_cast<PathOverlap*>(&keep_clear_overlap))) {
+    const auto obstacle_id = FLAGS_keep_clear_virtual_object_id_prefix +
+        keep_clear_overlap.object_id;
+    if (BuildKeepClearObstacle(
+        frame, reference_line_info,
+        const_cast<PathOverlap*>(&keep_clear_overlap),
+        obstacle_id)) {
       ADEBUG << "KEEP_CLAER for keep_clear_zone["
              << keep_clear_overlap.object_id << "] s["
              << keep_clear_overlap.start_s << ", " << keep_clear_overlap.end_s
@@ -59,8 +63,13 @@ bool KeepClear::ApplyRule(Frame* const frame,
   const std::vector<PathOverlap>& junction_overlaps =
       reference_line_info->reference_line().map_path().junction_overlaps();
   for (const auto& junction_overlap : junction_overlaps) {
-    if (BuildKeepClearObstacle(frame, reference_line_info,
-                               const_cast<PathOverlap*>(&junction_overlap))) {
+    const auto obstacle_id =
+        FLAGS_keep_clear_junction_virtual_object_id_prefix +
+        junction_overlap.object_id;
+    if (BuildKeepClearObstacle(
+        frame, reference_line_info,
+        const_cast<PathOverlap*>(&junction_overlap),
+        obstacle_id)) {
       ADEBUG << "KEEP_CLAER for junction[" << junction_overlap.object_id
              << "] s[" << junction_overlap.start_s << ", "
              << junction_overlap.end_s << "] BUILD";
@@ -71,8 +80,10 @@ bool KeepClear::ApplyRule(Frame* const frame,
 }
 
 bool KeepClear::BuildKeepClearObstacle(
-    Frame* const frame, ReferenceLineInfo* const reference_line_info,
-    PathOverlap* const keep_clear_overlap) {
+    Frame* const frame,
+    ReferenceLineInfo* const reference_line_info,
+    PathOverlap* const keep_clear_overlap,
+    const std::string& virtual_obstacle_id) {
   CHECK_NOTNULL(frame);
   CHECK_NOTNULL(reference_line_info);
   CHECK_NOTNULL(keep_clear_overlap);
@@ -119,16 +130,15 @@ bool KeepClear::BuildKeepClearObstacle(
   common::math::Box2d keep_clear_box{
       common::math::LineSegment2d(start_xy, end_xy),
       left_lane_width + right_lane_width};
-  const auto obstacle_id =
-      FLAGS_keep_clear_virtual_object_id_prefix + keep_clear_overlap->object_id;
-  auto* obstacle = frame->AddStaticVirtualObstacle(obstacle_id, keep_clear_box);
+  auto* obstacle =
+      frame->AddStaticVirtualObstacle(virtual_obstacle_id, keep_clear_box);
   if (!obstacle) {
-    AERROR << "Failed to create obstacle " << obstacle_id << " in frame";
+    AERROR << "Failed to create obstacle: " << virtual_obstacle_id;
     return false;
   }
   auto* path_obstacle = reference_line_info->AddObstacle(obstacle);
   if (!path_obstacle) {
-    AERROR << "Failed to create path_obstacle for " << obstacle_id;
+    AERROR << "Failed to create path_obstacle: " << virtual_obstacle_id;
     return false;
   }
   path_obstacle->SetReferenceLineStBoundaryType(

modules/planning/tasks/traffic_decider/keep_clear.h

@@ -21,6 +21,8 @@
 #ifndef MODULES_PLANNING_TASKS_TRAFFIC_DECIDER_KEEP_CLEAR_H_
 #define MODULES_PLANNING_TASKS_TRAFFIC_DECIDER_KEEP_CLEAR_H_
 
+#include <string>
+
 #include "modules/planning/tasks/traffic_decider/traffic_rule.h"
 
 namespace apollo {
@@ -41,7 +43,8 @@ class KeepClear : public TrafficRule {
   bool BuildKeepClearObstacle(
       Frame* const frame,
       ReferenceLineInfo* const reference_line_info,
-      hdmap::PathOverlap* const keep_clear_overlap);
+      hdmap::PathOverlap* const keep_clear_overlap,
+      const std::string& virtual_obstacle_id);
 };
 
 }  // namespace planning