Add lidar streaming support to SwiftyCar

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<Google_Protobuf_Empty> {
+        return context.eventLoop.makeSucceededFuture(Google_Protobuf_Empty())
+    }
+    
+    func stop_tracking(request: Google_Protobuf_Empty, context: StatusOnlyCallContext) -> EventLoopFuture<Google_Protobuf_Empty> {
+        shouldScan = false
+        return context.eventLoop.makeSucceededFuture(Google_Protobuf_Empty())
+    }
+    
+    func start_tracking(request: Google_Protobuf_Empty, context: StatusOnlyCallContext) -> EventLoopFuture<Google_Protobuf_Empty> {
+        return context.eventLoop.makeSucceededFuture(Google_Protobuf_Empty())
+    }
+    
+    func record(request: Google_Protobuf_BoolValue, context: StatusOnlyCallContext) -> EventLoopFuture<Google_Protobuf_Empty> {
+        return context.eventLoop.makeSucceededFuture(Google_Protobuf_Empty())
+    }
+    
+    func save_lidar(request: MotorControl_SaveRequest, context: StatusOnlyCallContext) -> EventLoopFuture<Google_Protobuf_Empty> {
+        return context.eventLoop.makeSucceededFuture(Google_Protobuf_Empty())
+    }
+    
+    func lidar_stream(request: Persontracking_StreamMessage, context: StreamingResponseCallContext<Persontracking_PointScan>) -> EventLoopFuture<GRPCStatus> {
+        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)
+    }
+}

SwiftyCar/Sources/SwiftyCar/MotorProvider.swift

@@ -19,8 +19,8 @@ class MotorProvider: MotorControl_CarControlProvider{
     
     func set_throttle(request: MotorControl_ThrottleRequest, context: StatusOnlyCallContext) -> EventLoopFuture<MotorControl_ThrottleResponse> {
         self.vehicle.throttle = request.throttle
-        return context.eventLoop.makeSucceededFuture(.with{
-            $0.throttleSet = true
+        return context.eventLoop.makeSucceededFuture(.with{ throttle in
+            throttle.throttleSet = true
             })
     }
     

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