Fix esp32 servo implementation, update rust service to work with it and support print vehicle

Replace swift implementation with rust

See merge request vato007/picar!10

.gitignore

@@ -33,3 +33,6 @@ xcuserdata/
 .settings
 .project
 .classpath
+
+.vscode/settings.json
+**/target/**

.gitlab-ci.yml

@@ -1,21 +1,89 @@
 stages:
+  - protoc
   - build
   - deploy
 
+cache:
+  key: ${CI_COMMIT_REF_SLUG}
+  paths:
+    - "**/Package.resolved"
+    - "**/.build/"
+    - .gradle/
+
+protoc_gen:
+  image: vato.ddns.net:8083/gradle
+  stage: protoc
+  rules:
+    - changes:
+        - protobuf/*
+  artifacts:
+    paths:
+      - "./**/*pb2*"
+      - "./SwiftyCar/Sources/**/*.pb.swift"
+      - "./SwiftyCar/Sources/**/*.grpc.swift"      
+      - "./CarControlleriOS/Sources/**/*.pb.swift"
+      - "./CarControlleriOS/Sources/**/*.grpc.swift"
+    expire_in: 30 days
+  script:
+    - gradle :pycar:copyPythonCode
+    - gradle :SwiftyCar:copySwiftCode
+    - gradle :CarControlleriOS:copySwiftCode
+
 build_pycar:
-  image: vato.ddns.net:8083/python:3
+  image: vato.ddns.net:8083/python-infra:buster
   stage: build
