Merge branch 'TrackingTesting' into 'master'

Interim Tracking testing

See merge request vato007/picar!4

.vscode/launch.json

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

SwiftyCar/Package.swift

@@ -13,13 +13,15 @@ let package = Package(
         // .package(url: /* package url */, from: "1.0.0"),
         .package(url: "https://github.com/grpc/grpc-swift.git", from: "1.0.0-alpha.12"),
         .package(url: "https://github.com/uraimo/SwiftyGPIO.git", from: "1.0.0"),
+        .package(url: "https://vato.ddns.net/gitlab/vato007/swiftrplidar.git", .branch("master")),
+        .package(url: "https://vato.ddns.net/gitlab/vato007/SwiftSerial.git", .branch("dtr_support"))
     ],
     targets: [
         // Targets are the basic building blocks of a package. A target can define a module or a test suite.
         // Targets can depend on other targets in this package, and on products in packages which this package depends on.
         .target(
             name: "SwiftyCar",
-            dependencies: ["SwiftyGPIO", .product(name: "GRPC", package: "grpc-swift")]),
+            dependencies: ["SwiftyGPIO", .product(name: "GRPC", package: "grpc-swift"), "SwiftRPLidar"]),
         .testTarget(
             name: "SwiftyCarTests",
             dependencies: ["SwiftyCar"]),

SwiftyCar/Sources/SwiftyCar/LidarProvider.swift

@@ -0,0 +1,61 @@
+//
+//  LidarProvider.swift
+//  
+//
+//  Created by Michael Pivato on 10/7/20.
+//
+
+import Foundation
+import GRPC
+import NIO
+import SwiftProtobuf
+import SwiftRPLidar
+
+class LidarProvider: Persontracking_PersonTrackingProvider {
+    
+    private let lidar: SwiftRPLidar
+    private var shouldScan: Bool = false
+    
+    init(lidar: SwiftRPLidar) {
+        self.lidar = lidar
+    }
+    
+    func set_tracking_group(request: Persontracking_Int32Value, context: StatusOnlyCallContext) -> EventLoopFuture<Google_Protobuf_Empty> {
+        return context.eventLoop.makeSucceededFuture(Google_Protobuf_Empty())
+    }
+    
+    func stop_tracking(request: Google_Protobuf_Empty, context: StatusOnlyCallContext) -> EventLoopFuture<Google_Protobuf_Empty> {
+        shouldScan = false
+        return context.eventLoop.makeSucceededFuture(Google_Protobuf_Empty())
+    }
+    
+    func start_tracking(request: Google_Protobuf_Empty, context: StatusOnlyCallContext) -> EventLoopFuture<Google_Protobuf_Empty> {
+        return context.eventLoop.makeSucceededFuture(Google_Protobuf_Empty())
+    }
+    
+    func record(request: Google_Protobuf_BoolValue, context: StatusOnlyCallContext) -> EventLoopFuture<Google_Protobuf_Empty> {
+        return context.eventLoop.makeSucceededFuture(Google_Protobuf_Empty())
+    }
+    
+    func save_lidar(request: MotorControl_SaveRequest, context: StatusOnlyCallContext) -> EventLoopFuture<Google_Protobuf_Empty> {
+        return context.eventLoop.makeSucceededFuture(Google_Protobuf_Empty())
+    }
+    
+    func lidar_stream(request: Persontracking_StreamMessage, context: StreamingResponseCallContext<Persontracking_PointScan>) -> EventLoopFuture<GRPCStatus> {
+        shouldScan = true
+        try! lidar.iterScans{scan in
+            _ = context.sendResponse(.with{protoScan in
+                protoScan.points = scan.map{ point in
+                    Persontracking_Point.with{ protoPoint in
+                        protoPoint.angle = Double(point.angle)
+                        protoPoint.distance = Double(point.distance)
+                        // Placeholder group number.
+                        protoPoint.groupNumber = 0
+                    }
+                }
+            })
+            return shouldScan
+        }
+        return context.eventLoop.makeSucceededFuture(.ok)
+    }
+}

SwiftyCar/Sources/SwiftyCar/MotorProvider.swift

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

SwiftyCar/Sources/SwiftyCar/main.swift

@@ -7,6 +7,8 @@
 
 import NIO
 import GRPC
+import SwiftRPLidar
+import SwiftSerial
 
 func doServer() throws {
     // Copied from examples
@@ -16,13 +18,18 @@ func doServer() throws {
       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])
+      .withServiceProviders([provider, trackingProvider])
       .bind(host: "localhost", port: 0)
 
     server.map {
@@ -37,11 +44,27 @@ func doServer() throws {
     }.wait()
 }
 
+func createLidar() -> SwiftRPLidar{
+    return try! SwiftRPLidar(onPort: SerialPort(path: "/dev/cu.usbserial0001"))
+    
+}
+
 // Entry-Point to the Swift Car Controller
 print("Starting Server")
 do{
-try doServer()
+    try doServer()
 }
 catch{
     print("Server failed")
 }
+
+extension SerialPort: LidarSerial{
+    public func setBaudrate(baudrate: Int) {
+        // TODO: handle different baudrates. Only need this for now.
+        switch baudrate{
+        default:
+            setSettings(receiveRate: .baud115200, transmitRate: .baud115200, minimumBytesToRead: 1)
+        }
+        
+    }
+}

app/build.gradle

