planning: add clear zone traffic rule.

modules/planning/common/path_obstacle.cc

@@ -391,5 +391,9 @@ void PathObstacle::SetStBoundary(const StBoundary& boundary) {
   st_boundary_ = boundary;
 }
 
+void PathObstacle::SetStBoundaryType(const StBoundary::BoundaryType type) {
+  st_boundary_.SetBoundaryType(type);
+}
+
 }  // namespace planning
 }  // namespace apollo

modules/planning/common/path_obstacle.h

@@ -98,6 +98,8 @@ class PathObstacle {
 
   void SetStBoundary(const StBoundary& boundary);
 
+  void SetStBoundaryType(const StBoundary::BoundaryType type);
+
   void EraseStBoundary();
 
   bool HasLongitudinalDecision() const;

modules/planning/common/planning_gflags.cc

@@ -136,6 +136,9 @@ DEFINE_double(stop_max_distance_buffer, 4.0,
               "distance buffer of passing stop line");
 DEFINE_double(stop_min_speed, 0.1, "min speed for computing stop");
 DEFINE_double(stop_max_deceleration, 6.0, "max deceleration");
+/// Clear Zone
+DEFINE_string(clear_zone_virtual_object_id_prefix, "CZ_",
+              "prefix for converting clear zone id to virtual object id");
 /// traffic light
 DEFINE_string(signal_light_virtual_object_id_prefix, "SL_",
               "prefix for converting signal id to virtual object id");

modules/planning/common/planning_gflags.h

@@ -101,6 +101,8 @@ DECLARE_bool(enable_follow_accel_constraint);
 DECLARE_double(stop_max_distance_buffer);
 DECLARE_double(stop_min_speed);
 DECLARE_double(stop_max_deceleration);
+/// Clear Zone
+DECLARE_string(clear_zone_virtual_object_id_prefix);
 /// triffic light
 DECLARE_string(signal_light_virtual_object_id_prefix);
 DECLARE_double(max_deacceleration_for_yellow_light_stop);

modules/planning/common/speed/st_boundary.h

@@ -59,6 +59,7 @@ class StBoundary : public common::math::Polygon2d {
     FOLLOW,
     YIELD,
     OVERTAKE,
+    KEEP_CLEAR,
   };
 
   bool IsEmpty() const { return lower_points_.empty(); }

modules/planning/proto/planning_config.proto

@@ -23,6 +23,7 @@ message RuleConfig {
         BACKSIDE_VEHICLE = 1;
         SIGNAL_LIGHT = 2;
         CROSSWALK = 3;
+        CLEAR_ZONE = 4;
     }
     optional RuleId rule_id = 1;
 }

modules/planning/proto/planning_internal.proto

@@ -25,6 +25,7 @@ message StGraphBoundaryDebug {
     ST_BOUNDARY_TYPE_FOLLOW = 3;
     ST_BOUNDARY_TYPE_YIELD = 4;
     ST_BOUNDARY_TYPE_OVERTAKE = 5;
+    ST_BOUNDARY_TYPE_KEEP_CLEAR = 6;
   }
   optional string name = 1;
   repeated apollo.common.SpeedPoint point = 2;

modules/planning/tasks/speed_optimizer.cc

