Rework decoding logic, fix a bug in destagger mode.
Further speed up by decoding data directly into messages instead of cv::Mat and pcl::PointCloud and then covert them to ros messages. Latency is now <0.15 ms compared to 0.7ms earlier and ouster's latency is 3ms.