From 486e9d459d4e08b9e399fd6cd23a2c507aae576f Mon Sep 17 00:00:00 2001
From: kalman <kalmanxiong@slightech.com>
Date: Wed, 12 Dec 2018 20:22:39 +0800
Subject: [PATCH] Fix timestamp bug and record bug in ros

---
 .../launch/mynteye.launch                     | 32 +++++++++++++++++++
 .../src/wrapper_nodelet.cc                    |  7 ++--
 2 files changed, 36 insertions(+), 3 deletions(-)

diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
index 15b79c7..9c6e49c 100644
--- a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
@@ -156,5 +156,37 @@
 
     </node>
 
+    <!-- disable compressed depth plugin for image topics -->
+    <group ns="$(arg left_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+    <group ns="$(arg left_rect_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+    <group ns="$(arg right_rect_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+    <group ns="$(arg right_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+    <group ns="$(arg disparity_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+    <group ns="$(arg disparity_norm_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+
   </group> <!-- mynteye -->
 </launch>
diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
index 189418e..8dd656d 100644
--- a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
@@ -322,9 +322,10 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
       api_->EnableStreamData(Stream::POINTS);
       api_->SetStreamCallback(
           Stream::POINTS, [this](const api::StreamData &data) {
+            ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
             static std::size_t count = 0;
             ++count;
-            publishPoints(data, count, ros::Time::now());
+            publishPoints(data, count, stamp);
           });
       is_published_[Stream::POINTS] = true;
     }
@@ -341,10 +342,10 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
       api_->EnableStreamData(stream);
       api_->SetStreamCallback(
           stream, [this, stream](const api::StreamData &data) {
-            // data.img is null, not hard timestamp
+            ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
             static std::size_t count = 0;
             ++count;
-            publishCamera(stream, data, count, ros::Time::now());
+            publishCamera(stream, data, count, stamp);
           });
       is_published_[stream] = true;
     }