planning: added ScenarioContext

modules/planning/conf/scenario_stop_sign_unprotected_config.pb.txt

@@ -61,7 +61,7 @@ stage_config: {
   }
 }
 
-{
+stage_config: {
   stage_type: STOP_SIGN_UNPROTECTED_INTERSECTION_CRUISE
   enabled : true
   task_type: DP_POLY_PATH_OPTIMIZER

modules/planning/scenarios/BUILD

@@ -46,8 +46,8 @@ cc_library(
         "//modules/common",
         "//modules/planning/common:planning_common",
         "//modules/planning/scenarios/lane_follow",
-        # "//modules/planning/scenarios/side_pass",
-        # "//modules/planning/scenarios/stop_sign_unprotected",
+        "//modules/planning/scenarios/side_pass",
+        "//modules/planning/scenarios/stop_sign_unprotected",
     ],
 )
 

modules/planning/scenarios/lane_follow/lane_follow_scenario.cc

@@ -64,15 +64,6 @@ constexpr double kSpeedOptimizationFallbackCost = 2e4;
 constexpr double kStraightForwardLineCost = 10.0;
 }  // namespace
 
-LaneFollowScenario::LaneFollowScenario()
-    : Scenario(FLAGS_scenario_lane_follow_config_file) {}
-
-LaneFollowScenario::LaneFollowScenario(const std::string& config_file)
-    : Scenario(config_file) {}
-
-LaneFollowScenario::LaneFollowScenario(const ScenarioConfig& config)
-    : Scenario(config) {}
-
 std::unique_ptr<Stage> LaneFollowScenario::CreateStage(
     const ScenarioConfig::StageConfig& stage_config) {
   if (stage_config.stage_type() != ScenarioConfig::LANE_FOLLOW_DEFAULT_STAGE) {

modules/planning/scenarios/lane_follow/lane_follow_scenario.h

@@ -42,9 +42,9 @@ namespace planning {
 
 class LaneFollowScenario : public Scenario {
  public:
-  LaneFollowScenario();
-  explicit LaneFollowScenario(const std::string& config_file);
-  explicit LaneFollowScenario(const ScenarioConfig& config);
+  explicit LaneFollowScenario(const ScenarioConfig& config,
+                              const ScenarioContext* context)
+      : Scenario(config, context) {}
 
   std::unique_ptr<Stage> CreateStage(
       const ScenarioConfig::StageConfig& stage_config) override;

modules/planning/scenarios/lane_follow/lane_follow_scenario_test.cc

@@ -58,7 +58,8 @@ TEST_F(LaneFollowScenarioTest, Init) {
   ScenarioConfig config;
   EXPECT_TRUE(apollo::common::util::GetProtoFromFile(
       FLAGS_scenario_lane_follow_config_file, &config));
-  scenario_.reset(new LaneFollowScenario(config));
+  ScenarioContext context;
+  scenario_.reset(new LaneFollowScenario(config, &context));
   EXPECT_EQ(scenario_->scenario_type(), ScenarioConfig::LANE_FOLLOW);
 }
 

modules/planning/scenarios/scenario.cc

@@ -26,11 +26,13 @@
 namespace apollo {
 namespace planning {
 
-Scenario::Scenario(const std::string& config_file) {
-  CHECK(apollo::common::util::GetProtoFromFile(config_file, &config_));
-}
+Scenario::Scenario(const ScenarioConfig& config, const ScenarioContext* context)
+    : config_(config), scenario_context_(context) {}
 
-Scenario::Scenario(const ScenarioConfig& config) : config_(config) {}
+bool Scenario::LoadConfig(const std::string& config_file,
+                          ScenarioConfig* config) {
+  return apollo::common::util::GetProtoFromFile(config_file, config);
+}
 
 void Scenario::Init() {
   CHECK(!config_.stage_type().empty());

modules/planning/scenarios/scenario.h

@@ -36,6 +36,8 @@
 namespace apollo {
 namespace planning {
 
+struct ScenarioContext {};
+
 class Scenario {
  public:
   enum ScenarioStatus {
@@ -44,9 +46,11 @@ class Scenario {
     STATUS_DONE = 2,
   };
 
-  explicit Scenario(const std::string& config_file);
+  explicit Scenario(const ScenarioConfig& config,
+                    const ScenarioContext* context);
 
-  explicit Scenario(const ScenarioConfig& config);
+  static bool LoadConfig(const std::string& config_file,
+                         ScenarioConfig* config);
 
   virtual ~Scenario() = default;
 
@@ -83,6 +87,7 @@ class Scenario {
   std::unordered_map<ScenarioConfig::StageType,
                      const ScenarioConfig::StageConfig*, std::hash<int>>
       stage_config_map_;
+  const ScenarioContext* scenario_context_ = nullptr;
 };
 
 }  // namespace planning

modules/planning/scenarios/scenario_manager.cc

@@ -18,10 +18,10 @@
 
 #include <utility>
 
+#include "modules/planning/common/planning_gflags.h"
 #include "modules/planning/scenarios/lane_follow/lane_follow_scenario.h"
-// #include "modules/planning/scenarios/side_pass/side_pass_scenario.h"
-// #include
-// "modules/planning/scenarios/stop_sign_unprotected/stop_sign_unprotected.h"
+#include "modules/planning/scenarios/side_pass/side_pass_scenario.h"
+#include "modules/planning/scenarios/stop_sign_unprotected/stop_sign_unprotected.h"
 
 namespace apollo {
 namespace planning {
@@ -31,33 +31,46 @@ bool ScenarioManager::Init(
   RegisterScenarios();
   default_scenario_type_ = ScenarioConfig::LANE_FOLLOW;
   supported_scenarios_ = supported_scenarios;
-  for (const auto scenario_type : supported_scenarios_) {
-    CHECK(scenario_factory_.Contains(scenario_type));
-  }
-  current_scenario_ = scenario_factory_.CreateObject(default_scenario_type_);
-  current_scenario_->Init();
+  current_scenario_ = CreateScenario(default_scenario_type_);
   return true;
 }
 
+std::unique_ptr<Scenario> ScenarioManager::CreateScenario(
+    ScenarioConfig::ScenarioType scenario_type) {
+  std::unique_ptr<Scenario> ptr;
+  if (scenario_type == ScenarioConfig::LANE_FOLLOW) {
+    ptr.reset(
+        new LaneFollowScenario(config_map_[scenario_type], &scenario_context_));
+  } else if (scenario_type == ScenarioConfig::SIDE_PASS) {
+    ptr.reset(
+        new SidePassScenario(config_map_[scenario_type], &scenario_context_));
+  } else if (scenario_type == ScenarioConfig::STOP_SIGN_UNPROTECTED) {
+    ptr.reset(new StopSignUnprotectedScenario(config_map_[scenario_type],
+                                              &scenario_context_));
+  } else {
+    return nullptr;
+  }
+  if (ptr != nullptr) {
+    ptr->Init();
+  }
+  return ptr;
+}
+
 void ScenarioManager::RegisterScenarios() {
-  scenario_factory_.Register(ScenarioConfig::LANE_FOLLOW, []() -> Scenario* {
-    return new LaneFollowScenario();
-  });
-  // scenario_factory_.Register(ScenarioConfig::SIDE_PASS, []() -> Scenario* {
-  //   return new SidePassScenario();
-  // });
-  // scenario_factory_.Register(
-  //     ScenarioConfig::STOP_SIGN_UNPROTECTED,
-  //     []() -> Scenario* { return new StopSignUnprotectedScenario(); });
+  CHECK(Scenario::LoadConfig(FLAGS_scenario_lane_follow_config_file,
+                             &config_map_[ScenarioConfig::LANE_FOLLOW]));
+  CHECK(Scenario::LoadConfig(FLAGS_scenario_side_pass_config_file,
+                             &config_map_[ScenarioConfig::SIDE_PASS]));
+  CHECK(Scenario::LoadConfig(
+      FLAGS_scenario_stop_sign_unprotected_config_file,
+      &config_map_[ScenarioConfig::STOP_SIGN_UNPROTECTED]));
 }
 
 bool ScenarioManager::SelectChangeLaneScenario(
     const common::TrajectoryPoint& ego_point, const Frame& frame) {
   if (frame.reference_line_info().size() > 1) {
     if (current_scenario_->scenario_type() != ScenarioConfig::LANE_FOLLOW) {
-      current_scenario_ =
-          scenario_factory_.CreateObject(ScenarioConfig::LANE_FOLLOW);
-      current_scenario_->Init();
+      current_scenario_ = CreateScenario(ScenarioConfig::LANE_FOLLOW);
     }
     return true;
   } else {
@@ -77,7 +90,7 @@ bool ScenarioManager::SelectScenario(const ScenarioConfig::ScenarioType type,
   if (current_scenario_->scenario_type() == type) {
     return true;
   } else {
-    auto scenario = scenario_factory_.CreateObject(type);
+    auto scenario = CreateScenario(type);
     if (scenario->IsTransferable(*current_scenario_, ego_point, frame)) {
       current_scenario_ = std::move(scenario);
       return true;
@@ -86,9 +99,14 @@ bool ScenarioManager::SelectScenario(const ScenarioConfig::ScenarioType type,
   return false;
 }
 
+void ScenarioManager::Observe(const Frame& frame) {
+  // TODO(all) fill in observe functions
+}
+
 void ScenarioManager::Update(const common::TrajectoryPoint& ego_point,
                              const Frame& frame) {
   CHECK(!frame.reference_line_info().empty());
+  Observe(frame);
   // change lane case, currently default to LANE_FOLLOW in change lane case.
   // TODO(all) implement change lane scenario.
   if (SelectChangeLaneScenario(ego_point, frame)) {
@@ -140,7 +158,7 @@ void ScenarioManager::Update(const common::TrajectoryPoint& ego_point,
 
   // finally use default transferrable scenario.
   if (current_scenario_->scenario_type() != default_scenario_type_) {
-    current_scenario_ = scenario_factory_.CreateObject(default_scenario_type_);
+    current_scenario_ = CreateScenario(default_scenario_type_);
   }
 }
 

modules/planning/scenarios/scenario_manager.h

@@ -18,6 +18,7 @@
 
 #include <memory>
 #include <set>
+#include <unordered_map>
 
 #include "modules/planning/proto/planning_config.pb.h"
 
@@ -38,6 +39,16 @@ class ScenarioManager final {
   void Update(const common::TrajectoryPoint& ego_point, const Frame& frame);
 
  private:
+  /**
+   * This function will wake up each scenario's observe function to cache
+   * neccessary information in planning context, even when the scenario is not
+   * scheduled.
+   */
+  void Observe(const Frame& frame);
+
+  std::unique_ptr<Scenario> CreateScenario(
+      ScenarioConfig::ScenarioType scenario_type);
+
   bool SelectChangeLaneScenario(const common::TrajectoryPoint& ego_point,
                                 const Frame& frame);
   bool ReuseCurrentScenario(const common::TrajectoryPoint& ego_point,
@@ -51,12 +62,14 @@ class ScenarioManager final {
   ScenarioConfig::ScenarioType DecideCurrentScenario(
       const common::TrajectoryPoint& ego_point, const Frame& frame);
 
-  common::util::Factory<ScenarioConfig::ScenarioType, Scenario>
-      scenario_factory_;
+  std::unordered_map<ScenarioConfig::ScenarioType, ScenarioConfig,
+                     std::hash<int>>
+      config_map_;
 
   std::unique_ptr<Scenario> current_scenario_;
   ScenarioConfig::ScenarioType default_scenario_type_;
   std::set<ScenarioConfig::ScenarioType> supported_scenarios_;
+  ScenarioContext scenario_context_;
 };
 
 }  // namespace planning

modules/planning/scenarios/side_pass/side_pass_scenario.cc

@@ -102,7 +102,7 @@ std::unique_ptr<Stage> SidePassScenario::CreateStage(
   }
   auto ptr = s_stage_factory_.CreateObjectOrNull(stage_config.stage_type(),
                                                  stage_config);
-  ptr->SetContext(&context_);
+  ptr->SetContext(&side_pass_context_);
   return ptr;
 }
 

modules/planning/scenarios/side_pass/side_pass_scenario.h

@@ -45,8 +45,9 @@ class SidePassScenario : public Scenario {
     PathData path_data_;
   };
 
-  SidePassScenario() : Scenario(FLAGS_scenario_side_pass_config_file) {}
-  explicit SidePassScenario(const ScenarioConfig& config) : Scenario(config) {}
+  explicit SidePassScenario(const ScenarioConfig& config,
+                            const ScenarioContext* scenario_context)
+      : Scenario(config, scenario_context) {}
 
   bool IsTransferable(const Scenario& current_scenario,
                       const common::TrajectoryPoint& ego_point,
@@ -72,7 +73,7 @@ class SidePassScenario : public Scenario {
       Stage* (*)(const ScenarioConfig::StageConfig& stage_config)>
       s_stage_factory_;
 
-  SidePassContext context_;
+  SidePassContext side_pass_context_;
   std::vector<std::unique_ptr<Task>> tasks_;
   ScenarioConfig config_;
   bool stage_init_ = false;

modules/planning/scenarios/side_pass/side_pass_scenario_test.cc

@@ -58,7 +58,8 @@ TEST_F(SidePassScenarioTest, Init) {
   ScenarioConfig config;
   EXPECT_TRUE(apollo::common::util::GetProtoFromFile(
       FLAGS_scenario_side_pass_config_file, &config));
-  scenario_.reset(new SidePassScenario(config));
+  ScenarioContext context;
+  scenario_.reset(new SidePassScenario(config, &context));
   EXPECT_EQ(scenario_->scenario_type(), ScenarioConfig::SIDE_PASS);
 }
 

modules/planning/scenarios/stop_sign_unprotected/stop_sign_unprotected.cc

@@ -105,23 +105,6 @@ std::unique_ptr<Stage> StopSignUnprotectedScenario::CreateStage(
   return ptr;
 }
 
-void StopSignUnprotectedScenario::Observe(Frame* const frame) {
-  CHECK_NOTNULL(frame);
-
-  const auto& reference_line_info = frame->reference_line_info().front();
-
-  if (!FindNextStopSign(reference_line_info)) {
-    ADEBUG << "no stop sign found";
-    return;
-  }
-
-  GetAssociatedLanes(*next_stop_sign_);
-
-  double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();
-  adc_distance_to_stop_sign_ =
-      context_.next_stop_sign_overlap.start_s - adc_front_edge_s;
-}
-
 bool StopSignUnprotectedScenario::IsTransferable(
     const Scenario& current_scenario, const common::TrajectoryPoint& ego_point,
     const Frame& frame) const {

modules/planning/scenarios/stop_sign_unprotected/stop_sign_unprotected.h

@@ -49,12 +49,9 @@ class StopSignUnprotectedScenario : public Scenario {
         associated_lanes;
   };
 
-  StopSignUnprotectedScenario()
-      : Scenario(FLAGS_scenario_stop_sign_unprotected_config_file) {}
-  explicit StopSignUnprotectedScenario(const ScenarioConfig& config)
-      : Scenario(config) {}
-
-  void Observe(Frame* const frame);
+  explicit StopSignUnprotectedScenario(const ScenarioConfig& config,
+                                       const ScenarioContext* context)
+      : Scenario(config, context) {}
 
   std::unique_ptr<Stage> CreateStage(
       const ScenarioConfig::StageConfig& stage_config);

modules/planning/scenarios/stop_sign_unprotected/stop_sign_unprotected_test.cc

@@ -59,8 +59,9 @@ TEST_F(StopSignUnprotectedScenarioTest, Init) {
   ScenarioConfig config;
   EXPECT_TRUE(apollo::common::util::GetProtoFromFile(
       FLAGS_scenario_stop_sign_unprotected_config_file, &config));
+  ScenarioContext context;
 
-  scenario_.reset(new StopSignUnprotectedScenario(config));
+  scenario_.reset(new StopSignUnprotectedScenario(config, &context));
   EXPECT_EQ(scenario_->scenario_type(), ScenarioConfig::STOP_SIGN_UNPROTECTED);
 }