If I read the code correctly, you stamp the ROS topics with the current system time, using the C++ clock_gettime. Is there a reason you use the local system time instead of the packet timestamp (when that is available)?