Conform to IteratorProtocol for scanning.

I've left the old callback implementations in for now, undecided whether these should be removed. The nice thing about the callback method is that it's more compact, however the logic behind it is less clear.

README.md

@@ -3,9 +3,8 @@
 This library interacts with the RPLidar A1 using pure swift. It is designed to closely replicate the python rplidar library by Skoltech: https://github.com/SkoltechRobotics/rplidar/
 There may be support for RPLidar A2, however it is not available for testing at this time.
 
-Documentation is coming soon.
-
 For a runnable example, see the Examples product.
 
-``` swift run --product Examples ```
+``` swift run LidarExamples ```
 
+Documentation can be generated using [swift-doc](https://github.com/SwiftDocOrg/swift-doc)

Sources/Examples/main.swift

@@ -11,16 +11,26 @@ catch {
 func main() throws {
     let serialPort = SerialPort(path: "/dev/cu.usbserial-0001")
     let lidar = try SwiftRPLidar(onPort: serialPort)
-    
+    var index = 0
     try lidar.iterMeasurements { measurement in
         print("Quality: ",measurement.quality, ", Angle: ", measurement.angle, ", Distance: ", measurement.distance)
-        return true
+        index += 1
+        return index <= 10
+    }
+    
+    index = 0
+    _ = try lidar.startScanning()
+    for measurement in lidar {
+        print("Quality: ",measurement.quality, ", Angle: ", measurement.angle, ", Distance: ", measurement.distance)
+        index += 1
+        if index > 10 {
+            break
+        }
     }
 }
 
 extension SerialPort: LidarSerial{
     public func setBaudrate(baudrate: Int) {
-        // TODO: handle different baudrates. Only need this for now.
         switch baudrate{
         default:
             setSettings(receiveRate: .baud115200, transmitRate: .baud115200, minimumBytesToRead: 3, timeout: 1)

Sources/SwiftRPLidar/SwiftRPLidar.swift

@@ -38,6 +38,19 @@ public enum RPLidarError: Error {
          INCORRECT_DESCRIPTOR_FORMAT
 }
 
+/**
+ Calculates the quality, angle and distance of a scan from the raw Data.
+ 
+ - returns:
+ LidarScan containing the quality, angle in degrees, and distance in mm.
+ 
+ - throws:
+ RPLidarError.SCAN_ERROR if the raw input is malformed or empty.
+ 
+ - parameters:
+ - raw: The bytes containing the scan data.
+ 
+ */
 func processScan(raw: Data) throws -> LidarScan {
     let newScan = raw[0] & 0b1
     let inversedNewScan = (raw[0] >> 1) & 0b1
@@ -61,7 +74,20 @@ public struct LidarScan{
 }
 
 
+/**
+ Function alias for measurement (single scan) closure. Use with iterMeasurements
+ 
+ - parameters:
+ - scan: Single lidar scan.
+ */
 public typealias MeasurementHandler = (_ scan: LidarScan) -> Bool
+
+/**
+ Function alias for scan (multiple scans) closure. Use with iterScans
+ 
+ - parameters:
+ - scans: Array of lidar scans for this batch.
+ */
 public typealias ScanHandler = (_ scans: [LidarScan]) -> Bool
 
 public class SwiftRPLidar {
@@ -70,6 +96,17 @@ public class SwiftRPLidar {
     private var motorRunning = false
     private let measurementDelayus: UInt32
     
+    private(set) var isScanning = false
+    private(set) var dataSize: UInt8 = 5
+    /**
+     A class to interact with the RPLidar A1 and A2.
+     
+     - parameters:
+     onPort: Serial Port the lidar is attached to
+     measurementDelayus: Delay that should be applied between each measurement. This is needed in case the serial port timeout is not working correctly.
+     
+     - throws If the serial port cannot be connected, or the motor cannot be started.
+     */
     public init(onPort serialPort: LidarSerial, measurementDelayus: UInt32 = 500) throws {
         self.serialPort = serialPort
         self.measurementDelayus = measurementDelayus
@@ -87,12 +124,18 @@ public class SwiftRPLidar {
         }
     }
     
+    /**
+     Connect and configure the serial port.
+     */
     public func connect() throws {
         disconnect()
         try serialPort.openPort()
         serialPort.setBaudrate(baudrate: 115200)
     }
     
+    /**
+     Close the serial port.
+     */
     public func disconnect(){
         // Need to close, SwiftSerial is blocking.
         serialPort.closePort()
@@ -112,12 +155,11 @@ public class SwiftRPLidar {
         motorRunning = false
     }
     
-    public func setPwm(_ pwm: Int) throws{
+    /**
-        assert(0 <= pwm && pwm <= Constants.MAX_MOTOR_PWM)
+     Gets information about the connected lidar.
-        try self.sendPayloadCommand(Constants.SET_PWM_BYTE, payload: pwm.asData())
-    }
      
-    // Returns (model, firmware, hardware, serialNumber)
+     - returns (model, firmware, hardware, serialNumber)
+     */
     public func getInfo() throws -> (UInt8, (UInt8, UInt8), UInt8, String){
         try sendCommand(Data([Constants.GET_HEALTH]))
         let (dataSize, isSingle, dataType) = try readDescriptor()!
@@ -130,6 +172,12 @@ public class SwiftRPLidar {
         return (raw[0], (raw[2], raw[1]), raw[3], serialNumber)
     }
     
+    /**
+     Get the current health status of the lidar.
+     
+     - returns (Health status, errorCode
+     - throws If the received health status is in the incorrect format.
+     */
     public func getHealth() throws -> (HEALTH_STATUSES, UInt8){
         try sendCommand(Constants.GET_HEALTH.asData())
         guard let (dataSize, isSingle, dataType) = try readDescriptor() else {
@@ -144,19 +192,66 @@ public class SwiftRPLidar {
         return (status, errorCode)
     }
     
+    /**
+     Clear all data in the serial port buffer.
+     - throws If the buffer could not be emptied.
+     */
     public func clearInput() throws{
         _ = try serialPort.readData(ofLength: serialPort.inWaiting)
     }
     
+    /**
+     Send the stop command to the lidar.
+     - throws If the stop command cannot be sent.
+     */
     public func stop() throws{
         try sendCommand(Constants.STOP.asData())
+        isScanning = false
     }
     
+    /**
+     Send the reset command to the lidar. This puts the lidar into a state as though it just connected and started scannning.
+     - throws If the reset command annot be reset.
+     */
     public func reset() throws{
         try sendCommand(Constants.RESET.asData())
     }
     
+    /**
+     Iterate over the lidar measurments, calling the given measurement handler each time scan is processed.
+     
+     - parameters:
+     maxBufferMeasurements: Max number of measurements to include in the buffer before flushing it.
+     onMeasure: Measurement handler to execute on every read.
+     
+     - throws If the input data is in the incorrect format, or the health retrieved errors.
+     */
     public func iterMeasurements(maxBufferMeasurements: Int = 500, _ onMeasure: MeasurementHandler) throws {
+        if !isScanning {
+            throw RPLidarError.SCAN_ERROR(message: "You must start scanning!")
+        }
+        var read = true
+        while read {
+            do {
+                let raw = try receiveMeasurementAndClearBuffer(dataSize: dataSize, maxBufferMeasurements: maxBufferMeasurements)
+                if raw.count > 0 {
+                    read = try onMeasure(processScan(raw: raw))
+                }
+                if measurementDelayus > 0 {
+                    usleep(measurementDelayus)
+                }
+            } catch RPLidarError.INCORRECT_DESCRIPTOR_FORMAT {
+                print("Incorrect Descriptor, skipping scan")
+            } catch RPLidarError.SCAN_ERROR(let message) {
+                print("Scan processing failed, skipping scan: \(message)")
+            }
+        }
+    }
+    
+    /**
+     Send the command to the lidar to start scanning. This should be called before using any iterator.
+     */
+    public func startScanning() throws -> UInt8  {
         try startMotor()
         let (status, _) = try getHealth()
         if status == .ERROR{
@@ -172,9 +267,21 @@ public class SwiftRPLidar {
         if dataSize != 5 || isSingle || dataType != Constants.SCAN_TYPE {
             throw RPLidarError.INCORRECT_DESCRIPTOR_FORMAT
         }
-        var read = true
+        isScanning = true
-        while read {
+        self.dataSize = dataSize
-            do {
+        return dataSize
+    }
+    
+    /**
+     Receive a measurement from the lidar. If the data in waiting exceeds the buffer, clear it.
+     - parameters:
+     dataSize: The length of the data in bytes for a single lidar measurement.
+     maxBufferMeasurements: The maximum length allowed in the buffer before it should be cleared.
+     */
+    public func receiveMeasurementAndClearBuffer(dataSize: UInt8, maxBufferMeasurements: Int = 500) throws -> Data{
+        if !isScanning {
+            throw RPLidarError.SCAN_ERROR(message: "Haven't started scanning")
+        }
         let raw = try readResponse(Int(dataSize))
         if maxBufferMeasurements > 0 {
             let dataInWaiting = serialPort.inWaiting
@@ -183,18 +290,18 @@ public class SwiftRPLidar {
                 _ = try serialPort.readData(ofLength: dataInWaiting / Int(dataSize) * Int(dataSize))
             }
         }
-                read = try onMeasure(processScan(raw: raw))
+        return raw
-                // TODO: Figure out why this delay is needed.
-                // If some delay is not present, then there is a high chance that the scan will be incorrect, and processScan will error.
-                usleep(measurementDelayus)
-            } catch RPLidarError.INCORRECT_DESCRIPTOR_FORMAT {
-                print("Incorrect Descriptor, skipping scan")
-            } catch RPLidarError.SCAN_ERROR(let message) {
-                print("Scan processing failed, skipping scan: \(message)")
-            }
-        }
     }
     
+    /**
+     Iterate over batches of measurements.
+     - parameters:
+     maxBufferMeasurements: Max number of measurements to include in the buffer before flushing it.
+     minLength: Minimum number of measurements to include in a scan.
+     onScan: Handler function to execute on each scan.
+     
+     - throws If the input data is in the incorrect format, or the health retrieved errors.
+     */
     public func iterScans(maxBufferMeasurements: Int = 500, minLength: Int = 5, _ onScan: ScanHandler) throws {
         var scan: [LidarScan] = []
         var read = true
@@ -203,7 +310,7 @@ public class SwiftRPLidar {
                 if scan.count > minLength {
                     read = onScan(scan)
                 }
-                scan = []
+                scan.removeAll()
             }
             if measurement.quality > 0 && measurement.distance > 0 {
                 scan.append(measurement)
@@ -212,6 +319,21 @@ public class SwiftRPLidar {
         }
     }
     
+    /**
+     Make an iterator that can be used to iterate over batches of measurements.
+     - parameters:
+     withLength: Minimum number of measurements to process in an iteration.
+     maxBufferMeasurements: The maximum number of measurements allowed in the buffer before clearing it.
+     */
+    public func makeScanIterator(withLength minLength: Int = 5, maxBufferMeasurements: Int = 500) -> LidarScanIterator {
+        return LidarScanIterator(lidar: self, measurementsPerBatch: minLength, maxBufferMeasurements: maxBufferMeasurements)
+    }
+    
+    private func setPwm(_ pwm: Int) throws{
+        assert(0 <= pwm && pwm <= Constants.MAX_MOTOR_PWM)
+        try self.sendPayloadCommand(Constants.SET_PWM_BYTE, payload: pwm.asData())
+    }
+    
     private func sendPayloadCommand(_ cmd: Int, payload: Data) throws{
         let size = UInt8(payload.count)
         var req = Constants.SYNC.asData() + cmd.asData() + size.asData() + payload
@@ -278,3 +400,84 @@ extension UInt8 {
     }
 }
 
+// MARK: Iterator Extensions
+
+extension SwiftRPLidar: Sequence {
+    public typealias Element = LidarScan
+    public typealias Iterator = LidarMeasurementIterator
+    
+    public func makeIterator() -> LidarMeasurementIterator {
+        return LidarMeasurementIterator(lidar: self, maxBufferMeasurements: 500)
+    }
+}
+
+
+/**
+ Iterator for individual lidar measurements.
+ A scan may be nil when data is not received from serialPort, or when the lidar is not ready for scanning.
+ */
+public struct LidarMeasurementIterator {
+    let lidar: SwiftRPLidar
+    let maxBufferMeasurements: Int
+}
+
+extension LidarMeasurementIterator: IteratorProtocol{
+    public typealias Element = LidarScan
+    
+    public mutating func next() -> LidarScan? {
+        if lidar.isScanning {
+            do {
+                let raw = try lidar.receiveMeasurementAndClearBuffer(dataSize: lidar.dataSize, maxBufferMeasurements: maxBufferMeasurements)
+                if raw.count > 0 {
+                    return try processScan(raw: raw)
+                }
+            } catch {
+                print("Failed to process lidar scan")
+            }
+        } else {
+            print("You must start scanning!")
+        }
+        return nil
+    }
+}
+
+/**
+ Iterator for batches of lidar measurements.
+ */
+public struct LidarScanIterator {
+    let lidar: SwiftRPLidar
+    let measurementsPerBatch: Int
+    let maxBufferMeasurements: Int
+    public typealias Element = [LidarScan]
+}
+
+extension LidarScanIterator: Sequence {
+    public typealias Iterator = LidarScanIterator
+    
+    public func makeIterator() -> LidarScanIterator {
+        return LidarScanIterator(lidar: lidar, measurementsPerBatch: measurementsPerBatch, maxBufferMeasurements: maxBufferMeasurements)
+    }
+}
+
+extension LidarScanIterator: IteratorProtocol {
+    
+    public mutating func next() -> [LidarScan]? {
+        
+        if lidar.isScanning {
+            var allScans: [LidarScan] = []
+            do {
+                while allScans.count < measurementsPerBatch {
+                    let raw = try lidar.receiveMeasurementAndClearBuffer(dataSize: lidar.dataSize, maxBufferMeasurements: maxBufferMeasurements)
+                    if raw.count > 0 {
+                        allScans.append(try processScan(raw: raw))
+                    }
+                }
+                return allScans
+            } catch {
+                print("Failed to process scan")
+            }
+        }
+        return nil
+    }
+}
+