planning: init tasks based on the same order defined in conf

modules/planning/scenarios/BUILD

@@ -13,6 +13,7 @@ cc_library(
     deps = [
         "//modules/common",
         "//modules/planning/common:planning_common",
+        "//modules/planning/toolkits:task",
     ],
 )
 

modules/planning/scenarios/lane_follow/BUILD

@@ -29,7 +29,6 @@ cc_library(
         "//modules/planning/reference_line",
         "//modules/planning/reference_line:qp_spline_reference_line_smoother",
         "//modules/planning/scenarios:scenario_lib",
-        "//modules/planning/toolkits:task",
         "//modules/planning/toolkits/optimizers:path_optimizer",
         "//modules/planning/toolkits/optimizers:speed_optimizer",
         "//modules/planning/toolkits/optimizers/dp_poly_path:dp_poly_path_optimizer",

modules/planning/scenarios/lane_follow/lane_follow_scenario.cc

@@ -57,6 +57,8 @@ using common::TrajectoryPoint;
 using common::math::Vec2d;
 using common::time::Clock;
 
+int LaneFollowScenario::current_stage_index_ = 0;
+
 namespace {
 constexpr double kPathOptimizationFallbackCost = 2e4;
 constexpr double kSpeedOptimizationFallbackCost = 2e4;
@@ -91,29 +93,13 @@ bool LaneFollowScenario::Init() {
   CHECK(apollo::common::util::GetProtoFromFile(
       FLAGS_scenario_lane_follow_config_file, &config_));
 
-  // get all active tasks
-  std::vector<TaskType> tasks;
-  CHECK_GT(config_.stage_size(), 0);
-  // TODO(All): deal with multiple stages.
-  for (int i = 0; i < config_.stage(0).task_size(); ++i) {
-    tasks.push_back(config_.stage(0).task(i));
-  }
-
-  // init task with conf
-  for (int i = 0; i < config_.scenario_task_config_size(); ++i) {
-    auto task_cfg = config_.scenario_task_config(i);
-    TaskType task_type = task_cfg.task_type();
-    if (std::find(tasks.begin(), tasks.end(), task_type) != tasks.end()) {
-      tasks_.emplace_back(task_factory_.CreateObject(task_type));
-      AINFO << "Created task:" << TaskType_Name(task_type);
-      if (!tasks_.back()->Init(task_cfg)) {
-        AERROR << "Init task[" << TaskType_Name(task_type) << "] failed.";
-        return false;
-      }
-    }
+  if (!InitTasks(config_, current_stage_index_, &tasks_)) {
+    return false;
   }
 
   is_init_ = true;
+  status_ = STATUS_INITED;
+
   return true;
 }
 
@@ -166,6 +152,8 @@ void LaneFollowScenario::RecordDebugInfo(ReferenceLineInfo* reference_line_info,
 
 Status LaneFollowScenario::Process(const TrajectoryPoint& planning_start_point,
                                    Frame* frame) {
+  status_ = STATUS_PROCESSING;
+
   bool has_drivable_reference_line = false;
   bool disable_low_priority_path = false;
   auto status =

modules/planning/scenarios/lane_follow/lane_follow_scenario.h

@@ -26,7 +26,6 @@
 
 #include "modules/common/proto/pnc_point.pb.h"
 #include "modules/planning/proto/planning.pb.h"
-#include "modules/planning/proto/planning_config.pb.h"
 
 #include "modules/common/status/status.h"
 #include "modules/common/util/factory.h"
@@ -75,12 +74,10 @@ class LaneFollowScenario : public Scenario {
   void RecordDebugInfo(ReferenceLineInfo* reference_line_info,
                        const std::string& name, const double time_diff_ms);
 
-  apollo::common::util::Factory<TaskType, Task> task_factory_;
-
+ private:
+  static int current_stage_index_;
   std::vector<std::unique_ptr<Task>> tasks_;
-
   ScenarioConfig config_;
-
   SpeedProfileGenerator speed_profile_generator_;
 };
 

modules/planning/scenarios/scenario.cc

@@ -27,5 +27,38 @@ ScenarioConfig::ScenarioType Scenario::scenario_type() const {
   return scenario_type_;
 }
 
+bool Scenario::InitTasks(const ScenarioConfig& config,
+                         const int current_stage_index,
+                         std::vector<std::unique_ptr<Task>>* tasks) {
+  CHECK_GT(config.stage_size(), current_stage_index);
+  ScenarioConfig::Stage stage = config.stage(current_stage_index);
+
+  // get all scenario_task_configs
+  std::vector<ScenarioConfig::ScenarioTaskConfig> task_configs;
+  for (int i = 0; i < config.scenario_task_config_size(); ++i) {
+    task_configs.push_back(config.scenario_task_config(i));
+  }
+
+  // init task in the same order as defined on conf
+  for (int i = 0; i < stage.task_size(); ++i) {
+    TaskType task = stage.task(i);
+    auto task_config_it = std::find_if(
+        task_configs.begin(), task_configs.end(),
+        [task](ScenarioConfig::ScenarioTaskConfig& task_config) {
+          return task_config.task_type() == task;
+        });
+    if (task_config_it != task_configs.end()) {
+      tasks->emplace_back(task_factory_.CreateObject(task));
+      AINFO << "Created task:" << TaskType_Name(task);
+      if (!tasks->back()->Init(*task_config_it)) {
+        AERROR << "Init task[" << TaskType_Name(task) << "] failed.";
+        return false;
+      }
+    }
+  }
+
+  return true;
+}
+
 }  // namespace planning
 }  // namespace apollo

modules/planning/scenarios/scenario.h

@@ -20,20 +20,29 @@
 
 #pragma once
 
-#include <set>
+#include <memory>
 #include <string>
+#include <vector>
 
 #include "modules/planning/proto/planning_config.pb.h"
 
 #include "modules/common/status/status.h"
 #include "modules/common/util/factory.h"
 #include "modules/planning/common/frame.h"
+#include "modules/planning/toolkits/task.h"
 
 namespace apollo {
 namespace planning {
 
 class Scenario {
  public:
+  enum ScenarioStatus {
+    STATUS_UNKNOWN = 0,
+    STATUS_INITED = 1,
+    STATUS_PROCESSING = 2,
+    STATUS_DONE = 3,
+  };
+
   Scenario() = default;
 
   explicit Scenario(const ScenarioConfig::ScenarioType& scenario_type)
@@ -52,8 +61,17 @@ class Scenario {
                               const common::TrajectoryPoint& ego_point,
                               const Frame& frame) const = 0;
 
+  const ScenarioStatus& GetStatus() const { return status_; }
+
+ protected:
+  bool InitTasks(const ScenarioConfig& config,
+                 const int current_stage_index,
+                 std::vector<std::unique_ptr<Task>>* tasks);
+
  protected:
   bool is_init_ = false;
+  apollo::common::util::Factory<TaskType, Task> task_factory_;
+  ScenarioStatus status_ = STATUS_UNKNOWN;
   const ScenarioConfig::ScenarioType scenario_type_;
 };
 

modules/planning/scenarios/side_pass/BUILD

@@ -29,7 +29,6 @@ cc_library(
         "//modules/planning/reference_line",
         "//modules/planning/reference_line:qp_spline_reference_line_smoother",
         "//modules/planning/scenarios:scenario_lib",
-        "//modules/planning/toolkits:task",
         "//modules/planning/toolkits/optimizers:path_optimizer",
         "//modules/planning/toolkits/optimizers:speed_optimizer",
         "//modules/planning/toolkits/optimizers/dp_poly_path:dp_poly_path_optimizer",

modules/planning/scenarios/side_pass/side_pass_scenario.cc

@@ -56,6 +56,8 @@ using common::TrajectoryPoint;
 using common::math::Vec2d;
 using common::time::Clock;
 
+int SidePassScenario::current_stage_index_ = 0;
+
 namespace {
 constexpr double kPathOptimizationFallbackCost = 2e4;
 constexpr double kSpeedOptimizationFallbackCost = 2e4;
@@ -85,34 +87,28 @@ bool SidePassScenario::Init() {
   CHECK(apollo::common::util::GetProtoFromFile(
       FLAGS_scenario_side_pass_config_file, &config_));
 
-  // get all active tasks
-  std::vector<TaskType> tasks;
-  CHECK_GT(config_.stage_size(), 0);
-  // TODO(All): deal with multiple stages.
-  for (int i = 0; i < config_.stage(0).task_size(); ++i) {
-    tasks.push_back(config_.stage(0).task(i));
-  }
-
-  // init task with conf
-  for (int i = 0; i < config_.scenario_task_config_size(); ++i) {
-    auto task_cfg = config_.scenario_task_config(i);
-    TaskType task_type = task_cfg.task_type();
-    if (std::find(tasks.begin(), tasks.end(), task_type) != tasks.end()) {
-      tasks_.emplace_back(task_factory_.CreateObject(task_type));
-      AINFO << "Created task:" << TaskType_Name(task_type);
-      if (!tasks_.back()->Init(task_cfg)) {
-        AERROR << "Init task[" << TaskType_Name(task_type) << "] failed.";
-        return false;
-      }
-    }
+  if (!InitTasks(config_, current_stage_index_, &tasks_)) {
+    return false;
   }
 
   is_init_ = true;
+  status_ = STATUS_INITED;
+
   return true;
 }
 
 Status SidePassScenario::Process(const TrajectoryPoint& planning_start_point,
                                  Frame* frame) {
+  status_ = STATUS_PROCESSING;
+
+  // TODO(all)
+
+  if (current_stage_index_ < config_.stage_size() - 1) {
+    current_stage_index_++;
+  } else {
+    status_ = STATUS_DONE;
+  }
+
   return Status::OK();
 }
 

modules/planning/scenarios/side_pass/side_pass_scenario.h

@@ -26,8 +26,6 @@
 
 #include "modules/common/proto/pnc_point.pb.h"
 #include "modules/planning/proto/planning.pb.h"
-#include "modules/planning/proto/planning_config.pb.h"
-
 #include "modules/common/status/status.h"
 #include "modules/common/util/factory.h"
 #include "modules/planning/common/reference_line_info.h"
@@ -65,12 +63,10 @@ class SidePassScenario : public Scenario {
 
   void RegisterTasks();
 
-  apollo::common::util::Factory<TaskType, Task> task_factory_;
-
+ private:
+  static int current_stage_index_;
   std::vector<std::unique_ptr<Task>> tasks_;
-
   ScenarioConfig config_;
-
   SpeedProfileGenerator speed_profile_generator_;
 
   SidePassStage stage_ = OBSTACLE_APPROACH;

modules/planning/scenarios/stop_sign_unprotected/BUILD

@@ -21,7 +21,6 @@ cc_library(
         "//modules/planning/math/curve1d:quartic_polynomial_curve1d",
         "//modules/planning/proto:planning_proto",
         "//modules/planning/scenarios:scenario_lib",
-        "//modules/planning/toolkits:task",
         "//modules/planning/toolkits/optimizers:path_optimizer",
         "//modules/planning/toolkits/optimizers:speed_optimizer",
         "//modules/planning/toolkits/optimizers/dp_poly_path:dp_poly_path_optimizer",

modules/planning/scenarios/stop_sign_unprotected/stop_sign_unprotected.cc

@@ -21,6 +21,8 @@
 #include "modules/planning/scenarios/stop_sign_unprotected/stop_sign_unprotected.h"  // NOINT
 
 
+#include "modules/planning/proto/planning_config.pb.h"
+
 #include "cybertron/common/log.h"
 #include "modules/common/util/file.h"
 #include "modules/planning/common/frame.h"
@@ -38,6 +40,8 @@ namespace planning {
 using common::Status;
 using common::TrajectoryPoint;
 
+int StopSignUnprotectedScenario::current_stage_index_ = 0;
+
 void StopSignUnprotectedScenario::RegisterTasks() {
   task_factory_.Register(DP_POLY_PATH_OPTIMIZER,
                          []() -> Task* { return new DpPolyPathOptimizer(); });
@@ -61,35 +65,28 @@ bool StopSignUnprotectedScenario::Init() {
   CHECK(apollo::common::util::GetProtoFromFile(
       FLAGS_scenario_stop_sign_unprotected_config_file, &config_));
 
-  // get all active tasks
-  std::vector<TaskType> tasks;
-  CHECK_GT(config_.stage_size(), 0);
-  // TODO(All): deal with multiple stages.
-  for (int i = 0; i < config_.stage(0).task_size(); ++i) {
-    tasks.push_back(config_.stage(0).task(i));
-  }
-
-  // init task with conf
-  for (int i = 0; i < config_.scenario_task_config_size(); ++i) {
-    auto task_cfg = config_.scenario_task_config(i);
-    TaskType task_type = task_cfg.task_type();
-    if (std::find(tasks.begin(), tasks.end(), task_type) != tasks.end()) {
-      tasks_.emplace_back(task_factory_.CreateObject(task_type));
-      AINFO << "Created task:" << TaskType_Name(task_type);
-      if (!tasks_.back()->Init(task_cfg)) {
-        AERROR << "Init task[" << TaskType_Name(task_type) << "] failed.";
-        return false;
-      }
-    }
+  if (!InitTasks(config_, current_stage_index_, &tasks_)) {
+    return false;
   }
 
   is_init_ = true;
+  status_ = STATUS_INITED;
+
   return true;
 }
 
 Status StopSignUnprotectedScenario::Process(
     const TrajectoryPoint& planning_start_point,
     Frame* frame) {
+  status_ = STATUS_PROCESSING;
+
+  // TODO(all)
+
+  if (current_stage_index_ < config_.stage_size() - 1) {
+    current_stage_index_++;
+  } else {
+    status_ = STATUS_DONE;
+  }
   return Status::OK();
 }
 

modules/planning/scenarios/stop_sign_unprotected/stop_sign_unprotected.h

@@ -24,13 +24,11 @@
 #include <vector>
 
 #include "modules/planning/proto/planning.pb.h"
-#include "modules/planning/proto/planning_config.pb.h"
 
 #include "modules/common/status/status.h"
 #include "modules/common/util/factory.h"
 #include "modules/planning/common/speed_profile_generator.h"
 #include "modules/planning/scenarios/scenario.h"
-#include "modules/planning/toolkits/task.h"
 
 namespace apollo {
 namespace planning {
@@ -53,12 +51,10 @@ class StopSignUnprotectedScenario : public Scenario {
  private:
   void RegisterTasks();
 
-  apollo::common::util::Factory<TaskType, Task> task_factory_;
-
+ private:
+  static int current_stage_index_;
   std::vector<std::unique_ptr<Task>> tasks_;
-
   ScenarioConfig config_;
-
   SpeedProfileGenerator speed_profile_generator_;
 };
 

modules/planning/toolkits/deciders/creep.cc

@@ -74,7 +74,7 @@ bool Creep::BuildStopDecision(
 
   double adc_front_edge_s = reference_line_info->AdcSlBoundary().end_s();
   const double creep_distance = FindCreepDistance(frame, reference_line_info);
-  double creep_stop_s = adc_front_edge_s + + creep_distance;
+  double creep_stop_s = adc_front_edge_s + creep_distance;
 
   // create virtual stop wall
   // TODO(all)