diff --git a/Package.resolved b/Package.resolved index c14dc95..6f2990e 100644 --- a/Package.resolved +++ b/Package.resolved @@ -5,9 +5,9 @@ "package": "SwiftSerial", "repositoryURL": "https://vato.ddns.net/gitlab/vato007/SwiftSerial.git", "state": { - "branch": "dtr_support", - "revision": "34e59a7d8766f7097eb68779ec039e77a1eec78a", - "version": null + "branch": null, + "revision": "4a167032f3070ac837fc729dd6ca0cb897a65457", + "version": "0.1.3" } } ] diff --git a/Package.swift b/Package.swift index 53e5e2a..0e0ab67 100644 --- a/Package.swift +++ b/Package.swift @@ -9,13 +9,21 @@ let package = Package( .library( name: "SwiftRPLidar", targets: ["SwiftRPLidar"]), + .executable(name: "LidarExamples", targets: ["Examples"]), ], dependencies: [ + .package(url: "https://vato.ddns.net/gitlab/vato007/SwiftSerial.git", from: "0.1.3") ], targets: [ .target( name: "SwiftRPLidar", dependencies: []), + .target( + name: "Examples", + dependencies: [ + "SwiftRPLidar", + "SwiftSerial" + ]), .testTarget( name: "SwiftRPLidarTests", dependencies: ["SwiftRPLidar"]), diff --git a/README.md b/README.md index 055cb49..9a1771c 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,11 @@ # SwiftRPLidar -A description of this package. +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 ``` + diff --git a/Sources/Examples/main.swift b/Sources/Examples/main.swift new file mode 100644 index 0000000..b563362 --- /dev/null +++ b/Sources/Examples/main.swift @@ -0,0 +1,30 @@ +import SwiftRPLidar +import SwiftSerial + +do { + try main() +} +catch { + print("Unexpected Error \(error)") +} + +func main() throws { + let serialPort = SerialPort(path: "/dev/cu.usbserial-0001") + let lidar = try SwiftRPLidar(onPort: serialPort) + + try lidar.iterMeasurements { measurement in + print("Quality: ",measurement.quality, ", Angle: ", measurement.angle, ", Distance: ", measurement.distance) + return true + } +} + +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 8e7f88d..d83dce3 100644 --- a/Sources/SwiftRPLidar/SwiftRPLidar.swift +++ b/Sources/SwiftRPLidar/SwiftRPLidar.swift @@ -30,18 +30,26 @@ public enum HEALTH_STATUSES: UInt8 { case GOOD = 0, WARNING, ERROR } +public enum RPLidarError: Error { + case FAILED_TO_START, + INCORRECT_INFO_FORMAT, + INCORRECT_HEALTH_FORMAT, + SCAN_ERROR(message: String), + INCORRECT_DESCRIPTOR_FORMAT +} + func processScan(raw: Data) throws -> LidarScan { let newScan = raw[0] & 0b1 let inversedNewScan = (raw[0] >> 1) & 0b1 let quality = raw[0] >> 2 - if (newScan == inversedNewScan){ - + if newScan == inversedNewScan { + throw RPLidarError.SCAN_ERROR(message: "New scan flags mismatch") } - if ((raw[1] & 0b1) != 1) { - + if (raw[1] & 0b1) != 1 { + throw RPLidarError.SCAN_ERROR(message: "Check bit not equal to 1") } - let angle = Float(raw[1] >> 1) + Float(raw[2] << 7) / 64 - let distance = Float(raw[3]) + Float(raw[4] << 8) / 4 + let angle = (Float(raw[1] >> 1) + Float(raw[2] << 7)) / 64 + let distance = (Float(raw[3]) + Float(raw[4] << 8)) / 4 return LidarScan(newScan: newScan == 1, quality: quality, angle: angle, distance: distance) } @@ -53,44 +61,46 @@ public struct LidarScan{ } - public typealias MeasurementHandler = (_ scan: LidarScan) -> Bool public typealias ScanHandler = (_ scans: [LidarScan]) -> Bool public class SwiftRPLidar { private var motor: Bool = false - private var serialPort: LidarSerial? = nil + private var serialPort: LidarSerial private var motorRunning = false + private let measurementDelayus: UInt32 - - public init(onPort serialPort: LidarSerial) throws { + public init(onPort serialPort: LidarSerial, measurementDelayus: UInt32 = 500) throws { self.serialPort = serialPort + self.measurementDelayus = measurementDelayus try connect() try startMotor() } deinit { - if(serialPort != nil){ - serialPort?.closePort() + disconnect() + do { + try stop() + try stopMotor() + } catch { + print("Failed to stop lidar/motor.") } } public func connect() throws { disconnect() - try serialPort!.openPort() - serialPort?.setBaudrate(baudrate: 115200) + try serialPort.openPort() + serialPort.setBaudrate(baudrate: 115200) } public func disconnect(){ - if(serialPort != nil){ - // Need to close, SwiftSerial is blocking. - serialPort!.closePort() - } + // Need to close, SwiftSerial is blocking. + serialPort.closePort() } public func startMotor() throws{ // A1 - serialPort?.dtr = true + serialPort.dtr = true // A2 try self.setPwm(Constants.DEFAULT_MOTOR_PWM) self.motorRunning = true @@ -98,7 +108,7 @@ public class SwiftRPLidar { public func stopMotor() throws{ try setPwm(0) - serialPort?.dtr = false + serialPort.dtr = false motorRunning = false } @@ -107,35 +117,26 @@ public class SwiftRPLidar { try self.sendPayloadCommand(Constants.SET_PWM_BYTE, payload: pwm.asData()) } - public func getInfo() throws -> (UInt8, UInt8, UInt8, String){ + // 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()! - if (dataSize != Constants.INFO_LEN){ - - } - if(!isSingle){ - - } - if(dataType != Constants.INFO_TYPE){ - + if dataSize != Constants.INFO_LEN || !isSingle || dataType != Constants.INFO_TYPE { + throw RPLidarError.INCORRECT_INFO_FORMAT } let raw = try readResponse(dataSize) let serialNumber = String(bytes: raw[4...], encoding: .ascii)! - return (raw[0], raw[2], raw[3], serialNumber) + return (raw[0], (raw[2], raw[1]), raw[3], serialNumber) } public func getHealth() throws -> (HEALTH_STATUSES, UInt8){ try sendCommand(Constants.GET_HEALTH.asData()) - let (dataSize, isSingle, dataType) = try readDescriptor()! - if (dataSize != Constants.HEALTH_LEN){ - + guard let (dataSize, isSingle, dataType) = try readDescriptor() else { + throw RPLidarError.INCORRECT_HEALTH_FORMAT } - if (!isSingle){ - - } - if(dataType != Constants.HEALTH_TYPE){ - + if dataSize != Constants.HEALTH_LEN || !isSingle || dataType != Constants.HEALTH_TYPE{ + throw RPLidarError.INCORRECT_HEALTH_FORMAT } let raw = try readResponse(dataSize) let status = HEALTH_STATUSES(rawValue: raw[0])! @@ -144,7 +145,7 @@ public class SwiftRPLidar { } public func clearInput() throws{ - _ = try serialPort?.readData(ofLength: (serialPort?.inWaiting)!) + _ = try serialPort.readData(ofLength: serialPort.inWaiting) } public func stop() throws{ @@ -159,44 +160,45 @@ public class SwiftRPLidar { try startMotor() let (status, _) = try getHealth() if status == .ERROR{ - // Throw Exception + throw RPLidarError.INCORRECT_HEALTH_FORMAT } - + else if status == .WARNING { - + print("Warning given when checking health.") } try sendCommand(Constants.SCAN.asData()) let (dataSize, isSingle, dataType) = try readDescriptor()! - if dataSize != 5 { - - } - if isSingle { - - } - if dataType != Constants.SCAN_TYPE { - + if dataSize != 5 || isSingle || dataType != Constants.SCAN_TYPE { + throw RPLidarError.INCORRECT_DESCRIPTOR_FORMAT } var read = true - // Need to check in waiting or something... while read { - 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)) + 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)) + } } + 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) + } catch RPLidarError.INCORRECT_DESCRIPTOR_FORMAT { + print("Incorrect Descriptor, skipping scan") + } catch RPLidarError.SCAN_ERROR(let message) { + print("Scan processing failed, skipping scan: \(message)") } - // TODO: Support cancelling of measurements. Would it already work though? - read = try onMeasure(processScan(raw: raw)) } } public func iterScans(maxBufferMeasurements: Int = 500, minLength: Int = 5, _ onScan: ScanHandler) throws { var scan: [LidarScan] = [] var read = true - try iterMeasurements{ measurement in + try iterMeasurements(maxBufferMeasurements: maxBufferMeasurements){ measurement in if measurement.newScan { if scan.count > minLength { read = onScan(scan) @@ -208,7 +210,6 @@ public class SwiftRPLidar { } return read } - } private func sendPayloadCommand(_ cmd: Int, payload: Data) throws{ @@ -216,7 +217,7 @@ public class SwiftRPLidar { var req = Constants.SYNC.asData() + cmd.asData() + size.asData() + payload let checksum = calcChecksum(req) req += checksum.asData() - _ = try serialPort?.writeData(req) + _ = try serialPort.writeData(req) } private func sendPayloadCommand(_ cmd: UInt8, payload: Data) throws { @@ -225,7 +226,7 @@ public class SwiftRPLidar { private func calcChecksum(_ data: Data) -> Int{ var checksum = 0; - data.forEach{ body in + data.forEach { body in checksum ^= Int(body) } return checksum @@ -234,13 +235,11 @@ public class SwiftRPLidar { private func sendCommand(_ command: Data) throws{ var req = Constants.SYNC.asData() req.append(command) - _ = try serialPort?.writeData(req) + _ = try serialPort.writeData(req) } private func readDescriptor() throws -> (UInt8, Bool, UInt8)?{ - guard let descriptor = try serialPort?.readData(ofLength: Constants.DESCRIPTOR_LEN) else { - return nil - } + let descriptor = try serialPort.readData(ofLength: Constants.DESCRIPTOR_LEN) if (descriptor.count != Constants.DESCRIPTOR_LEN){ return nil } @@ -252,11 +251,9 @@ public class SwiftRPLidar { } private func readResponse(_ dataSize: Int) throws -> Data { - guard let data = try serialPort?.readData(ofLength: dataSize) else{ - return Data() - } + let data = try serialPort.readData(ofLength: dataSize) if(data.count != dataSize){ - return Data() + throw RPLidarError.INCORRECT_DESCRIPTOR_FORMAT } return data } @@ -264,10 +261,10 @@ public class SwiftRPLidar { private func readResponse(_ dataSize: UInt8) throws -> Data { return try readResponse(Int(dataSize)) } - - } +// MARK: Convenience packing extensions. + extension Int{ func asData() -> Data { var int = self