planning: add a decider to add virtual stop fence for stop sign

modules/planning/conf/scenario_stop_sign_unprotected_config.pb.txt

@@ -7,6 +7,7 @@ stage_type: STOP_SIGN_UNPROTECTED_INTERSECTION_CRUISE
 stage_config: {
   stage_type: STOP_SIGN_UNPROTECTED_PRE_STOP
   enabled : true
+  task_type: DECIDER_STOP_SIGN
   task_type: DP_POLY_PATH_OPTIMIZER
   task_type: PATH_DECIDER
   task_type: DP_ST_SPEED_OPTIMIZER
@@ -197,372 +198,14 @@ stage_config: {
 stage_config: {
   stage_type: STOP_SIGN_UNPROTECTED_STOP
   enabled : true
-  task_type: DP_POLY_PATH_OPTIMIZER
-  task_type: PATH_DECIDER
-  task_type: DP_ST_SPEED_OPTIMIZER
-  task_type: SPEED_DECIDER
-  task_type: QP_SPLINE_ST_SPEED_OPTIMIZER
-  task_config : {
-    task_type : DP_POLY_PATH_OPTIMIZER
-    dp_poly_path_config {
-      waypoint_sampler_config {
-        sample_points_num_each_level: 7
-        step_length_max: 40.0
-        step_length_min: 20.0
-        lateral_sample_offset: 0.5
-        lateral_adjust_coeff: 0.5
-        sidepass_distance: 2.8
-        navigator_sample_num_each_level: 3
-      }
-      eval_time_interval: 0.1
-      path_resolution: 1.0
-      obstacle_ignore_distance: 20.0
-      obstacle_collision_distance: 0.5
-      obstacle_risk_distance: 2.0
-      obstacle_collision_cost: 1e8
-      path_l_cost: 6.5
-      path_dl_cost: 8e3
-      path_ddl_cost: 5e1
-      path_l_cost_param_l0: 1.50
-      path_l_cost_param_b: 0.40
-      path_l_cost_param_k: 1.5
-      path_out_lane_cost: 1e8
-      path_end_l_cost: 1.0e4
-    }
-  }
-  task_config : {
-    task_type : PATH_DECIDER
-  }
-  task_config : {
-    task_type : DP_ST_SPEED_OPTIMIZER
-    dp_st_speed_config {
-      total_path_length: 149
-      total_time: 7.0
-      matrix_dimension_s: 150
-      matrix_dimension_t: 8
-
-      speed_weight: 0.0
-      accel_weight: 10.0
-      jerk_weight: 10.0
-      obstacle_weight: 1.0
-      reference_weight: 0.0
-      go_down_buffer: 5.0
-      go_up_buffer: 5.0
-
-      default_obstacle_cost: 1e3
-
-      default_speed_cost: 1.0e3
-      exceed_speed_penalty: 10.0
-      low_speed_penalty: 10.0
-      keep_clear_low_speed_penalty: 10.0
-      accel_penalty: 1.0
-      decel_penalty: 1.0
-
-      positive_jerk_coeff: 1.0
-      negative_jerk_coeff: 1.0
-
-      max_acceleration: 3.0
-      max_deceleration: -4.0
-
-      st_boundary_config {
-        boundary_buffer: 0.1
-        high_speed_centric_acceleration_limit: 0.8
-        low_speed_centric_acceleration_limit: 1.2
-        high_speed_threshold: 12.58
-        low_speed_threshold: 7.5
-        minimal_kappa: 0.00001
-        point_extension: 1.0
-        lowest_speed: 2.5
-        num_points_to_avg_kappa: 2
-        static_obs_nudge_speed_ratio: 0.6
-        dynamic_obs_nudge_speed_ratio: 0.8
-        centri_jerk_speed_coeff: 1.0
-      }
-    }
-  }
-  task_config : {
-    task_type : SPEED_DECIDER
-    dp_st_speed_config {
-      total_path_length: 149
-      total_time: 7.0
-      matrix_dimension_s: 150
-      matrix_dimension_t: 8
-      speed_weight: 0.0
-      accel_weight: 10.0
-      jerk_weight: 10.0
-      obstacle_weight: 1.0
-      reference_weight: 0.0
-      go_down_buffer: 5.0
-      go_up_buffer: 5.0
-      default_obstacle_cost: 1e3
-      default_speed_cost: 1.0e3
-      exceed_speed_penalty: 10.0
-      low_speed_penalty: 10.0
-      keep_clear_low_speed_penalty: 10.0
-      accel_penalty: 1.0
-      decel_penalty: 1.0
-      positive_jerk_coeff: 1.0
-      negative_jerk_coeff: 1.0
-      max_acceleration: 3.0
-      max_deceleration: -4.0
-      st_boundary_config {
-        boundary_buffer: 0.1
-        high_speed_centric_acceleration_limit: 0.8
-        low_speed_centric_acceleration_limit: 1.2
-        high_speed_threshold: 12.58
-        low_speed_threshold: 7.5
-        minimal_kappa: 0.00001
-        point_extension: 1.0
-        lowest_speed: 2.5
-        num_points_to_avg_kappa: 2
-        static_obs_nudge_speed_ratio: 0.6
-        dynamic_obs_nudge_speed_ratio: 0.8
-        centri_jerk_speed_coeff: 1.0
-      }
-    }
-  }
-  task_config : {
-    task_type : QP_SPLINE_ST_SPEED_OPTIMIZER
-    qp_st_speed_config {
-      total_path_length: 250.0
-      total_time: 7.0
-      preferred_max_acceleration: 2.5
-      preferred_min_deceleration: -3.3
-      max_acceleration: 3.0
-      min_deceleration: -4.0
-
-      qp_spline_config {
-        number_of_discrete_graph_t: 4
-        spline_order: 5
-        speed_kernel_weight: 0.0
-        accel_kernel_weight: 1000.0
-        jerk_kernel_weight: 1000.0
-        follow_weight: 5.0
-        stop_weight: 0.2
-        cruise_weight: 0.3
-        regularization_weight: 0.1
-        follow_drag_distance: 17.0
-        dp_st_reference_weight: 0.0
-        init_jerk_kernel_weight: 5e4
-        yield_weight: 1e2
-        yield_drag_distance: 20.0
-      }
-      qp_piecewise_config {
-        number_of_evaluated_graph_t: 40
-        accel_kernel_weight: 1000.0
-        jerk_kernel_weight: 100.0
-        follow_weight: 5.0
-        stop_weight: 0.2
-        cruise_weight: 0.5
-        regularization_weight: 0.1
-        follow_drag_distance: 17.0
-      }
-      st_boundary_config {
-        boundary_buffer: 0.1
-        high_speed_centric_acceleration_limit: 0.8
-        low_speed_centric_acceleration_limit: 1.2
-        high_speed_threshold: 12.58
-        low_speed_threshold: 7.5
-        minimal_kappa: 0.00001
-        point_extension: 1.0
-        lowest_speed: 2.5
-        num_points_to_avg_kappa: 2
-        static_obs_nudge_speed_ratio: 0.6
-        dynamic_obs_nudge_speed_ratio: 0.8
-        centri_jerk_speed_coeff: 1.0
-      }
-    }
-  }
-  task_config : {
-    task_type : DECIDER_CREEP
-    decider_creep_config : {
-      stop_distance: 0.3
-      speed_limit: 1.0
-      max_valid_stop_distance: 0.4
-      min_boundary_t: 6.0
-    }
-  }
+  task_type: DECIDER_STOP_SIGN
 }
 
 stage_config: {
   stage_type: STOP_SIGN_UNPROTECTED_CREEP
   enabled : true
-  task_type: DP_POLY_PATH_OPTIMIZER
-  task_type: PATH_DECIDER
-  task_type: DP_ST_SPEED_OPTIMIZER
-  task_type: SPEED_DECIDER
-  task_type: QP_SPLINE_ST_SPEED_OPTIMIZER
-  task_config : {
-    task_type : DP_POLY_PATH_OPTIMIZER
-    dp_poly_path_config {
-      waypoint_sampler_config {
-        sample_points_num_each_level: 7
-        step_length_max: 40.0
-        step_length_min: 20.0
-        lateral_sample_offset: 0.5
-        lateral_adjust_coeff: 0.5
-        sidepass_distance: 2.8
-        navigator_sample_num_each_level: 3
-      }
-      eval_time_interval: 0.1
-      path_resolution: 1.0
-      obstacle_ignore_distance: 20.0
-      obstacle_collision_distance: 0.5
-      obstacle_risk_distance: 2.0
-      obstacle_collision_cost: 1e8
-      path_l_cost: 6.5
-      path_dl_cost: 8e3
-      path_ddl_cost: 5e1
-      path_l_cost_param_l0: 1.50
-      path_l_cost_param_b: 0.40
-      path_l_cost_param_k: 1.5
-      path_out_lane_cost: 1e8
-      path_end_l_cost: 1.0e4
-    }
-  }
-  task_config : {
-    task_type : PATH_DECIDER
-  }
-  task_config : {
-    task_type : DP_ST_SPEED_OPTIMIZER
-    dp_st_speed_config {
-      total_path_length: 149
-      total_time: 7.0
-      matrix_dimension_s: 150
-      matrix_dimension_t: 8
-
-      speed_weight: 0.0
-      accel_weight: 10.0
-      jerk_weight: 10.0
-      obstacle_weight: 1.0
-      reference_weight: 0.0
-      go_down_buffer: 5.0
-      go_up_buffer: 5.0
-
-      default_obstacle_cost: 1e3
-
-      default_speed_cost: 1.0e3
-      exceed_speed_penalty: 10.0
-      low_speed_penalty: 10.0
-      keep_clear_low_speed_penalty: 10.0
-      accel_penalty: 1.0
-      decel_penalty: 1.0
-
-      positive_jerk_coeff: 1.0
-      negative_jerk_coeff: 1.0
-
-      max_acceleration: 3.0
-      max_deceleration: -4.0
-
-      st_boundary_config {
-        boundary_buffer: 0.1
-        high_speed_centric_acceleration_limit: 0.8
-        low_speed_centric_acceleration_limit: 1.2
-        high_speed_threshold: 12.58
-        low_speed_threshold: 7.5
-        minimal_kappa: 0.00001
-        point_extension: 1.0
-        lowest_speed: 2.5
-        num_points_to_avg_kappa: 2
-        static_obs_nudge_speed_ratio: 0.6
-        dynamic_obs_nudge_speed_ratio: 0.8
-        centri_jerk_speed_coeff: 1.0
-      }
-    }
-  }
-  task_config : {
-    task_type : SPEED_DECIDER
-    dp_st_speed_config {
-      total_path_length: 149
-      total_time: 7.0
-      matrix_dimension_s: 150
-      matrix_dimension_t: 8
-      speed_weight: 0.0
-      accel_weight: 10.0
-      jerk_weight: 10.0
-      obstacle_weight: 1.0
-      reference_weight: 0.0
-      go_down_buffer: 5.0
-      go_up_buffer: 5.0
-      default_obstacle_cost: 1e3
-      default_speed_cost: 1.0e3
-      exceed_speed_penalty: 10.0
-      low_speed_penalty: 10.0
-      keep_clear_low_speed_penalty: 10.0
-      accel_penalty: 1.0
-      decel_penalty: 1.0
-      positive_jerk_coeff: 1.0
-      negative_jerk_coeff: 1.0
-      max_acceleration: 3.0
-      max_deceleration: -4.0
-      st_boundary_config {
-        boundary_buffer: 0.1
-        high_speed_centric_acceleration_limit: 0.8
-        low_speed_centric_acceleration_limit: 1.2
-        high_speed_threshold: 12.58
-        low_speed_threshold: 7.5
-        minimal_kappa: 0.00001
-        point_extension: 1.0
-        lowest_speed: 2.5
-        num_points_to_avg_kappa: 2
-        static_obs_nudge_speed_ratio: 0.6
-        dynamic_obs_nudge_speed_ratio: 0.8
-        centri_jerk_speed_coeff: 1.0
-      }
-    }
-  }
-  task_config : {
-    task_type : QP_SPLINE_ST_SPEED_OPTIMIZER
-    qp_st_speed_config {
-      total_path_length: 250.0
-      total_time: 7.0
-      preferred_max_acceleration: 2.5
-      preferred_min_deceleration: -3.3
-      max_acceleration: 3.0
-      min_deceleration: -4.0
-
-      qp_spline_config {
-        number_of_discrete_graph_t: 4
-        spline_order: 5
-        speed_kernel_weight: 0.0
-        accel_kernel_weight: 1000.0
-        jerk_kernel_weight: 1000.0
-        follow_weight: 5.0
-        stop_weight: 0.2
-        cruise_weight: 0.3
-        regularization_weight: 0.1
-        follow_drag_distance: 17.0
-        dp_st_reference_weight: 0.0
-        init_jerk_kernel_weight: 5e4
-        yield_weight: 1e2
-        yield_drag_distance: 20.0
-      }
-      qp_piecewise_config {
-        number_of_evaluated_graph_t: 40
-        accel_kernel_weight: 1000.0
-        jerk_kernel_weight: 100.0
-        follow_weight: 5.0
-        stop_weight: 0.2
-        cruise_weight: 0.5
-        regularization_weight: 0.1
-        follow_drag_distance: 17.0
-      }
-      st_boundary_config {
-        boundary_buffer: 0.1
-        high_speed_centric_acceleration_limit: 0.8
-        low_speed_centric_acceleration_limit: 1.2
-        high_speed_threshold: 12.58
-        low_speed_threshold: 7.5
-        minimal_kappa: 0.00001
-        point_extension: 1.0
-        lowest_speed: 2.5
-        num_points_to_avg_kappa: 2
-        static_obs_nudge_speed_ratio: 0.6
-        dynamic_obs_nudge_speed_ratio: 0.8
-        centri_jerk_speed_coeff: 1.0
-      }
-    }
-  }
+  task_type: DECIDER_CREEP
+  # task_type: PROCEED_WITH_CAUTION
   task_config : {
     task_type : DECIDER_CREEP
     decider_creep_config : {
@@ -753,13 +396,4 @@ stage_config: {
       }
     }
   }
-  task_config : {
-    task_type : DECIDER_CREEP
-    decider_creep_config : {
-      stop_distance: 0.3
-      speed_limit: 1.0
-      max_valid_stop_distance: 0.4
-      min_boundary_t: 6.0
-    }
-  }
 }

modules/planning/proto/decider_config.proto

@@ -12,3 +12,8 @@ message DeciderCreepConfig {
   // min boundary t to ignore obstacles while creeping
   optional double min_boundary_t = 4 [default = 6.0];  // second
 }
+
+message DeciderStopSignConfig {
+  // stop distance(m) to the stop line of the stop sign
+  optional double stop_distance = 1 [default = 1.0];  // meter
+}
\ No newline at end of file

modules/planning/proto/planning_config.proto

@@ -37,6 +37,7 @@ message TaskConfig {
      NAVI_OBSTACLE_DECIDER = 9;
      QP_PIECEWISE_JERK_PATH_OPTIMIZER = 10;
      DECIDER_CREEP = 11;
+     DECIDER_STOP_SIGN = 12;
    };
   optional TaskType task_type = 1;
   oneof task_config {
@@ -49,6 +50,7 @@ message TaskConfig {
     ProceedWithCautionSpeedConfig proceed_with_caution_speed_config = 8;
     QpPiecewiseJerkPathConfig qp_piecewise_jerk_path_config = 9;
     DeciderCreepConfig decider_creep_config = 10;
+    DeciderStopSignConfig decider_stop_sign_config = 11;
   }
 }
 

modules/planning/scenarios/lane_follow/lane_follow_scenario_test.cc

@@ -50,7 +50,7 @@ TEST_F(LaneFollowScenarioTest, VerifyConf) {
       FLAGS_scenario_side_pass_config_file, &config));
 }
 
