Add timestamp, frame_id