@@ -36,9 +36,9 @@ dependencies {
     testImplementation 'junit:junit:4.12'
     androidTestImplementation 'androidx.test:runner:1.2.0'
     androidTestImplementation 'androidx.test.espresso:espresso-core:3.2.0'
-    implementation 'io.grpc:grpc-okhttp:1.28.1' // CURRENT_GRPC_VERSION
-    implementation 'io.grpc:grpc-protobuf-lite:1.28.1' // CURRENT_GRPC_VERSION
-    implementation 'io.grpc:grpc-stub:1.28.1' // CURRENT_GRPC_VERSION
+    implementation 'io.grpc:grpc-okhttp:1.29.0' // CURRENT_GRPC_VERSION
+    implementation 'io.grpc:grpc-protobuf-lite:1.29.0' // CURRENT_GRPC_VERSION
+    implementation 'io.grpc:grpc-stub:1.29.0' // CURRENT_GRPC_VERSION
     implementation 'javax.annotation:javax.annotation-api:1.2'
     implementation 'org.zeromq:jeromq:0.5.2'
 }

app/src/main/java/org/vato/carcontroller/LIDAR/LidarView.java

@@ -2,11 +2,11 @@ package org.vato.carcontroller.LIDAR;
 
 import android.content.Context;
 import android.content.SharedPreferences;
-import android.graphics.Bitmap;
 import android.graphics.Canvas;
 import android.graphics.Color;
 import android.graphics.Paint;
 import android.util.AttributeSet;
+import android.util.Log;
 import android.view.MotionEvent;
 import android.view.SurfaceHolder;
 import android.view.SurfaceView;
@@ -17,9 +17,14 @@ import com.google.protobuf.Empty;
 
 import org.vato.carcontroller.PersonTrackingGrpc;
 import org.vato.carcontroller.PointScan;
+import org.vato.carcontroller.StreamMessage;
 import org.vato.carcontroller.Updaters.AbstractUpdater;
+import org.vato.carcontroller.Updaters.GrpcUpdater;
 import org.vato.carcontroller.Updaters.ZmqUpdater;
 
+import java.util.HashMap;
+import java.util.Map;
+import java.util.Objects;
 import java.util.stream.Collectors;
 
 import io.grpc.ManagedChannel;
@@ -27,7 +32,8 @@ import io.grpc.ManagedChannelBuilder;
 import io.grpc.stub.StreamObserver;
 
 public class LidarView extends SurfaceView