+  rules:
+    - changes:
+        - pycar/*
+  needs:
+    - protoc_gen
   script:
     - cd pycar && python setup.py bdist_wheel && cd ..
 
 build_pycar_docker:
   image: vato.ddns.net:8083/docker
   stage: build
+  rules:
+    - changes:
+        - pycar/*
+  needs:
+    - protoc_gen
   script:
     - echo ${DOCKER_PASSWORD} | docker login vato.ddns.net:8083 --username ${DOCKER_USERNAME} --password-stdin
     - docker build -f pycar/Dockerfile --build-arg PYPI_USERNAME=${PYPI_USERNAME} --build-arg PYPI_PASSWORD=${PYPI_PASSWORD} -t vato.ddns.net:8082/pycar:latest pycar
 
+built_swift_car:
+  image: vato.ddns.net:8083/swift
+  stage: build
+  rules:
+    - changes:
+        - SwiftyCar/*
+        - protobuf/*
+  needs:
+    - protoc_gen
+  script:
+    - cd SwiftyCar && swift build
+
+build_esp32:
+  image: vato.ddns.net:8083/shaguarger/platformio
+  stage: build
+  rules:
+    - changes:
+        - esp32/*
+  script:
+    - platformio ci --project-conf esp32/platformio.ini esp32
+
 deploy_pycar:
+  image: vato.ddns.net:8083/python-infra:buster
+  stage: deploy
+  needs:
+    - build_pycar
+  script:
+    - cd pycar && python -m twine upload -u $TWINE_COMMON_CREDS_USR -p $TWINE_COMMON_CREDS_PSW dist/* && cd ..
+  only:
+    - tags
+
+deploy_pycar_docker:
   image: vato.ddns.net:8083/docker
   stage: deploy
   script:
@@ -25,3 +93,8 @@ deploy_pycar:
     - build_pycar_docker
   only:
     - tags
+
+deploy_pycar_docker_arm:
+  extends: deploy_pycar_docker
+  tags:
+    - docker-arm

.idea/.name

@@ -0,0 +1 @@
+CarController

.idea/codeStyles/Project.xml

@@ -1,5 +1,23 @@
 <component name="ProjectCodeStyleConfiguration">
   <code_scheme name="Project" version="173">
+    <JetCodeStyleSettings>
+      <option name="PACKAGES_TO_USE_STAR_IMPORTS">
+        <value>
+          <package name="java.util" alias="false" withSubpackages="false" />
+          <package name="kotlinx.android.synthetic" alias="false" withSubpackages="true" />
+          <package name="io.ktor" alias="false" withSubpackages="true" />
+        </value>
+      </option>
+      <option name="PACKAGES_IMPORT_LAYOUT">
+        <value>
+          <package name="" alias="false" withSubpackages="true" />
+          <package name="java" alias="false" withSubpackages="true" />
+          <package name="javax" alias="false" withSubpackages="true" />
+          <package name="kotlin" alias="false" withSubpackages="true" />
+          <package name="" alias="true" withSubpackages="true" />
+        </value>
+      </option>
+    </JetCodeStyleSettings>
     <codeStyleSettings language="XML">
       <indentOptions>
         <option name="CONTINUATION_INDENT_SIZE" value="4" />

.idea/compiler.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="CompilerConfiguration">
+    <bytecodeTargetLevel target="21" />
+  </component>
+</project>

.idea/deploymentTargetDropDown.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="deploymentTargetDropDown">
+    <targetSelectedWithDropDown>
+      <Target>
+        <type value="QUICK_BOOT_TARGET" />
+        <deviceKey>
+          <Key>
+            <type value="VIRTUAL_DEVICE_PATH" />
+            <value value="$USER_HOME$/.android/avd/Pixel_4_XL_API_31.avd" />
+          </Key>
+        </deviceKey>
+      </Target>
+    </targetSelectedWithDropDown>
+    <timeTargetWasSelectedWithDropDown value="2023-01-22T10:14:55.330012Z" />
+  </component>
+</project>

.idea/gradle.xml

@@ -4,19 +4,20 @@
   <component name="GradleSettings">
     <option name="linkedExternalProjectsSettings">
       <GradleProjectSettings>
-        <option name="testRunner" value="PLATFORM" />
+        <option name="testRunner" value="CHOOSE_PER_TEST" />
-        <option name="distributionType" value="DEFAULT_WRAPPED" />
         <option name="externalProjectPath" value="$PROJECT_DIR$" />
+        <option name="gradleJvm" value="#GRADLE_LOCAL_JAVA_HOME" />
         <option name="modules">
           <set>
             <option value="$PROJECT_DIR$" />
             <option value="$PROJECT_DIR$/CarControlleriOS" />
+            <option value="$PROJECT_DIR$/SwiftyCar" />
             <option value="$PROJECT_DIR$/app" />
-            <option value="$PROJECT_DIR$/car" />
             <option value="$PROJECT_DIR$/protobuf" />
+            <option value="$PROJECT_DIR$/pycar" />
           </set>
         </option>
-        <option name="resolveModulePerSourceSet" value="false" />
+        <option name="resolveExternalAnnotations" value="false" />
       </GradleProjectSettings>
     </option>
   </component>

.idea/jarRepositories.xml

@@ -0,0 +1,40 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="RemoteRepositoriesConfiguration">
+    <remote-repository>
+      <option name="id" value="central" />
+      <option name="name" value="Maven Central repository" />
+      <option name="url" value="https://repo1.maven.org/maven2" />
+    </remote-repository>
+    <remote-repository>
+      <option name="id" value="jboss.community" />
+      <option name="name" value="JBoss Community repository" />
+      <option name="url" value="https://repository.jboss.org/nexus/content/repositories/public/" />
+    </remote-repository>
+    <remote-repository>
+      <option name="id" value="MavenLocal" />
+      <option name="name" value="MavenLocal" />
+      <option name="url" value="file:$USER_HOME$/.m2/repository" />
+    </remote-repository>
+    <remote-repository>
+      <option name="id" value="BintrayJCenter" />
+      <option name="name" value="BintrayJCenter" />
+      <option name="url" value="https://jcenter.bintray.com/" />
+    </remote-repository>
+    <remote-repository>
+      <option name="id" value="Google" />
+      <option name="name" value="Google" />
+      <option name="url" value="https://dl.google.com/dl/android/maven2/" />
+    </remote-repository>
+    <remote-repository>
+      <option name="id" value="MavenLocal" />
+      <option name="name" value="MavenLocal" />
+      <option name="url" value="file:$USER_HOME$/.m2/repository/" />
+    </remote-repository>
+    <remote-repository>
+      <option name="id" value="MavenRepo" />
+      <option name="name" value="MavenRepo" />
+      <option name="url" value="https://repo.maven.apache.org/maven2/" />
+    </remote-repository>
+  </component>
+</project>

.idea/misc.xml

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <project version="4">
-  <component name="ProjectRootManager" version="2" languageLevel="JDK_1_8" project-jdk-name="1.8" project-jdk-type="JavaSDK">
+  <component name="ProjectRootManager" version="2" languageLevel="JDK_21" default="true" project-jdk-name="jbr-21" project-jdk-type="JavaSDK">
     <output url="file://$PROJECT_DIR$/build/classes" />
   </component>
   <component name="ProjectType">

.idea/runConfigurations.xml

@@ -1,12 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="RunConfigurationProducerService">
-    <option name="ignoredProducers">
-      <set>
-        <option value="org.jetbrains.plugins.gradle.execution.test.runner.AllInPackageGradleConfigurationProducer" />
-        <option value="org.jetbrains.plugins.gradle.execution.test.runner.TestClassGradleConfigurationProducer" />
-        <option value="org.jetbrains.plugins.gradle.execution.test.runner.TestMethodGradleConfigurationProducer" />
-      </set>
-    </option>
-  </component>
-</project>

.vscode/launch.json

@@ -10,7 +10,8 @@
             "request": "launch",
             "module": "car",
             "env": {
-                "CAR_VEHICLE": "CAR_MOCK",
+                "CAR_VEHICLE": "VEHICLE_MOCK",
+                // "CAR_VEHICLE": "VEHICLE_SERIAL",
                 // "CAR_LIDAR": "/dev/tty.usbserial-0001",
                 "CAR_LIDAR": "LIDAR_MOCK"
             }

README.md

@@ -0,0 +1,41 @@
+# PiCar
+
+This software allows for the control of an RC Car using a linux system. Currently it's been tested to work on a Traxxas Slash and raspberry pi 3b+.
+
+See the individual modules for more detailed READMEs, including build instructions.
+
+### Current Functinoality
+
+- 2D Servo (Steering + Throttle) control.
+- 2D SLAM streaming
+- 2D LiDAR streaming
+- LiDAR object tracking
+    - Currently the grouping works great, tracking needs some work.
+- Control via smartphone applications
+    - Android is fully supported, iOS currently only supports servo control.
+
+## Modules
+
+### PyCar
+
+This is the main (and original) project to control the raspberry pi. It supports all functions listed in Current Functionality.
+
+Tracking through the LiDAR is somewhat supported, however has been found to be quite poor at the moment, due to the current assignment algorithm.
+
+Currently this module is only designed to work standalone, as it is seen to be quite trivial to create the core functionality in a custom application.
+
+### SwiftyCar
+
+The plan for this module is to eventually replace the python one, as Swift is known to be faster, whilst being nicer to program than C. Currently it's only meant to support 2D servo control, as a separate library is being created for LiDAR functionality.
+
+### CarControlleriOS
+
+This is the iOS application to control the car. Due to the use of gRPC, it can work with 
+
+### app
+
+This contains the code for the Android application. You should use gradle for this, and Java must be installed to use it.
+
+### protobuf
+
+This contains the gRPC and protobuf definitions for controlling the car, so arbitrary clients (that must support gRPC) can be used with the car. The recommended way to generate the code at the moment is to use Gradle, which will do Python, Swift and Java codegen.

SwiftyCar/Package.resolved

@@ -1,79 +0,0 @@
-{
-  "object": {
-    "pins": [
-      {
-        "package": "grpc-swift",
-        "repositoryURL": "https://github.com/grpc/grpc-swift.git",
-        "state": {
-          "branch": null,
-          "revision": "b83ee1ee2caa0660eb02444977b9b6e353c2adbf",
-          "version": "1.0.0-alpha.12"
-        }
-      },
-      {
-        "package": "swift-log",
-        "repositoryURL": "https://github.com/apple/swift-log.git",
-        "state": {
-          "branch": null,
-          "revision": "74d7b91ceebc85daf387ebb206003f78813f71aa",
-          "version": "1.2.0"
-        }
-      },
-      {
-        "package": "swift-nio",
-        "repositoryURL": "https://github.com/apple/swift-nio.git",
-        "state": {
-          "branch": null,
-          "revision": "40bdad80882d307abe2c0bb36cf3bd4d3e03fe04",
-          "version": "2.16.1"
-        }
-      },
-      {
-        "package": "swift-nio-http2",
-        "repositoryURL": "https://github.com/apple/swift-nio-http2.git",
-        "state": {
-          "branch": null,
-          "revision": "82eb3fa0974b838358ee46bc6c5381e5ae5de6b9",
-          "version": "1.11.0"
-        }
-      },
-      {
-        "package": "swift-nio-ssl",
-        "repositoryURL": "https://github.com/apple/swift-nio-ssl.git",
-        "state": {
-          "branch": null,
-          "revision": "ae213938e151964aa691f0e902462fbe06baeeb6",
-          "version": "2.7.1"
-        }
-      },
-      {
-        "package": "swift-nio-transport-services",
-        "repositoryURL": "https://github.com/apple/swift-nio-transport-services.git",
-        "state": {
-          "branch": null,
-          "revision": "85a67aea7caf5396ed599543dd23cffeb6dbbf96",
-          "version": "1.5.1"
-        }
-      },
-      {
-        "package": "SwiftProtobuf",
-        "repositoryURL": "https://github.com/apple/swift-protobuf.git",
-        "state": {
-          "branch": null,
-          "revision": "7790acf0a81d08429cb20375bf42a8c7d279c5fe",
-          "version": "1.8.0"
-        }
-      },
-      {
-        "package": "SwiftyGPIO",
-        "repositoryURL": "https://github.com/uraimo/SwiftyGPIO.git",
-        "state": {
-          "branch": null,
-          "revision": "4127ff9dd5c6aa8acb6be34b7ce2af2f6e0b942d",
-          "version": "1.1.14"
-        }
-      }
-    ]
-  },
-  "version": 1
-}

SwiftyCar/Package.swift

@@ -1,29 +0,0 @@
-// swift-tools-version:5.1
-// The swift-tools-version declares the minimum version of Swift required to build this package.
-
-import PackageDescription
-
-let package = Package(
-    name: "SwiftyCar",
-    products: [
-        // Products define the executables and libraries produced by a package, and make them visible to other packages.
-    ],
-    dependencies: [
-        // Dependencies declare other packages that this package depends on.
-        // .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"), "SwiftRPLidar"]),
-        .testTarget(
-            name: "SwiftyCarTests",
-            dependencies: ["SwiftyCar"]),
-    ]
-)

SwiftyCar/Sources/SwiftyCar/LidarProvider.swift

@@ -1,61 +0,0 @@
-//
-//  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

@@ -1,57 +0,0 @@
-//
-//  MotorProvider.swift
-//  
-//
-//  Created by Michael Pivato on 13/5/20.
-//
-
-import Foundation
-import GRPC
-import NIO
-import SwiftProtobuf
-
-class MotorProvider: MotorControl_CarControlProvider{
-    private var vehicle: Vehicle2D
-    
-    init(vehicle: Vehicle2D){
-        self.vehicle = vehicle
-    }
-    
-    func set_throttle(request: MotorControl_ThrottleRequest, context: StatusOnlyCallContext) -> EventLoopFuture<MotorControl_ThrottleResponse> {
-        self.vehicle.throttle = request.throttle
-        return context.eventLoop.makeSucceededFuture(.with{ throttle in
-            throttle.throttleSet = true
-            })
-    }
-    
-    func set_steering(request: MotorControl_SteeringRequest, context: StatusOnlyCallContext) -> EventLoopFuture<MotorControl_SteeringResponse> {
-        self.vehicle.steering = request.steering
-        return context.eventLoop.makeSucceededFuture(.with{
-            $0.steeringSet = true
-            })
-    }
-    
-    func stream_vehicle_2d(context: UnaryResponseCallContext<Google_Protobuf_Empty>) -> EventLoopFuture<(StreamEvent<MotorControl_Vehicle2DRequest>) -> Void> {
-        return context.eventLoop.makeSucceededFuture({event in
-            switch event{
-            case .message(let movement):
-                self.vehicle.throttle = movement.throttle.throttle
-                self.vehicle.steering = movement.steering.steering
-            case .end:
-                context.responsePromise.succeed(Google_Protobuf_Empty())
-            }
-            
-        })
-    }
-    
-    func record(request: MotorControl_RecordingReqeust, context: StatusOnlyCallContext) -> EventLoopFuture<Google_Protobuf_Empty> {
-        // TODO: Recording...
-        return context.eventLoop.makeSucceededFuture(Google_Protobuf_Empty())
-    }
-    
-    func save_recorded_data(request: MotorControl_SaveRequest, context: StatusOnlyCallContext) -> EventLoopFuture<Google_Protobuf_Empty> {
-        // TODO Recording...
-        return context.eventLoop.makeSucceededFuture(Google_Protobuf_Empty())
-    }
-}
-

SwiftyCar/Sources/SwiftyCar/PwmWrappers.swift

@@ -1,104 +0,0 @@
-//
-//  PwmWrappers.swift
-//
-//  Simple wrappers for PwmOutput I'll create as I go along, to match gpioZero's implementation I'm currently using.
-//  See gpiozero code here: https://github.com/gpiozero/gpiozero/blob/master/gpiozero/output_devices.py
-//
-//  Created by Michael Pivato on 9/5/20.
-//
-
-import Foundation
-import SwiftyGPIO
-
-enum ServoError: Error{
-    case invalidMinPulseWidth
-    case invalidMaxPulseWidth
-}
-
-class Servo{
-    private (set) var frameWidth : Float
-    private var minDc: Float
-    private var dcRange: Float
-    private var minValue: Float
-    private var valueRange: Float
-    private var internalValue: Float
-    
-    // TODO: Use a factory to provide the correct pwm rather than passing the pin in.
-    private var device: PWMOutput
-    private var currentPulseWIdth: Float
-    
-    var minPulseWidth: Float {
-        get{
-            return self.minDc * self.frameWidth
-        }
-    }
-    
-    var maxPulseWidth: Float{
-        get{
-            return (self.dcRange * self.frameWidth) + self.minPulseWidth
-        }
-    }
-    
-    // TODO: Also store the pulse width (needs to be within this class)
-    //    var pulseWidth: Float{
-    //        get{
-    //            return self.device//self.pwm_device.pin.state * self.frame_width
-    //        }
-    //        set (value){
-    //
-    //        }
-    //    }
-    
-    var value: Float{
-        get{
-            internalValue
-        }
-        set(newValue){
-            self.internalValue = newValue
-            self.device.startPWM(period: Int(self.frameWidth),
-                                 // Multiple by 100 to get into percentage. I don't like that...
-                                 duty: self.minDc + self.dcRange * ((internalValue - self.minValue) / self.valueRange) * 100)
-        }
-    }
-    
-    // TODO: Change this to nanoseconds to match PWMOutput
-    init?(forPin pin: PWMOutput, startingAt initialValue: Float = 0, withMinPulseWidth minPulseWidth: Float = 1000000,
-          withMaxPulseWidth maxPulseWidth: Float = 2000000, withFrameWidth frameWidth: Float = 20000000) throws {
-        if(minPulseWidth >= maxPulseWidth){
-            throw ServoError.invalidMinPulseWidth
-        }
-        
-        if(maxPulseWidth >= frameWidth){
-            throw ServoError.invalidMaxPulseWidth
-        }
-        self.frameWidth = frameWidth
-        self.minDc = minPulseWidth / frameWidth
-        self.dcRange = (maxPulseWidth - minPulseWidth) / frameWidth
-        self.minValue = -1
-        self.valueRange = 2
-        
-        self.currentPulseWIdth = 0
-        
-        // Initialise pin immediately.
-        self.device = pin
-        self.device.initPWM()
-        self.internalValue = initialValue
-        self.device.startPWM(period: Int(self.frameWidth), duty: self.minDc + self.dcRange * ((internalValue - self.minValue) / self.valueRange) * 100)
-    }
-    
-    func min(){
-        self.value = -1
-    }
-    
-    func mid(){
-        self.value = 0
-    }
-    
-    func max() {
-        self.value = 1
-    }
-    
-    func detach(){
-        self.device.stopPWM()
-    }
-}

SwiftyCar/Sources/SwiftyCar/Vehicle.swift

@@ -1,65 +0,0 @@
-//
-//  Vehicle.swift
-//
-//
-//  Created by Michael Pivato on 8/5/20.
-//
-
-import Foundation
-import SwiftyGPIO
-
-protocol Vehicle{
-    mutating func move2D(magnitude: Float, angle: Float)
-}
-
-protocol Vehicle2D: Vehicle{
-    var throttle: Float {get set}
-    var steering: Float {get set}
-}
-
-class MockVehicle: Vehicle2D{
-    var throttle: Float = 0
-    var steering: Float = 0
-    
-    func move2D(magnitude: Float, angle: Float) {
-        
-    }
-}
-
-class RPiVehicle2D: Vehicle2D{
-    public let pwmThrottle: Servo
-    public let pwmSteering: Servo
-    
-    var throttle: Float{
-        get{
-            return pwmThrottle.value
-        }
-        set(value){
-            pwmThrottle.value = value
-        }
-    }
-    
-    var steering: Float{
-        get{
-            return pwmSteering.value
-        }
-        set(value){
-            pwmSteering.value = value
-        }
-    }
-    
-    init(withThrottlePin: Servo, withSteeringPin: Servo){
-        pwmThrottle = withThrottlePin
-        pwmSteering = withSteeringPin
-    }
-    
-    func move2D(magnitude: Float, angle: Float) {
-        
-    }
-    
-    func stop(){
-        pwmThrottle.detach()
-        pwmSteering.detach()
-    }
-    
-}

SwiftyCar/Sources/SwiftyCar/VehicleFactory.swift

@@ -1,27 +0,0 @@
-//
-//  File.swift
-//  
-//
-//  Created by Michael Pivato on 20/5/20.
-//
-
-import Foundation
-import SwiftyGPIO
-
-
-
-func getVehicle2D() throws -> Vehicle2D {
-    if let value = ProcessInfo.processInfo.environment["CAR_VEHICLE"] {
-        switch value{
-        case "CAR_2D":
-            // Get car for rpi.
-            let pwms = SwiftyGPIO.hardwarePWMs(for:.RaspberryPi3)!
-            
-            // Read the feature database.
-            return try RPiVehicle2D(withThrottlePin: Servo(forPin: (pwms[0]?[.P18])!)!, withSteeringPin:Servo(forPin: (pwms[1]?[.P19])!)!)
-        default:
-            return MockVehicle()
-        }
-    }
-    return MockVehicle()
-}

SwiftyCar/Sources/SwiftyCar/main.swift

@@ -1,70 +0,0 @@
-//
-//  main.swift
-//  
-//
-//  Created by Michael Pivato on 8/5/20.
-//
-
-import NIO
-import GRPC
-import SwiftRPLidar
-import SwiftSerial
-
-func doServer() throws {
-    // Copied from examples
-    // Create an event loop group for the server to run on.
-    let group = MultiThreadedEventLoopGroup(numberOfThreads: System.coreCount)
-    defer {
-      try! group.syncShutdownGracefully()
-    }
-    
-    let lidar = createLidar()
-    lidar.iterMeasurements{measruement in
-        print(measruement.quality)
-        return false
-    }
-    // Create a provider using the features we read.
-    let provider = try MotorProvider(vehicle: getVehicle2D())
-    let trackingProvider = LidarProvider(lidar: lidar)
-
-    // Start the server and print its address once it has started.
-    let server = Server.insecure(group: group)
-      .withServiceProviders([provider, trackingProvider])
-      .bind(host: "localhost", port: 0)
-
-    server.map {
-      $0.channel.localAddress
-    }.whenSuccess { address in
-      print("server started on port \(address!.port!)")
-    }
-
-    // Wait on the server's `onClose` future to stop the program from exiting.
-    _ = try server.flatMap {
-      $0.onClose
-    }.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()
-}
-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)
-        }
-        
-    }
-}

SwiftyCar/Tests/LinuxMain.swift

@@ -1,7 +0,0 @@
-import XCTest
-
-import SwiftyCarTests
-
-var tests = [XCTestCaseEntry]()
-tests += SwiftyCarTests.allTests()
-XCTMain(tests)

SwiftyCar/Tests/SwiftyCarTests/SwiftyCarTests.swift

@@ -1,15 +0,0 @@
-import XCTest
-@testable import SwiftyCar
-
-final class SwiftyCarTests: XCTestCase {
-    func testExample() {
-        // This is an example of a functional test case.
-        // Use XCTAssert and related functions to verify your tests produce the correct
-        // results.
-//        XCTAssertEqual(SwiftyCar().text, "Hello, World!")
-    }
-
-    static var allTests = [
-        ("testExample", testExample),
-    ]
-}

SwiftyCar/Tests/SwiftyCarTests/XCTestManifests.swift

@@ -1,9 +0,0 @@
-import XCTest
-
-#if !canImport(ObjectiveC)
-public func allTests() -> [XCTestCaseEntry] {
-    return [
-        testCase(SwiftyCarTests.allTests),
-    ]
-}
-#endif

SwiftyCar/build.gradle

@@ -1,16 +0,0 @@
-configurations{
-        swift {
-            canBeConsumed = false
-            canBeResolved = true
-    }
-}
-
-dependencies {
-    swift project(path: ':protobuf', configuration: 'swift')
-}
-
-task copySwiftCode(type: Copy, dependsOn: configurations.swift) {
-    // Copy python protobuf code from proto project.
-    from zipTree(configurations.swift.asPath)
-    into './Sources/SwiftyCar'
-}

app/build.gradle

@@ -4,12 +4,11 @@ plugins{
 }
 
 android {
-    compileSdkVersion 29
-    buildToolsVersion "29.0.2"
     defaultConfig {
         applicationId "org.vato.carcontroller"
-        minSdkVersion 26
+        compileSdk 35
-        targetSdkVersion 29
+        minSdkVersion 35
+        targetSdkVersion 35
         versionCode 1
         versionName "1.0"
         testInstrumentationRunner "androidx.test.runner.AndroidJUnitRunner"
@@ -21,24 +20,31 @@ android {
         }
     }
     compileOptions {
-        sourceCompatibility = 1.8
+        sourceCompatibility = 21
-        targetCompatibility = 1.8
+        targetCompatibility = 21
     }
+    buildFeatures {
+        mlModelBinding true
+    }
+    namespace 'org.vato.carcontroller'
 }
 
 dependencies {
     implementation project(':protobuf')
     implementation fileTree(dir: 'libs', include: ['*.jar'])
-    implementation 'androidx.appcompat:appcompat:1.1.0'
+    implementation 'androidx.appcompat:appcompat:1.7.0'
-    implementation 'androidx.constraintlayout:constraintlayout:1.1.3'
+    implementation 'androidx.constraintlayout:constraintlayout:2.2.0'
-    implementation 'com.google.android.material:material:1.1.0'
+    implementation 'com.google.android.material:material:1.12.0'
-    implementation 'androidx.preference:preference:1.1.1'
+    implementation 'androidx.preference:preference:1.2.1'
-    testImplementation 'junit:junit:4.12'
+    implementation 'org.tensorflow:tensorflow-lite-support:0.1.0'
-    androidTestImplementation 'androidx.test:runner:1.2.0'
+    implementation 'org.tensorflow:tensorflow-lite-metadata:0.1.0-rc2'
-    androidTestImplementation 'androidx.test.espresso:espresso-core:3.2.0'
+    implementation 'org.tensorflow:tensorflow-lite-gpu:2.3.0'
-    implementation 'io.grpc:grpc-okhttp:1.29.0' // CURRENT_GRPC_VERSION
+    testImplementation 'junit:junit:4.13.2'
-    implementation 'io.grpc:grpc-protobuf-lite:1.29.0' // CURRENT_GRPC_VERSION
+    androidTestImplementation 'androidx.test:runner:1.6.2'
-    implementation 'io.grpc:grpc-stub:1.29.0' // CURRENT_GRPC_VERSION
+    androidTestImplementation 'androidx.test.espresso:espresso-core:3.6.1'
-    implementation 'javax.annotation:javax.annotation-api:1.2'
+    implementation 'io.grpc:grpc-okhttp:1.57.0'
+    implementation 'io.grpc:grpc-protobuf-lite:1.57.0'
+    implementation 'io.grpc:grpc-stub:1.57.0'
+    implementation 'javax.annotation:javax.annotation-api:1.3.2'
     implementation 'org.zeromq:jeromq:0.5.2'
 }

app/src/androidTest/java/com/example/carcontroller/ExampleInstrumentedTest.java

@@ -1,27 +0,0 @@
-package com.example.carcontroller;
-
-import android.content.Context;
-
-import androidx.test.platform.app.InstrumentationRegistry;
-import androidx.test.ext.junit.runners.AndroidJUnit4;
-
-import org.junit.Test;
-import org.junit.runner.RunWith;
-
-import static org.junit.Assert.*;
-
-/**
- * Instrumented test, which will execute on an Android device.
- *
- * @see <a href="http://d.android.com/tools/testing">Testing documentation</a>
- */
-@RunWith(AndroidJUnit4.class)
-public class ExampleInstrumentedTest {
-    @Test
-    public void useAppContext() {
-        // Context of the app under test.
-        Context appContext = InstrumentationRegistry.getInstrumentation().getTargetContext();
-
-        assertEquals("com.example.carcontroller", appContext.getPackageName());
-    }
-}

