From be85d7d39ee6a01e69bb0991610d7258c28243f0 Mon Sep 17 00:00:00 2001 From: Piv <18462828+Piv200@users.noreply.github.com> Date: Tue, 22 Sep 2020 21:41:10 +0930 Subject: [PATCH] Ignore bad scans, add documentation. --- Sources/Examples/main.swift | 1 - Sources/SwiftRPLidar/SwiftRPLidar.swift | 105 +++++++++++++++++++++--- 2 files changed, 94 insertions(+), 12 deletions(-) diff --git a/Sources/Examples/main.swift b/Sources/Examples/main.swift index b563362..4fd7da1 100644 --- a/Sources/Examples/main.swift +++ b/Sources/Examples/main.swift @@ -20,7 +20,6 @@ func main() throws { 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) diff --git a/Sources/SwiftRPLidar/SwiftRPLidar.swift b/Sources/SwiftRPLidar/SwiftRPLidar.swift index b92adcc..c2f2034 100644 --- a/Sources/SwiftRPLidar/SwiftRPLidar.swift +++ b/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,15 @@ public class SwiftRPLidar { private var motorRunning = false private let measurementDelayus: UInt32 + /** + 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 +122,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 +153,11 @@ public class SwiftRPLidar { motorRunning = false } - public func setPwm(_ pwm: Int) throws{ - assert(0 <= pwm && pwm <= Constants.MAX_MOTOR_PWM) - try self.sendPayloadCommand(Constants.SET_PWM_BYTE, payload: pwm.asData()) - } - - // Returns (model, firmware, hardware, serialNumber) + /** + Gets information about the connected lidar. + + - 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 +170,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,18 +190,39 @@ 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()) } + /** + 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 { try startMotor() let (status, _) = try getHealth() @@ -183,10 +250,12 @@ public class SwiftRPLidar { _ = try serialPort.readData(ofLength: dataInWaiting / Int(dataSize) * Int(dataSize)) } } - read = try onMeasure(processScan(raw: 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) + 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) { @@ -195,6 +264,15 @@ public class SwiftRPLidar { } } + /** + 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 +281,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 +290,11 @@ public class SwiftRPLidar { } } + 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