Control: add timestep in core controllers

modules/control/submodules/lat_lon_controller_submodule.cc

@@ -80,6 +80,13 @@ bool LatLonControllerSubmodule::Proc(
                                             &control_core_command);
   AERROR_IF(!status.ok()) << "Failed to produce control command:"
                           << status.error_message();
+
+  control_core_command.mutable_header()->set_lidar_timestamp(
+      preprocessor_status->header().lidar_timestamp());
+  control_core_command.mutable_header()->set_camera_timestamp(
+      preprocessor_status->trajectory().header().camera_timestamp());
+  control_core_command.mutable_header()->set_radar_timestamp(
+      preprocessor_status->trajectory().header().radar_timestamp());
   common::util::FillHeader(Name(), &control_core_command);
   control_core_writer_->Write(
       std::make_shared<ControlCommand>(control_core_command));

modules/control/submodules/mpc_controller_submodule.cc

@@ -73,8 +73,13 @@ bool MPCControllerSubmodule::Proc(
   AERROR_IF(!status.ok()) << "Failed to produce control command:"
                           << status.error_message();
 
+  control_core_command.mutable_header()->set_lidar_timestamp(
+      preprocessor_status->header().lidar_timestamp());
+  control_core_command.mutable_header()->set_camera_timestamp(
+      preprocessor_status->trajectory().header().camera_timestamp());
+  control_core_command.mutable_header()->set_radar_timestamp(
+      preprocessor_status->trajectory().header().radar_timestamp());
   common::util::FillHeader(Name(), &control_core_command);
-
   control_core_writer_->Write(control_core_command);
   return status.ok();
 }