From 779d3ed1cbd96aa9868183e2b6a35a7970344893 Mon Sep 17 00:00:00 2001
From: Zachary Taylor <zacharyjeremytaylor@gmail.com>
Date: Wed, 12 Sep 2018 09:30:16 +0200
Subject: [PATCH] added ptp sync

---
 ouster_client/src/os1.cpp   |  2 ++
 ouster_ros/src/os1_node.cpp | 26 +++++++++++++++++++++++---
 2 files changed, 25 insertions(+), 3 deletions(-)

diff --git a/ouster_client/src/os1.cpp b/ouster_client/src/os1.cpp
index cc695c7b..35291890 100644
--- a/ouster_client/src/os1.cpp
+++ b/ouster_client/src/os1.cpp
@@ -142,6 +142,8 @@ std::shared_ptr<client> init_client(const std::string& hostname,
     success &= do_cmd("set_udp_port_lidar", std::to_string(lidar_port));
     success &= do_cmd("set_udp_port_imu", std::to_string(imu_port));
     success &= do_cmd("set_udp_ip", udp_dest_host);
+    success &= do_cmd("set_timestamp_mode", "TIME_FROM_PTP_1588");
+    success &= do_cmd("reinitialize", "");
 
     if (!success) return std::shared_ptr<client>();
 
diff --git a/ouster_ros/src/os1_node.cpp b/ouster_ros/src/os1_node.cpp
index 77064013..7e4167d2 100644
--- a/ouster_ros/src/os1_node.cpp
+++ b/ouster_ros/src/os1_node.cpp
@@ -33,6 +33,20 @@
 using ns = std::chrono::nanoseconds;
 using PacketMsg = ouster_ros::PacketMsg;
 
+bool validTimestamp(const ros::Time& msg_time) {
+  const ros::Duration kMaxTimeOffset(1.0);
+
+  const ros::Time now = ros::Time::now();
+  if (msg_time < (now - kMaxTimeOffset)) {
+    ROS_WARN_STREAM_THROTTLE(
+        1, "OS1 clock is currently not in sync with host. Current host time: "
+               << now << " OS1 message time: " << msg_time
+               << ". Rejecting measurement.");
+    return false;
+  }
+  return true;
+}
+
 int main(int argc, char** argv) {
     ros::init(argc, argv, "ouster_driver");
     ros::NodeHandle nh("~");
@@ -49,12 +63,18 @@ int main(int argc, char** argv) {
 
     auto lidar_handler = ouster_ros::OS1::batch_packets(
         scan_dur, [&](ns scan_ts, const ouster_ros::OS1::CloudOS1& cloud) {
-            lidar_pub.publish(
-                ouster_ros::OS1::cloud_to_cloud_msg(cloud, scan_ts));
+          sensor_msgs::PointCloud2 msg =
+              ouster_ros::OS1::cloud_to_cloud_msg(cloud, scan_ts);
+          if (validTimestamp(msg.header.stamp)) {
+            lidar_pub.publish(msg);
+          }
         });
 
     auto imu_handler = [&](const PacketMsg& p) {
-        imu_pub.publish(ouster_ros::OS1::packet_to_imu_msg(p));
+        sensor_msgs::Imu msg = ouster_ros::OS1::packet_to_imu_msg(p);
+        if(validTimestamp(msg.header.stamp)){
+            imu_pub.publish(msg);
+        }
     };
 
     if (replay_mode) {