Added Python support for GetSurreal XVLidar

examples/xvslam.py

@@ -0,0 +1,74 @@
+#!/usr/bin/env python
+
+'''
+xvslam.py : BreezySLAM Python with GetSurreal XV 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/ttyACM0'
+
+from breezyslam.algorithms import RMHC_SLAM
+from breezyslam.components import XVLidar as LaserModel
+
+from xvlidar import XVLidar as Lidar
+
+from cvslamshow 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)
+
+    while True:
+
+        # 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.displayRobot((x, y, theta))
+
+        trajectory.append((x,y))
+
+        # Display trajectory
+        display.displayTrajectory(trajectory)
+
+        # Exit on ESCape
+        key = display.refresh()
+        if key != None and (key&0x1A):
+            exit(0)
+     

python/breezyslam/components.py

@@ -61,4 +61,12 @@ class URG04LX(Laser):
         
         Laser.__init__(self, 682, 10, 240, 4000, detectionMargin, offsetMillimeters)
 
+class XVLidar(Laser):
+    '''
+    A class for the GetSurreal XVLidar
+    '''
+    def __init__(self, detectionMargin = 0, offsetMillimeters = 0):
+        
+        Laser.__init__(self, 360, 5.5, 360, 6000, detectionMargin, offsetMillimeters)
+