diff --git a/multisense_ros/include/multisense_ros/camera.h b/multisense_ros/include/multisense_ros/camera.h
index 9f10c22..d8e6b6d 100644
--- a/multisense_ros/include/multisense_ros/camera.h
+++ b/multisense_ros/include/multisense_ros/camera.h
@@ -348,11 +348,21 @@ class Camera {
     //
     // Diagnostics
     diagnostic_updater::Updater diagnostic_updater_;
-    void deviceInfoDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat);
-    void deviceStatusDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat);
+    void deviceInfoDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat);
+    void deviceStatusDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat);
+    void ptpStatusDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat);
 
-    void diagnosticTimerCallback(const ros::TimerEvent &);
+    void diagnosticTimerCallback(const ros::TimerEvent&);
     ros::Timer diagnostic_trigger_;
+
+    //
+    // Timestamping and timesync settings
+    ros::Time imageTimestampToRosTime(uint32_t time_secs, uint32_t time_microsecs);
+
+    bool ptp_time_sync_ = false;
+    bool network_time_sync_ = false;
+    std::atomic_bool ptp_time_stamp_in_use_;
+    int32_t ptp_time_offset_secs_ = 0;
 };
 
 }
diff --git a/multisense_ros/src/camera.cpp b/multisense_ros/src/camera.cpp
index 22db581..6c8b2c3 100644
--- a/multisense_ros/src/camera.cpp
+++ b/multisense_ros/src/camera.cpp
@@ -302,7 +302,8 @@ Camera::Camera(Channel* driver, const std::string& tf_prefix) :
     pointcloud_max_range_(15.0),
     last_frame_id_(-1),
     border_clip_type_(BorderClip::NONE),
-    border_clip_value_(0.0)
+    border_clip_value_(0.0),
+    ptp_time_stamp_in_use_(false)
 {
     //
     // Query device and version information from sensor
@@ -319,6 +320,12 @@ Camera::Camera(Channel* driver, const std::string& tf_prefix) :
         return;
     }
 
+    //
+    // Get timesync parameters
+    device_nh_.getParam("ptp_time_offset_secs", ptp_time_offset_secs_);
+    device_nh_.getParam("ptp_time_sync", ptp_time_sync_);
+    device_nh_.getParam("network_time_sync", network_time_sync_);
+
     //
     // Get the camera config
 
@@ -735,6 +742,7 @@ Camera::Camera(Channel* driver, const std::string& tf_prefix) :
     diagnostic_updater_.setHardwareID(device_info_.name + " " + std::to_string(device_info_.hardwareRevision));
     diagnostic_updater_.add("device_info", this, &Camera::deviceInfoDiagnostic);
     diagnostic_updater_.add("device_status", this, &Camera::deviceStatusDiagnostic);
+    diagnostic_updater_.add("ptp_status", this, &Camera::ptpStatusDiagnostic);
     diagnostic_trigger_ = device_nh_.createTimer(ros::Duration(1), &Camera::diagnosticTimerCallback, this);
 }
 
@@ -827,8 +835,7 @@ void Camera::histogramCallback(const image::Header& header)
         Status status = driver_->getImageHistogram(header.frameId, mh);
         if (Status_Ok == status) {
             rh.frame_count = header.frameId;
-            rh.time_stamp  = ros::Time(header.timeSeconds,
-                                       1000 * header.timeMicroSeconds);
+            rh.time_stamp = imageTimestampToRosTime(header.timeSeconds, header.timeMicroSeconds);
             rh.width  = header.width;
             rh.height = header.height;
             switch(header.source) {
@@ -855,7 +862,7 @@ void Camera::jpegImageCallback(const image::Header& header)
         return;
     }
 
-    const ros::Time t = ros::Time(header.timeSeconds, 1000 * header.timeMicroSeconds);
+    ros::Time t = imageTimestampToRosTime(header.timeSeconds, header.timeMicroSeconds);
 
     const uint32_t height    = header.height;
     const uint32_t width     = header.width;
@@ -925,7 +932,7 @@ void Camera::disparityImageCallback(const image::Header& header)
 
     const uint32_t imageSize = (header.width * header.height * header.bitsPerPixel) / 8;
 
-    const ros::Time t = ros::Time(header.timeSeconds, 1000 * header.timeMicroSeconds);
+    ros::Time t = imageTimestampToRosTime(header.timeSeconds, header.timeMicroSeconds);
 
     switch(header.source) {
     case Source_Disparity:
@@ -1094,7 +1101,7 @@ void Camera::monoCallback(const image::Header& header)
         return;
     }
 
-    ros::Time t = ros::Time(header.timeSeconds, 1000 * header.timeMicroSeconds);
+    ros::Time t = imageTimestampToRosTime(header.timeSeconds, header.timeMicroSeconds);
 
     switch(header.source) {
     case Source_Luma_Left:
@@ -1202,7 +1209,7 @@ void Camera::rectCallback(const image::Header& header)
         return;
     }
 
-    ros::Time t = ros::Time(header.timeSeconds, 1000 * header.timeMicroSeconds);
+    ros::Time t = imageTimestampToRosTime(header.timeSeconds, header.timeMicroSeconds);
 
     switch(header.source) {
     case Source_Luma_Rectified_Left:
@@ -1334,7 +1341,7 @@ void Camera::depthCallback(const image::Header& header)
         return;
     }
 
-    const ros::Time t(header.timeSeconds, 1000 * header.timeMicroSeconds);
+    ros::Time t = imageTimestampToRosTime(header.timeSeconds, header.timeMicroSeconds);
 
     const float    bad_point = std::numeric_limits<float>::quiet_NaN();
     const uint32_t depthSize = header.height * header.width * sizeof(float);
@@ -1507,7 +1514,7 @@ void Camera::pointCloudCallback(const image::Header& header)
         return;
     }
 
