Planning: Added check for prediction time.

modules/planning/common/frame.cc

@@ -319,17 +319,11 @@ void Frame::AlignPredictionTime(const double trajectory_header_time) {
   double prediction_header_time = prediction_.header().timestamp_sec();
   ADEBUG << "prediction header: " << std::to_string(prediction_header_time);
 
-  for (int i = 0; i < prediction_.prediction_obstacle_size(); ++i) {
-    prediction::PredictionObstacle *obstacle =
-        prediction_.mutable_prediction_obstacle(i);
-    for (int j = 0; j < obstacle->trajectory_size(); ++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);
-        double abs_relative_time = point->relative_time();
-        point->set_relative_time(prediction_header_time + abs_relative_time -
-                                 trajectory_header_time);
+  for (auto &obstacle : *prediction_.mutable_prediction_obstacle()) {
+    for (auto &trajectory : *obstacle.mutable_trajectory()) {
+      for (auto &point : *trajectory.mutable_trajectory_point()) {
+        point.set_relative_time(prediction_header_time + point.relative_time() -
+                                trajectory_header_time);
       }
     }
   }

modules/planning/tasks/st_graph/st_boundary_mapper.cc

@@ -295,6 +295,16 @@ bool StBoundaryMapper::GetOverlapBoundaryPoints(
     }
     for (int i = 0; i < trajectory.trajectory_point_size(); ++i) {
       const auto& trajectory_point = trajectory.trajectory_point(i);
+      if (i > 0) {
+        const auto& pre_point = trajectory.trajectory_point(i - 1);
+        if (trajectory_point.relative_time() <= pre_point.relative_time()) {
+          AERROR << "Fail to map because prediction time is not increasing."
+                 << "current point: " << trajectory_point.ShortDebugString()
+                 << "previous point: " << pre_point.ShortDebugString();
+          return false;
+        }
+      }
+
       const Box2d obs_box = obstacle.GetBoundingBox(trajectory_point);
 
       double trajectory_point_time = trajectory_point.relative_time();