Use data acquisition timestamps for Pico sensor data per nasa#412 (na…

…sa#413)
frsstar · Mar 3, 2022 · 32b420a · 32b420a
1 parent 4bfe947
commit 32b420a
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 8 deletions.
diff --git a/hardware/pico_driver/src/pico_driver.cc b/hardware/pico_driver/src/pico_driver.cc
@@ -306,7 +306,7 @@ class PicoDriverL1 : public PicoDriver, public royale::IDepthDataListener, publi
     }
     // If we have depth data, use the same mechanism as L1 to push it
     if (pub_cloud_.getNumSubscribers() > 0) {
-      cloud_.header.stamp = ros::Time::now();
+      cloud_.header.stamp.fromNSec(std::chrono::duration_cast<std::chrono::nanoseconds>(data->timeStamp).count());
       std::copy(
         reinterpret_cast<const uint8_t*>(data->points.data()),
         reinterpret_cast<const uint8_t*>(data->points.data()) + cloud_.row_step * cloud_.height,
@@ -328,7 +328,9 @@ class PicoDriverL1 : public PicoDriver, public royale::IDepthDataListener, publi
     }
     // If we have depth data, use the same mechanism as L1 to push it
     if (pub_depth_image_.getNumSubscribers() > 0) {
-      depth_image_.header.stamp = ros::Time::now();
+      // units not documented in DepthImage.hpp, maybe usecs like DepthData?
+      uint64_t stampUsecs = data->timestamp;
+      depth_image_.header.stamp.fromNSec(stampUsecs * 1000);
       std::copy(
         reinterpret_cast<const uint8_t*>(data->data.data()),
         reinterpret_cast<const uint8_t*>(data->data.data()) + depth_image_.height * depth_image_.step,
@@ -430,10 +432,21 @@ class PicoDriverL2 : public PicoDriver, public royale::IExtendedDataListener {
       ROS_WARN("data pointer = nullptr");
       return;
     }
+
+    ros::Time commonStamp(0, 0);
+    if (data->hasDepthData() && data->getDepthData() != nullptr) {
+      commonStamp.fromNSec(
+        std::chrono::duration_cast<std::chrono::nanoseconds>(data->getDepthData()->timeStamp).count());
+    } else if (data->hasIntermediateData()
+               && data->getIntermediateData() != nullptr) {
+      commonStamp.fromNSec(
+        std::chrono::duration_cast<std::chrono::nanoseconds>(data->getIntermediateData()->timeStamp).count());
+    }
+
     // If we have depth data, use the same mechanism as L1 to push it
     if (data->hasDepthData() && pub_cloud_.getNumSubscribers() > 0
       && data->getDepthData() != nullptr) {
-      cloud_.header.stamp = ros::Time::now();
+      cloud_.header.stamp = commonStamp;
        std::copy(
           reinterpret_cast<const uint8_t*>(data->getDepthData()->points.data()),
           reinterpret_cast<const uint8_t*>(data->getDepthData()->points.data()) + cloud_.row_step * cloud_.height,
@@ -443,7 +456,7 @@ class PicoDriverL2 : public PicoDriver, public royale::IExtendedDataListener {
     // If we have a listener and the extended data contains intermediate data, publish it
     if (data->hasIntermediateData() && pub_extended_.getNumSubscribers() > 0
         && data->getIntermediateData() != nullptr) {
-      extended_.header.stamp = ros::Time::now();
+      extended_.header.stamp = commonStamp;
       // Populate the modulation frequencies and exposures used to produce this data
       extended_.frequency.resize(data->getIntermediateData()->modulationFrequencies.size());
       for (size_t i = 0; i < data->getIntermediateData()->modulationFrequencies.size(); i++)

diff --git a/hardware/pico_driver/src/pico_proxy.cc b/hardware/pico_driver/src/pico_proxy.cc
@@ -182,8 +182,7 @@ class PicoProxyNodelet : public ff_util::FreeFlyerNodelet  {
   // Called when depth image data arrives
   void DepthImageCallback(const sensor_msgs::ImageConstPtr& msg) {
     // Prepare distance image
-    // TODO(oalexan1): keep same timestamp as the input?
-    distance_.header.stamp = ros::Time::now();
+    distance_.header.stamp = msg->header.stamp;
     distance_.header.frame_id = msg->header.frame_id;
     distance_.height = msg->height;
     distance_.width = msg->width;
@@ -192,8 +191,7 @@ class PicoProxyNodelet : public ff_util::FreeFlyerNodelet  {
     distance_.step = distance_.width * sizeof(uint16_t);
     distance_.data.resize(distance_.height * distance_.step);
     // Prepare confidence image
-    // TODO(oalexan1): keep same timestamp as the input?
-    confidence_.header.stamp = ros::Time::now();
+    confidence_.header.stamp = msg->header.stamp;
     confidence_.header.frame_id = msg->header.frame_id;
     confidence_.height = msg->height;
     confidence_.width = msg->width;