-    const ros::Time t(header.timeSeconds, 1000 * header.timeMicroSeconds);
+    ros::Time t = imageTimestampToRosTime(header.timeSeconds, header.timeMicroSeconds);
 
     //
     // Resize our corresponding pointclouds if we plan on publishing them
@@ -1760,7 +1767,7 @@ void Camera::rawCamDataCallback(const image::Header& header)
             raw_cam_data_.gain              = left_luma_rect.gain;
             raw_cam_data_.exposure_time     = left_luma_rect.exposure;
             raw_cam_data_.frame_count       = left_luma_rect.frameId;
-            raw_cam_data_.time_stamp        = ros::Time(left_luma_rect.timeSeconds, 1000 * left_luma_rect.timeMicroSeconds);
+            raw_cam_data_.time_stamp        = imageTimestampToRosTime(header.timeSeconds, header.timeMicroSeconds);
             raw_cam_data_.width             = left_luma_rect.width;
             raw_cam_data_.height            = left_luma_rect.height;
 
@@ -1789,7 +1796,7 @@ void Camera::colorImageCallback(const image::Header& header)
         return;
     }
 
-    const ros::Time t(header.timeSeconds, 1000 * header.timeMicroSeconds);
+    ros::Time t = imageTimestampToRosTime(header.timeSeconds, header.timeMicroSeconds);
 
     switch (header.source)
     {
@@ -2080,6 +2087,25 @@ void Camera::groundSurfaceSplineCallback(const ground_surface::Header& header)
     ground_surface_spline_pub_.publish(ground_surface_utilities::eigenToPointcloud(eigen_pcl, frame_id_origin_));
 }
 
+ros::Time Camera::imageTimestampToRosTime(uint32_t time_secs, uint32_t time_microsecs)
+{
+    // When camera has ptp enabled and there is no ptp lock,
+    // the image timestamp equals 0
+    if (time_secs == 0)
+    {
+        return ros::Time::now();
+    }
+
+    auto time_stamp = ros::Time(time_secs, 1000 * time_microsecs);
+    if (ptp_time_sync_ && time_secs > std::abs(ptp_time_offset_secs_))
+    {
+        time_stamp += ros::Duration(ptp_time_offset_secs_);
+        ptp_time_stamp_in_use_ = true;
+    }
+
+    return time_stamp;
+}
+
 void Camera::updateConfig(const image::Config& config)
 {
     stereo_calibration_manager_->updateConfig(config);
@@ -2264,6 +2290,25 @@ void Camera::deviceStatusDiagnostic(diagnostic_updater::DiagnosticStatusWrapper&
     }
 }
 
+void Camera::ptpStatusDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat)
+{
+    stat.add("ptp enabled", ptp_time_sync_);
+    stat.add("ptp status",  ptp_time_stamp_in_use_);
+    if (!ptp_time_sync_)
+    {
+        stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "PTP status: Disabled.");
+        return;
+    }
+
+    if (ptp_time_stamp_in_use_)
+    {
+        stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "PTP status: OK");
+    } else {
+        stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "PTP status: Uncalibrated");
+    }
+    ptp_time_stamp_in_use_ = false;
+}
+
 void Camera::diagnosticTimerCallback(const ros::TimerEvent&)
 {
     diagnostic_updater_.update();