diff --git a/libuvc_camera/src/camera_driver.cpp b/libuvc_camera/src/camera_driver.cpp
index 34024f197ba15c2eaabf428653ba64d8951fd290..ade51c8aee87ea7bf5b44846ac4d84cd3a5f435d 100644
--- a/libuvc_camera/src/camera_driver.cpp
+++ b/libuvc_camera/src/camera_driver.cpp
@@ -162,7 +162,10 @@ void CameraDriver::ReconfigureCallback(UVCCameraConfig &new_config, uint32_t lev
 }
 
 void CameraDriver::ImageCallback(uvc_frame_t *frame) {
-  ros::Time timestamp = ros::Time::now();
+  ros::Time timestamp = ros::Time(frame->capture_time.tv_sec, frame->capture_time.tv_usec);
+  if ( timestamp == ros::Time(0) ) {
+    timestamp = ros::Time::now();
+  }
 
   boost::recursive_mutex::scoped_lock(mutex_);