planning: fix the accumulating trajectory points bug

modules/planning/common/trajectory/trajectory_stitcher.cc

@@ -24,8 +24,8 @@
 #include <list>
 #include <utility>
 
-#include "modules/common/configs/config_gflags.h"
 #include "cybertron/common/log.h"
+#include "modules/common/configs/config_gflags.h"
 #include "modules/common/math/angle.h"
 #include "modules/common/math/quaternion.h"
 #include "modules/common/util/util.h"
@@ -169,13 +169,9 @@ std::vector<TrajectoryPoint> TrajectoryStitcher::ComputeStitchingTrajectory(
   std::size_t forward_time_index =
       prev_trajectory->QueryLowerBoundPoint(forward_rel_time);
 
-  ADEBUG << "Position matched index: " << position_matched_index;
-  ADEBUG << "Time matched index: " << time_matched_index;
-
-  auto matched_index = std::min(time_matched_index, position_matched_index);
   std::vector<TrajectoryPoint> stitching_trajectory(
       prev_trajectory->trajectory_points().begin() +
-          std::max(0, static_cast<int>(matched_index - 1)),
+          std::max(0, static_cast<int>(time_matched_index - 1)),
       prev_trajectory->trajectory_points().begin() + forward_time_index + 1);
 
   const double zero_s = stitching_trajectory.back().path_point().s();

modules/planning/std_planning.cc

@@ -395,23 +395,11 @@ Status StdPlanning::Plan(
 
   ADEBUG << "current_time_stamp: " << std::to_string(current_time_stamp);
 
-  // Navi Panner doesn't need to stitch the last path planning
-  // trajectory.Otherwise, it will cause the Dremview planning track to display
-  // flashing or bouncing
   if (FLAGS_enable_stitch_last_trajectory) {
     last_publishable_trajectory_->PrependTrajectoryPoints(
         stitching_trajectory.begin(), stitching_trajectory.end() - 1);
   }
 
-  for (size_t i = 0; i < last_publishable_trajectory_->NumOfPoints(); ++i) {
-    if (last_publishable_trajectory_->TrajectoryPointAt(i).relative_time() >
-        FLAGS_trajectory_time_high_density_period) {
-      break;
-    }
-    ADEBUG << last_publishable_trajectory_->TrajectoryPointAt(i)
-                  .ShortDebugString();
-  }
-
   last_publishable_trajectory_->PopulateTrajectoryProtobuf(trajectory_pb);
 
   best_ref_info->ExportEngageAdvice(trajectory_pb->mutable_engage_advice());