Fixed timestamps and used them in logdemocv

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 ---------------------------------------------------------