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

Get lidar to a working state, add errors, example.

Browse Source
This commit is contained in:
Piv
2020-09-19 19:50:56 +09:30
parent 761eb0bc1a
commit ef84c19f92
5 changed files with 120 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
Package.resolved
View File

@@ -5,9 +5,9 @@
"package": "SwiftSerial", "package": "SwiftSerial",
"repositoryURL": "https://vato.ddns.net/gitlab/vato007/SwiftSerial.git", "repositoryURL": "https://vato.ddns.net/gitlab/vato007/SwiftSerial.git",
"state": { "state": {
"branch": "dtr_support", "branch": null,
"revision": "34e59a7d8766f7097eb68779ec039e77a1eec78a", "revision": "4a167032f3070ac837fc729dd6ca0cb897a65457",
"version": null "version": "0.1.3"
} }
} }
] ]

8
Package.swift
View File

@@ -9,13 +9,21 @@ let package = Package(
.library( .library(
name: "SwiftRPLidar", name: "SwiftRPLidar",
targets: ["SwiftRPLidar"]), targets: ["SwiftRPLidar"]),
.executable(name: "LidarExamples", targets: ["Examples"]),
], ],
dependencies: [ dependencies: [
.package(url: "https://vato.ddns.net/gitlab/vato007/SwiftSerial.git", from: "0.1.3")
], ],
targets: [ targets: [
.target( .target(
name: "SwiftRPLidar", name: "SwiftRPLidar",
dependencies: []), dependencies: []),
.target(
name: "Examples",
dependencies: [
"SwiftRPLidar",
"SwiftSerial"
]),
.testTarget( .testTarget(
name: "SwiftRPLidarTests", name: "SwiftRPLidarTests",
dependencies: ["SwiftRPLidar"]), dependencies: ["SwiftRPLidar"]),

10
README.md
View File

@@ -1,3 +1,11 @@
# SwiftRPLidar # 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 ```

30
Sources/Examples/main.swift Normal file
View File

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

129
Sources/SwiftRPLidar/SwiftRPLidar.swift
View File

@@ -30,18 +30,26 @@ public enum HEALTH_STATUSES: UInt8 {
case GOOD = 0, WARNING, ERROR 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 { func processScan(raw: Data) throws -> LidarScan {
let newScan = raw[0] & 0b1 let newScan = raw[0] & 0b1
let inversedNewScan = (raw[0] >> 1) & 0b1 let inversedNewScan = (raw[0] >> 1) & 0b1
let quality = raw[0] >> 2 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 angle = (Float(raw[1] >> 1) + Float(raw[2] << 7)) / 64
let distance = Float(raw[3]) + Float(raw[4] << 8) / 4 let distance = (Float(raw[3]) + Float(raw[4] << 8)) / 4
return LidarScan(newScan: newScan == 1, quality: quality, angle: angle, distance: distance) 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 MeasurementHandler = (_ scan: LidarScan) -> Bool
public typealias ScanHandler = (_ scans: [LidarScan]) -> Bool public typealias ScanHandler = (_ scans: [LidarScan]) -> Bool
public class SwiftRPLidar { public class SwiftRPLidar {
private var motor: Bool = false private var motor: Bool = false
private var serialPort: LidarSerial? = nil private var serialPort: LidarSerial
private var motorRunning = false private var motorRunning = false
private let measurementDelayus: UInt32
public init(onPort serialPort: LidarSerial, measurementDelayus: UInt32 = 500) throws {
public init(onPort serialPort: LidarSerial) throws {
self.serialPort = serialPort self.serialPort = serialPort
self.measurementDelayus = measurementDelayus
try connect() try connect()
try startMotor() try startMotor()
} }
deinit { deinit {
if(serialPort != nil){ disconnect()
serialPort?.closePort() do {
try stop()
try stopMotor()
} catch {
print("Failed to stop lidar/motor.")
} }
} }
public func connect() throws { public func connect() throws {
disconnect() disconnect()
try serialPort!.openPort() try serialPort.openPort()
serialPort?.setBaudrate(baudrate: 115200) serialPort.setBaudrate(baudrate: 115200)
} }
public func disconnect(){ public func disconnect(){
if(serialPort != nil){
// Need to close, SwiftSerial is blocking. // Need to close, SwiftSerial is blocking.
serialPort!.closePort() serialPort.closePort()
}
} }
public func startMotor() throws{ public func startMotor() throws{
// A1 // A1
serialPort?.dtr = true serialPort.dtr = true
// A2 // A2
try self.setPwm(Constants.DEFAULT_MOTOR_PWM) try self.setPwm(Constants.DEFAULT_MOTOR_PWM)
self.motorRunning = true self.motorRunning = true
@@ -98,7 +108,7 @@ public class SwiftRPLidar {
public func stopMotor() throws{ public func stopMotor() throws{
try setPwm(0) try setPwm(0)
serialPort?.dtr = false serialPort.dtr = false
motorRunning = false motorRunning = false
} }
@@ -107,35 +117,26 @@ public class SwiftRPLidar {
try self.sendPayloadCommand(Constants.SET_PWM_BYTE, payload: pwm.asData()) 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])) try sendCommand(Data([Constants.GET_HEALTH]))
let (dataSize, isSingle, dataType) = try readDescriptor()! let (dataSize, isSingle, dataType) = try readDescriptor()!
if (dataSize != Constants.INFO_LEN){ if dataSize != Constants.INFO_LEN || !isSingle || dataType != Constants.INFO_TYPE {
throw RPLidarError.INCORRECT_INFO_FORMAT
}
if(!isSingle){
}
if(dataType != Constants.INFO_TYPE){
} }
let raw = try readResponse(dataSize) let raw = try readResponse(dataSize)
let serialNumber = String(bytes: raw[4...], encoding: .ascii)! 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){ public func getHealth() throws -> (HEALTH_STATUSES, UInt8){
try sendCommand(Constants.GET_HEALTH.asData()) try sendCommand(Constants.GET_HEALTH.asData())
let (dataSize, isSingle, dataType) = try readDescriptor()! guard let (dataSize, isSingle, dataType) = try readDescriptor() else {
if (dataSize != Constants.HEALTH_LEN){ throw RPLidarError.INCORRECT_HEALTH_FORMAT
} }
if (!isSingle){ if dataSize != Constants.HEALTH_LEN || !isSingle || dataType != Constants.HEALTH_TYPE{
throw RPLidarError.INCORRECT_HEALTH_FORMAT
}
if(dataType != Constants.HEALTH_TYPE){
} }
let raw = try readResponse(dataSize) let raw = try readResponse(dataSize)
let status = HEALTH_STATUSES(rawValue: raw[0])! let status = HEALTH_STATUSES(rawValue: raw[0])!
@@ -144,7 +145,7 @@ public class SwiftRPLidar {
} }
public func clearInput() throws{ public func clearInput() throws{
_ = try serialPort?.readData(ofLength: (serialPort?.inWaiting)!) _ = try serialPort.readData(ofLength: serialPort.inWaiting)
} }
public func stop() throws{ public func stop() throws{
@@ -159,44 +160,45 @@ public class SwiftRPLidar {
try startMotor() try startMotor()
let (status, _) = try getHealth() let (status, _) = try getHealth()
if status == .ERROR{ if status == .ERROR{
// Throw Exception throw RPLidarError.INCORRECT_HEALTH_FORMAT
} }
else if status == .WARNING { else if status == .WARNING {
print("Warning given when checking health.")
} }
try sendCommand(Constants.SCAN.asData()) try sendCommand(Constants.SCAN.asData())
let (dataSize, isSingle, dataType) = try readDescriptor()! let (dataSize, isSingle, dataType) = try readDescriptor()!
if dataSize != 5 { if dataSize != 5 || isSingle || dataType != Constants.SCAN_TYPE {
throw RPLidarError.INCORRECT_DESCRIPTOR_FORMAT
}
if isSingle {
}
if dataType != Constants.SCAN_TYPE {
} }
var read = true var read = true
// Need to check in waiting or something...
while read { while read {
do {
let raw = try readResponse(Int(dataSize)) let raw = try readResponse(Int(dataSize))
if maxBufferMeasurements > 0 { if maxBufferMeasurements > 0 {
let dataInWaiting = serialPort?.inWaiting let dataInWaiting = serialPort.inWaiting
if dataInWaiting! > maxBufferMeasurements { if dataInWaiting > maxBufferMeasurements {
print("Too many measurements in the input buffer. Clearing Buffer") print("Too many measurements in the input buffer. Clearing Buffer")
_ = try serialPort?.readData(ofLength: dataInWaiting! / Int(dataSize) * Int(dataSize)) _ = try serialPort.readData(ofLength: dataInWaiting / Int(dataSize) * Int(dataSize))
} }
} }
// TODO: Support cancelling of measurements. Would it already work though?
read = try onMeasure(processScan(raw: raw)) 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)")
}
} }
} }
public func iterScans(maxBufferMeasurements: Int = 500, minLength: Int = 5, _ onScan: ScanHandler) throws { public func iterScans(maxBufferMeasurements: Int = 500, minLength: Int = 5, _ onScan: ScanHandler) throws {
var scan: [LidarScan] = [] var scan: [LidarScan] = []
var read = true var read = true
try iterMeasurements{ measurement in try iterMeasurements(maxBufferMeasurements: maxBufferMeasurements){ measurement in
if measurement.newScan { if measurement.newScan {
if scan.count > minLength { if scan.count > minLength {
read = onScan(scan) read = onScan(scan)
@@ -208,7 +210,6 @@ public class SwiftRPLidar {
} }
return read return read
} }
} }
private func sendPayloadCommand(_ cmd: Int, payload: Data) throws{ 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 var req = Constants.SYNC.asData() + cmd.asData() + size.asData() + payload
let checksum = calcChecksum(req) let checksum = calcChecksum(req)
req += checksum.asData() req += checksum.asData()
_ = try serialPort?.writeData(req) _ = try serialPort.writeData(req)
} }
private func sendPayloadCommand(_ cmd: UInt8, payload: Data) throws { private func sendPayloadCommand(_ cmd: UInt8, payload: Data) throws {
@@ -234,13 +235,11 @@ public class SwiftRPLidar {
private func sendCommand(_ command: Data) throws{ private func sendCommand(_ command: Data) throws{
var req = Constants.SYNC.asData() var req = Constants.SYNC.asData()
req.append(command) req.append(command)
_ = try serialPort?.writeData(req) _ = try serialPort.writeData(req)
} }
private func readDescriptor() throws -> (UInt8, Bool, UInt8)?{ private func readDescriptor() throws -> (UInt8, Bool, UInt8)?{
guard let descriptor = try serialPort?.readData(ofLength: Constants.DESCRIPTOR_LEN) else { let descriptor = try serialPort.readData(ofLength: Constants.DESCRIPTOR_LEN)
return nil
}
if (descriptor.count != Constants.DESCRIPTOR_LEN){ if (descriptor.count != Constants.DESCRIPTOR_LEN){
return nil return nil
} }
@@ -252,11 +251,9 @@ public class SwiftRPLidar {
} }
private func readResponse(_ dataSize: Int) throws -> Data { private func readResponse(_ dataSize: Int) throws -> Data {
guard let data = try serialPort?.readData(ofLength: dataSize) else{ let data = try serialPort.readData(ofLength: dataSize)
return Data()
}
if(data.count != dataSize){ if(data.count != dataSize){
return Data() throw RPLidarError.INCORRECT_DESCRIPTOR_FORMAT
} }
return data return data
} }
@@ -264,10 +261,10 @@ public class SwiftRPLidar {
private func readResponse(_ dataSize: UInt8) throws -> Data { private func readResponse(_ dataSize: UInt8) throws -> Data {
return try readResponse(Int(dataSize)) return try readResponse(Int(dataSize))
} }
} }
// MARK: Convenience packing extensions.
extension Int{ extension Int{
func asData() -> Data { func asData() -> Data {
var int = self var int = self