app/src/main/AndroidManifest.xml

@@ -1,6 +1,5 @@
 <?xml version="1.0" encoding="utf-8"?>
-<manifest xmlns:android="http://schemas.android.com/apk/res/android"
+<manifest xmlns:android="http://schemas.android.com/apk/res/android">
-    package="org.vato.carcontroller">
 
     <uses-permission android:name="android.permission.INTERNET" />
     <uses-permission android:name="android.permission.ACCESS_NETWORK_STATE" />
@@ -12,7 +11,7 @@
         android:roundIcon="@mipmap/ic_launcher_round"
         android:supportsRtl="true"
         android:theme="@style/AppTheme">
-        <activity android:name="org.vato.carcontroller.SLAM.SlamController"></activity>
+        <activity android:name="org.vato.carcontroller.SLAM.SlamController" />
         <activity android:name="org.vato.carcontroller.LIDAR.LidarTrackingController" />
         <activity
             android:name="org.vato.carcontroller.SettingsActivity"
@@ -20,6 +19,7 @@
             android:parentActivityName="org.vato.carcontroller.MainActivity" />
         <activity android:name="org.vato.carcontroller.SimpleController" />
         <activity
+            android:exported="true"
             android:name="org.vato.carcontroller.MainActivity"
             android:theme="@style/AppTheme.NoActionBar">
             <intent-filter>

app/src/main/java/org/vato/carcontroller/PiLoader.java

