planning: initialized stages in hailing scenario

modules/planning/common/planning_gflags.cc

@@ -63,10 +63,10 @@ DEFINE_string(scenario_valet_parking_config_file,
               "/apollo/modules/planning/conf/"
               "scenario/valet_parking_config.pb.txt",
               "valet_parking scenario config file");
-DEFINE_string(scenario_hailing_config_file,
+DEFINE_string(scenario_park_and_go_config_file,
               "/apollo/modules/planning/conf/"
-              "scenario/hailing_config.pb.txt",
-              "hailing scenario config file");
+              "scenario/park_and_go_config.pb.txt",
+              "park_and_go scenario config file");
 
 DEFINE_bool(enable_scenario_bare_intersection, false,
             "enable bare_intersection scenarios in planning");
@@ -90,6 +90,9 @@ DEFINE_bool(enable_scenario_traffic_light, true,
 DEFINE_bool(enable_force_pull_over_open_space_parking_test, false,
             "enable force_pull_over_open_space_parking_test");
 
+DEFINE_bool(enable_scenario_park_and_go, false,
+            "enable park-and-go scenario in planning");
+
 DEFINE_string(traffic_rule_config_filename,
               "/apollo/modules/planning/conf/traffic_rule_config.pb.txt",
               "Traffic rule config filename");

modules/planning/common/planning_gflags.h

@@ -29,7 +29,7 @@ DECLARE_string(scenario_lane_follow_config_file);
 DECLARE_string(scenario_narrow_street_u_turn_config_file);
 DECLARE_string(scenario_pull_over_config_file);
 DECLARE_string(scenario_stop_sign_unprotected_config_file);
-DECLARE_string(scenario_hailing_config_file);
+DECLARE_string(scenario_park_and_go_config_file);
 DECLARE_string(scenario_traffic_light_protected_config_file);
 DECLARE_string(scenario_traffic_light_unprotected_left_turn_config_file);
 DECLARE_string(scenario_traffic_light_unprotected_right_turn_config_file);
@@ -38,6 +38,7 @@ DECLARE_string(scenario_valet_parking_config_file);
 
 DECLARE_bool(enable_scenario_bare_intersection);
 DECLARE_bool(enable_scenario_pull_over);
+DECLARE_bool(enable_scenario_park_and_go);
 DECLARE_bool(enable_scenario_stop_sign);
 DECLARE_bool(enable_scenario_traffic_light);
 DECLARE_bool(enable_pull_over_exit);

modules/planning/conf/scenario/hailing_config.pb.txt

-scenario_type: HAILING
\ No newline at end of file

modules/planning/conf/scenario/park_and_go_config.pb.txt

+scenario_type: PARK_AND_GO
+park_and_go_config:
+{
+}
+
+stage_type: PARK_AND_GO_CHECK
+stage_type: PARK_AND_GO_ADJUST
+stage_type: PARK_AND_GO_CRUISE
+
+stage_config:{
+    stage_type: PARK_AND_GO_CHECK
+    enabled: true
+}

modules/planning/proto/planning_config.proto

@@ -154,7 +154,7 @@ message ScenarioValetParkingConfig {
 
 message ScenarioNarrowStreetUTurnConfig {}
 
-message ScenarioHailingConfig {}
+message ScenarioParkAndGoConfig {}
 
 // scenario configs
 message ScenarioConfig {
@@ -177,7 +177,7 @@ message ScenarioConfig {
 
     // misc
     NARROW_STREET_U_TURN = 12;
-    HAILING = 13;
+    PARK_AND_GO = 13;
   }
 
   // StageType is a superset of stages from all scenarios.
@@ -219,6 +219,11 @@ message ScenarioConfig {
     // valet parking scenario
     VALET_PARKING_APPROACHING_PARKING_SPOT = 510;
     VALET_PARKING_PARKING = 511;
+
+    // park_and_go scenario
+    PARK_AND_GO_CHECK = 601;
+    PARK_AND_GO_CRUISE = 602;
+    PARK_AND_GO_ADJUST = 603;
   };
 
   message StageConfig {
@@ -228,7 +233,7 @@ message ScenarioConfig {
     repeated TaskConfig.TaskType task_type = 3;
     // an unordered task configurations
     repeated TaskConfig task_config = 4;
-  }
+  } 
 
   optional ScenarioType scenario_type = 1;
   oneof scenario_config {
@@ -241,7 +246,7 @@ message ScenarioConfig {
     ScenarioPullOverConfig pull_over_config = 9;
     ScenarioValetParkingConfig valet_parking_config = 10;
     ScenarioNarrowStreetUTurnConfig narrow_street_u_turn_config = 11;
-    ScenarioHailingConfig hailing_config = 12;
+    ScenarioParkAndGoConfig park_and_go_config = 12;
   }
   // a list of stages that are used at runtime.  The first one is default stage.
   repeated StageType stage_type = 13;

modules/planning/scenarios/hailing/BUILD → modules/planning/scenarios/park_and_go/BUILD

@@ -3,41 +3,65 @@ load("//tools:cpplint.bzl", "cpplint")
 package(default_visibility = ["//visibility:public"])
 
 cc_library(
-    name = "hailing",
+    name = "park_and_go",
     copts = ["-DMODULE_NAME=\\\"planning\\\""],
     deps = [
-        ":hailing_scenario",
+        ":park_and_go_scenario",
     ],
 )
 
 cc_library(
-    name = "hailing_scenario",
+    name = "park_and_go_scenario",
     srcs = [
-        "hailing_scenario.cc",
+        "park_and_go_scenario.cc",
+        "stage_check.cc",
     ],
     hdrs = [
-        "hailing_scenario.h",
+        "park_and_go_scenario.h",
+        "stage_check.h",
     ],
     copts = ["-DMODULE_NAME=\\\"planning\\\""],
     deps = [
         "//cyber/common:log",
+        "//external:gflags",
         "//modules/common",
+        "//modules/common/util:factory",
+        "//modules/common/vehicle_state:vehicle_state_provider",
+        "//modules/planning/common:planning_common",
+        "//modules/planning/common/util:common_lib",
+        "//modules/planning/common/util:util_lib",
         "//modules/planning/proto:planning_proto",
         "//modules/planning/scenarios:scenario",
+        "//modules/planning/scenarios/util:scenario_util_lib",
     ],
 )
 
 cc_test(
-    name = "hailing_scenario_test",
+    name = "park_and_go_scenario_test",
     size = "small",
     srcs = [
-        "hailing_scenario_test.cc",
+        "park_and_go_scenario_test.cc",
     ],
     data = [
         "//modules/planning:planning_conf",
     ],
     deps = [
-        ":hailing",
+        ":park_and_go",
+        "@gtest//:main",
+    ],
+)
+
+cc_test(
+    name = "stage_check_test",
+    size = "small",
+    srcs = [
+        "stage_check_test.cc",
+    ],
+    data = [
+        "//modules/planning:planning_conf",
+    ],
+    deps = [
+        ":park_and_go",
         "@gtest//:main",
     ],
 )

modules/planning/scenarios/park_and_go/park_and_go_scenario.cc

+/******************************************************************************
+ * Copyright 2019 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.
+ *****************************************************************************/
+
+#include "modules/planning/scenarios/park_and_go/park_and_go_scenario.h"
+
+#include "cyber/common/log.h"
+#include "modules/planning/scenarios/park_and_go/stage_check.h"
+
+namespace apollo {
+namespace planning {
+namespace scenario {
+namespace park_and_go {
+
+apollo::common::util::Factory<
+    ScenarioConfig::StageType, Stage,
+    Stage* (*)(const ScenarioConfig::StageConfig& stage_config)>
+    ParkAndGoScenario::s_stage_factory_;
+
+void ParkAndGoScenario::Init() {
+  if (init_) {
+    return;
+  }
+
+  Scenario::Init();
+
+  if (!GetScenarioConfig()) {
+    AERROR << "fail to get scenario specific config";
+    return;
+  }
+
+  init_ = true;
+}
+
+void ParkAndGoScenario::RegisterStages() {
+  if (!s_stage_factory_.Empty()) {
+    s_stage_factory_.Clear();
+  }
+  s_stage_factory_.Register(
+      ScenarioConfig::PARK_AND_GO_CHECK,
+      [](const ScenarioConfig::StageConfig& config) -> Stage* {
+        return new ParkAndGoStageCheck(config);
+      });
+}
+
+std::unique_ptr<Stage> ParkAndGoScenario::CreateStage(
+    const ScenarioConfig::StageConfig& stage_config) {
+  if (s_stage_factory_.Empty()) {
+    RegisterStages();
+  }
+  auto ptr = s_stage_factory_.CreateObjectOrNull(stage_config.stage_type(),
+                                                 stage_config);
+  if (ptr) {
+    ptr->SetContext(&context_);
+  }
+  return ptr;
+}
+
+bool ParkAndGoScenario::GetScenarioConfig() {
+  if (!config_.has_park_and_go_config()) {
+    AERROR << "miss scenario specific config";
+    return false;
+  }
+  context_.scenario_config.CopyFrom(config_.park_and_go_config());
+  return true;
+}
+
+}  // namespace park_and_go
+}  // namespace scenario
+}  // namespace planning
+}  // namespace apollo

modules/planning/scenarios/hailing/hailing_scenario.h → modules/planning/scenarios/park_and_go/park_and_go_scenario.h

@@ -17,7 +17,6 @@
 /**
  * @file
  **/
-
 #pragma once
 
 #include <memory>
@@ -27,18 +26,40 @@
 namespace apollo {
 namespace planning {
 namespace scenario {
-namespace hailing {
+namespace park_and_go {
+
+// stage context
+struct ParkAndGoContext {
+  ScenarioParkAndGoConfig scenario_config;
+};
 
-class HailingScenario : public Scenario {
+class ParkAndGoScenario : public Scenario {
  public:
-  HailingScenario(const ScenarioConfig& config, const ScenarioContext* context)
+  ParkAndGoScenario(const ScenarioConfig& config,
+                    const ScenarioContext* context)
       : Scenario(config, context) {}
 
+  void Init() override;
+
   std::unique_ptr<Stage> CreateStage(
       const ScenarioConfig::StageConfig& stage_config) override;
+
+  ParkAndGoContext* GetContext() { return &context_; }
+
+ private:
+  static void RegisterStages();
+  bool GetScenarioConfig();
+
+ private:
+  static apollo::common::util::Factory<
+      ScenarioConfig::StageType, Stage,
+      Stage* (*)(const ScenarioConfig::StageConfig& stage_config)>
+      s_stage_factory_;
+  bool init_ = false;
+  ParkAndGoContext context_;
 };
 
-}  // namespace hailing
+}  // namespace park_and_go
 }  // namespace scenario
 }  // namespace planning
 }  // namespace apollo

modules/planning/scenarios/hailing/hailing_scenario_test.cc → modules/planning/scenarios/park_and_go/park_and_go_scenario_test.cc

@@ -18,7 +18,7 @@
  * @file
  **/
 
-#include "modules/planning/scenarios/hailing/hailing_scenario.h"
+#include "modules/planning/scenarios/park_and_go/park_and_go_scenario.h"
 
 #include "gtest/gtest.h"
 
@@ -29,23 +29,23 @@
 namespace apollo {
 namespace planning {
 namespace scenario {
-namespace hailing {
+namespace park_and_go {
 
-class HailingTest : public ::testing::Test {
+class ParkAndGoTest : public ::testing::Test {
  public:
   virtual void SetUp() {}
 
  protected:
-  std::unique_ptr<HailingScenario> scenario_;
+  std::unique_ptr<ParkAndGoScenario> scenario_;
 };
 
-TEST_F(HailingTest, VerifyConf) {
+TEST_F(ParkAndGoTest, VerifyConf) {
   ScenarioConfig config;
   EXPECT_TRUE(apollo::cyber::common::GetProtoFromFile(
-      FLAGS_scenario_hailing_config_file, &config));
+      FLAGS_scenario_park_and_go_config_file, &config));
 }
 
-}  // namespace hailing
+}  // namespace park_and_go
 }  // namespace scenario
 }  // namespace planning
 }  // namespace apollo

modules/planning/scenarios/park_and_go/stage_check.cc

+/******************************************************************************
+ * Copyright 2019 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.
+ *****************************************************************************/
+
+#include "modules/planning/scenarios/park_and_go/stage_check.h"
+
+#include <string>
+#include <vector>
+
+#include "cyber/common/log.h"
+
+#include "modules/planning/common/frame.h"
+#include "modules/planning/common/planning_context.h"
+#include "modules/planning/common/util/common.h"
+#include "modules/planning/scenarios/util/util.h"
+#include "modules/planning/tasks/deciders/path_bounds_decider/path_bounds_decider.h"
+
+namespace apollo {
+namespace planning {
+namespace scenario {
+namespace park_and_go {
+
+using common::TrajectoryPoint;
+
+Stage::StageStatus ParkAndGoStageCheck::Process(
+    const TrajectoryPoint& planning_init_point, Frame* frame) {
+  return FinishStage(true);
+}
+
+Stage::StageStatus ParkAndGoStageCheck::FinishStage(const bool success) {
+  if (success) {
+    next_stage_ = ScenarioConfig::PARK_AND_GO_CRUISE;
+  } else {
+    next_stage_ = ScenarioConfig::PARK_AND_GO_ADJUST;
+  }
+  // TODO(SHU) add implementation
+  return Stage::FINISHED;
+}
+
+}  // namespace park_and_go
+}  // namespace scenario
+}  // namespace planning
+}  // namespace apollo

modules/planning/scenarios/park_and_go/stage_check.h

+/******************************************************************************
+ * Copyright 2019 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.
+ *****************************************************************************/
+
+#pragma once
+
+#include "modules/planning/proto/planning_config.pb.h"
+
+#include "modules/planning/scenarios/park_and_go/park_and_go_scenario.h"
+#include "modules/planning/scenarios/stage.h"
+
+namespace apollo {
+namespace planning {
+namespace scenario {
+namespace park_and_go {
+
+struct ParkAndGoContext;
+
+class ParkAndGoStageCheck : public Stage {
+ public:
+  explicit ParkAndGoStageCheck(const ScenarioConfig::StageConfig& config)
+      : Stage(config) {}
+
+  Stage::StageStatus Process(const common::TrajectoryPoint& planning_init_point,
+                             Frame* frame) override;
+
+  ParkAndGoContext* GetContext() {
+    return Stage::GetContextAs<ParkAndGoContext>();
+  }
+
+  Stage::StageStatus FinishStage(const bool success);
+
+ private:
+  ScenarioParkAndGoConfig scenario_config_;
+};
+
+}  // namespace park_and_go
+}  // namespace scenario
+}  // namespace planning
+}  // namespace apollo

modules/planning/scenarios/hailing/hailing_scenario.cc → modules/planning/scenarios/park_and_go/stage_check_test.cc

@@ -17,19 +17,33 @@
 /**
  * @file
  **/
-#include "modules/planning/scenarios/hailing/hailing_scenario.h"
+#include "modules/planning/scenarios/park_and_go/stage_check.h"
+
+#include "gtest/gtest.h"
+#include "modules/planning/proto/planning_config.pb.h"
 
 namespace apollo {
 namespace planning {
 namespace scenario {
-namespace hailing {
+namespace park_and_go {
+
+class ParkAndGoStageCheckTest : public ::testing::Test {
+ public:
+  virtual void SetUp() {
+    config_.set_stage_type(ScenarioConfig::PARK_AND_GO_CHECK);
+  }
+
+ protected:
+  ScenarioConfig::StageConfig config_;
+};
 
-std::unique_ptr<Stage> HailingScenario::CreateStage(
-    const ScenarioConfig::StageConfig& stage_config) {
-  return nullptr;
+TEST_F(ParkAndGoStageCheckTest, Init) {
+  ParkAndGoStageCheck park_and_go_stage_check(config_);
+  EXPECT_EQ(park_and_go_stage_check.Name(),
+            ScenarioConfig::StageType_Name(config_.stage_type()));
 }
 
-}  // namespace hailing
+}  // namespace park_and_go
 }  // namespace scenario
 }  // namespace planning
 }  // namespace apollo