From 2432a7b8f254f098d911eed92f3937e33ed93c58 Mon Sep 17 00:00:00 2001
From: "C. Andy Martin" <cam@myfastmail.com>
Date: Sat, 23 Feb 2019 16:25:39 -0500
Subject: [PATCH] fixup time stamps channel to be correct

The time stamps channel "stamps" from laser_geometry was not being
adjusted to keep the stamps channel consistent with the header stamp.
Keep the header stamp the same (the time of the last assembled scan),
and adjust all the stamps to be relative to the header stamp.
---
 include/laser_assembler/base_assembler.h     | 16 +++++++++++++++-
 include/laser_assembler/base_assembler_srv.h | 15 ++++++++++++++-
 2 files changed, 29 insertions(+), 2 deletions(-)

diff --git a/include/laser_assembler/base_assembler.h b/include/laser_assembler/base_assembler.h
index 2481dd7..6f7e353 100644
--- a/include/laser_assembler/base_assembler.h
+++ b/include/laser_assembler/base_assembler.h
@@ -291,6 +291,8 @@ bool BaseAssembler<T>::assembleScans(AssembleScans::Request& req, AssembleScans:
   while ( i < scan_hist_.size() &&                                                    // Don't go past end of deque
           scan_hist_[i].header.stamp < req.end )                                      // Don't go past the end-time of the request
   {
+    // The timestamp of the cloud is the final scan
+    resp.cloud.header.stamp = scan_hist_[i].header.stamp;
     req_pts += (scan_hist_[i].points.size ()+downsample_factor_-1)/downsample_factor_ ;
     i += downsample_factor_ ;
   }
@@ -310,10 +312,15 @@ bool BaseAssembler<T>::assembleScans(AssembleScans::Request& req, AssembleScans:
     resp.cloud.points.resize (req_pts);
     const unsigned int num_channels = scan_hist_[start_index].channels.size ();
     resp.cloud.channels.resize (num_channels) ;
+    int stamps_channel = -1;
     for (i = 0; i<num_channels; i++)
     {
       resp.cloud.channels[i].name = scan_hist_[start_index].channels[i].name ;
       resp.cloud.channels[i].values.resize (req_pts) ;
+      if (resp.cloud.channels[i].name == "stamps")
+      {
+        stamps_channel = i;
+      }
     }
     //resp.cloud.header.stamp = req.end ;
     resp.cloud.header.frame_id = fixed_frame_ ;
@@ -328,6 +335,7 @@ bool BaseAssembler<T>::assembleScans(AssembleScans::Request& req, AssembleScans:
           ROS_FATAL("Trying to add a malformed point cloud. Cloud has %u points, but channel %u has %u elems", (int)scan_hist_[i].points.size (), chan_ind, (int)scan_hist_[i].channels[chan_ind].values.size ());
       }
 
+      double stamps_offset = (scan_hist_[i].header.stamp - resp.cloud.header.stamp).toSec();
       for(unsigned int j=0; j<scan_hist_[i].points.size (); j+=downsample_factor_)
       {
         resp.cloud.points[cloud_count].x = scan_hist_[i].points[j].x ;
@@ -335,11 +343,17 @@ bool BaseAssembler<T>::assembleScans(AssembleScans::Request& req, AssembleScans:
         resp.cloud.points[cloud_count].z = scan_hist_[i].points[j].z ;
 
         for (unsigned int k=0; k<num_channels; k++)
+        {
           resp.cloud.channels[k].values[cloud_count] = scan_hist_[i].channels[k].values[j] ;
+          if (stamps_channel >= 0 && k ==stamps_channel)
+          {
+            // adjust stamps to be an offset from the published header stamp
+            resp.cloud.channels[k].values[cloud_count] += stamps_offset;
+          }
+        }
 
         cloud_count++ ;
       }
-      resp.cloud.header.stamp = scan_hist_[i].header.stamp;
     }
   }
   scan_hist_mutex_.unlock() ;
diff --git a/include/laser_assembler/base_assembler_srv.h b/include/laser_assembler/base_assembler_srv.h
index 36dbb64..33eaaac 100644
--- a/include/laser_assembler/base_assembler_srv.h
+++ b/include/laser_assembler/base_assembler_srv.h
@@ -279,6 +279,7 @@ bool BaseAssemblerSrv<T>::buildCloud(AssembleScans::Request& req, AssembleScans:
   while ( i < scan_hist_.size() &&                                                    // Don't go past end of deque
           scan_hist_[i].header.stamp < req.end )                                      // Don't go past the end-time of the request
   {
+    resp.cloud.header.stamp = scan_hist_[i].header.stamp;
     req_pts += (scan_hist_[i].points.size()+downsample_factor_-1)/downsample_factor_ ;
     i += downsample_factor_ ;
   }
@@ -298,27 +299,39 @@ bool BaseAssemblerSrv<T>::buildCloud(AssembleScans::Request& req, AssembleScans:
     resp.cloud.points.resize( req_pts ) ;
     const unsigned int num_channels = scan_hist_[start_index].channels.size() ;
     resp.cloud.channels.resize(num_channels) ;
+    int stamps_channel = -1;
     for (i = 0; i<num_channels; i++)
     {
       resp.cloud.channels[i].name = scan_hist_[start_index].channels[i].name ;
       resp.cloud.channels[i].values.resize(req_pts) ;
+      if (resp.cloud.channels[i].name == "stamps")
+      {
+        stamps_channel = i;
+      }
     }
     //resp.cloud.header.stamp = req.end ;
     resp.cloud.header.frame_id = fixed_frame_ ;
     unsigned int cloud_count = 0 ;
     for (i=start_index; i<past_end_index; i+=downsample_factor_)
     {
+      double stamps_offset = (scan_hist_[i].header.stamp - resp.cloud.header.stamp).toSec();
       for(unsigned int j=0; j<scan_hist_[i].points.size(); j+=downsample_factor_)
       {
         resp.cloud.points[cloud_count].x = scan_hist_[i].points[j].x ;
         resp.cloud.points[cloud_count].y = scan_hist_[i].points[j].y ;
         resp.cloud.points[cloud_count].z = scan_hist_[i].points[j].z ;
         for (unsigned int k=0; k<num_channels; k++)
+        {
           resp.cloud.channels[k].values[cloud_count] = scan_hist_[i].channels[k].values[j] ;
+          if (stamps_channel >= 0 && k ==stamps_channel)
+          {
+            // adjust stamps to be an offset from the published header stamp
+            resp.cloud.channels[k].values[cloud_count] += stamps_offset;
+          }
+        }
 
         cloud_count++ ;
       }
-      resp.cloud.header.stamp = scan_hist_[i].header.stamp;
     }
   }
   scan_hist_mutex_.unlock() ;