@@ -17,12 +17,12 @@ public class PiLoader implements Runnable {
 
     private Integer steeringValue = 50;
     private Integer throttleValue = 50;
-    private ManagedChannel mChannel;
+    private final ManagedChannel mChannel;
 
-    private CarControlGrpc.CarControlBlockingStub stub;
+    private final CarControlGrpc.CarControlBlockingStub stub;
-    private AtomicBoolean stop = new AtomicBoolean(false);
+    private final AtomicBoolean stop = new AtomicBoolean(false);
     private Thread piUpdaterThread;
-    private boolean useGrpcStream;
+    private final boolean useGrpcStream;
 
     public PiLoader(String host, Integer port, boolean useGrpcStream) {
         mChannel = ManagedChannelBuilder.forAddress(host, port).usePlaintext().build();
@@ -93,9 +93,9 @@ public class PiLoader implements Runnable {
     private void doStreamUpdates() {
         // Stream if user sets use_grpc_streams to true. This method is more efficient but less compatible.
         final CountDownLatch finishLatch = new CountDownLatch(1);
-        StreamObserver<Empty> responseObserver = new StreamObserver<Empty>() {
+        StreamObserver<Vehicle2DResponse> responseObserver = new StreamObserver<Vehicle2DResponse>() {
             @Override
-            public void onNext(Empty value) {
+            public void onNext(Vehicle2DResponse value) {
                 finishLatch.countDown();
                 Log.d("PiLoader", "Finished streaming");
             }
@@ -141,12 +141,12 @@ public class PiLoader implements Runnable {
 
     public void saveRecording() {
         // Ideally don't want to use a blocking stub here, android may complain.
-        Empty done = stub.saveRecordedData(SaveRequest.newBuilder().setFile("Test").build());
+        SaveResponse done = stub.saveRecordedData(SaveRequest.newBuilder().setFile("Test").build());
     }
 
 
     public void record(boolean record) {
         // Ideally don't want to use a blocking stub here, android may complain.
-        Empty done = stub.record(RecordingReqeust.newBuilder().setRecord(record).build());
+        RecordingResponse done = stub.record(RecordingReqeust.newBuilder().setRecord(record).build());
     }
 }

app/src/main/java/org/vato/carcontroller/SLAM/SlamView.java

@@ -14,12 +14,14 @@ import android.view.SurfaceView;
 import androidx.preference.PreferenceManager;
 
 import com.google.protobuf.ByteString;
-import com.google.protobuf.Empty;
 
 import org.vato.carcontroller.SlamControlGrpc;
 import org.vato.carcontroller.SlamDetails;
 import org.vato.carcontroller.SlamLocation;
 import org.vato.carcontroller.SlamScan;
+import org.vato.carcontroller.StartMapStreamingResponse;
+import org.vato.carcontroller.StopStreamingRequest;
+import org.vato.carcontroller.StopStreamingResponse;
 import org.vato.carcontroller.Updaters.AbstractUpdater;
 import org.vato.carcontroller.Updaters.GrpcUpdater;
 import org.vato.carcontroller.Updaters.ZmqUpdater;
@@ -102,9 +104,9 @@ public class SlamView extends SurfaceView implements AbstractUpdater.MapChangedL
 
     private void doZmqSlamStream() {
         // TODO: See if this bootstrapping can be integrated into updaters, as grpc requires a similar thing.
-        StreamObserver<Empty> response = new StreamObserver<Empty>() {
+        StreamObserver<StartMapStreamingResponse> response = new StreamObserver<StartMapStreamingResponse>() {
             @Override
-            public void onNext(Empty value) {
+            public void onNext(StartMapStreamingResponse value) {
                 mapThread.start();
             }
 
@@ -129,9 +131,9 @@ public class SlamView extends SurfaceView implements AbstractUpdater.MapChangedL
     public void stop() {
         // TODO: Use grpc to tell zmq to stop.
         slam.stop();
-        stub.stopStreaming(Empty.newBuilder().build(), new StreamObserver<Empty>() {
+        stub.stopStreaming(StopStreamingRequest.newBuilder().build(), new StreamObserver<StopStreamingResponse>() {
             @Override
-            public void onNext(Empty value) {
+            public void onNext(StopStreamingResponse value) {
             }
 
             @Override

app/src/main/java/org/vato/carcontroller/depth/DepthPredictionModel.java

@@ -0,0 +1,60 @@
+package org.vato.carcontroller.depth;
+
+import android.content.Context;
+import android.graphics.Bitmap;
+
+import org.tensorflow.lite.gpu.CompatibilityList;
+import org.tensorflow.lite.support.image.ColorSpaceType;
+import org.tensorflow.lite.support.image.TensorImage;
+import org.tensorflow.lite.support.model.Model;
+import org.vato.carcontroller.ml.MobilenetNnconv5;
+
+import java.io.IOException;
+
+/**
+ * Predicts the depth
+ */
+public class DepthPredictionModel implements AutoCloseable {
+
+    private MobilenetNnconv5 model = null;
+
+    private DepthPredictionModel() {
+    }
+
+    public static DepthPredictionModel newModel(Context context) {
+        try {
+            DepthPredictionModel created = new DepthPredictionModel();
+            Model.Options.Builder options = new Model.Options.Builder();
+            CompatibilityList compatList = new CompatibilityList();
+            if (compatList.isDelegateSupportedOnThisDevice()) {
+                // TODO: Choice of gpu and nnapi?
+                options.setDevice(Model.Device.GPU);
+            } else {
+                // TODO: User configurable?
+                options.setNumThreads(4);
+            }
+            created.model = MobilenetNnconv5.newInstance(context, options.build());
+            return created;
+        } catch (IOException e) {
+            return null;
+        }
+    }
+
+    public Bitmap predict(Bitmap bitmap) {
+        // TODO: Resizing/cropping, handle it all in the framework
+        TensorImage image = TensorImage.fromBitmap(bitmap);
+        // Runs model inference and gets result.
+        MobilenetNnconv5.Outputs outputs = model.process(image.getTensorBuffer());
+        TensorImage depth = new TensorImage();
+        depth.load(outputs.getOutputFeature0AsTensorBuffer(), ColorSpaceType.GRAYSCALE);
+        // TODO: Resize back to something similar to original (ignore crop obviously)?
+        return depth.getBitmap();
+
+    }
+
+
+    @Override
+    public void close() {
+        model.close();
+    }
+}

app/src/main/res/layout/activity_main.xml

@@ -4,6 +4,7 @@
     xmlns:tools="http://schemas.android.com/tools"
     android:layout_width="match_parent"
     android:layout_height="match_parent"
+    android:fitsSystemWindows="true"
     tools:context=".MainActivity">
 
     <com.google.android.material.appbar.AppBarLayout

build.gradle

@@ -3,11 +3,11 @@
 buildscript {
     repositories {
         google()
-        jcenter()
+        mavenCentral()
     }
     dependencies {
-        classpath 'com.android.tools.build:gradle:4.0.1'
+        classpath 'com.android.tools.build:gradle:8.8.1'
-        classpath 'com.google.protobuf:protobuf-gradle-plugin:0.8.10'
+        classpath 'com.google.protobuf:protobuf-gradle-plugin:0.9.4'
 
         // NOTE: Do not place your application dependencies here; they belong
         // in the individual module build.gradle files
@@ -17,8 +17,7 @@ buildscript {
 allprojects {
     repositories {
         google()
-        jcenter()
+        mavenCentral()
-        mavenLocal()
     }
 }
 

car-rs/Cargo.toml

@@ -0,0 +1,33 @@
+[package]
+name = "car-rs"
+version = "0.1.0"
+edition = "2021"
+
+# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
+
+[dependencies]
+# https://github.com/golemparts/rppal
+rppal = { version = "0.13.1", optional = true }
+futures-core = "0.3"
+futures-util = "0.3"
+tokio = { version = "1", features = ["full"] }
+tokio-stream = "0.1.11"
+prost = "0.11"
+
+# https://github.com/hyperium/tonic
+tonic = "0.8.3"
+
+# https://docs.rs/serialport/4.0.1/serialport/index.html
+serialport = "4.3.0"
+
+clap = { version = "4.1.8", features = ["derive"] }
+
+[build-dependencies]
+tonic-build = "0.8.3"
+
+[features]
+rppal = ["dep:rppal"]
+
+# How to get dependencies for my own projects, so I don't need to upload to crates.io
+# or create my own rust package repo.
+# https://doc.rust-lang.org/cargo/reference/specifying-dependencies.html#specifying-dependencies-from-git-repositories

car-rs/build.rs

@@ -0,0 +1,8 @@
+fn main() -> Result<(), Box<dyn std::error::Error>> {
+    let protos = ["slam/SlamController.proto", "control/motorService.proto"];
+    tonic_build::configure().compile(
+        &protos.map(|proto| "../protobuf/src/main/proto/car/".to_owned() + proto),
+        &["../protobuf/src/main/proto"],
+    )?;
+    Ok(())
+}

car-rs/src/grpcserver.rs

@@ -0,0 +1,141 @@
+pub mod motor_control_service {
+    tonic::include_proto!("motor_control");
+}
+
+pub mod slam_controller_service {
+    tonic::include_proto!("slam_control");
+}
+
+use std::{sync::Mutex, time::Duration};
+
+use car_rs::Vehicle;
+use futures_util::StreamExt;
+use motor_control_service::car_control_server::CarControl;
+use tokio::time;
+use tokio_stream::wrappers::ReceiverStream;
+use tonic::{Request, Response, Status, Streaming};
+
+use self::{
+    motor_control_service::{
+        RecordingReqeust, RecordingResponse, SaveRequest, SaveResponse, SteeringRequest,
+        SteeringResponse, ThrottleRequest, ThrottleResponse, Vehicle2DRequest, Vehicle2DResponse,
+    },
+    slam_controller_service::{
+        slam_control_server::SlamControl, SlamDetails, SlamLocation, SlamScan,
+        StartMapStreamingResponse, StopStreamingRequest, StopStreamingResponse,
+    },
+};
+
+#[derive(Debug)]
+pub struct MotorControlService<T>
+where
+    T: Vehicle,
+{
+    vehicle: Mutex<T>,
+}
+
+impl<T: Vehicle> MotorControlService<T> {
+    pub fn new(vehicle: T) -> MotorControlService<T> {
+        MotorControlService {
+            vehicle: Mutex::new(vehicle),
+        }
+    }
+}
+
+#[tonic::async_trait]
+impl<T: Vehicle + Send + Sync + 'static> CarControl for MotorControlService<T> {
+    async fn set_throttle(
+        &self,
+        _request: Request<ThrottleRequest>,
+    ) -> Result<Response<ThrottleResponse>, Status> {
+        self.vehicle
+            .lock()
+            .unwrap()
+            .set_throttle(_request.into_inner().throttle as f64);
+        Ok(Response::new(ThrottleResponse { throttle_set: true }))
+    }
+
+    async fn set_steering(
+        &self,
+        _request: Request<SteeringRequest>,
+    ) -> Result<Response<SteeringResponse>, Status> {
+        self.vehicle
+            .lock()
+            .unwrap()
+            .set_steering(_request.into_inner().steering as f64);
+        Ok(Response::new(SteeringResponse { steering_set: true }))
+    }
+
+    async fn stream_vehicle_2d(
+        &self,
+        request: Request<Streaming<Vehicle2DRequest>>,
+    ) -> Result<Response<Vehicle2DResponse>, Status> {
+        let mut stream = request.into_inner();
+
+        // If we don't a request for 3 seconds, timeout and stop the vehicle
+        while let Ok(Some(Ok(req))) = time::timeout(Duration::from_secs(3), stream.next()).await {
+            let mut vehicle = self.vehicle.lock().unwrap();
+            if let Some(throttle) = req.throttle {
+                vehicle.set_throttle(throttle.throttle as f64);
+            }
+            if let Some(steering) = req.steering {
+                vehicle.set_steering(steering.steering as f64);
+            }
+        }
+
+        self.vehicle.lock().unwrap().set_throttle(0.);
+
+        Ok(Response::new(Vehicle2DResponse {}))
+    }
+
+    async fn record(
+        &self,
+        _request: Request<RecordingReqeust>,
+    ) -> Result<Response<RecordingResponse>, Status> {
+        unimplemented!()
+    }
+
+    async fn save_recorded_data(
+        &self,
+        _request: Request<SaveRequest>,
+    ) -> Result<Response<SaveResponse>, Status> {
+        unimplemented!()
+    }
+}
+
+#[derive(Debug)]
+pub struct SlamControlService {}
+
+#[tonic::async_trait]
+impl SlamControl for SlamControlService {
+    async fn start_map_streaming(
+        &self,
+        request: Request<SlamDetails>,
+    ) -> Result<Response<StartMapStreamingResponse>, Status> {
+        todo!()
+    }
+
+    type map_streamStream = ReceiverStream<Result<SlamScan, Status>>;
+
+    async fn map_stream(
+        &self,
+        request: Request<SlamDetails>,
+    ) -> Result<Response<Self::map_streamStream>, Status> {
+        let scan = SlamScan {
+            map: vec![],
+            location: Some(SlamLocation {
+                theta: 1.,
+                x: 1.,
+                y: 1.,
+            }),
+        };
+        todo!()
+    }
+
+    async fn stop_streaming(
+        &self,
+        request: Request<StopStreamingRequest>,
+    ) -> Result<Response<StopStreamingResponse>, Status> {
+        todo!()
+    }
+}

car-rs/src/lib.rs

@@ -0,0 +1,202 @@
+use serialport::SerialPort;
+
+mod lidar;
+
+// TODO: Should be returning results in these traits
+pub trait Servo {
+    fn get_value(&self) -> f64;
+
+    fn set_value(&mut self, value: f64);
+
+    fn min(&mut self) {
+        self.set_value(-1.);
+    }
+    fn mid(&mut self) {
+        self.set_value(0.);
+    }
+    fn max(&mut self) {
+        self.set_value(1.);
+    }
+}
+
+pub trait Vehicle {
+    fn get_throttle(&self) -> f64;
+    fn set_throttle(&mut self, throttle: f64);
+    fn get_steering(&self) -> f64;
+    fn set_steering(&mut self, steering: f64);
+}
+#[cfg(feature = "rppal")]
+pub mod rppal {
+    use rppal::pwm::{Channel, Pwm};
+
+    pub struct RpiPwmServo {
+        pwm: Pwm,
+        min_duty_cycle: f64,
+        duty_cycle_range: f64,
+        value: f64,
+        frame_width: f64,
+    }
+
+    impl RpiPwmServo {
+        pub fn new(pwm: Pwm) -> RpiPwmServo {
+            RpiPwmServo::new(pwm, 1000000)
+        }
+
+        pub fn new(pwm: Pwm, min_pulse_width: f64) -> RpiPwmServo {}
+
+        pub fn new(
+            pwm: Pwm,
+            min_pulse_width: f64,
+            max_pulse_width: f64,
+            frame_width: f64,
+        ) -> RpiPwmServo {
+            RpiPwmServo {
+                pwm,
+                min_duty_cycle: min_pulse_width / frame_width,
+                duty_cycle_range: (max_pulse_width - min_pulse_width) / frame_width,
+                frame_width,
+            }
+        }
+    }
+
+    impl Default for RpiPwmServo {
+        fn default() -> Self {
+            Self {
+                pwm: Pwm::new(Channel::Pwm0).expect("Failed to initialise Pwm servo"),
+            }
+        }
+    }
+
+    impl Servo for RpiPwmServo {
+        fn get_duty_cycle(&self) -> f64 {
+            self.pwm.duty_cycle().unwrap_or(0.)
+        }
+
+        fn set_duty_cycle(&self, pwm: f64) {
+            self.pwm
+                .set_duty_cycle(pwm)
+                .expect("Failed to write duty cycle");
+        }
+
+        fn get_frequency(&self) -> f64 {
+            self.pwm.duty_cycle().unwrap_or(0.0)
+        }
+
+        fn set_frequency(&self, frequency: f64) {
+            self.pwm
+                .set_frequency(frequency, self.get_duty_cycle())
+                .expect("Failed to set Frequency")
+        }
+    }
+}
+
+pub struct Esp32SerialPwmServo<T: SerialPort> {
+    serial_port: T,
+    value: f64,
+    channel: u8,
+    pin: u8,
+}
+
+impl<T: SerialPort> Esp32SerialPwmServo<T> {
+    pub fn new(serial_port: T, channel: u8, pin: u8) -> Esp32SerialPwmServo<T> {
+        let mut servo = Esp32SerialPwmServo {
+            serial_port,
+            value: 0.,
+            channel,
+            pin,
+        };
+        servo.init_pwm();
+        servo
+    }
+
+    fn init_pwm(&mut self) {
+        let bytes_written = self.serial_port.write(&[0, 1, self.channel, self.pin]);
+        // TODO: Better error handling (even anyhow would be better)
+        match bytes_written {
+            Ok(size) => println!("{}", size),
+            Err(err) => eprintln!("{}", err),
+        }
+    }
+}
+
+impl<T: SerialPort> Servo for Esp32SerialPwmServo<T> {
+    fn get_value(&self) -> f64 {
+        self.value
+    }
+
+    fn set_value(&mut self, value: f64) {
+        let mut temp_value = value;
+        if temp_value < -1. {
+            temp_value = -1.;
+        } else if temp_value > 1. {
+            temp_value = 1.;
+        }
+        self.value = temp_value;
+        let bytes_written = self
+            .serial_port
+            .write(&[self.channel, ((value + 1.) / 2. * 14. + 7.) as u8]);
+        // TODO: Better error handling
+        match bytes_written {
+            Ok(size) => println!("{}", size),
+            Err(err) => eprintln!("{}", err),
+        }
+    }
+}
+
+pub struct ServoVehicle<T: Servo> {
+    steering_servo: T,
+    throttle_servo: T,
+}
+
+impl<T: Servo> ServoVehicle<T> {
+    pub fn new(steering_servo: T, throttle_servo: T) -> ServoVehicle<T> {
+        ServoVehicle {
+            steering_servo,
+            throttle_servo,
+        }
+    }
+}
+
+impl<T: Servo> Vehicle for ServoVehicle<T> {
+    fn get_throttle(&self) -> f64 {
+        self.throttle_servo.get_value()
+    }
+
+    fn set_throttle(&mut self, throttle: f64) {
+        self.throttle_servo.set_value(throttle);
+    }
+
+    fn get_steering(&self) -> f64 {
+        self.steering_servo.get_value()
+    }
+
+    fn set_steering(&mut self, steering: f64) {
+        self.steering_servo.set_value(steering);
+    }
+}
+
+#[derive(Default, Debug)]
+pub struct PrintVehicle {
+    throttle: f64,
+    steering: f64,
+}
+
+impl Vehicle for PrintVehicle {
+    fn get_throttle(&self) -> f64 {
+        self.throttle
+    }
+
+    fn set_throttle(&mut self, throttle: f64) {
+        println!("New Throttle: {}", throttle);
+        self.throttle = throttle;
+    }
+
+    fn get_steering(&self) -> f64 {
+        self.steering
+    }
+
+    fn set_steering(&mut self, steering: f64) {
+        println!("New Steering: {}", steering);
+        self.steering = steering;
+    }
+}

car-rs/src/lidar.rs

@@ -0,0 +1,324 @@
+use core::fmt;
+use serialport::ClearBuffer;
+use serialport::SerialPort;
+use std::io;
+use std::io::Error;
+use std::string::FromUtf8Error;
+
+const SYNC: u8 = 0xA5;
+const SYNC2: u8 = 0x5A;
+
+const GET_INFO: u8 = 0x50;
+const GET_HEALTH: u8 = 0x52;
+
+const STOP: u8 = 0x25;
+const RESET: u8 = 0x40;
+
+const SCAN: u8 = 0x20;
+#[allow(dead_code)]
+const FORCE_SCAN: u8 = 0x21;
+
+const DESCRIPTOR_LEN: usize = 7;
+const INFO_LEN: u8 = 20;
+const HEALTH_LEN: u8 = 3;
+
+const INFO_TYPE: u8 = 4;
+const HEALTH_TYPE: u8 = 6;
+const SCAN_TYPE: u8 = 129;
+
+const SET_PWM: u8 = 0xF0;
+const MAX_MOTOR_PWM: usize = 1023;
+#[allow(dead_code)]
+const DEFAULT_MOTOR_PWM: usize = 660;
+
+const SCAN_SIZE: u8 = 5;
+
+pub enum HealthStatus {
+    Good,
+    Warning,
+    Error,
+}
+
+impl HealthStatus {
+    pub fn from_raw(raw: u8) -> HealthStatus {
+        match raw {
+            0 => HealthStatus::Good,
+            1 => HealthStatus::Warning,
+            _ => HealthStatus::Error,
+        }
+    }
+}
+
+pub enum RPLidarError {
+    FailedToStart,
+    IncorrectInfoFormat,
+    IncorrectHealthFormat,
+    ScanError(String),
+    IncorrectDescriptorFormat,
+    IOError(Error),
+}
+
+impl From<Error> for RPLidarError {
+    fn from(err: Error) -> Self {
+        RPLidarError::IOError(err)
+    }
+}
+
+impl From<FromUtf8Error> for RPLidarError {
+    fn from(_: FromUtf8Error) -> Self {
+        RPLidarError::IncorrectDescriptorFormat
+    }
+}
+
+impl fmt::Debug for RPLidarError {
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+        match self {
+            Self::FailedToStart => write!(f, "FailedToStart"),
+            Self::IncorrectInfoFormat => write!(f, "IncorrectInfoFormat"),
+            Self::IncorrectHealthFormat => write!(f, "IncorrectHealthFormat"),
+            Self::ScanError(arg0) => f.debug_tuple("ScanError").field(arg0).finish(),
+            Self::IncorrectDescriptorFormat => write!(f, "IncorrectDescriptorFormat"),
+            Self::IOError(arg0) => f.debug_tuple("IOError").field(arg0).finish(),
+        }
+    }
+}
+
+pub struct LidarScan {
+    pub new_scan: bool,
+    pub quality: u8,
+    pub angle: f64,
+    pub distance: f64,
+}
+
+fn process_scan(raw: [u8; SCAN_SIZE as usize]) -> Result<LidarScan, RPLidarError> {
+    let new_scan = raw[0] & 0b1;
+    let inversed_new_scan = (raw[0] >> 1) & 0b1;
+    let quality = raw[0] >> 2;
+    if new_scan == inversed_new_scan {
+        return Err(RPLidarError::ScanError(String::from(
+            "New scan flags mismatch",
+        )));
+    }
+
+    if (raw[1] & 0b1) != 1 {
+        return Err(RPLidarError::ScanError(String::from(
+            "Check bit not equal to 1",
+        )));
+    }
+
+    let angle = ((((raw[1] as isize) >> 1) + (raw[2] as isize) << 7) as f64) / 64.;
+    let distance = (((raw[3] as isize) + ((raw[4] as isize) << 8)) as f64) / 4.;
+
+    Ok(LidarScan {
+        new_scan: new_scan == 1,
+        quality,
+        angle,
+        distance,
+    })
+}
+
+pub struct Lidar<T: io::Read + io::Write> {
+    motor_running: bool,
+    serial_port: T,
+    is_scanning: bool,
+    measurements_per_batch: usize,
+    max_buffer_measurements: u32,
+}
+
+impl<T: SerialPort> Lidar<T> {
+    pub fn new(serial_port: T) -> Lidar<T> {
+        Lidar {
+            motor_running: false,
+            serial_port,
+            is_scanning: false,
+            measurements_per_batch: 200,
+            max_buffer_measurements: 500,
+        }
+    }
+
+    pub fn connect(&mut self) {
+        self.serial_port
+            .set_baud_rate(115200)
+            .expect("Failed to set baudrate");
+    }
+
+    pub fn start_motor(&mut self) {
+        self.serial_port
+            .write_data_terminal_ready(true)
+            .expect("Failed to write dtr");
+        // self.set_pwm(DEFAULT_MOTOR_PWM);
+        self.motor_running = true;
+    }
+
+    pub fn start_scanning(&mut self) -> Result<u8, RPLidarError> {
+        self.start_motor();
+        let (status, _) = self.get_health()?;
+
+        match status {
+            HealthStatus::Error => Err(RPLidarError::IncorrectHealthFormat),
+            _ => {
+                self.send_command(vec![SCAN]);
+                let (data_size, is_single, data_type) = self.read_descriptor()?;
+                if data_size != SCAN_SIZE || is_single || data_type != SCAN_TYPE {
+                    return Err(RPLidarError::IncorrectDescriptorFormat);
+                }
+                self.is_scanning = true;
+                Ok(data_size)
+            }
+        }
+    }
+
+    pub fn stop_motor(&mut self) {
+        self.set_pwm(0);
+        self.serial_port
+            .write_data_terminal_ready(false)
+            .expect("Failed to write dtr");
+        self.motor_running = false;
+    }
+
+    pub fn get_info(&mut self) -> Result<(u8, (u8, u8), u8, String), RPLidarError> {
+        self.send_command(vec![GET_INFO]);
+
+        let (data_size, is_single, data_type) = self.read_descriptor()?;
+
+        if data_size != INFO_LEN || !is_single || data_type != INFO_TYPE {
+            return Err(RPLidarError::IncorrectInfoFormat);
+        }
+        let mut buf = [0; INFO_LEN as usize];
+        self.serial_port.read(&mut buf)?;
+        let raw = buf;
+        let serial_number = String::from_utf8(Vec::from(&raw[4..]))?;
+        return Ok((raw[0], (raw[2], raw[1]), raw[3], serial_number));
+    }
+
+    pub fn get_health(&mut self) -> Result<(HealthStatus, usize), RPLidarError> {
+        self.send_command(vec![GET_HEALTH]);
+
+        let (data_size, is_single, data_type) = self.read_descriptor()?;
+
+        if data_size != HEALTH_LEN || !is_single || data_type != HEALTH_TYPE {
+            return Err(RPLidarError::IncorrectHealthFormat);
+        }
+
+        let mut buf = [0; HEALTH_LEN as usize];
+        self.serial_port.read(&mut buf)?;
+        let raw = buf;
+        let status = HealthStatus::from_raw(raw[0]);
+        let error_code = (raw[1] as usize) << 8 + raw[2];
+        return Ok((status, error_code));
+    }
+
+    pub fn clear_input(&mut self) {
+        self.serial_port
+            .clear(ClearBuffer::Input)
+            .expect("Failed to clear input buffer");
+    }
+
+    pub fn stop(&mut self) {
+        self.send_command(vec![STOP]);
+        self.is_scanning = false;
+    }
+
+    pub fn reset(&mut self) {
+        self.send_command(vec![RESET]);
+    }
+
+    pub fn receive_measurement_and_clear_buffer(
+        &mut self,
+        max_buffer_measurements: u32,
+    ) -> Result<[u8; SCAN_SIZE as usize], RPLidarError> {
+        if !self.is_scanning {
+            return Err(RPLidarError::ScanError(String::from(
+                "Haven't started scanning",
+            )));
+        }
+        let mut buf = [0; SCAN_SIZE as usize];
+        self.serial_port.read(&mut buf)?;
+        let raw = buf;
+        if max_buffer_measurements > 0 {
+            if self
+                .serial_port
+                .bytes_to_read()
+                .expect("Failed to get bytes to read")
+                > max_buffer_measurements * SCAN_SIZE as u32
+            {
+                println!("Too many measurements in the input buffer. Clearing Buffer");
+                self.serial_port
+                    .clear(ClearBuffer::Input)
+                    .expect("Failed to clear input buffer.");
+            }
+        }
+        Ok(raw)
+    }
+
+    fn send_command(&mut self, command: Vec<u8>) {
+        let mut vec = vec![SYNC];
+        vec.extend(command);
+        self.serial_port
+            .write(&vec)
+            .expect("Failed to send command");
+    }
+
+    fn send_payload_command(&mut self, cmd: u8, payload: Vec<u8>) {
+        let size: u8 = payload
+            .len()
+            .try_into()
+            .expect("Failed to convert payload length");
+        let mut req = vec![SYNC];
+        req.push(cmd);
+        req.push(size);
+        req.extend(payload);
+        let checksum = self.calc_checksum(&req);
+        req.push(checksum);
+        self.serial_port
+            .write(&req)
+            .expect("Failed to send payload");
+    }
+
+    fn calc_checksum(&self, data: &Vec<u8>) -> u8 {
+        data.iter()
+            .copied()
+            .reduce(|accum, next| accum ^ next)
+            .expect("Failed to calculate checksum")
+            .to_owned()
+    }
+
+    fn read_descriptor(&mut self) -> Result<(u8, bool, u8), RPLidarError> {
+        let mut descriptor: [u8; DESCRIPTOR_LEN] = [0; DESCRIPTOR_LEN];
+        let ret = self.serial_port.read(&mut descriptor)?;
+        if ret != DESCRIPTOR_LEN || (descriptor[0] != SYNC && descriptor[1] != SYNC2) {
+            eprintln!("Failed to read enough or something");
+            Err(RPLidarError::IncorrectDescriptorFormat)
+        } else {
+            let is_single = descriptor[DESCRIPTOR_LEN - 2] == 0;
+            Ok((descriptor[2], is_single, descriptor[DESCRIPTOR_LEN - 1]))
+        }
+    }
+
+    fn set_pwm(&mut self, pwm: usize) {
+        assert!(pwm <= MAX_MOTOR_PWM);
+        self.send_payload_command(SET_PWM, Vec::from(pwm.to_ne_bytes()));
+    }
+}
+
+// Probably want to have separate iterator types like we do in swift for measurements vs scans
+impl<T: SerialPort> Iterator for Lidar<T> {
+    type Item = Vec<LidarScan>;
+
+    fn next(&mut self) -> Option<Self::Item> {
+        if self.is_scanning {
+            let mut all_scans: Vec<LidarScan> = Vec::new();
+            while all_scans.len() < self.measurements_per_batch {
+                if let Ok(raw) =
+                    self.receive_measurement_and_clear_buffer(self.max_buffer_measurements)
+                {
+                    if let Ok(scan) = process_scan(raw) {
+                        all_scans.push(scan);
+                    }
+                }
+            }
+            return Some(all_scans);
+        }
+        None
+    }
+}

car-rs/src/main.rs

@@ -0,0 +1,73 @@
+use std::net::SocketAddr;
+
+use car_rs::{Esp32SerialPwmServo, PrintVehicle, ServoVehicle, Vehicle};
+use clap::{Parser, ValueEnum};
+use grpcserver::{
+    motor_control_service::car_control_server::CarControlServer, MotorControlService,
+};
+
+use tonic::transport::Server;
+
+mod grpcserver;
+
+#[derive(Debug, Copy, Clone, PartialEq, Eq, PartialOrd, Ord, ValueEnum)]
+enum VehicleType {
+    /// Use esp32 to control the car connected via usb
+    Esp32Serial,
+    /// Debug vehicle to print changes to std out.
+    Print,
+}
+
+#[derive(Parser, Debug)]
+#[command(version, about, long_about = None)]
+struct Args {
+    #[arg(short, long)]
+    serial_port: String,
+
+    #[arg(short, long, default_value_t = 10000)]
+    web_port: u32,
+
+    #[arg(short, long, default_value_t = 115200)]
+    baud_rate: u32,
+
+    #[arg(value_enum, short, long, default_value_t = VehicleType::Print)]
+    vehicle_type: VehicleType,
+}
+
+#[tokio::main]
+async fn main() -> Result<(), Box<dyn std::error::Error>> {
+    let args = Args::parse();
+    let addr = format!("[::1]:{}", &args.web_port).parse().unwrap();
+    match args.vehicle_type {
+        VehicleType::Esp32Serial => {
+            let mut steering_port = serialport::new(&args.serial_port, args.baud_rate)
+                .open_native()
+                .expect("Could not open serial port");
+            steering_port.set_exclusive(false)?;
+            let mut throttle_port = serialport::new(&args.serial_port, args.baud_rate)
+                .open_native()
+                .expect("Could not open serial port");
+            throttle_port.set_exclusive(false)?;
+            let steering_servo = Esp32SerialPwmServo::new(steering_port, 1, 12);
+            let throttle_servo = Esp32SerialPwmServo::new(throttle_port, 2, 18);
+            create_service(ServoVehicle::new(steering_servo, throttle_servo), addr).await?;
+        }
+        VehicleType::Print => {
+            let vehicle = PrintVehicle::default();
+            create_service(vehicle, addr).await?;
+        }
+    }
+
+    Ok(())
+}
+
+async fn create_service<T>(vehicle: T, addr: SocketAddr) -> Result<(), Box<dyn std::error::Error>>
+where
+    T: Vehicle + Send + Sync + 'static,
+{
+    let motor_control = MotorControlService::new(vehicle);
+
+    let svc = CarControlServer::new(motor_control);
+    Server::builder().add_service(svc).serve(addr).await?;
+    Ok(())
+}

esp32/.gitignore

@@ -0,0 +1,5 @@
+.pio
+.vscode/.browse.c_cpp.db*
+.vscode/c_cpp_properties.json
+.vscode/launch.json
+.vscode/ipch

esp32/.vscode/extensions.json

@@ -0,0 +1,7 @@
+{
+    // See http://go.microsoft.com/fwlink/?LinkId=827846
+    // for the documentation about the extensions.json format
+    "recommendations": [
+        "platformio.platformio-ide"
+    ]
+}

esp32/README.md

@@ -0,0 +1,32 @@
+This module includes an arduino sketch designed to run on the ESP32.
+
+The sketch takes simple input from serial (ESP32 microusb port), and converts the input to a duty cycle that can be used on servos.
+
+The protocol specification is as follows, assuming an array of bytes:
+
+| Byte Number |                                                                                                 Value Description |
+| :---------- | ----------------------------------------------------------------------------------------------------------------: |
+| 0           |                            0 if Calibrating a servo. Higher values indicates channel number to set duty cycle on. |
+| 1           | If byte 0 = 0: number of servos to calibrate. Else, the new duty cycle value for the channel specified in byte 0. |
+
+When setting the duty cycle, the current min angle is 0, and the max angle is 255, which allows the entire byte to be used for setting the duty cycle.
+
+The table below describes the byte format for calibrating a servo. This array of bytes can be repeated for the given number of servos in byte 1:
+
+| Byte Number |                                                                   Value Description |
+| :---------- | ----------------------------------------------------------------------------------: |
+| 0           | Servo channel to set (this will be byte 0 when setting duty cycle, so don't use 0). |
+| 1           |                                                             The pin number to setup |
+
+The min/max pulse widths are hardcoded to 1000us/2000us respectively.
+
+At the end of each loop, the entire array will be read, to flush any data that may cause issues later.
+
+Upcoming (TODO):
+
+- Use bit shift to allow 12 bits to be used for the duty cycle range, as there can only be a max of 16 channels anyway (4 bits).
+- Consider protobuf or msgpack for serialisation format, for more advanced use cases, and more maintainable communication formats (currently changing the message format will require changes to the entire protocol)
+
+## Deployment
+
+Sometimes you'll need to hold the boot button on the ESP32, then press upload (right arrow) on platformIO. Once the upload starts, you may release the boot button and wait for upload to complete.

esp32/include/README

@@ -0,0 +1,39 @@
+
+This directory is intended for project header files.
+
+A header file is a file containing C declarations and macro definitions
+to be shared between several project source files. You request the use of a
+header file in your project source file (C, C++, etc) located in `src` folder
+by including it, with the C preprocessing directive `#include'.
+
+```src/main.c
+
+#include "header.h"
+
+int main (void)
+{
+ ...
+}
+```
+
+Including a header file produces the same results as copying the header file
+into each source file that needs it. Such copying would be time-consuming
+and error-prone. With a header file, the related declarations appear
+in only one place. If they need to be changed, they can be changed in one
+place, and programs that include the header file will automatically use the
+new version when next recompiled. The header file eliminates the labor of
+finding and changing all the copies as well as the risk that a failure to
+find one copy will result in inconsistencies within a program.
+
+In C, the usual convention is to give header files names that end with `.h'.
+It is most portable to use only letters, digits, dashes, and underscores in
+header file names, and at most one dot.
+
+Read more about using header files in official GCC documentation:
+
+* Include Syntax
+* Include Operation
+* Once-Only Headers
+* Computed Includes
+
+https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html

esp32/lib/README

@@ -0,0 +1,46 @@
+
+This directory is intended for project specific (private) libraries.
+PlatformIO will compile them to static libraries and link into executable file.
+
+The source code of each library should be placed in a an own separate directory
+("lib/your_library_name/[here are source files]").
+
+For example, see a structure of the following two libraries `Foo` and `Bar`:
+
+|--lib
+|  |
+|  |--Bar
+|  |  |--docs
+|  |  |--examples
+|  |  |--src
+|  |     |- Bar.c
+|  |     |- Bar.h
+|  |  |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
+|  |
+|  |--Foo
+|  |  |- Foo.c
+|  |  |- Foo.h
+|  |
+|  |- README --> THIS FILE
+|
+|- platformio.ini
+|--src
+   |- main.c
+
+and a contents of `src/main.c`:
+```
+#include <Foo.h>
+#include <Bar.h>
+
+int main (void)
+{
+  ...
+}
+
+```
+
+PlatformIO Library Dependency Finder will find automatically dependent
+libraries scanning project source files.
+
+More information about PlatformIO Library Dependency Finder
+- https://docs.platformio.org/page/librarymanager/ldf.html

esp32/platformio.ini

@@ -0,0 +1,15 @@
+; PlatformIO Project Configuration File
+;
+;   Build options: build flags, source filter
+;   Upload options: custom upload port, speed and extra flags
+;   Library options: dependencies, extra library storages
+;   Advanced options: extra scripting
+;
+; Please visit documentation for the other options and examples
+; https://docs.platformio.org/page/projectconf.html
+
+[env:esp32doit-devkit-v1]
+platform = espressif32
+board = esp32doit-devkit-v1
+framework = arduino
+monitor_speed = 115200

esp32/src/main.cpp

@@ -0,0 +1,56 @@
+#include <Arduino.h>
+
+// Min/max widths, used to calculate min/max duty cycles.
+const int MIN_ANGLE = 12;
+const int MAX_ANGLE = 26;
+
+void setup()
+{
+    Serial.begin(115200);
+}
+
+void setupServos(uint8_t size, uint8_t *calibrationValues)
+{
+    // We assume there are 2 bytes per servo [channel number, pin number]. Ignore if there aren't.
+    if (size % 2 == 0)
+    {
+        for (int i = 0; i < size; i += 2)
+        {
+            int channel = calibrationValues[i];
+            int pin = calibrationValues[i + 1];
+            pinMode(pin, OUTPUT);
+            ledcSetup(channel, 50, 8);
+            ledcAttachPin(pin, channel);
+        }
+    }
+}
+
+void modifyServo(uint8_t channel, uint8_t newAngle)
+{
+    if (newAngle > MAX_ANGLE)
+    {
+        newAngle = MAX_ANGLE;
+    }
+    else if (newAngle < MIN_ANGLE)
+    {
+        newAngle = MIN_ANGLE;
+    }
+    ledcWrite(channel, newAngle);
+}
+
+void loop()
+{
+    uint8_t header[2];
+    Serial.readBytes(header, 2);
+
+    if (header[0] == 0)
+    {
+        uint8_t calibration[2 * header[1]];
+        Serial.readBytes(calibration, header[1]);
+        setupServos(header[1], calibration);
+    }
+    else
+    {
+        modifyServo(header[0], header[1]);
+    }
+}

esp32/test/README

@@ -0,0 +1,11 @@
+
+This directory is intended for PlatformIO Unit Testing and project tests.
+
+Unit Testing is a software testing method by which individual units of
+source code, sets of one or more MCU program modules together with associated
+control data, usage procedures, and operating procedures, are tested to
+determine whether they are fit for use. Unit testing finds problems early
+in the development cycle.
+
+More information about PlatformIO Unit Testing:
+- https://docs.platformio.org/page/plus/unit-testing.html

gradle.properties

@@ -17,4 +17,5 @@ org.gradle.jvmargs=-Xmx1536m
 android.useAndroidX=true
 # Automatically convert third-party libraries to use AndroidX
 android.enableJetifier=true
-
+android.nonTransitiveRClass=false
+android.nonFinalResIds=false

gradle/wrapper/gradle-wrapper.properties

@@ -1,5 +1,7 @@
 distributionBase=GRADLE_USER_HOME
 distributionPath=wrapper/dists
-distributionUrl=https\://services.gradle.org/distributions/gradle-6.4.1-bin.zip
+distributionUrl=https\://services.gradle.org/distributions/gradle-8.10.2-bin.zip
+networkTimeout=10000
+validateDistributionUrl=true
 zipStoreBase=GRADLE_USER_HOME
 zipStorePath=wrapper/dists

gradlew

@@ -1,7 +1,7 @@
-#!/usr/bin/env sh
+#!/bin/sh
 
 #
-# Copyright 2015 the original author or authors.
+# Copyright © 2015-2021 the original authors.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
@@ -15,69 +15,104 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 #
+# SPDX-License-Identifier: Apache-2.0
+#
 
 ##############################################################################
-##
+#
-##  Gradle start up script for UN*X
+#   Gradle start up script for POSIX generated by Gradle.
-##
+#
+#   Important for running:
+#
+#   (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is
+#       noncompliant, but you have some other compliant shell such as ksh or
+#       bash, then to run this script, type that shell name before the whole
+#       command line, like:
+#
+#           ksh Gradle
+#
+#       Busybox and similar reduced shells will NOT work, because this script
+#       requires all of these POSIX shell features:
+#         * functions;
+#         * expansions «$var», «${var}», «${var:-default}», «${var+SET}»,
+#           «${var#prefix}», «${var%suffix}», and «$( cmd )»;
+#         * compound commands having a testable exit status, especially «case»;
+#         * various built-in commands including «command», «set», and «ulimit».
+#
+#   Important for patching:
+#
+#   (2) This script targets any POSIX shell, so it avoids extensions provided
+#       by Bash, Ksh, etc; in particular arrays are avoided.
+#
+#       The "traditional" practice of packing multiple parameters into a
+#       space-separated string is a well documented source of bugs and security
+#       problems, so this is (mostly) avoided, by progressively accumulating
+#       options in "$@", and eventually passing that to Java.
+#
+#       Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS,
+#       and GRADLE_OPTS) rely on word-splitting, this is performed explicitly;
+#       see the in-line comments for details.
+#
+#       There are tweaks for specific operating systems such as AIX, CygWin,
+#       Darwin, MinGW, and NonStop.
+#
+#   (3) This script is generated from the Groovy template
+#       https://github.com/gradle/gradle/blob/HEAD/platforms/jvm/plugins-application/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt
+#       within the Gradle project.
+#
+#       You can find Gradle at https://github.com/gradle/gradle/.
+#
 ##############################################################################
 
 # Attempt to set APP_HOME
+
 # Resolve links: $0 may be a link
-PRG="$0"
+app_path=$0
-# Need this for relative symlinks.
+
-while [ -h "$PRG" ] ; do
+# Need this for daisy-chained symlinks.
-    ls=`ls -ld "$PRG"`
+while
-    link=`expr "$ls" : '.*-> \(.*\)$'`
+    APP_HOME=${app_path%"${app_path##*/}"}  # leaves a trailing /; empty if no leading path
-    if expr "$link" : '/.*' > /dev/null; then
+    [ -h "$app_path" ]
-        PRG="$link"
+do
-    else
+    ls=$( ls -ld "$app_path" )
-        PRG=`dirname "$PRG"`"/$link"
+    link=${ls#*' -> '}
-    fi
+    case $link in             #(
+      /*)   app_path=$link ;; #(
+      *)    app_path=$APP_HOME$link ;;
+    esac
 done
-SAVED="`pwd`"
-cd "`dirname \"$PRG\"`/" >/dev/null
-APP_HOME="`pwd -P`"
-cd "$SAVED" >/dev/null
 
-APP_NAME="Gradle"
+# This is normally unused
-APP_BASE_NAME=`basename "$0"`
+# shellcheck disable=SC2034
-
+APP_BASE_NAME=${0##*/}
-# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036)
-DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"'
+APP_HOME=$( cd -P "${APP_HOME:-./}" > /dev/null && printf '%s
+' "$PWD" ) || exit
 
 # Use the maximum available, or set MAX_FD != -1 to use that value.
-MAX_FD="maximum"
+MAX_FD=maximum
 
 warn () {
     echo "$*"
-}
+} >&2
 
 die () {
     echo
     echo "$*"
     echo
     exit 1
-}
+} >&2
 
 # OS specific support (must be 'true' or 'false').
 cygwin=false
 msys=false
 darwin=false
 nonstop=false
-case "`uname`" in
+case "$( uname )" in                #(
-  CYGWIN* )
+  CYGWIN* )         cygwin=true  ;; #(
-    cygwin=true
+  Darwin* )         darwin=true  ;; #(
-    ;;
+  MSYS* | MINGW* )  msys=true    ;; #(
-  Darwin* )
+  NONSTOP* )        nonstop=true ;;
-    darwin=true
-    ;;
-  MINGW* )
-    msys=true
-    ;;
-  NONSTOP* )
-    nonstop=true
-    ;;
 esac
 
 CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar
@@ -87,9 +122,9 @@ CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar
 if [ -n "$JAVA_HOME" ] ; then
     if [ -x "$JAVA_HOME/jre/sh/java" ] ; then
         # IBM's JDK on AIX uses strange locations for the executables
-        JAVACMD="$JAVA_HOME/jre/sh/java"
+        JAVACMD=$JAVA_HOME/jre/sh/java
     else
-        JAVACMD="$JAVA_HOME/bin/java"
+        JAVACMD=$JAVA_HOME/bin/java
     fi
     if [ ! -x "$JAVACMD" ] ; then
         die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME
@@ -98,88 +133,120 @@ Please set the JAVA_HOME variable in your environment to match the
 location of your Java installation."
     fi
 else
-    JAVACMD="java"
+    JAVACMD=java
-    which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+    if ! command -v java >/dev/null 2>&1
+    then
+        die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
 
 Please set the JAVA_HOME variable in your environment to match the
 location of your Java installation."
     fi
+fi
 
 # Increase the maximum file descriptors if we can.
-if [ "$cygwin" = "false" -a "$darwin" = "false" -a "$nonstop" = "false" ] ; then
+if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then
-    MAX_FD_LIMIT=`ulimit -H -n`
+    case $MAX_FD in #(
-    if [ $? -eq 0 ] ; then
+      max*)
-        if [ "$MAX_FD" = "maximum" -o "$MAX_FD" = "max" ] ; then
+        # In POSIX sh, ulimit -H is undefined. That's why the result is checked to see if it worked.
-            MAX_FD="$MAX_FD_LIMIT"
+        # shellcheck disable=SC2039,SC3045
-        fi
+        MAX_FD=$( ulimit -H -n ) ||
-        ulimit -n $MAX_FD
+            warn "Could not query maximum file descriptor limit"
-        if [ $? -ne 0 ] ; then
+    esac
-            warn "Could not set maximum file descriptor limit: $MAX_FD"
+    case $MAX_FD in  #(
-        fi
+      '' | soft) :;; #(
-    else
+      *)
-        warn "Could not query maximum file descriptor limit: $MAX_FD_LIMIT"
+        # In POSIX sh, ulimit -n is undefined. That's why the result is checked to see if it worked.
-    fi
+        # shellcheck disable=SC2039,SC3045
-fi
+        ulimit -n "$MAX_FD" ||
-
+            warn "Could not set maximum file descriptor limit to $MAX_FD"
-# For Darwin, add options to specify how the application appears in the dock
-if $darwin; then
-    GRADLE_OPTS="$GRADLE_OPTS \"-Xdock:name=$APP_NAME\" \"-Xdock:icon=$APP_HOME/media/gradle.icns\""
-fi
-
-# For Cygwin or MSYS, switch paths to Windows format before running java
-if [ "$cygwin" = "true" -o "$msys" = "true" ] ; then
-    APP_HOME=`cygpath --path --mixed "$APP_HOME"`
-    CLASSPATH=`cygpath --path --mixed "$CLASSPATH"`
-    
-    JAVACMD=`cygpath --unix "$JAVACMD"`
-
-    # We build the pattern for arguments to be converted via cygpath
-    ROOTDIRSRAW=`find -L / -maxdepth 1 -mindepth 1 -type d 2>/dev/null`
-    SEP=""
-    for dir in $ROOTDIRSRAW ; do
-        ROOTDIRS="$ROOTDIRS$SEP$dir"
-        SEP="|"
-    done
-    OURCYGPATTERN="(^($ROOTDIRS))"
-    # Add a user-defined pattern to the cygpath arguments
-    if [ "$GRADLE_CYGPATTERN" != "" ] ; then
-        OURCYGPATTERN="$OURCYGPATTERN|($GRADLE_CYGPATTERN)"
-    fi
-    # Now convert the arguments - kludge to limit ourselves to /bin/sh
-    i=0
-    for arg in "$@" ; do
-        CHECK=`echo "$arg"|egrep -c "$OURCYGPATTERN" -`
-        CHECK2=`echo "$arg"|egrep -c "^-"`                                 ### Determine if an option
-
-        if [ $CHECK -ne 0 ] && [ $CHECK2 -eq 0 ] ; then                    ### Added a condition
-            eval `echo args$i`=`cygpath --path --ignore --mixed "$arg"`
-        else
-            eval `echo args$i`="\"$arg\""
-        fi
-        i=`expr $i + 1`
-    done
-    case $i in
-        0) set -- ;;
-        1) set -- "$args0" ;;
-        2) set -- "$args0" "$args1" ;;
-        3) set -- "$args0" "$args1" "$args2" ;;
-        4) set -- "$args0" "$args1" "$args2" "$args3" ;;
-        5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;;
-        6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;;
-        7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;;
-        8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;;
-        9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;;
     esac
 fi
 
-# Escape application args
+# Collect all arguments for the java command, stacking in reverse order:
-save () {
+#   * args from the command line
-    for i do printf %s\\n "$i" | sed "s/'/'\\\\''/g;1s/^/'/;\$s/\$/' \\\\/" ; done
+#   * the main class name
-    echo " "
+#   * -classpath
-}
+#   * -D...appname settings
-APP_ARGS=`save "$@"`
+#   * --module-path (only if needed)
+#   * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables.
 
-# Collect all arguments for the java command, following the shell quoting and substitution rules
+# For Cygwin or MSYS, switch paths to Windows format before running java
-eval set -- $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS "\"-Dorg.gradle.appname=$APP_BASE_NAME\"" -classpath "\"$CLASSPATH\"" org.gradle.wrapper.GradleWrapperMain "$APP_ARGS"
+if "$cygwin" || "$msys" ; then
+    APP_HOME=$( cygpath --path --mixed "$APP_HOME" )
+    CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" )
+
+    JAVACMD=$( cygpath --unix "$JAVACMD" )
+
+    # Now convert the arguments - kludge to limit ourselves to /bin/sh
+    for arg do
+        if
+            case $arg in                                #(
+              -*)   false ;;                            # don't mess with options #(
+              /?*)  t=${arg#/} t=/${t%%/*}              # looks like a POSIX filepath
+                    [ -e "$t" ] ;;                      #(
+              *)    false ;;
+            esac
+        then
+            arg=$( cygpath --path --ignore --mixed "$arg" )
+        fi
+        # Roll the args list around exactly as many times as the number of
+        # args, so each arg winds up back in the position where it started, but
+        # possibly modified.
+        #
+        # NB: a `for` loop captures its iteration list before it begins, so
+        # changing the positional parameters here affects neither the number of
+        # iterations, nor the values presented in `arg`.
+        shift                   # remove old arg
+        set -- "$@" "$arg"      # push replacement arg
+    done
+fi
+
+
+# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"'
+
+# Collect all arguments for the java command:
+#   * DEFAULT_JVM_OPTS, JAVA_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments,
+#     and any embedded shellness will be escaped.
+#   * For example: A user cannot expect ${Hostname} to be expanded, as it is an environment variable and will be
+#     treated as '${Hostname}' itself on the command line.
+
+set -- \
+        "-Dorg.gradle.appname=$APP_BASE_NAME" \
+        -classpath "$CLASSPATH" \
+        org.gradle.wrapper.GradleWrapperMain \
+        "$@"
+
+# Stop when "xargs" is not available.
+if ! command -v xargs >/dev/null 2>&1
+then
+    die "xargs is not available"
+fi
+
+# Use "xargs" to parse quoted args.
+#
+# With -n1 it outputs one arg per line, with the quotes and backslashes removed.
+#
+# In Bash we could simply go:
+#
+#   readarray ARGS < <( xargs -n1 <<<"$var" ) &&
+#   set -- "${ARGS[@]}" "$@"
+#
+# but POSIX shell has neither arrays nor command substitution, so instead we
+# post-process each arg (as a line of input to sed) to backslash-escape any
+# character that might be a shell metacharacter, then use eval to reverse
+# that process (while maintaining the separation between arguments), and wrap
+# the whole thing up as a single "set" statement.
+#
+# This will of course break if any of these variables contains a newline or
+# an unmatched quote.
+#
+
+eval "set -- $(
+        printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" |
+        xargs -n1 |
+        sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' |
+        tr '\n' ' '
+    )" '"$@"'
 
 exec "$JAVACMD" "$@"

