diff --git a/examples/xvslam.py b/examples/xvslam.py new file mode 100755 index 0000000..83f5420 --- /dev/null +++ b/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 . +''' + +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) + diff --git a/python/breezyslam/components.py b/python/breezyslam/components.py index 2e62bcf..b234ea4 100644 --- a/python/breezyslam/components.py +++ b/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) +