Planning: only estimate current vehicle state if the time difference is small. (#1410)

modules/planning/planning.cc

@@ -209,7 +209,12 @@ void Planning::RunOnce() {
       VehicleStateProvider::instance()->vehicle_state();
 
   // estimate (x, y) at current timestamp
-  if (FLAGS_estimate_current_vehicle_state) {
+  // This estimate is only valid if the current time and vehicle state timestamp
+  // differs only a small amount (20ms). When the different is too large, the
+  // estimation is invalid.
+  DCHECK_GT(start_timestamp, vehicle_state.timestamp());
+  if (FLAGS_estimate_current_vehicle_state &&
+      start_timestamp - vehicle_state.timestamp() < 0.020) {
     auto future_xy = VehicleStateProvider::instance()->EstimateFuturePosition(
         start_timestamp - vehicle_state.timestamp());
     vehicle_state.set_x(future_xy.x());