feature : add clear function for path data and speed data

modules/planning/common/path/path_data.cc

@@ -90,6 +90,11 @@ bool PathData::get_path_point_with_ref_s(
   return true;
 }
 
+void PathData::Clear() {
+  discretized_path_ = DiscretizedPath();
+  frenet_path_ = FrenetFramePath();
+}
+
 std::string PathData::DebugString() const {
   std::ostringstream sout;
   sout << "[" << std::endl;

modules/planning/common/path/path_data.h

@@ -49,9 +49,10 @@ class PathData {
   bool get_path_point_with_ref_s(const double ref_s,
                                  common::PathPoint *const path_point) const;
 
+  void Clear();
+
   std::string DebugString() const;
 
-  // TODO(fanhaoyang) add check if the path data is valid
  private:
   DiscretizedPath discretized_path_;
 

modules/planning/common/speed/speed_data.cc

@@ -88,6 +88,10 @@ double SpeedData::total_time() const {
   return speed_vector_.back().t() - speed_vector_.front().t();
 }
 
+void SpeedData::Clear() {
+  speed_vector_.clear();
+}
+
 std::string SpeedData::DebugString() const {
   std::ostringstream sout;
   sout << "[" << std::endl;

modules/planning/common/speed/speed_data.h

@@ -48,6 +48,7 @@ class SpeedData {
 
   double total_time() const;
 
+  void Clear();
  private:
   SpeedPoint interpolate(const SpeedPoint& left, const SpeedPoint& right,
                          const double weight) const;

modules/planning/common/trajectory/publishable_trajectory.cc

@@ -50,7 +50,7 @@ void PublishableTrajectory::set_header_time(const double header_time) {
 ADCTrajectory PublishableTrajectory::to_trajectory_protobuf() const {
   ADCTrajectory trajectory_pb;
   trajectory_pb.mutable_header()->set_timestamp_sec(_header_time);
-  trajectory_pb.mutable_trajectory_point()->MergeFrom(
+  trajectory_pb.mutable_trajectory_point()->CopyFrom(
       {_trajectory_points.begin(), _trajectory_points.end()});
   return trajectory_pb;
 }
@@ -58,7 +58,7 @@ ADCTrajectory PublishableTrajectory::to_trajectory_protobuf() const {
 void PublishableTrajectory::populate_trajectory_protobuf(
     ADCTrajectory* trajectory_pb) const {
   trajectory_pb->mutable_header()->set_timestamp_sec(_header_time);
-  trajectory_pb->mutable_trajectory_point()->MergeFrom(
+  trajectory_pb->mutable_trajectory_point()->CopyFrom(
       {_trajectory_points.begin(), _trajectory_points.end()});
 }
 

modules/planning/conf/planning_config.pb.txt

 planner_type : EM em_planner_config {
 optimizer : DP_POLY_PATH_OPTIMIZER
-#optimizer : DP_ST_SPEED_OPTIMIZER
-#optimizer : QP_SPLINE_PATH_OPTIMIZER
+optimizer : DP_ST_SPEED_OPTIMIZER
+optimizer : QP_SPLINE_PATH_OPTIMIZER
 optimizer : QP_SPLINE_ST_SPEED_OPTIMIZER
 }

modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.cc

@@ -80,6 +80,7 @@ Status QpSplineStGraph::search(const StGraphData& st_graph_data,
   }
 
   // extract output
+  speed_data->Clear();
   const Spline1d& spline = _spline_generator->spline();
 
   double time_resolution = _qp_spline_st_speed_config.output_time_resolution();