@@ -130,6 +130,10 @@ void SpeedOptimizer::RecordSTGraphDebug(const StGraphData& st_graph_data,
       case StBoundary::BoundaryType::YIELD:
         boundary_debug->set_type(StGraphBoundaryDebug::ST_BOUNDARY_TYPE_YIELD);
         break;
+      case StBoundary::BoundaryType::KEEP_CLEAR:
+        boundary_debug->set_type(
+            StGraphBoundaryDebug::ST_BOUNDARY_TYPE_KEEP_CLEAR);
+        break;
     }
 
     for (const auto& point : boundary->points()) {

modules/planning/tasks/traffic_decider/BUILD

@@ -6,11 +6,13 @@ cc_library(
     name = "traffic_rules",
     srcs = [
         "backside_vehicle.cc",
+        "clear_zone.cc",
         "crosswalk.cc",
         "signal_light.cc",
     ],
     hdrs = [
         "backside_vehicle.h",
+        "clear_zone.h",
         "crosswalk.h",
         "signal_light.h",
         "traffic_rule.h",

modules/planning/tasks/traffic_decider/clear_zone.cc

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * @file
+ **/
+
+#include "modules/planning/tasks/traffic_decider/clear_zone.h"
+
+#include <limits>
+#include <vector>
+
+#include "modules/common/proto/pnc_point.pb.h"
+
+#include "modules/common/adapters/adapter_manager.h"
+#include "modules/common/vehicle_state/vehicle_state.h"
+#include "modules/planning/common/planning_gflags.h"
+
+namespace apollo {
+namespace planning {
+
+using apollo::common::adapter::AdapterManager;
+using apollo::perception::TrafficLight;
+using apollo::perception::TrafficLightDetection;
+
+ClearZone::ClearZone(const RuleConfig& config) : TrafficRule(config) {}
+
+bool ClearZone::ApplyRule(Frame* frame,
+                          ReferenceLineInfo* const reference_line_info) {
+  frame_ = frame;
+  reference_line_info_ = reference_line_info;
+  const auto& map_path = reference_line_info_->reference_line().map_path();
+  const auto& clear_zones = map_path.junction_overlaps();
+  for (const auto& clear_zone : clear_zones) {
+    if (!BuildClearZoneObstacle(clear_zone)) {
+      AERROR << "Failed to build clear zone : " << clear_zone.object_id;
+      return false;
+    }
+  }
+  return true;
+}
+
+bool ClearZone::BuildClearZoneObstacle(
+    const hdmap::PathOverlap& clear_zone_overlap) {
+  const double front_edge_s = reference_line_info_->AdcSlBoundary().end_s();
+  if (clear_zone_overlap.start_s < front_edge_s) {
+    ADEBUG << "adc is already inside clear zone "
+           << clear_zone_overlap.object_id << ", skip this clear zone";
+    return true;
+  }
+  common::SLPoint sl_point;
+  sl_point.set_s(clear_zone_overlap.start_s);
+  sl_point.set_l(0.0);
+  common::math::Vec2d start_xy;
+  const auto& reference_line = reference_line_info_->reference_line();
+  if (!reference_line.SLToXY(sl_point, &start_xy)) {
+    AERROR << "Failed to get xy from sl: " << sl_point.DebugString();
+    return false;
+  }
+  double left_lane_width = 0.0;
+  double right_lane_width = 0.0;
+  if (!reference_line.GetLaneWidth(clear_zone_overlap.start_s, &left_lane_width,
+                                   &right_lane_width)) {
+    AERROR << "Failed to get lane width at s = " << clear_zone_overlap.start_s;
+    return false;
+  }
+  common::math::Vec2d end_xy;
+  sl_point.set_s(clear_zone_overlap.end_s);
+  if (!reference_line_info_->reference_line().SLToXY(sl_point, &end_xy)) {
+    AERROR << "Failed to get xy from sl: " << sl_point.DebugString();
+    return false;
+  }
+  common::math::Box2d clear_zone_box{
+      common::math::LineSegment2d(start_xy, end_xy),
+      left_lane_width + right_lane_width};
+  const auto obstacle_id =
+      FLAGS_clear_zone_virtual_object_id_prefix + clear_zone_overlap.object_id;
+  auto* obstacle =
+      frame_->AddStaticVirtualObstacle(obstacle_id, clear_zone_box);
+  if (!obstacle) {
+    AERROR << "Failed to create obstacle " << obstacle_id << " in frame";
+    return false;
+  }
+  auto* path_obstacle = reference_line_info_->AddObstacle(obstacle);
+  if (!path_obstacle) {
+    AERROR << "Failed to create path_obstacle for " << obstacle_id;
+    return false;
+  }
+  path_obstacle->SetStBoundaryType(StBoundary::BoundaryType::KEEP_CLEAR);
+  return true;
+}
+
+}  // namespace planning
+}  // namespace apollo

modules/planning/tasks/traffic_decider/clear_zone.h

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * @file
+ **/
+
+#ifndef MODULES_PLANNING_TASKS_TRAFFIC_DECIDER_CLEAR_ZONE_H_
+#define MODULES_PLANNING_TASKS_TRAFFIC_DECIDER_CLEAR_ZONE_H_
+
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#include "modules/perception/proto/traffic_light_detection.pb.h"
+
+#include "modules/planning/tasks/traffic_decider/traffic_rule.h"
+
+namespace apollo {
+namespace planning {
+
+/**
+ * This class creates a virtual obstacle for each keep clear region.
+ */
+class ClearZone : public TrafficRule {
+ public:
+  explicit ClearZone(const RuleConfig& config);
+  virtual ~ClearZone() = default;
+
+  bool ApplyRule(Frame* frame, ReferenceLineInfo* const reference_line_info);
+
+ private:
+  bool BuildClearZoneObstacle(const hdmap::PathOverlap& clear_zone_overlap);
+  ReferenceLineInfo* reference_line_info_;
+  Frame* frame_;
+};
+
+}  // namespace planning
+}  // namespace apollo
+
+#endif  // MODULES_PLANNING_TASKS_TRAFFIC_DECIDER_CLEAR_ZONE_H_