From a42b6158078f759ee83fdd8d7ec5bb11a1a3c7a0 Mon Sep 17 00:00:00 2001
From: "Dr. Simon Levy" <slevy@fred.cs.wlu.edu>
Date: Fri, 29 Jun 2018 17:45:01 -0400
Subject: [PATCH] Begin RPLidar example

---
 examples/rpslam.py | 81 ++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 81 insertions(+)
 create mode 100755 examples/rpslam.py

diff --git a/examples/rpslam.py b/examples/rpslam.py
new file mode 100755
index 0000000..e713d98
--- /dev/null
+++ b/examples/rpslam.py
@@ -0,0 +1,81 @@
+#!/usr/bin/env python3
+
+'''
+rpslam.py : BreezySLAM Python with SLAMTECH RP A1 Lidar
+                 
+Copyright (C) 2016 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/>.
+'''
+
+MAP_SIZE_PIXELS         = 500
+MAP_SIZE_METERS         = 10
+LIDAR_DEVICE            = '/dev/ttyUSB0'
+
+from breezyslam.algorithms import RMHC_SLAM
+#from breezyslam.sensors import XVLidar as LaserModel
+
+from rplidar import RPLidar as Lidar
+
+from pltslamshow import SlamShow
+
+if __name__ == '__main__':
+
+    # Connect to Lidar unit
+    lidar = Lidar(LIDAR_DEVICE)
+
+    # Create an RMHC SLAM object with a laser model and optional robot model
+    #slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
+
+    # Set up a SLAM display
+    #display = SlamShow(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, 'SLAM')
+
+    # Initialize an empty trajectory
+    trajectory = []
+
+    # Initialize empty map
+    mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
+
+    # Create an iterator to collect data from the RPLidar
+    iterator = lidar.iter_scans()
+
+    while True:
+
+        try:
+            print(next(iterator))
+
+        except KeyboardInterrupt:
+            break
+
+
+        # Update SLAM with current Lidar scan, using first element of (scan, quality) pairs
+        #slam.update([pair[0] for pair in lidar.getScan()])
+
+        # Get current robot position
+        #x, y, theta = slam.getpos()
+
+        # Get current map bytes as grayscale
+        #slam.getmap(mapbytes)
+
+        #display.displayMap(mapbytes)
+
+        #display.setPose(x, y, theta)
+
+        # Exit on ESCape
+        #key = display.refresh()
+        #if key != None and (key&0x1A):
+        #    exit(0)
+
+    lidar.stop()
+    lidar.disconnect()