From 404772946eeab57d5c9e08dbf74f58200357f7a3 Mon Sep 17 00:00:00 2001
From: "Simon D. Levy" <simon.d.levy@gmail.com>
Date: Sun, 21 Sep 2014 15:04:27 -0400
Subject: [PATCH] Create log2png.py

---
 examples/log2png.py | 155 ++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 155 insertions(+)
 create mode 100644 examples/log2png.py

diff --git a/examples/log2png.py b/examples/log2png.py
new file mode 100644
index 0000000..79eb613
--- /dev/null
+++ b/examples/log2png.py
@@ -0,0 +1,155 @@
+#!/usr/bin/env python
+
+'''
+log2png.py : BreezySLAM Python demo.  Reads logfile with odometry and scan data
+             from Paris Mines Tech and produces a .PNG image file showing robot 
+             trajectory and final map.
+             
+For details see
+
+    @inproceedings{coreslam-2010,
+      author    = {Bruno Steux and Oussama El Hamzaoui},
+      title     = {CoreSLAM: a SLAM Algorithm in less than 200 lines of C code},
+      booktitle = {11th International Conference on Control, Automation, 
+                   Robotics and Vision, ICARCV 2010, Singapore, 7-10 
+                   December 2010, Proceedings},
+      pages     = {1975-1979},
+      publisher = {IEEE},
+      year      = {2010}
+    }
+                 
+Copyright (C) 2014 Simon D. Levy
+
+This code is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as 
+published by the Free Software Foundation, either version 3 of the 
+License, or (at your option) any later version.
+
+This code is distributed in the hope that it will be useful,     
+but WITHOUT ANY WARRANTY without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License 
+along with this code.  If not, see <http://www.gnu.org/licenses/>.
+
+Change log:
+
+20-APR-2014 - Simon D. Levy - Get params from command line
+05-JUN-2014 - SDL - get random seed from command line
+'''
+
+# Map size, scale
+MAP_SIZE_PIXELS          = 800
+MAP_SIZE_METERS          =  32
+
+from breezyslam.algorithms import Deterministic_SLAM, RMHC_SLAM
+from breezyslam.components import Laser
+from breezyslam.robots import WheeledRobot
+
+from mines import MinesLaser, Rover, load_data
+from progressbar import ProgressBar
+
+from sys import argv, exit, stdout
+from time import time
+
+import Image
+
+def main():
+	    
+    # Bozo filter for input args
+    if len(argv) < 3:
+        print('Usage:   %s <dataset> <use_odometry> <random_seed>' % argv[0])
+        print('Example: %s exp2 1 9999' % argv[0])
+        exit(1)
+    
+    # Grab input args
+    dataset = argv[1]
+    use_odometry  =  True if int(argv[2]) else False
+    seed =  int(argv[3]) if len(argv) > 3 else 0
+    
+	# Load the data from the file    
+    lidars, odometries = load_data('.', dataset)
+    
+    # Build a robot model if we want odometry
+    robot = Rover() if use_odometry else None
+        
+    # Create a CoreSLAM object with laser params and optional robot object
+    slam = RMHC_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed=seed) \
+           if seed \
+           else Deterministic_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
+           
+    # Report what we're doing
+    nscans = len(lidars)
+    print('Processing %d scans with%s odometry / with%s particle filter...' % \
+        (nscans, \
+         '' if use_odometry else 'out', '' if seed else 'out'))
+    progbar = ProgressBar(0, nscans, 80)
+    
+    # Start with an empty trajectory of positions
+    trajectory = []
+
+    # Start timing
+    start_sec = time()
+    
+    # Loop over scans    
+    for scanno in range(nscans):
+    
+        if use_odometry:
+                  
+            # Convert odometry to velocities
+            velocities = robot.computeVelocities(odometries[scanno])
+                                 
+            # Update SLAM with lidar and velocities
+            slam.update(lidars[scanno], velocities)
+            
+        else:
+        
+            # Update SLAM with lidar alone
+            slam.update(lidars[scanno])
+                    
+        # Get new position
+        x_mm, y_mm, theta_degrees = slam.getpos()    
+        
+        # Add new position to trajectory
+        trajectory.append((x_mm, y_mm))
+        
+        # Tame impatience
+        progbar.updateAmount(scanno)
+        stdout.write('\r%s' % str(progbar))
+        stdout.flush()
+
+    # Report elapsed time
+    elapsed_sec = time() - start_sec
+    print('\n%d scans in %f sec = %f scans / sec' % (nscans, elapsed_sec, nscans/elapsed_sec))
+                    
+                                
+    # Create a byte array to receive the computed maps
+    mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
+    
+    # Get final map    
+    slam.getmap(mapbytes)
+    
+    # Put trajectory into map as black pixels
+    for coords in trajectory:
+                
+        x_mm, y_mm = coords
+                               
+        x_pix = mm2pix(x_mm)
+        y_pix = mm2pix(y_mm)
+                                                                                              
+        mapbytes[y_pix * MAP_SIZE_PIXELS + x_pix] = 0;
+                    
+    # Save map and trajectory as PNG file
+    image = Image.frombuffer('L', (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS), mapbytes, 'raw', 'L', 0, 1)
+    image.save('%s.png' % dataset)
+
+            
+# Helpers ---------------------------------------------------------        
+
+def mm2pix(mm):
+        
+    return int(mm / (MAP_SIZE_METERS * 1000. / MAP_SIZE_PIXELS))  
+    
+                    
+main()