-        implements AbstractUpdater.MapChangedListener<PointScan> {
+        implements AbstractUpdater.MapChangedListener<PointScan>,
+                   GrpcUpdater.GrpcUpdateBootstrapper<PointScan> {
 
     private static final String LIDAR_TOPIC = "lidar_map";
 
@@ -35,10 +41,12 @@ public class LidarView extends SurfaceView
     private Thread lidarThread;
     private String port;
     private SurfaceHolder surfaceHolder;
+    private boolean useGrpcStreams;
     PersonTrackingGrpc.PersonTrackingStub stub;
+    private float timeBetweenMessages;
+    private Map<Integer, Paint> groupNumPaints = new HashMap<>();
 
-    private int mBitmapX, mBitmapY, mViewWidth, mViewHeight;
-    private Bitmap mBitmap;
+    private int mViewWidth, mViewHeight, centreX, centreY;
 
     public LidarView(Context context) {
         super(context);
@@ -60,8 +68,15 @@ public class LidarView extends SurfaceView
         String host = prefs.getString("host", "10.0.0.53");
         port = prefs.getString("zmqPort", "5050");
         String gRPCPort = prefs.getString("port", "50051");
+        useGrpcStreams = prefs.getBoolean("use_grpc_streams", false);
+        timeBetweenMessages = prefs.getFloat("lidar_timeout", 0.1f);
+
+        if (useGrpcStreams) {
+            lidar = new GrpcUpdater<>(PointScan.getDefaultInstance().getParserForType(), this);
+        } else {
             lidar = new ZmqUpdater<>(PointScan.getDefaultInstance().getParserForType(), LIDAR_TOPIC,
                                      host, port);
+        }
         lidar.addMapChangedListener(this);
         surfaceHolder = getHolder();
         lidarThread = new Thread(lidar);
@@ -74,7 +89,16 @@ public class LidarView extends SurfaceView
      * Called by MainActivity.onResume() to start a thread.
      */
     public void resume() {
-        StreamObserver<Empty> response = new StreamObserver<Empty>() {
+        if (useGrpcStreams) {
+            lidarThread.start();
+        } else {
+            doZmqLidarStream();
+        }
+    }
+
+    private void doZmqLidarStream() {
+        // use async grpc method, ZMQ doesn't need to connect straight away.
+        stub.startTracking(Empty.newBuilder().build(), new StreamObserver<Empty>() {
             @Override
             public void onNext(Empty value) {
                 lidarThread.start();
@@ -90,14 +114,16 @@ public class LidarView extends SurfaceView
             public void onCompleted() {
                 // Don't care.
             }
-        };
-        // use async grpc method, ZMQ doesn't need to connect straight away.
-        stub.startTracking(Empty.newBuilder().build(), response);
+        });
     }
 
     @Override
     protected void onSizeChanged(int w, int h, int oldw, int oldh) {
         super.onSizeChanged(w, h, oldw, oldh);
+        mViewWidth = w;
+        mViewHeight = h;
+        centreX = w / 2;
+        centreY = h / 2;
     }
 
     @Override
@@ -115,11 +141,26 @@ public class LidarView extends SurfaceView
 
 
     public void stop() {
-        // TODO: Use grpc to tell zmq to stop.
         lidar.stop();
+        StreamObserver<Empty> responseObserver = new StreamObserver<Empty>() {
+            @Override
+            public void onNext(Empty value) {
+            }
+
+            @Override
+            public void onError(Throwable t) {
+                Log.d("LIDAR", "Failed to stop SLAM", t);
+            }
+
+            @Override
+            public void onCompleted() {
+            }
+        };
+        stub.stopTracking(Empty.newBuilder().build(), responseObserver);
         try {
             lidarThread.join(1000);
         } catch (InterruptedException e) {
+            Log.d("LIDAR", "Lidar failed to join", e);
         }
     }
 
@@ -129,21 +170,62 @@ public class LidarView extends SurfaceView
             Canvas canvas = surfaceHolder.lockCanvas();
             canvas.save();
             canvas.drawColor(Color.WHITE);
-            for (Point point : points.getPointsList().stream().map(Point::fromProtoPoint).collect(
+            // TODO: Do an initial pass to find the max distance, which will be a scale factor for the other points.
+            double maxDistance = 0;
+            for (org.vato.carcontroller.Point point : points.getPointsList()) {
+                if (point.getDistance() > maxDistance) {
+                    maxDistance = point.getDistance();
+                }
+            }
+
+            final double maxDistanceFinal = maxDistance;
+
+            // Apply scaling factor from max distance.
+            for (Point point : points.getPointsList().stream().map(
+                    point -> org.vato.carcontroller.Point.newBuilder(point).setDistance(
+                            point.getDistance() / maxDistanceFinal * mViewHeight).build()).map(
+                    Point::fromProtoPoint)
+                                     .collect(
                                              Collectors.toList())) {
                 // Now for each point, draw a circle for the point (so it's big enough) in the correct spot,
                 // and create a colour for that point to paint it correctly.
-                // TODO: Dynamically change the colour of the paint object based on the point group number.
-                canvas.drawCircle((float) point.x, (float) point.y, 5, new Paint());
+                if (!groupNumPaints.containsKey(point.groupNumber)) {
+                    Paint paint = new Paint();
+                    paint.setColor(
+                            Color.HSVToColor(new float[]{convertGroupNumberToHue(
+                                    point.groupNumber), 1f, 1f}));
+                    groupNumPaints.put(point.groupNumber, paint);
+                }
+                canvas.drawCircle((float) point.x + centreX,
+                                  (float) point.y + centreY, 5,
+                                  Objects.requireNonNull(groupNumPaints
+                                                                 .get(point.groupNumber))); // Can't be null as we just added it.
             }
             canvas.restore();
             surfaceHolder.unlockCanvasAndPost(canvas);
         }
     }
 
+
+    /**
+     * @param groupNumber
+     * @return
+     */
+    private static int convertGroupNumberToHue(int groupNumber) {
+        return (43 * groupNumber) % 360;
+    }
+
+    @Override
+    public void bootstrap(StreamObserver<PointScan> responseObserver) {
+        stub.lidarStream(
+                StreamMessage.newBuilder().setTimeBetweenMessages(timeBetweenMessages).build(),
+                responseObserver);
+    }
+
     private static class Point {
         private double x;
         private double y;
+        private int groupNumber;
 
         private Point(double x, double y) {
             this.x = x;
@@ -151,7 +233,9 @@ public class LidarView extends SurfaceView
         }
 
         static Point fromProtoPoint(org.vato.carcontroller.Point point) {
-            return fromHist(point.getDistance(), point.getAngle());
+            Point p = fromHist(point.getDistance(), point.getAngle());
+            p.groupNumber = point.getGroupNumber();
+            return p;
         }
 
         static Point fromHist(double distance, double angle) {
@@ -159,8 +243,8 @@ public class LidarView extends SurfaceView
         }
 
         static Point fromHist(double distance, double angle, Point offset) {
-            return new Point(distance * Math.sin(angle) + offset.x,
-                             distance * Math.cos(angle) + offset.y);
+            return new Point(distance * Math.sin(Math.toRadians(angle)) + offset.x,
+                             distance * Math.cos(Math.toRadians(angle)) + offset.y);
         }
 
     }

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

@@ -7,6 +7,7 @@ import android.graphics.Canvas;
 import android.graphics.Color;
 import android.graphics.Paint;
 import android.util.AttributeSet;
+import android.util.Log;
 import android.view.SurfaceHolder;
 import android.view.SurfaceView;
 
@@ -128,6 +129,20 @@ 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>() {
+            @Override
+            public void onNext(Empty value) {
+            }
+
+            @Override
+            public void onError(Throwable t) {
+                Log.d("SLAM", "Failed to stop SLAM", t);
+            }
+
+            @Override
+            public void onCompleted() {
+            }
+        });
         try {
             mapThread.join(1000);
         } catch (InterruptedException e) {

app/src/main/java/org/vato/carcontroller/Updaters/GrpcUpdater.java

@@ -10,13 +10,14 @@ import io.grpc.stub.StreamObserver;
 public class GrpcUpdater<T extends MessageLite> extends AbstractUpdater<T> {
     GrpcUpdateBootstrapper<T> bootstrapper;
 
-    public GrpcUpdater(Parser parser, GrpcUpdateBootstrapper bootstrapper) {
+    public GrpcUpdater(Parser<T> parser, GrpcUpdateBootstrapper<T> bootstrapper) {
         super(parser);
         this.bootstrapper = bootstrapper;
     }
 
     @Override
     public void stop() {
+        // TODO... may not be needed here.
 
     }
 

app/src/main/res/xml/root_preferences.xml

@@ -46,6 +46,14 @@
 
     </PreferenceCategory>
 
+    <PreferenceCategory>
+        <EditTextPreference
+            android:defaultValue="0.1"
+            android:title="LiDAR time between scan fetches."
+            app:key="lidar_timeout"
+            app:useSimpleSummaryProvider="true" />
+    </PreferenceCategory>
+
     <PreferenceCategory android:title="0MQ SLAM Connection">
         <EditTextPreference
             android:key="zmqPort"

build.gradle

@@ -6,7 +6,7 @@ buildscript {
         jcenter()
     }
     dependencies {
-        classpath 'com.android.tools.build:gradle:3.6.3'
+        classpath 'com.android.tools.build:gradle:4.0.1'
         classpath 'com.google.protobuf:protobuf-gradle-plugin:0.8.10'
 
         // NOTE: Do not place your application dependencies here; they belong

car/MyRaft/raft_pb2.py

@@ -1,296 +0,0 @@
-# Generated by the protocol buffer compiler.  DO NOT EDIT!
-# source: raft.proto
-
-import sys
-_b=sys.version_info[0]<3 and (lambda x:x) or (lambda x:x.encode('latin1'))
-from google.protobuf import descriptor as _descriptor
-from google.protobuf import message as _message
-from google.protobuf import reflection as _reflection
-from google.protobuf import symbol_database as _symbol_database
-# @@protoc_insertion_point(imports)
-
-_sym_db = _symbol_database.Default()
-
-
-
-
-DESCRIPTOR = _descriptor.FileDescriptor(
-  name='raft.proto',
-  package='raft',
-  syntax='proto3',
-  serialized_options=None,
-  serialized_pb=_b('\n\nraft.proto\x12\x04raft\"\x7f\n\rAppendEntries\x12\x0c\n\x04term\x18\x01 \x01(\r\x12\x10\n\x08leaderId\x18\x02 \x01(\t\x12\x14\n\x0cprevLogIndex\x18\x03 \x01(\r\x12\x13\n\x0bprevLogTerm\x18\x04 \x01(\r\x12\x14\n\x0cleaderCommit\x18\x05 \x01(\r\x12\r\n\x05\x65ntry\x18\x06 \x03(\t\"6\n\x15\x41ppendEntriesResponse\x12\x0c\n\x04term\x18\x01 \x01(\r\x12\x0f\n\x07success\x18\x02 \x01(\x08\"[\n\x0bRequestVote\x12\x0c\n\x04term\x18\x01 \x01(\r\x12\x13\n\x0b\x63\x61ndidateId\x18\x02 \x01(\t\x12\x14\n\x0clastLogIndex\x18\x03 \x01(\r\x12\x13\n\x0blastLogTerm\x18\x04 \x01(\r\"I\n\x13RequestVoteResponse\x12\x0c\n\x04term\x18\x01 \x01(\r\x12\x13\n\x0bvoteGranted\x18\x02 \x01(\x08\x12\x0f\n\x07voterId\x18\x03 \x01(\t2\x90\x01\n\x04Raft\x12\x46\n\x10\x41ppendEntriesRPC\x12\x13.raft.AppendEntries\x1a\x1b.raft.AppendEntriesResponse\"\x00\x12@\n\x0eRequestVoteRPC\x12\x11.raft.RequestVote\x1a\x19.raft.RequestVoteResponse\"\x00\x62\x06proto3')
-)
-
-
-
-
-_APPENDENTRIES = _descriptor.Descriptor(
-  name='AppendEntries',
-  full_name='raft.AppendEntries',
-  filename=None,
-  file=DESCRIPTOR,
-  containing_type=None,
-  fields=[
-    _descriptor.FieldDescriptor(
-      name='term', full_name='raft.AppendEntries.term', index=0,
-      number=1, type=13, cpp_type=3, label=1,
-      has_default_value=False, default_value=0,
-      message_type=None, enum_type=None, containing_type=None,
-      is_extension=False, extension_scope=None,
-      serialized_options=None, file=DESCRIPTOR),
-    _descriptor.FieldDescriptor(
-      name='leaderId', full_name='raft.AppendEntries.leaderId', index=1,
-      number=2, type=9, cpp_type=9, label=1,
-      has_default_value=False, default_value=_b("").decode('utf-8'),
-      message_type=None, enum_type=None, containing_type=None,
-      is_extension=False, extension_scope=None,
-      serialized_options=None, file=DESCRIPTOR),
-    _descriptor.FieldDescriptor(
-      name='prevLogIndex', full_name='raft.AppendEntries.prevLogIndex', index=2,
-      number=3, type=13, cpp_type=3, label=1,
-      has_default_value=False, default_value=0,
-      message_type=None, enum_type=None, containing_type=None,
-      is_extension=False, extension_scope=None,
-      serialized_options=None, file=DESCRIPTOR),
-    _descriptor.FieldDescriptor(
-      name='prevLogTerm', full_name='raft.AppendEntries.prevLogTerm', index=3,
-      number=4, type=13, cpp_type=3, label=1,
-      has_default_value=False, default_value=0,
-      message_type=None, enum_type=None, containing_type=None,
-      is_extension=False, extension_scope=None,
-      serialized_options=None, file=DESCRIPTOR),
-    _descriptor.FieldDescriptor(
-      name='leaderCommit', full_name='raft.AppendEntries.leaderCommit', index=4,
-      number=5, type=13, cpp_type=3, label=1,
-      has_default_value=False, default_value=0,
-      message_type=None, enum_type=None, containing_type=None,
-      is_extension=False, extension_scope=None,
-      serialized_options=None, file=DESCRIPTOR),
-    _descriptor.FieldDescriptor(
-      name='entry', full_name='raft.AppendEntries.entry', index=5,
-      number=6, type=9, cpp_type=9, label=3,
-      has_default_value=False, default_value=[],
-      message_type=None, enum_type=None, containing_type=None,
-      is_extension=False, extension_scope=None,
-      serialized_options=None, file=DESCRIPTOR),
-  ],
-  extensions=[
-  ],
-  nested_types=[],
-  enum_types=[
-  ],
-  serialized_options=None,
-  is_extendable=False,
-  syntax='proto3',
-  extension_ranges=[],
-  oneofs=[
-  ],
-  serialized_start=20,
-  serialized_end=147,
-)
-
-
-_APPENDENTRIESRESPONSE = _descriptor.Descriptor(
-  name='AppendEntriesResponse',
-  full_name='raft.AppendEntriesResponse',
-  filename=None,
-  file=DESCRIPTOR,
-  containing_type=None,
-  fields=[
-    _descriptor.FieldDescriptor(
-      name='term', full_name='raft.AppendEntriesResponse.term', index=0,
-      number=1, type=13, cpp_type=3, label=1,
-      has_default_value=False, default_value=0,
-      message_type=None, enum_type=None, containing_type=None,
-      is_extension=False, extension_scope=None,
-      serialized_options=None, file=DESCRIPTOR),
-    _descriptor.FieldDescriptor(
-      name='success', full_name='raft.AppendEntriesResponse.success', index=1,
-      number=2, type=8, cpp_type=7, label=1,
-      has_default_value=False, default_value=False,
-      message_type=None, enum_type=None, containing_type=None,
-      is_extension=False, extension_scope=None,
-      serialized_options=None, file=DESCRIPTOR),
-  ],
-  extensions=[
-  ],
-  nested_types=[],
-  enum_types=[
-  ],
-  serialized_options=None,
-  is_extendable=False,
-  syntax='proto3',
-  extension_ranges=[],
-  oneofs=[
-  ],
-  serialized_start=149,
-  serialized_end=203,
-)
-
-
-_REQUESTVOTE = _descriptor.Descriptor(
-  name='RequestVote',
-  full_name='raft.RequestVote',
-  filename=None,
-  file=DESCRIPTOR,
-  containing_type=None,
-  fields=[
-    _descriptor.FieldDescriptor(
-      name='term', full_name='raft.RequestVote.term', index=0,
-      number=1, type=13, cpp_type=3, label=1,
-      has_default_value=False, default_value=0,
-      message_type=None, enum_type=None, containing_type=None,
-      is_extension=False, extension_scope=None,
-      serialized_options=None, file=DESCRIPTOR),
-    _descriptor.FieldDescriptor(
-      name='candidateId', full_name='raft.RequestVote.candidateId', index=1,
-      number=2, type=9, cpp_type=9, label=1,
-      has_default_value=False, default_value=_b("").decode('utf-8'),
-      message_type=None, enum_type=None, containing_type=None,
-      is_extension=False, extension_scope=None,
-      serialized_options=None, file=DESCRIPTOR),
-    _descriptor.FieldDescriptor(
-      name='lastLogIndex', full_name='raft.RequestVote.lastLogIndex', index=2,
-      number=3, type=13, cpp_type=3, label=1,
-      has_default_value=False, default_value=0,
-      message_type=None, enum_type=None, containing_type=None,
-      is_extension=False, extension_scope=None,
-      serialized_options=None, file=DESCRIPTOR),
-    _descriptor.FieldDescriptor(
-      name='lastLogTerm', full_name='raft.RequestVote.lastLogTerm', index=3,
-      number=4, type=13, cpp_type=3, label=1,
-      has_default_value=False, default_value=0,
-      message_type=None, enum_type=None, containing_type=None,
-      is_extension=False, extension_scope=None,
-      serialized_options=None, file=DESCRIPTOR),
-  ],
-  extensions=[
-  ],
-  nested_types=[],
-  enum_types=[
-  ],
-  serialized_options=None,
-  is_extendable=False,
-  syntax='proto3',
-  extension_ranges=[],
-  oneofs=[
-  ],
-  serialized_start=205,
-  serialized_end=296,
-)
-
-
-_REQUESTVOTERESPONSE = _descriptor.Descriptor(
-  name='RequestVoteResponse',
-  full_name='raft.RequestVoteResponse',
-  filename=None,
-  file=DESCRIPTOR,
-  containing_type=None,
-  fields=[
-    _descriptor.FieldDescriptor(
-      name='term', full_name='raft.RequestVoteResponse.term', index=0,
-      number=1, type=13, cpp_type=3, label=1,
-      has_default_value=False, default_value=0,
-      message_type=None, enum_type=None, containing_type=None,
-      is_extension=False, extension_scope=None,
-      serialized_options=None, file=DESCRIPTOR),
-    _descriptor.FieldDescriptor(
-      name='voteGranted', full_name='raft.RequestVoteResponse.voteGranted', index=1,
-      number=2, type=8, cpp_type=7, label=1,
-      has_default_value=False, default_value=False,
-      message_type=None, enum_type=None, containing_type=None,
-      is_extension=False, extension_scope=None,
-      serialized_options=None, file=DESCRIPTOR),
-    _descriptor.FieldDescriptor(
-      name='voterId', full_name='raft.RequestVoteResponse.voterId', index=2,
-      number=3, type=9, cpp_type=9, label=1,
-      has_default_value=False, default_value=_b("").decode('utf-8'),
-      message_type=None, enum_type=None, containing_type=None,
-      is_extension=False, extension_scope=None,
-      serialized_options=None, file=DESCRIPTOR),
-  ],
-  extensions=[
-  ],
-  nested_types=[],
-  enum_types=[
-  ],
-  serialized_options=None,
-  is_extendable=False,
-  syntax='proto3',
-  extension_ranges=[],
-  oneofs=[
-  ],
-  serialized_start=298,
-  serialized_end=371,
-)
-
-DESCRIPTOR.message_types_by_name['AppendEntries'] = _APPENDENTRIES
-DESCRIPTOR.message_types_by_name['AppendEntriesResponse'] = _APPENDENTRIESRESPONSE
-DESCRIPTOR.message_types_by_name['RequestVote'] = _REQUESTVOTE
-DESCRIPTOR.message_types_by_name['RequestVoteResponse'] = _REQUESTVOTERESPONSE
-_sym_db.RegisterFileDescriptor(DESCRIPTOR)
-
-AppendEntries = _reflection.GeneratedProtocolMessageType('AppendEntries', (_message.Message,), dict(
-  DESCRIPTOR = _APPENDENTRIES,
-  __module__ = 'raft_pb2'
-  # @@protoc_insertion_point(class_scope:raft.AppendEntries)
-  ))
-_sym_db.RegisterMessage(AppendEntries)
-
-AppendEntriesResponse = _reflection.GeneratedProtocolMessageType('AppendEntriesResponse', (_message.Message,), dict(
-  DESCRIPTOR = _APPENDENTRIESRESPONSE,
-  __module__ = 'raft_pb2'
-  # @@protoc_insertion_point(class_scope:raft.AppendEntriesResponse)
-  ))
-_sym_db.RegisterMessage(AppendEntriesResponse)
-
-RequestVote = _reflection.GeneratedProtocolMessageType('RequestVote', (_message.Message,), dict(
-  DESCRIPTOR = _REQUESTVOTE,
-  __module__ = 'raft_pb2'
-  # @@protoc_insertion_point(class_scope:raft.RequestVote)
-  ))
-_sym_db.RegisterMessage(RequestVote)
-
-RequestVoteResponse = _reflection.GeneratedProtocolMessageType('RequestVoteResponse', (_message.Message,), dict(
-  DESCRIPTOR = _REQUESTVOTERESPONSE,
-  __module__ = 'raft_pb2'
-  # @@protoc_insertion_point(class_scope:raft.RequestVoteResponse)
-  ))
-_sym_db.RegisterMessage(RequestVoteResponse)
-
-
-
-_RAFT = _descriptor.ServiceDescriptor(
-  name='Raft',
-  full_name='raft.Raft',
-  file=DESCRIPTOR,
-  index=0,
-  serialized_options=None,
-  serialized_start=374,
-  serialized_end=518,
-  methods=[
-  _descriptor.MethodDescriptor(
-    name='AppendEntriesRPC',
-    full_name='raft.Raft.AppendEntriesRPC',
-    index=0,
-    containing_service=None,
-    input_type=_APPENDENTRIES,
-    output_type=_APPENDENTRIESRESPONSE,
-    serialized_options=None,
-  ),
-  _descriptor.MethodDescriptor(
-    name='RequestVoteRPC',
-    full_name='raft.Raft.RequestVoteRPC',
-    index=1,
-    containing_service=None,
-    input_type=_REQUESTVOTE,
-    output_type=_REQUESTVOTERESPONSE,
-    serialized_options=None,
-  ),
-])
-_sym_db.RegisterServiceDescriptor(_RAFT)
-
-DESCRIPTOR.services_by_name['Raft'] = _RAFT
-
-# @@protoc_insertion_point(module_scope)

car/MyRaft/raft_pb2_grpc.py

@@ -1,63 +0,0 @@
-# Generated by the gRPC Python protocol compiler plugin. DO NOT EDIT!
-import grpc
-
-import MyRaft.raft_pb2 as raft__pb2
-
-
-class RaftStub(object):
-  # missing associated documentation comment in .proto file
-  pass
-
-  def __init__(self, channel):
-    """Constructor.
-
-    Args:
-      channel: A grpc.Channel.
-    """
-    self.AppendEntriesRPC = channel.unary_unary(
-        '/raft.Raft/AppendEntriesRPC',
-        request_serializer=raft__pb2.AppendEntries.SerializeToString,
-        response_deserializer=raft__pb2.AppendEntriesResponse.FromString,
-        )
-    self.RequestVoteRPC = channel.unary_unary(
-        '/raft.Raft/RequestVoteRPC',
-        request_serializer=raft__pb2.RequestVote.SerializeToString,
-        response_deserializer=raft__pb2.RequestVoteResponse.FromString,
-        )
-
-
-class RaftServicer(object):
-  # missing associated documentation comment in .proto file
-  pass
-
-  def AppendEntriesRPC(self, request, context):
-    # missing associated documentation comment in .proto file
-    pass
-    context.set_code(grpc.StatusCode.UNIMPLEMENTED)
-    context.set_details('Method not implemented!')
-    raise NotImplementedError('Method not implemented!')
-
-  def RequestVoteRPC(self, request, context):
-    # missing associated documentation comment in .proto file
-    pass
-    context.set_code(grpc.StatusCode.UNIMPLEMENTED)
-    context.set_details('Method not implemented!')
-    raise NotImplementedError('Method not implemented!')
-
-
-def add_RaftServicer_to_server(servicer, server):
-  rpc_method_handlers = {
-      'AppendEntriesRPC': grpc.unary_unary_rpc_method_handler(
-          servicer.AppendEntriesRPC,
-          request_deserializer=raft__pb2.AppendEntries.FromString,
-          response_serializer=raft__pb2.AppendEntriesResponse.SerializeToString,
-      ),
-      'RequestVoteRPC': grpc.unary_unary_rpc_method_handler(
-          servicer.RequestVoteRPC,
-          request_deserializer=raft__pb2.RequestVote.FromString,
-          response_serializer=raft__pb2.RequestVoteResponse.SerializeToString,
-      ),
-  }
-  generic_handler = grpc.method_handlers_generic_handler(
-      'raft.Raft', rpc_method_handlers)
-  server.add_generic_rpc_handlers((generic_handler,))

car/car.iml

@@ -1,28 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<module external.linked.project.id=":car" external.linked.project.path="$MODULE_DIR$" external.root.project.path="$MODULE_DIR$/.." external.system.id="GRADLE" type="JAVA_MODULE" version="4">
-  <component name="FacetManager">
-    <facet type="android-gradle" name="Android-Gradle">
-      <configuration>
-        <option name="GRADLE_PROJECT_PATH" value=":car" />
-        <option name="LAST_SUCCESSFUL_SYNC_AGP_VERSION" />
-        <option name="LAST_KNOWN_AGP_VERSION" />
-      </configuration>
-    </facet>
-    <facet type="java-gradle" name="Java-Gradle">
-      <configuration>
-        <option name="BUILD_FOLDER_PATH" value="$MODULE_DIR$/build" />
-        <option name="BUILDABLE" value="false" />
-      </configuration>
-    </facet>
-  </component>
-  <component name="NewModuleRootManager" LANGUAGE_LEVEL="JDK_1_8" inherit-compiler-output="true">
-    <exclude-output />
-    <content url="file://$MODULE_DIR$">
-      <excludeFolder url="file://$MODULE_DIR$/.gradle" />
-      <excludeFolder url="file://$MODULE_DIR$/build" />
-    </content>
-    <orderEntry type="inheritedJdk" />
-    <orderEntry type="sourceFolder" forTests="false" />
-    <orderEntry type="library" name="Bundled Protobuf Distribution" level="application" />
-  </component>
-</module>

gradle/wrapper/gradle-wrapper.properties

@@ -1,5 +1,5 @@
 distributionBase=GRADLE_USER_HOME
 distributionPath=wrapper/dists
-distributionUrl=https\://services.gradle.org/distributions/gradle-6.3-bin.zip
+distributionUrl=https\://services.gradle.org/distributions/gradle-6.4.1-bin.zip
 zipStoreBase=GRADLE_USER_HOME
 zipStorePath=wrapper/dists

gradlew

@@ -82,6 +82,7 @@ esac
 
 CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar
 
+
 # Determine the Java command to use to start the JVM.
 if [ -n "$JAVA_HOME" ] ; then
     if [ -x "$JAVA_HOME/jre/sh/java" ] ; then
@@ -129,6 +130,7 @@ fi
 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

gradlew.bat

@@ -84,6 +84,7 @@ set CMD_LINE_ARGS=%*
 
 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%
 

protobuf/src/main/proto/car/tracking/lidar_tracker.proto

@@ -16,7 +16,7 @@ message Int32Value{
 
 message Point{
     double angle = 1;
-    int32 distance = 2;
+    double distance = 2;
     int32 group_number = 3;
 }
 
@@ -24,6 +24,9 @@ message PointScan{
     repeated Point points = 1;
 }
 
+message StreamMessage{
+    float time_between_messages = 1;
+}
 
 service PersonTracking{
     rpc set_tracking_group(Int32Value) returns (google.protobuf.Empty) {}
@@ -36,4 +39,6 @@ service PersonTracking{
 
     rpc save_lidar(MotorControl.SaveRequest) returns (google.protobuf.Empty) {}
 
+    rpc lidar_stream(StreamMessage) returns (stream PointScan) {}
+
 }

pycar/Dockerfile

@@ -1,4 +1,7 @@
-FROM python:3.6-slim
+FROM vato.ddns.net:8083/python:3
+
+ARG PYPI_USERNAME
+ARG PYPI_PASSWORD
 
 RUN apt-get update
 # OpenCV has a LOT of dependencies.
@@ -17,14 +20,14 @@ RUN apt-get install -y \
     && rm -rf /var/lib/apt/lists/*
 
 COPY requirements.txt /
-RUN pip install --trusted-host pypi.python.org -r requirements.txt
+RUN pip install --index-url https://${PYPI_USERNAME}:${PYPI_PASSWORD}@vato.ddns.net/nexus/repository/pypi-grouped/simple -r requirements.txt
 
 WORKDIR /app
 
-COPY . /app
+COPY ./src /app
 
 # We aren't listening, just connecting, so probs won't need this.
 # EXPOSE 1883
 ENV PYTHONPATH=/app
 
-CMD ["python", "DecisionSystem/CentralisedDecision/cameraserver.py", "-V", "/app/HandRecognitionMacbookFixed.mp4"]
+CMD ["python", "-m", "car"]

pycar/requirements.txt

@@ -1,9 +1,10 @@
 numpy
 opencv-python
 six
+wheel
 paho-mqtt
 u-msgpack-python
 grpcio-tools
 rplidar
+breezyslam
 pyzmq
-wheel

pycar/src/car/slam/slam_servicer.py

@@ -1,6 +1,6 @@
 import car.slam.SlamController_pb2_grpc as grpc
 import car.slam.SlamController_pb2 as proto
-import car.empty_pb2 as empty
+import google.protobuf.empty_pb2 as empty
 import car.slam.slam_streamer as slam
 from .slam_processor import SlamProcessor
 

pycar/src/car/tracking/algorithms.py

@@ -1,10 +1,12 @@
 import math
+import numpy as np
+from . import icp
 
 
 class Group:
 
-    def __init__(self, number, points=[]):
-        self._points = points
+    def __init__(self, number):
+        self._points = []
         self._number = number
         self._minX = None
         self._maxX = None
@@ -28,7 +30,7 @@ class Group:
 
     def _update_min_max(self, new_point):
         """
-        Updates the in and max points for this group.
+        Updates the min and max points for this group.
         This is to determine when assigning groups whether the
         same group is selected.
         """
@@ -104,35 +106,46 @@ def calc_groups(scan):
 
     Returns
     -------
-    list
-        List of groups that were found.
+    ndarray
+        Array of groups that were found.
     """
     prevPoint = None
-    currentGroup = None
-    allGroups = []
+    currentGroup = Group(0)
+    allGroups = [currentGroup]
     currentGroupNumber = 0
-
     # assume the list is already sorted.
     for point in scan:
         if prevPoint is None:
             prevPoint = point
+            currentGroup.add_point(point)
             continue
 
         # Distances are in mm.
-        # within 1cm makes a group. Will need to play around with this.
-        if (point[2] - prevPoint[2]) ** 2 < 10 ** 2:
-            if currentGroup is None:
-                currentGroup = Group(currentGroupNumber)
-                allGroups.append(currentGroup)
+        # within 10cm makes a group. Will need to play around with this.
+        if (point[2] - prevPoint[2]) ** 2 < 100 ** 2:
             currentGroup.add_point(point)
         else:
-            if currentGroup is not None:
             currentGroupNumber += 1
-                currentGroup = None
+            currentGroup = Group(currentGroupNumber)
+            currentGroup.add_point(point)
+            allGroups.append(currentGroup)
 
         prevPoint = point
+    return np.array(allGroups)
 
-    return allGroups
+
+def calc_groups_edge_algorithm(scan):
+    """
+    Calculates groups using an edge algorithm. This takes advantage of numpy arrays
+    and vectorisation, rather than the primitive python loop grouping, resulting in
+    faster grouping speeds.
+    """
+    allGroups = []
+    scanArray = np.array(scan)
+
+
+def edge_algorithm():
+    pass
 
 
 def find_centre(group):
@@ -156,13 +169,59 @@ def assign_groups(prev_groups, new_groups):
     """
     Assigns group numbers to a new scan based on the groups of an old scan.
     """
+    max_group_number = 0
+    unassigned_groups = []
     for group in prev_groups:
         old_centre = find_centre(group)
         for new_group in new_groups:
             new_centre = find_centre(new_group)
-            # They are considered the same if the new group and old group centres are within 5cm.
+            # They are considered the same if the new group and old group centres are within 10cm.
             if ((new_centre[0] - old_centre[0]) ** 2 + (new_centre[1] - old_centre[1]) ** 2) < 50 ** 2:
                 new_group.number = group.number
+                if group.number > max_group_number:
+                    max_group_number = group.number
+                continue
+        # If this is reached, then no matching groups were found.
+        unassigned_groups.append(new_group)
+
+    for group in unassigned_groups:
+        max_group_number += 1
+        group.number = max_group_number
+
+    return new_groups
+
+
+def assign_groups_II(prev_groups, new_groups):
+    """
+    Performs the assign groups algorithm, but instead of being greedy to assign, it will match up the
+    closest groups for each group.
+
+    Additionally, the centre of mass for a group of points is now used, which is less prone to the effects of 
+    outliers as the existing find_centre algorithm.
+
+    An ICP rotation/translation is not made in this algorithm, as it's assumed that the scans are quick enough for
+    there to not be a significant difference between scans that would require ICP. 
+    """
+    max_group_number = 0
+    unassigned_groups = []
+
+    def centres_from_groups(groups):
+        return np.array([icp.calc_mass_centre(np.array([convert_lidar_to_cartesian(point) for point in group.get_points()])) for group in groups])
+
+    old_group_centres = centres_from_groups(prev_groups)
+    old_group_indexes = np.arange(len(old_group_centres))
+
+    new_group_centers = centres_from_groups(new_groups)
+    new_group_indexes = np.arange(len(new_group_centers))
+
+    closest_points = icp.closest_points(new_group_centers, old_group_centres)
+    # Now assign the new groups to the closest matching old group, if the distance is within a certain threshold.
+    for i, point in enumerate(closest_points):
+        matching_groups = prev_groups[old_group_centres == point]
+        # TODO: Check the centres are within a certain threshold.
+        new_groups[i].number = prev_groups[0].number
+
+    # TODO: Go through to put all groups into one (if multiple groups get same number) to avoid splits.
 
     return new_groups
 

pycar/src/car/tracking/animate.py

@@ -1,6 +1,6 @@
 #!/usr/bin/env python3
 '''Animates distances and measurment quality'''
-from car.tracking.mock_lidar import MockLidar
+from car.tracking..devices.mock_lidar import MockLidar
 import matplotlib.pyplot as plt
 import numpy as np
 import matplotlib.animation as animation

pycar/src/car/tracking/animate_alg.py

@@ -2,7 +2,7 @@
 Animates distances and angle of lidar
 Uses model-free algorithms to track grouping of points (objects/groups)
 """
-from tracking.mock_lidar import MockLidar
+from car.tracking.devices.mock_lidar import MockLidar
 import matplotlib.pyplot as plt
 import numpy as np
 import matplotlib.animation as animation
@@ -35,7 +35,7 @@ class Bunch:
 
 
 def run():
-    lidar = MockLidar(loader.load_scans_bytes_file("tracking/out.pickle"))
+    lidar = MockLidar(loader.load_scans_bytes_file("pycar/src/car/tracking/out.pickle"))
     fig = plt.figure()
     ax = plt.subplot(111, projection='polar')
     line = ax.scatter([0, 0], [0, 0], s=5, c=[IMIN, IMAX],

pycar/src/car/tracking/devices/factory.py

@@ -3,10 +3,9 @@ from .. import lidar_loader as loader
 import os
 
 MOCK_DEVICE = "LIDAR_MOCK"
-RPLIDAR = "LIDAR_RPLIDAR"
 
 
-def get_lidar(device=None, connection='/dev/ttyUSB0'):
+def get_lidar(device=None):
     actual_device = None
     try:
         actual_device = device if device is not None else os.environ["CAR_LIDAR"]
@@ -14,17 +13,14 @@ def get_lidar(device=None, connection='/dev/ttyUSB0'):
         print(
             'No lidar device specified and the CAR_LIDAR environment variable is not set.')
     if actual_device == MOCK_DEVICE:
-        return MockLidar(loader.load_scans_bytes_file("car/src/car/tracking/out.pickle"))
-    elif actual_device == RPLIDAR:
+        return MockLidar(loader.load_scans_bytes_file("pycar/src/car/tracking/out.pickle"))
+    elif actual_device != '':
         try:
-            # TODO: Cleanup connection setting, probably don't need to pass it into the method.
             from rplidar import RPLidar
-            if "LIDAR_DEVICE" in os.environ:
-                return RPLidar(os.environ['LIDAR_DEVICE'])
-            return RPLidar(connection)
+            return RPLidar(device)
         except ImportError:
             print('Could not import RPLidar. Have you downloaded rplidar?')
     else:
-        print('No valid lidar device found. Please choose one of ' +
-              MOCK_DEVICE + ' or ' + RPLIDAR)
+        print('No valid lidar device found. Please choose ' +
+              MOCK_DEVICE + ' or a dn address for the lidar device.')
     return None