vato007/swiftrplidar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Conform to IteratorProtocol for scanning.

Browse Source 
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.
This commit is contained in:
Piv
2020-10-02 23:01:53 +09:30
parent 3cc661542b
commit d849fdef4e
2 changed files with 155 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
Sources/Examples/main.swift
View File

@@ -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
}
}
}

164
Sources/SwiftRPLidar/SwiftRPLidar.swift
View File

@@ -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
}
}