planning: fix CREEP and PROCEED_WITH_CAUTION

modules/planning/conf/scenario_stop_sign_unprotected_config.pb.txt

@@ -52,7 +52,11 @@ stage_config: {
   stage_type: STOP_SIGN_UNPROTECTED_CREEP
   enabled : true
   task_type: DECIDER_CREEP
+  task_type: DP_POLY_PATH_OPTIMIZER
+  task_type: PATH_DECIDER
+  task_type: DP_ST_SPEED_OPTIMIZER
   task_type: PROCEED_WITH_CAUTION_SPEED
+  task_type: QP_SPLINE_ST_SPEED_OPTIMIZER
   task_config : {
     task_type : DECIDER_CREEP
     decider_creep_config : {
@@ -62,6 +66,18 @@ stage_config: {
       min_boundary_t: 6.0
     }
   }
+  task_config : {
+    task_type : DP_POLY_PATH_OPTIMIZER
+  }
+  task_config : {
+    task_type : PATH_DECIDER
+  }
+  task_config : {
+    task_type : DP_ST_SPEED_OPTIMIZER
+  }
+  task_config : {
+    task_type : QP_SPLINE_ST_SPEED_OPTIMIZER
+  }
   task_config : {
     task_type : PROCEED_WITH_CAUTION_SPEED
     proceed_with_caution_speed_config : {

modules/planning/scenarios/stop_sign_unprotected/stop_sign_unprotected_stage.cc

@@ -55,6 +55,7 @@ using StopSignLaneVehicles =
 
 Stage::StageStatus StopSignUnprotectedPreStop::Process(
     const TrajectoryPoint& planning_init_point, Frame* frame) {
+  ADEBUG << "stage: PreStop";
   CHECK_NOTNULL(frame);
 
   const auto& reference_line_info = frame->reference_line_info().front();
@@ -81,6 +82,7 @@ Stage::StageStatus StopSignUnprotectedPreStop::Process(
 
 Stage::StageStatus StopSignUnprotectedStop::Process(
     const TrajectoryPoint& planning_init_point, Frame* frame) {
+  ADEBUG << "stage: Stop";
   CHECK_NOTNULL(frame);
 
   auto start_time = GetContext()->stop_start_time;
@@ -118,6 +120,7 @@ Stage::StageStatus StopSignUnprotectedStop::Process(
 
 Stage::StageStatus StopSignUnprotectedCreep::Process(
     const common::TrajectoryPoint& planning_init_point, Frame* frame) {
+  ADEBUG << "stage: Creep";
   CHECK_NOTNULL(frame);
 
   if (!config_.enabled()) {
@@ -136,7 +139,8 @@ Stage::StageStatus StopSignUnprotectedCreep::Process(
 
   // set param for PROCEED_WITH_CAUTION_SPEED
   dynamic_cast<DeciderCreep*>(FindTask(TaskConfig::DECIDER_CREEP))
-      ->SetProceedWithCautionSpeedParam(*frame, reference_line_info);
+      ->SetProceedWithCautionSpeedParam(*frame, reference_line_info,
+                                        stop_sign_overlap_end_s);
 
   bool plan_ok = PlanningOnReferenceLine(planning_init_point, frame);
   if (!plan_ok) {
@@ -147,6 +151,7 @@ Stage::StageStatus StopSignUnprotectedCreep::Process(
 
 Stage::StageStatus StopSignUnprotectedIntersectionCruise::Process(
     const common::TrajectoryPoint& planning_init_point, Frame* frame) {
+  ADEBUG << "stage: IntersectionCruise";
   CHECK_NOTNULL(frame);
 
   if (GetContext()->stop_sign_id !=

modules/planning/toolkits/deciders/decider_creep.cc

@@ -107,6 +107,7 @@ bool DeciderCreep::CheckCreepDone(const Frame& frame,
                                   const double stop_sign_overlap_end_s) {
   const auto& creep_config = config_.decider_creep_config();
   bool creep_done = false;
+
   double creep_stop_s =
       stop_sign_overlap_end_s + FindCreepDistance(frame, reference_line_info);
   const double distance =
@@ -130,17 +131,23 @@ bool DeciderCreep::CheckCreepDone(const Frame& frame,
 }
 
 void DeciderCreep::SetProceedWithCautionSpeedParam(
-    const Frame& frame, const ReferenceLineInfo& reference_line_info) {
+    const Frame& frame,
+    const ReferenceLineInfo& reference_line_info,
+    const double stop_sign_overlap_end_s) {
   common::SLPoint adc_center_sl;
   reference_line_info.reference_line().XYToSL(
       {frame.vehicle_state().x(), frame.vehicle_state().y()}, &adc_center_sl);
-  const double creep_distance =
-      adc_center_sl.s() + FindCreepDistance(frame, reference_line_info);
+  double creep_stop_s =
+      stop_sign_overlap_end_s + FindCreepDistance(frame, reference_line_info);
+  const double creep_distance = creep_stop_s - adc_center_sl.s();
 
   PlanningContext::GetScenarioInfo()
       ->proceed_with_caution_speed.is_fixed_distance = true;
   PlanningContext::GetScenarioInfo()->proceed_with_caution_speed.distance =
       creep_distance;
+  ADEBUG << "creep_stop_s[" << creep_stop_s
+      << "] adc_s[" <<  adc_center_sl.s()
+      << "] creep distance[" << creep_distance << "]";
 }
 
 }  // namespace planning

modules/planning/toolkits/deciders/decider_creep.h

@@ -45,7 +45,8 @@ class DeciderCreep : public Decider {
 
   void SetProceedWithCautionSpeedParam(
       const Frame& frame,
-      const ReferenceLineInfo& reference_line_info);
+      const ReferenceLineInfo& reference_line_info,
+      const double stop_sign_overlap_end_s);
 
  private:
   double FindCreepDistance(const Frame& frame,

modules/planning/toolkits/optimizers/proceed_with_caution_speed/proceed_with_caution_speed_generator.cc

@@ -57,14 +57,9 @@ Status ProceedWithCautionSpeedGenerator::Process(
   const bool is_fixed_distance = proceed_param.is_fixed_distance;
   double proceed_distance = is_fixed_distance ?
       proceed_param.distance : path_data.discretized_path().Length();
-  const double max_distance =
-      config_.proceed_with_caution_speed_config().max_distance();
-  if (proceed_distance > max_distance) {
-    AERROR << "required distance[" << proceed_distance
-        << "] is too long. max[" << max_distance << "]";
-    return Status(ErrorCode::PLANNING_ERROR,
-                  "required proceed distance is too long");
-  }
+  proceed_distance = std::min(
+      proceed_distance,
+      config_.proceed_with_caution_speed_config().max_distance());
 
   *speed_data = SpeedProfileGenerator::GenerateFixedDistanceCreepProfile(
       proceed_distance, proceeding_speed_);