gradlew.bat

@@ -13,6 +13,8 @@
 @rem See the License for the specific language governing permissions and
 @rem limitations under the License.
 @rem
+@rem SPDX-License-Identifier: Apache-2.0
+@rem
 
 @if "%DEBUG%"=="" @echo off
 @rem ##########################################################################
@@ -26,6 +28,7 @@ if "%OS%"=="Windows_NT" setlocal
 
 set DIRNAME=%~dp0
 if "%DIRNAME%"=="" set DIRNAME=.
+@rem This is normally unused
 set APP_BASE_NAME=%~n0
 set APP_HOME=%DIRNAME%
 
@@ -40,13 +43,13 @@ if defined JAVA_HOME goto findJavaFromJavaHome
 
 set JAVA_EXE=java.exe
 %JAVA_EXE% -version >NUL 2>&1
-if "%ERRORLEVEL%" == "0" goto init
+if %ERRORLEVEL% equ 0 goto execute
 
-echo.
+echo. 1>&2
-echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 1>&2
-echo.
+echo. 1>&2
-echo Please set the JAVA_HOME variable in your environment to match the
+echo Please set the JAVA_HOME variable in your environment to match the 1>&2
-echo location of your Java installation.
+echo location of your Java installation. 1>&2
 
 goto fail
 
