From 8ec235baf3439a05d01c8d4e5839aa6c4016b8d6 Mon Sep 17 00:00:00 2001
From: Matt Lubas <mattlubas@gmail.com>
Date: Mon, 15 Aug 2016 15:31:27 -0400
Subject: [PATCH] Fixed timestamps and used them in logdemocv

---
 examples/logdemocv.py | 15 ++++++++++++---
 examples/mines.py     |  5 ++++-
 2 files changed, 16 insertions(+), 4 deletions(-)

diff --git a/examples/logdemocv.py b/examples/logdemocv.py
index 391f324..c0772db 100755
--- a/examples/logdemocv.py
+++ b/examples/logdemocv.py
@@ -43,6 +43,7 @@ from mines import MinesLaser, Rover, load_data
 from cvslamshow import SlamShow
 
 from sys import argv, exit
+from time import sleep
 
 def main():
 	    
@@ -58,7 +59,7 @@ def main():
     seed =  int(argv[3]) if len(argv) > 3 else 0
     
 	# Load the data from the file    
-    timestamp, lidars, odometries = load_data('.', dataset)
+    timestamps, lidars, odometries = load_data('.', dataset)
     
     # Build a robot model if we want odometry
     robot = Rover() if use_odometry else None
@@ -76,6 +77,9 @@ def main():
 
     # Set up a SLAM display
     display = SlamShow(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, 'SLAM')
+
+    # Store previous timestamp to create delay for realistic movie
+    prevtime = 0
     
     # Loop over scans    
     for scanno in range(nscans):
@@ -96,7 +100,8 @@ def main():
         # Get new position
         x_mm, y_mm, theta_degrees = slam.getpos()    
 
-        print(scanno, x_mm, y_mm, theta_degrees)
+
+        #print(scanno, x_mm, y_mm, theta_degrees)
 
         # Get current map    
         slam.getmap(mapbytes)
@@ -110,7 +115,11 @@ def main():
         if key != None and (key&0x1A):
             exit(0)
  
-        # XXX Add delay for real-time plot
+        # Add delay for real-time plot
+        currtime = timestamps[scanno] / 1.e6 # Convert usec to sec
+        if prevtime > 0:
+            sleep(currtime-prevtime)
+        prevtime = currtime
     
             
 # Helpers ---------------------------------------------------------        
diff --git a/examples/mines.py b/examples/mines.py
index 33c626d..a17f5da 100644
--- a/examples/mines.py
+++ b/examples/mines.py
@@ -65,10 +65,13 @@ def load_data(datadir, dataset):
             
         toks = s.split()[0:-1] # ignore ''
 
-        odometry = (int(toks[0]), int(toks[2]), int(toks[3]))
+        timestamp = int(toks[0])
+
+        odometry = timestamp, int(toks[2]), int(toks[3])
                         
         lidar = [int(tok) for tok in toks[24:]]
 
+        timestamps.append(timestamp)
         scans.append(lidar)
         odometries.append(odometry)