From 57e90c495f79488f93850da8bfb5ea4633d79a76 Mon Sep 17 00:00:00 2001 From: Piv <18462828+Piv200@users.noreply.github.com> Date: Fri, 10 Jul 2020 22:55:32 +0930 Subject: [PATCH] Add lidar streaming support to SwiftyCar --- SwiftyCar/Package.swift | 4 +- .../Sources/SwiftyCar/LidarProvider.swift | 61 +++++++++++++++++++ .../Sources/SwiftyCar/MotorProvider.swift | 4 +- SwiftyCar/Sources/SwiftyCar/main.swift | 23 ++++++- 4 files changed, 87 insertions(+), 5 deletions(-) create mode 100644 SwiftyCar/Sources/SwiftyCar/LidarProvider.swift diff --git a/SwiftyCar/Package.swift b/SwiftyCar/Package.swift index 22eebe7..a46de07 100644 --- a/SwiftyCar/Package.swift +++ b/SwiftyCar/Package.swift @@ -13,13 +13,15 @@ let package = Package( // .package(url: /* package url */, from: "1.0.0"), .package(url: "https://github.com/grpc/grpc-swift.git", from: "1.0.0-alpha.12"), .package(url: "https://github.com/uraimo/SwiftyGPIO.git", from: "1.0.0"), + .package(url: "https://vato.ddns.net/gitlab/vato007/swiftrplidar.git", .branch("master")), + .package(url: "https://vato.ddns.net/gitlab/vato007/SwiftSerial.git", .branch("dtr_support")) ], targets: [ // Targets are the basic building blocks of a package. A target can define a module or a test suite. // Targets can depend on other targets in this package, and on products in packages which this package depends on. .target( name: "SwiftyCar", - dependencies: ["SwiftyGPIO", .product(name: "GRPC", package: "grpc-swift")]), + dependencies: ["SwiftyGPIO", .product(name: "GRPC", package: "grpc-swift"), "SwiftRPLidar"]), .testTarget( name: "SwiftyCarTests", dependencies: ["SwiftyCar"]), diff --git a/SwiftyCar/Sources/SwiftyCar/LidarProvider.swift b/SwiftyCar/Sources/SwiftyCar/LidarProvider.swift new file mode 100644 index 0000000..4397fba --- /dev/null +++ b/SwiftyCar/Sources/SwiftyCar/LidarProvider.swift @@ -0,0 +1,61 @@ +// +// LidarProvider.swift +// +// +// Created by Michael Pivato on 10/7/20. +// + +import Foundation +import GRPC +import NIO +import SwiftProtobuf +import SwiftRPLidar + +class LidarProvider: Persontracking_PersonTrackingProvider { + + private let lidar: SwiftRPLidar + private var shouldScan: Bool = false + + init(lidar: SwiftRPLidar) { + self.lidar = lidar + } + + func set_tracking_group(request: Persontracking_Int32Value, context: StatusOnlyCallContext) -> EventLoopFuture { + return context.eventLoop.makeSucceededFuture(Google_Protobuf_Empty()) + } + + func stop_tracking(request: Google_Protobuf_Empty, context: StatusOnlyCallContext) -> EventLoopFuture { + shouldScan = false + return context.eventLoop.makeSucceededFuture(Google_Protobuf_Empty()) + } + + func start_tracking(request: Google_Protobuf_Empty, context: StatusOnlyCallContext) -> EventLoopFuture { + return context.eventLoop.makeSucceededFuture(Google_Protobuf_Empty()) + } + + func record(request: Google_Protobuf_BoolValue, context: StatusOnlyCallContext) -> EventLoopFuture { + return context.eventLoop.makeSucceededFuture(Google_Protobuf_Empty()) + } + + func save_lidar(request: MotorControl_SaveRequest, context: StatusOnlyCallContext) -> EventLoopFuture { + return context.eventLoop.makeSucceededFuture(Google_Protobuf_Empty()) + } + + func lidar_stream(request: Persontracking_StreamMessage, context: StreamingResponseCallContext) -> EventLoopFuture { + shouldScan = true + try! lidar.iterScans{scan in + _ = context.sendResponse(.with{protoScan in + protoScan.points = scan.map{ point in + Persontracking_Point.with{ protoPoint in + protoPoint.angle = Double(point.angle) + protoPoint.distance = Double(point.distance) + // Placeholder group number. + protoPoint.groupNumber = 0 + } + } + }) + return shouldScan + } + return context.eventLoop.makeSucceededFuture(.ok) + } +} diff --git a/SwiftyCar/Sources/SwiftyCar/MotorProvider.swift b/SwiftyCar/Sources/SwiftyCar/MotorProvider.swift index fadc40a..e8eb906 100644 --- a/SwiftyCar/Sources/SwiftyCar/MotorProvider.swift +++ b/SwiftyCar/Sources/SwiftyCar/MotorProvider.swift @@ -19,8 +19,8 @@ class MotorProvider: MotorControl_CarControlProvider{ func set_throttle(request: MotorControl_ThrottleRequest, context: StatusOnlyCallContext) -> EventLoopFuture { self.vehicle.throttle = request.throttle - return context.eventLoop.makeSucceededFuture(.with{ - $0.throttleSet = true + return context.eventLoop.makeSucceededFuture(.with{ throttle in + throttle.throttleSet = true }) } diff --git a/SwiftyCar/Sources/SwiftyCar/main.swift b/SwiftyCar/Sources/SwiftyCar/main.swift index 7afc07c..5dc69d6 100644 --- a/SwiftyCar/Sources/SwiftyCar/main.swift +++ b/SwiftyCar/Sources/SwiftyCar/main.swift @@ -7,6 +7,8 @@ import NIO import GRPC +import SwiftRPLidar +import SwiftSerial func doServer() throws { // Copied from examples @@ -19,10 +21,11 @@ func doServer() throws { // Create a provider using the features we read. let provider = try MotorProvider(vehicle: getVehicle2D()) + let trackingProvider = LidarProvider(lidar: createLidar()) // Start the server and print its address once it has started. let server = Server.insecure(group: group) - .withServiceProviders([provider]) + .withServiceProviders([provider, trackingProvider]) .bind(host: "localhost", port: 0) server.map { @@ -37,11 +40,27 @@ func doServer() throws { }.wait() } +func createLidar() -> SwiftRPLidar{ + return try! SwiftRPLidar(onPort: SerialPort(path: "/dev/cu.usbserial0001")) + +} + // Entry-Point to the Swift Car Controller print("Starting Server") do{ -try doServer() + try doServer() } catch{ print("Server failed") } + +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: 1) + } + + } +}