@@ -54,31 +57,16 @@ goto fail
 set JAVA_HOME=%JAVA_HOME:"=%
 set JAVA_EXE=%JAVA_HOME%/bin/java.exe
 
-if exist "%JAVA_EXE%" goto init
+if exist "%JAVA_EXE%" goto execute
 
-echo.
+echo. 1>&2
-echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%
+echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2
-echo.
+echo. 1>&2
-echo Please set the JAVA_HOME variable in your environment to match the
+echo Please set the JAVA_HOME variable in your environment to match the 1>&2
-echo location of your Java installation.
+echo location of your Java installation. 1>&2
 
 goto fail
 
-:init
-@rem Get command-line arguments, handling Windows variants
-
-if not "%OS%" == "Windows_NT" goto win9xME_args
-
-:win9xME_args
-@rem Slurp the command line arguments.
-set CMD_LINE_ARGS=
-set _SKIP=2
-
-:win9xME_args_slurp
-if "x%~1" == "x" goto execute
-
-set CMD_LINE_ARGS=%*
-
 :execute
 @rem Setup the command line
 
@@ -86,17 +74,19 @@ set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
 
 
 @rem Execute Gradle
-"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS%
+"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
 
 :end
 @rem End local scope for the variables with windows NT shell
-if "%ERRORLEVEL%"=="0" goto mainEnd
+if %ERRORLEVEL% equ 0 goto mainEnd
 
 :fail
 rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
 rem the _cmd.exe /c_ return code!