-TEST_F(LaneFollowScenarioTest, InitTasks) {
+TEST_F(LaneFollowScenarioTest, Init) {
   FLAGS_scenario_lane_follow_config_file =
       "//apollo/modules/planning/testdata/conf/"
       "scenario_lane_follow_config.pb.txt";

modules/planning/scenarios/side_pass/side_pass_scenario_test.cc

@@ -50,7 +50,7 @@ TEST_F(SidePassScenarioTest, VerifyConf) {
       FLAGS_scenario_side_pass_config_file, &config));
 }
 
-TEST_F(SidePassScenarioTest, InitTasks) {
+TEST_F(SidePassScenarioTest, Init) {
   FLAGS_scenario_side_pass_config_file =
       "//apollo/modules/planning/testdata/conf/"
       "scenario_side_pass_config.pb.txt";

modules/planning/scenarios/stop_sign_unprotected/stop_sign_unprotected_test.cc

@@ -51,7 +51,7 @@ TEST_F(StopSignUnprotectedScenarioTest, VerifyConf) {
       FLAGS_scenario_side_pass_config_file, &config));
 }
 
-TEST_F(StopSignUnprotectedScenarioTest, InitTasks) {
+TEST_F(StopSignUnprotectedScenarioTest, Init) {
   FLAGS_scenario_stop_sign_unprotected_config_file =
       "//apollo/modules/planning/testdata/conf/"
       "scenario_stop_sign_unprotected_config.pb.txt";

modules/planning/toolkits/deciders/BUILD

@@ -7,14 +7,14 @@ cc_library(
     srcs = [
         "decider.cc",
         "decider_creep.cc",
-        "decider_stop_sign_monitor.cc",
+        "decider_stop_sign.cc",
         "side_pass_path_decider.cc",
         "side_pass_safety.cc",
     ],
     hdrs = [
         "decider.h",
         "decider_creep.h",
-        "decider_stop_sign_monitor.h",
+        "decider_stop_sign.h",
         "side_pass_path_decider.h",
         "side_pass_safety.h",
     ],

modules/planning/toolkits/deciders/decider.h

@@ -20,9 +20,6 @@
 
 #pragma once
 
-#include <string>
-#include <vector>
-
 #include "modules/common/status/status.h"
 #include "modules/planning/toolkits/task.h"
 

modules/planning/toolkits/deciders/decider_creep.cc

@@ -22,9 +22,6 @@
 
 #include <string>
 
-#include "modules/common/proto/pnc_point.pb.h"
-#include "modules/planning/common/frame.h"
-
 namespace apollo {
 namespace planning {
 

modules/planning/toolkits/deciders/decider_stop_sign_monitor.cc → modules/planning/toolkits/deciders/decider_stop_sign.cc

@@ -18,41 +18,91 @@
  * @file
  **/
 
-#include "modules/planning/toolkits/deciders/decider_stop_sign_monitor.h"
+#include "modules/planning/toolkits/deciders/decider_stop_sign.h"
 
 #include <string>
 
-#include "modules/common/proto/pnc_point.pb.h"
-#include "modules/planning/common/frame.h"
+#include "modules/common/util/util.h"
 
 namespace apollo {
 namespace planning {
 
 using apollo::common::ErrorCode;
 using apollo::common::Status;
+using apollo::common::util::WithinBound;
 using apollo::hdmap::PathOverlap;
 
-DeciderStopSignMonitor::DeciderStopSignMonitor(const TaskConfig& config)
-    : Decider(config) {
-  SetName("DeciderStopSignMonitor");
+DeciderStopSign::DeciderStopSign(const TaskConfig& config) : Decider(config) {
+  CHECK(config.has_decider_stop_sign_config());
+  config_ = config.decider_stop_sign_config();
+  SetName("DeciderCreep");
 }
 
-Status DeciderStopSignMonitor::Process(Frame* frame,
-                                       ReferenceLineInfo* reference_line_info) {
+Status DeciderStopSign::Process(Frame* frame,
+                             ReferenceLineInfo* reference_line_info) {
   CHECK_NOTNULL(frame);
   CHECK_NOTNULL(reference_line_info);
 
+  // TODO(all): to read from scenario context
+  const std::string stop_sign_id = "TEMP";
+  const double stop_line_s = 100;
+
+  const double stop_distance = config_.stop_distance();
+  const std::string stop_wall_id = STOP_SIGN_VO_ID_PREFIX + stop_sign_id;
+  BuildStopDecision(frame, reference_line_info,
+                    stop_wall_id, stop_line_s,
+                    stop_distance);
+
   return Status::OK();
 }
 
-void DeciderStopSignMonitor::UpdateWatchVehicles(
-    StopSignLaneVehicles* watch_vehicles) {
-  // TODo(all)
-}
+bool DeciderStopSign::BuildStopDecision(
+    Frame* const frame,
+    ReferenceLineInfo* const reference_line_info,
+    const std::string& stop_wall_id,
+    const double stop_line_s,
+    const double stop_distance) {
+  CHECK_NOTNULL(frame);
+  CHECK_NOTNULL(reference_line_info);
+
+  // check
+  const auto& reference_line = reference_line_info->reference_line();
+  if (!WithinBound(0.0, reference_line.Length(), stop_line_s)) {
+    AERROR << "stop_line_s[" << stop_line_s << "] is not on reference line";
+    return 0;
+  }
+
+  // create virtual stop wall
+  auto* obstacle =
+      frame->CreateStopObstacle(reference_line_info, stop_wall_id, stop_line_s);
+  if (!obstacle) {
+    AERROR << "Failed to create obstacle [" << stop_wall_id << "]";
+    return -1;
+  }
+  PathObstacle* stop_wall = reference_line_info->AddObstacle(obstacle);
+  if (!stop_wall) {
+    AERROR << "Failed to create path_obstacle for: " << stop_wall_id;
+    return -1;
+  }
+
+  // build stop decision
+  const double stop_s = stop_line_s - stop_distance;
+  auto stop_point = reference_line.GetReferencePoint(stop_s);
+  double stop_heading = reference_line.GetReferencePoint(stop_s).heading();
+
+  ObjectDecisionType stop;
+  auto stop_decision = stop.mutable_stop();
+  stop_decision->set_reason_code(StopReasonCode::STOP_REASON_STOP_SIGN);
+  stop_decision->set_distance_s(-stop_distance);
+  stop_decision->set_stop_heading(stop_heading);
+  stop_decision->mutable_stop_point()->set_x(stop_point.x());
+  stop_decision->mutable_stop_point()->set_y(stop_point.y());
+  stop_decision->mutable_stop_point()->set_z(0.0);
+
+  auto* path_decision = reference_line_info->path_decision();
+  path_decision->AddLongitudinalDecision(
+      "DeciderStopSign", stop_wall->Id(), stop);
 
-int DeciderStopSignMonitor::AddWatchVehicle(
-    const PathObstacle& path_obstacle, StopSignLaneVehicles* watch_vehicles) {
-  // TODo(all)
   return 0;
 }
 

modules/planning/toolkits/deciders/decider_stop_sign_monitor.h → modules/planning/toolkits/deciders/decider_stop_sign.h

@@ -21,14 +21,11 @@
 #pragma once
 
 #include <string>
-#include <unordered_map>
-#include <utility>
-#include <vector>
 
-#include "modules/planning/proto/planning_status.pb.h"
+#include "modules/planning/proto/decider_config.pb.h"
 
 #include "cybertron/common/macros.h"
-#include "modules/map/hdmap/hdmap.h"
+
 #include "modules/planning/common/frame.h"
 #include "modules/planning/common/reference_line_info.h"
 #include "modules/planning/toolkits/deciders/decider.h"
@@ -36,27 +33,23 @@
 namespace apollo {
 namespace planning {
 
-class DeciderStopSignMonitor : public Decider {
-  typedef std::unordered_map<std::string, std::vector<std::string>>
-      StopSignLaneVehicles;
-
+class DeciderStopSign : public Decider {
  public:
-  explicit DeciderStopSignMonitor(const TaskConfig& config);
+  explicit DeciderStopSign(const TaskConfig& config);
 
  private:
   apollo::common::Status Process(
       Frame* frame, ReferenceLineInfo* reference_line_info) override;
 
-  void UpdateWatchVehicles(StopSignLaneVehicles* watch_vehicles);
-  int AddWatchVehicle(const PathObstacle& path_obstacle,
-                      StopSignLaneVehicles* watch_vehicles);
+    bool BuildStopDecision(Frame* const frame,
+                         ReferenceLineInfo* const reference_line_info,
+                         const std::string& stop_wall_id,
+                         const double stop_line_s,
+                         const double stop_distance);
 
  private:
-  hdmap::PathOverlap next_stop_sign_overlap_;
-  hdmap::StopSignInfoConstPtr next_stop_sign_ = nullptr;
-  StopSignStatus::Status stop_status_;
-  std::vector<std::pair<hdmap::LaneInfoConstPtr, hdmap::OverlapInfoConstPtr>>
-      associated_lanes_;
+  static constexpr const char* STOP_SIGN_VO_ID_PREFIX = "SS_";
+  DeciderStopSignConfig config_;
 };
 
 }  // namespace planning