Planning: record frame history in planning.cc

modules/planning/common/frame.cc

@@ -130,12 +130,12 @@ const PlanningData &Frame::planning_data() const { return _planning_data; }
 
 PlanningData *Frame::mutable_planning_data() { return &_planning_data; }
 
-void Frame::set_computed_trajectory(const PublishableTrajectory &trajectory) {
-  _computed_trajectory = trajectory;
+void Frame::SetComputedTrajectory(const PublishableTrajectory &trajectory) {
+  computed_trajectory_ = trajectory;
 }
 
 const PublishableTrajectory &Frame::computed_trajectory() const {
-  return _computed_trajectory;
+  return computed_trajectory_;
 }
 
 const ReferenceLine &Frame::reference_line() const { return reference_line_; }
@@ -199,17 +199,16 @@ void Frame::AlignPredictionTime(const double trajectory_header_time) {
   double prediction_header_time = prediction_.header().timestamp_sec();
 
   for (int i = 0; i < prediction_.prediction_obstacle_size(); i++) {
-    prediction::PredictionObstacle* obstacle =
+    prediction::PredictionObstacle *obstacle =
         prediction_.mutable_prediction_obstacle(i);
     for (int j = 0; j < obstacle->trajectory_size(); j++) {
-      prediction::Trajectory* trajectory = obstacle->mutable_trajectory(j);
+      prediction::Trajectory *trajectory = obstacle->mutable_trajectory(j);
       for (int k = 0; k < trajectory->trajectory_point_size(); k++) {
-        common::TrajectoryPoint* point
-            = trajectory->mutable_trajectory_point(k);
+        common::TrajectoryPoint *point =
+            trajectory->mutable_trajectory_point(k);
         double abs_relative_time = point->relative_time();
-        point->set_relative_time(prediction_header_time
-                                     + abs_relative_time
-                                     - trajectory_header_time);
+        point->set_relative_time(prediction_header_time + abs_relative_time -
+                                 trajectory_header_time);
       }
     }
   }

modules/planning/common/frame.h

@@ -69,7 +69,7 @@ class Frame {
   const ADCTrajectory &GetADCTrajectory() const;
   ADCTrajectory *MutableADCTrajectory();
 
-  void set_computed_trajectory(const PublishableTrajectory &trajectory);
+  void SetComputedTrajectory(const PublishableTrajectory &trajectory);
   const PublishableTrajectory &computed_trajectory() const;
   const localization::Pose &VehicleInitPose() const;
 
@@ -131,7 +131,7 @@ class Frame {
   std::unique_ptr<PathDecision> path_decision_;
   uint32_t sequence_num_ = 0;
   localization::Pose init_pose_;
-  PublishableTrajectory _computed_trajectory;
+  PublishableTrajectory computed_trajectory_;
   ReferenceLine reference_line_;
   PlanningData _planning_data;
   static const hdmap::PncMap *pnc_map_;

modules/planning/common/indexed_queue.h

@@ -32,7 +32,7 @@ template <typename I, typename T>
 class IndexedQueue {
  public:
   IndexedQueue(std::size_t max_queue_size) : max_queue_size_(max_queue_size){};
-  T *Find(const I id) const {
+  const T *Find(const I id) const {
     auto iter = map_.find(id);
     if (iter == map_.end()) {
       return nullptr;

modules/planning/planning.cc

@@ -139,6 +139,7 @@ Status Planning::Start() {
   static ros::Rate loop_rate(FLAGS_planning_loop_rate);
   while (ros::ok()) {
     RunOnce();
+    FrameHistory::instance()->Add(frame_->sequence_num(), std::move(frame_));
     ros::spinOnce();
     loop_rate.sleep();
   }
@@ -253,6 +254,7 @@ bool Planning::Plan(const bool is_on_auto_mode, const double current_time_stamp,
 
   // update last publishable trajectory;
   last_publishable_trajectory_ = std::move(publishable_trajectory);
+
   return true;
 }
 

modules/planning/reference_line/reference_line_smoother.cc

@@ -70,7 +70,7 @@ bool ReferenceLineSmoother::smooth(
     return false;
   }
 
-  if (!apply_kernel()) {
+  if (!ApplyKernel()) {
     AERROR << "Add kernel for spline smoother failed.";
     return false;
   }
@@ -177,7 +177,7 @@ bool ReferenceLineSmoother::apply_constraint(
   return true;
 }
 
-bool ReferenceLineSmoother::apply_kernel() {
+bool ReferenceLineSmoother::ApplyKernel() {
   Spline2dKernel* kernel = spline_solver_->mutable_kernel();
 
   // add spline kernel

modules/planning/reference_line/reference_line_smoother.h

@@ -49,7 +49,7 @@ class ReferenceLineSmoother {
 
   bool apply_constraint(const ReferenceLine& raw_reference_line);
 
-  bool apply_kernel();
+  bool ApplyKernel();
 
   bool solve();