diff --git a/Sources/Examples/main.swift b/Sources/Examples/main.swift index 4fd7da1..fc50962 100644 --- a/Sources/Examples/main.swift +++ b/Sources/Examples/main.swift @@ -11,10 +11,21 @@ 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 + } } } diff --git a/Sources/SwiftRPLidar/SwiftRPLidar.swift b/Sources/SwiftRPLidar/SwiftRPLidar.swift index c2f2034..ddd4a23 100644 --- a/Sources/SwiftRPLidar/SwiftRPLidar.swift +++ b/Sources/SwiftRPLidar/SwiftRPLidar.swift @@ -96,6 +96,8 @@ 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. @@ -154,7 +156,7 @@ public class SwiftRPLidar { } /** - Gets information about the connected lidar. + Gets information about the connected lidar. - returns (model, firmware, hardware, serialNumber) */ @@ -204,6 +206,7 @@ public class SwiftRPLidar { */ public func stop() throws{ try sendCommand(Constants.STOP.asData()) + isScanning = false } /** @@ -224,6 +227,31 @@ public class SwiftRPLidar { - 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{ @@ -239,29 +267,30 @@ public class SwiftRPLidar { if dataSize != 5 || isSingle || dataType != Constants.SCAN_TYPE { throw RPLidarError.INCORRECT_DESCRIPTOR_FORMAT } - var read = true - while read { - do { - let raw = try readResponse(Int(dataSize)) - if maxBufferMeasurements > 0 { - let dataInWaiting = serialPort.inWaiting - if dataInWaiting > maxBufferMeasurements { - print("Too many measurements in the input buffer. Clearing Buffer") - _ = try serialPort.readData(ofLength: dataInWaiting / Int(dataSize) * Int(dataSize)) - } - } - 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)") + isScanning = true + self.dataSize = dataSize + 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 + if dataInWaiting > maxBufferMeasurements { + print("Too many measurements in the input buffer. Clearing Buffer") + _ = try serialPort.readData(ofLength: dataInWaiting / Int(dataSize) * Int(dataSize)) } } + return raw } /** @@ -290,6 +319,16 @@ 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()) @@ -361,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 + } +} +