added Init functions for planning optimizers.

modules/planning/optimizer/dp_poly_path/dp_poly_path_optimizer.h

@@ -34,7 +34,7 @@ class DpPolyPathOptimizer : public PathOptimizer {
  public:
   explicit DpPolyPathOptimizer(const std::string &name);
 
-  virtual bool Init();
+  virtual bool Init() override;
 
  private:
   apollo::common::Status Process(const SpeedData &speed_data,

modules/planning/optimizer/dp_st_speed/dp_st_speed_optimizer.cc

@@ -41,6 +41,11 @@ using ::apollo::localization::LocalizationEstimate;
 DpStSpeedOptimizer::DpStSpeedOptimizer(const std::string& name)
     : SpeedOptimizer(name) {}
 
+bool DpStSpeedOptimizer::Init() {
+  // TOOD: complete this function.
+  return true;
+}
+
 Status DpStSpeedOptimizer::Process(const PathData& path_data,
                                    const TrajectoryPoint& init_point,
                                    DecisionData* const decision_data,

modules/planning/optimizer/dp_st_speed/dp_st_speed_optimizer.h

@@ -32,6 +32,9 @@ class DpStSpeedOptimizer : public SpeedOptimizer {
  public:
   explicit DpStSpeedOptimizer(const std::string& name);
 
+  virtual bool Init() override;
+
+ private:
   virtual apollo::common::Status Process(const PathData& path_data,
                                          const TrajectoryPoint& init_point,
                                          DecisionData* const decision_data,

modules/planning/optimizer/qp_spline_path/qp_spline_path_optimizer.cc

@@ -30,10 +30,16 @@ using apollo::common::Status;
 QPSplinePathOptimizer::QPSplinePathOptimizer(const std::string& name)
     : PathOptimizer(name) {}
 
-Status QPSplinePathOptimizer::Process(
-    const SpeedData& speed_data, const ReferenceLine& reference_line,
-    const common::TrajectoryPoint& init_point,
-    DecisionData* const decision_data, PathData* const path_data) {
+bool QPSplinePathOptimizer::Init() {
+  is_init_ = true;
+  return true;
+}
+
+Status QPSplinePathOptimizer::Process(const SpeedData& speed_data,
+                                      const ReferenceLine& reference_line,
+                                      const common::TrajectoryPoint& init_point,
+                                      DecisionData* const decision_data,
+                                      PathData* const path_data) {
   _path_generator.SetConfig(FLAGS_qp_spline_path_config_file);
   if (!_path_generator.generate(reference_line, *decision_data, speed_data,
                                 init_point, path_data)) {

modules/planning/optimizer/qp_spline_path/qp_spline_path_optimizer.h

@@ -31,6 +31,7 @@ namespace planning {
 class QPSplinePathOptimizer : public PathOptimizer {
  public:
   explicit QPSplinePathOptimizer(const std::string& name);
+  virtual bool Init() override;
 
  private:
   apollo::common::Status Process(const SpeedData& speed_data,

modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc

@@ -39,39 +39,45 @@ using TrajectoryPoint = apollo::common::TrajectoryPoint;
 QpSplineStSpeedOptimizer::QpSplineStSpeedOptimizer(const std::string& name)
     : SpeedOptimizer(name) {}
 
-Status QpSplineStSpeedOptimizer::process(const PathData& path_data,
-                                         const TrajectoryPoint& init_point,
-                                         DecisionData* const decision_data,
-                                         SpeedData* const speed_data) const {
+bool QpSplineStSpeedOptimizer::Init() {
   // load boundary mapper
-  StBoundaryConfig st_boundary_config;
   if (!common::util::GetProtoFromFile(FLAGS_st_boundary_config_file,
-                                      &st_boundary_config)) {
+                                      &st_boundary_config_)) {
     AERROR << "Failed to load config file: " << FLAGS_st_boundary_config_file;
-    return Status(ErrorCode::PLANNING_ERROR, "Failed to load config file.");
+    return false;
   }
 
-  // load qp_spline_st_speed_config
-  QpSplineStSpeedConfig qp_spline_st_speed_config;
+  // load qp_spline_st_speed_config_
   if (!common::util::GetProtoFromFile(FLAGS_qp_spline_st_speed_config_file,
-                                      &qp_spline_st_speed_config)) {
+                                      &qp_spline_st_speed_config_)) {
     AERROR << "Failed to load config file: "
            << FLAGS_qp_spline_st_speed_config_file;
-    return Status(ErrorCode::PLANNING_ERROR, "Failed to load config file.");
+    return false;
   }
+  is_init_ = true;
+  return true;
+}
 
-  double total_length = std::min(qp_spline_st_speed_config.total_path_length(),
+Status QpSplineStSpeedOptimizer::process(const PathData& path_data,
+                                         const TrajectoryPoint& init_point,
+                                         DecisionData* const decision_data,
+                                         SpeedData* const speed_data) const {
+  if (!is_init_) {
+    AERROR << "Please call Init() before Process.";
+    return Status(ErrorCode::PLANNING_ERROR, "Not init.");
+  }
+  double total_length = std::min(qp_spline_st_speed_config_.total_path_length(),
                                  path_data.path().param_length());
 
   // step 1 get boundaries
   const auto& veh_param =
       common::config::VehicleConfigHelper::GetConfig().vehicle_param();
-  QpSplineStBoundaryMapper st_mapper(st_boundary_config, veh_param);
+  QpSplineStBoundaryMapper st_mapper(st_boundary_config_, veh_param);
   std::vector<StGraphBoundary> boundaries;
   if (st_mapper.get_graph_boundary(
           init_point, *decision_data, path_data,
-          qp_spline_st_speed_config.total_path_length(),
-          qp_spline_st_speed_config.total_time(),
+          qp_spline_st_speed_config_.total_path_length(),
+          qp_spline_st_speed_config_.total_time(),
           &boundaries) != Status::OK()) {
     return Status(ErrorCode::PLANNING_ERROR,
                   "Mapping obstacle for dp st speed optimizer failed!");
@@ -82,15 +88,15 @@ Status QpSplineStSpeedOptimizer::process(const PathData& path_data,
       apollo::common::vehicle_state::VehicleState::instance()->pose();
   const auto& hdmap = apollo::planning::DataCenter::instance()->map();
   if (st_mapper.get_speed_limits(pose, hdmap, path_data, total_length,
-                                 qp_spline_st_speed_config.total_time(),
-                                 qp_spline_st_speed_config.max_speed(),
+                                 qp_spline_st_speed_config_.total_time(),
+                                 qp_spline_st_speed_config_.max_speed(),
                                  &speed_limits) != Status::OK()) {
     return Status(ErrorCode::PLANNING_ERROR,
                   "Mapping obstacle for dp st speed optimizer failed!");
   }
 
   // step 2 perform graph search
-  QpSplineStGraph st_graph(qp_spline_st_speed_config, veh_param);
+  QpSplineStGraph st_graph(qp_spline_st_speed_config_, veh_param);
 
   StGraphData st_graph_data(boundaries, init_point, speed_limits,
                             path_data.path().param_length());

modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_speed_optimizer.h

@@ -24,8 +24,8 @@
 #include "modules/planning/optimizer/speed_optimizer.h"
 
 #include "modules/common/configs/proto/vehicle_config.pb.h"
-#include "modules/localization/proto/pose.pb.h"
 #include "modules/planning/proto/qp_spline_st_speed_config.pb.h"
+#include "modules/planning/proto/st_boundary_config.pb.h"
 
 #include "modules/map/hdmap/hdmap.h"
 
@@ -36,11 +36,15 @@ class QpSplineStSpeedOptimizer : public SpeedOptimizer {
  public:
   explicit QpSplineStSpeedOptimizer(const std::string& name);
 
+  virtual bool Init() override;
+
  private:
   virtual common::Status process(
       const PathData& path_data,
       const apollo::common::TrajectoryPoint& init_point,
       DecisionData* const decision_data, SpeedData* const speed_data) const;
+  StBoundaryConfig st_boundary_config_;
+  QpSplineStSpeedConfig qp_spline_st_speed_config_;
 };
 
 }  // namespace planning