-if  not "" == "%GRADLE_EXIT_CONSOLE%" exit 1
+set EXIT_CODE=%ERRORLEVEL%
-exit /b 1
+if %EXIT_CODE% equ 0 set EXIT_CODE=1
+if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE%
+exit /b %EXIT_CODE%
 
 :mainEnd
 if "%OS%"=="Windows_NT" endlocal

protobuf/README.md

@@ -0,0 +1,11 @@
+# Protobuf
+
+This module contains the protobuf definitions used by PiCar.
+
+## Build
+
+Run the gradle task
+
+`./gradlew generateProto`
+
+It's not recommended to run this alone, unless you plan on manually copying the generated files. The build.gradle files in the other projects will automatically copy the correct generated code across, which is recommended to use instead.

protobuf/build.gradle

@@ -20,22 +20,14 @@ configurations {
 }
 
 protobuf {
-    protoc { artifact = 'com.google.protobuf:protoc:3.11.0' }
+    protoc { artifact = 'com.google.protobuf:protoc:4.29.3' }
     plugins {
         grpc {
-            artifact = 'io.grpc:protoc-gen-grpc-java:1.28.1' // CURRENT_GRPC_VERSION
+            artifact = 'io.grpc:protoc-gen-grpc-java:1.57.0' // CURRENT_GRPC_VERSION
-        }
-        grpc_python {
-            path = "$projectDir/grpc_plugins/grpc_python_plugin_1.28.1-${osdetector.classifier}"
-        }
-        if(osdetector.os != 'windows'){
-            swift {
-                path = "$projectDir/grpc_plugins/protoc-gen-swift-$osdetector.classifier"
-            }
-            grpc_swift {
-                path = "$projectDir/grpc_plugins/protoc-gen-grpc-swift-$osdetector.classifier"
-            }
         }
+//        grpc_python {
+//            path = "$projectDir/grpc_plugins/grpc_python_plugin_1.28.1-${osdetector.classifier}"
+//        }
     }
     generateProtoTasks {
         all().each { task ->
@@ -48,15 +40,6 @@ protobuf {
                 grpc { // Options added to --grpc_out
                     option 'lite'
                 }
-                grpc_python {
-                    outputSubDir = 'python'
-                }
-                if(osdetector.os != 'windows'){
-                    swift{}
-                    grpc_swift {
-                        outputSubDir = 'swift'
-                    }
-                }
             }
         }
     }
@@ -76,8 +59,8 @@ artifacts {
 }
 
 dependencies {
-    implementation 'io.grpc:grpc-okhttp:1.28.1' // CURRENT_GRPC_VERSION
+    implementation 'io.grpc:grpc-okhttp:1.29.0' // CURRENT_GRPC_VERSION
-    implementation 'io.grpc:grpc-protobuf-lite:1.28.1' // CURRENT_GRPC_VERSION
+    implementation 'io.grpc:grpc-protobuf-lite:1.57.0' // CURRENT_GRPC_VERSION
-    implementation 'io.grpc:grpc-stub:1.28.1' // CURRENT_GRPC_VERSION
+    implementation 'io.grpc:grpc-stub:1.57.0' // CURRENT_GRPC_VERSION
-    implementation 'javax.annotation:javax.annotation-api:1.2'
+    implementation 'javax.annotation:javax.annotation-api:1.3.2'
 }

protobuf/src/main/proto/car/control/motorService.proto

@@ -7,8 +7,6 @@ option java_multiple_files = true;
 option java_package = "org.vato.carcontroller";
 option java_outer_classname = "MotorServiceProto";
 
-import "google/protobuf/empty.proto";
-
 message ThrottleRequest{
     float throttle = 1;
 }
@@ -38,10 +36,22 @@ message SaveRequest{
     string file = 1;
 }
 
+message Vehicle2DResponse {
+
+}
+
+message RecordingResponse {
+
+}
+
+message SaveResponse {
+
+}
+
 service CarControl{
     rpc set_throttle(ThrottleRequest) returns (ThrottleResponse){}
     rpc set_steering(SteeringRequest) returns (SteeringResponse){}
-    rpc stream_vehicle_2d(stream Vehicle2DRequest) returns (google.protobuf.Empty){}
+    rpc stream_vehicle_2d(stream Vehicle2DRequest) returns (Vehicle2DResponse){}
-    rpc record(RecordingReqeust) returns (google.protobuf.Empty) {}
+    rpc record(RecordingReqeust) returns (RecordingResponse) {}
-    rpc save_recorded_data(SaveRequest) returns (google.protobuf.Empty) {}
+    rpc save_recorded_data(SaveRequest) returns (SaveResponse) {}
 }

protobuf/src/main/proto/car/slam/SlamController.proto

@@ -1,11 +1,11 @@
 syntax = "proto3";
 
+package SlamControl;
+
 option java_multiple_files = true;
 option java_package = "org.vato.carcontroller";
 option java_outer_classname = "SlamControllerProto";
 
-import "google/protobuf/empty.proto";
-
 message SlamDetails {
     int32 map_size_pixels = 1;
     int32 map_size_meters = 2;
@@ -26,10 +26,22 @@ message SlamScan{
     SlamLocation location = 2;
 }
 
+message StartMapStreamingResponse {
+
+}
+
+message StopStreamingRequest {
+
+}
+
+message StopStreamingResponse {
+
+}
+
 service SlamControl {
-    rpc start_map_streaming(SlamDetails) returns (google.protobuf.Empty) {}
+    rpc start_map_streaming(SlamDetails) returns (StartMapStreamingResponse) {}
 
     rpc map_stream(SlamDetails) returns (stream SlamScan) {}
 
-    rpc stop_streaming(google.protobuf.Empty) returns (google.protobuf.Empty) {}
+    rpc stop_streaming(StopStreamingRequest) returns (StopStreamingResponse) {}
 }

pycar/Dockerfile

@@ -16,6 +16,13 @@ RUN apt-get install -y \
     libpq-dev \
     && rm -rf /var/lib/apt/lists/*
 
+# PIGPIO - used to control servos.
+RUN wget https://github.com/joan2937/pigpio/archive/master.zip \
+ && unzip master.zip \
+ && cd pigpio-master \
+ && make \
+ && make install \
+ && cd .. && rm -rf pigpio-master master.zip
 
 ARG PYPI_USERNAME
 ARG PYPI_PASSWORD

pycar/README.md

@@ -0,0 +1,51 @@
+# PyCar
+
+This module is the original project for RC Car control, and as such contains the most functionality.
+
+Note that only python 3+ has been tested for support (up to and including python 3.8).
+
+## Build
+
+Before building with python or Docker, you should generate protobuf code. If you have Java installed, the easiest way to do this is with the :pycar:copyPythonCode gradle task
+
+`./gradlew :pycar:copyPythonCode`
+
+Build using setup tools.
+The easiest way to build is using the gradle task. This will also generate protobuf code if it hasn't already been generated.
+
+`./gradlew :pycar:build`
+
+Otherwise you can run:
+
+`python setup.py bdist_wheel`
+
+### Docker
+
+A docker image has been created to ease deployment on the raspberry pi.
+
+The docker image is always built on a tag in GitLab CI.
+
+Otherwise, the image can be built by running
+
+`docker build --build-arg PYPI_USERNAME=$PYPI_USERNAME --build-arg PYPI_PASSWORD=$PYPI_PASSWORD .`
+
+or running the gradle task (which will also generate protobuf code)
+
+`./gradlew :pycar:buildDocker`
+
+## Runtime environment variables
+
+There are two key environment variables that should probably be configured at runtime:
+CAR_VEHICLE
+- This variable can have two values, and is used to control whether 
+- Possible Values:
+    - CAR_MOCK (Default, log changes to stdout) 
+    - CAR_2D 
+        - Use gpiozero and pigpio to control steering/throttle servos)
+
+CAR_LIDAR
+- This variable controls whether an RPLidar will be used, or a file to load LiDAR scans from.
+- Possible Values:
+    - LIDAR_MOCK (Default)
+    - {Serial Port}
+        - You must have an RPLidar connected via this port to access. For most linux systems such as raspberry pi, this will probably be /dev/ttyUSB0

pycar/build.gradle

@@ -20,6 +20,12 @@ task build(type: Exec, dependsOn: copyPythonCode) {
     args 'setup.py', 'bdist_wheel'
 }
 
+task buildDocker(type: Exec, dependsOn: copyPythonCode) {
+    executable 'docker'
+    workingDir projectDir
+    args 'build', '--build-arg', "PYPI_USERNAME=$System.env.PYPI_USERNAME", '--build-arg', "PYPI_PASSWORD=$System.env.PYPI_PASSWORD", '-t', 'vato.ddns.net:8082/pycar:latest', '.' 
+}
+
 task clean {
     doLast {
         delete 'dist'

pycar/requirements.txt

@@ -6,7 +6,8 @@ paho-mqtt
 u-msgpack-python
 grpcio-tools
 rplidar
-breezyslam
+# breezyslam
 pyzmq
 gpiozero
 pigpio
+esptool

pycar/src/car/control/gpio/abstract_vehicle.py

@@ -0,0 +1,26 @@
+from abc import ABC, abstractmethod, abstractproperty
+
+class AbstractVehicle(ABC):
+    
+    @abstractmethod
+    @property
+    def throttle(self):
+        pass
+
+    @abstractmethod
+    @throttle.setter
+    def throttle(self):
+        pass
+
+    @abstractmethod
+    @property
+    def steering(self):
+        pass
+
+    @abstractmethod
+    @steering.setter
+    def throttle(self):
+        pass
+
+    def stop(self):
+        pass

pycar/src/car/control/gpio/factory.py

@@ -1,18 +1,25 @@
+from car.control.gpio.abstract_vehicle import AbstractVehicle
+from car.control.gpio.serial_vehicle import SerialVehicle
 from .mockvehicle import MockVehicle
 import os
 
 
-def get_vehicle(motor_pin=19, steering_pin=18):
+# TODO: Remove need for motor/steering pin, instead retrieve from env variable.
+# TODO: Dependency injectino in python?
+def get_vehicle(motor_pin=19, steering_pin=18) -> AbstractVehicle:
     ENV_CAR = None if 'CAR_VEHICLE' not in os.environ else os.environ['CAR_VEHICLE']
-    if ENV_CAR == "CAR_2D":
+    if ENV_CAR == "VEHICLE_2D":
         try:
             from .vehicle import Vehicle
             return Vehicle(motor_pin, steering_pin)
         except ImportError:
             print(
                 'Could not import CAR_2D vehicle. Have you installed the GPIOZERO package?')
-    elif ENV_CAR == "CAR_MOCK":
+    elif ENV_CAR == "VEHICLE_MOCK":
-        return MockVehicle(motor_pin, steering_pin)
+        return MockVehicle()
+    elif ENV_CAR == "VEHICLE_SERIAL":
+        # TODO: Pins in environment variables.
+        return SerialVehicle()
     else:
         print('No valid vehicle found. Have you set the CAR_VEHICLE environment variable?')
         return None

pycar/src/car/control/gpio/mockvehicle.py

@@ -1,10 +1,11 @@
 
 
-# A dummy vehicle class to use when 
+# A dummy vehicle class to use when testing/not connected to a real device.
-class MockVehicle:
+from car.control.gpio.abstract_vehicle import AbstractVehicle
-    def __init__(self, motor_pin=19, servo_pin=18):
+
-        self.motor_pin = motor_pin
+
-        self.steering_pin = servo_pin
+class MockVehicle(AbstractVehicle):
+    def __init__(self):
         print('Using Mock Vehicle')
 
     @property
@@ -23,21 +24,5 @@ class MockVehicle:
     def steering(self, value):
         self._steering = value
 
-    @property
-    def motor_pin(self):
-        return self._motor_pin
-
-    @motor_pin.setter
-    def motor_pin(self, value):
-        self._motor_pin = value
-
-    @property
-    def steering_pin(self):
-        return self._steering_pin
-
-    @steering_pin.setter
-    def steering_pin(self, value):
-        self._steering_pin = value
-
     def stop(self):
         self.throttle = 0

pycar/src/car/control/gpio/recording_vehicle_decorator.py

@@ -1,11 +1,12 @@
 import datetime
+from .abstract_vehicle import AbstractVehicle
 
 
-class VehicleRecordingDecorator:
+class VehicleRecordingDecorator(AbstractVehicle):
     def __init__(self, vehicle):
         """
         A decorator for a vehicle object to record the changes in steering/throttle.
-        This will be recorded to memory, and will save to the given file when save is called.
+        This will be recorded to memory, and will save to the given file when save_data is called.
 
         Parameters
         ----------
@@ -66,21 +67,5 @@ class VehicleRecordingDecorator:
                 's,' + str(value) + ',' + datetime.datetime.now().isoformat(sep=' ', timespec='seconds'))
         self._vehicle.steering = value
 
-    @property
-    def motor_pin(self):
-        return self._vehicle.motor_pin
-
-    @motor_pin.setter
-    def motor_pin(self, value):
-        self._vehicle.motor_pin = value
-
-    @property
-    def steering_pin(self):
-        return self._vehicle.steering_pin
-
-    @steering_pin.setter
-    def steering_pin(self, value):
-        self._vehicle.steering_pin = value
-
     def stop(self):
         self.throttle = 0

pycar/src/car/control/gpio/serial_vehicle.py

@@ -0,0 +1,42 @@
+from .abstract_vehicle import AbstractVehicle
+from serial import Serial
+
+STEERING_CHANNEL = 1
+THROTTLE_CHANNEL = 2
+
+
+class SerialVehicle(AbstractVehicle):
+
+    def __init__(self, serial_port='/dev/ttyUSB0', steering_pin=12, throttle_pin=14):
+        self.serial_port = Serial(port=serial_port, baudrate=115200)
+
+        # Initialise the channels and pins on esp32.
+        self._init_esp32_pwm(steering_pin, throttle_pin)
+        self.throttle = 0
+        self.steering = 0
+
+    @property
+    def throttle(self) -> float:
+        return self.throttle
+
+    @throttle.setter
+    def throttle(self, new_throttle: float):
+        self.throttle = new_throttle
+        self._set_servo_value(THROTTLE_CHANNEL, new_throttle)
+
+    @property
+    def steering(self) -> float:
+        return self.steering
+
+    @steering.setter
+    def steering(self, new_steering: float):
+        self.steering = new_steering
+        self._set_servo_value(STEERING_CHANNEL, new_steering)
+
+    def _set_servo_value(self, channel, value):
+        # Scale the value to a byte, as 0-255 is the angle range for the esp32 servo.
+        self.serial_port.write(bytes[channel, (value + 1) / 2 * 255])
+
+    def _init_esp32_pwm(self, steering_pin, throttle_pin):
+        self.serial_port.write(bytes([0, 2, STEERING_CHANNEL,
+                                      steering_pin, THROTTLE_CHANNEL, throttle_pin]))

pycar/src/car/control/gpio/vehicle.py

@@ -1,3 +1,4 @@
+from .abstract_vehicle import AbstractVehicle
 from gpiozero import Servo, Device
 from gpiozero.pins.pigpio import PiGPIOFactory
 import subprocess
@@ -29,9 +30,9 @@ def _is_pin_valid(pin):
 # two servos for controls (e.g. drone, dog)
 
 
-class Vehicle:
+class Vehicle(AbstractVehicle):
     def __init__(self, motor_pin=19, servo_pin=18):
-        subprocess.call(['sudo', 'pigpiod'])
+        subprocess.call(['pigpiod'])
         Device.pin_factory = PiGPIOFactory()
         print('Using pin factory:')
         print(Device.pin_factory)

pycar/src/car/control/pid.py

@@ -0,0 +1,48 @@
+"""
+PID Controller implementation.
+"""
+
+import time
+
+
+class PIDController:
+    def __init__(self, set_point=0, kp=3, ki=1, kd=1):
+        """
+        Simple implementation of PID control. All calculations are serial.
+        Updates will only occur when you call the compute_timestep function, so
+        call it as often as is needed when using the control (ideally a set timestep).
+        You must ensure you set the set_point before calculations, otherwise you will see
+        that nothing will happen.
+        """
+        self._set_point = set_point
+        self._kp = kp
+        self._ki = ki
+        self._kd = kd
+        self._reset_errors()
+
+    @properrty
+    def set_point(self) -> float:
+        return self._set_point
+
+    @set_point.setter
+    def set_point(self, new_point: float):
+        self._set_point = new_point
+        self._reset_errors()
+
+    def _reset_errors(self):
+        self._previous_error = 0
+        self._sum_errors = 0
+        self._start_timestep = 0
+
+    def compute_timestep(self, process_value: float):
+        """
+        Compute the manipulated value at the current timestep, given the current Process Value.
+        This is very simple, only considering the previous timestep for the derivative,
+        and not performing any smoothing on the integral
+        """
+        error = self._set_point - process_value
+        dt = time.time - self._start_timestep
+        self._sum_errors += error * dt
+        derivative = (error - self._previous_error) / dt
+        self._previous_error = error
+        return self._kp * error + self._ki * self._sum_errors + self._kd * derivative