Rework lidar cache to support grpc streaming

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

@@ -18,9 +18,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;
@@ -28,7 +33,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";
 
@@ -36,7 +42,10 @@ public class LidarView extends SurfaceView
     private Thread lidarThread;
     private String port;
     private SurfaceHolder surfaceHolder;
+    private boolean useGrpcStreams;
     PersonTrackingGrpc.PersonTrackingStub stub;
+    private float timeBetweenMessages = 0.01f;
+    private Map<Integer, Paint> groupNumPaints = new HashMap<>();
 
     private int mBitmapX, mBitmapY, mViewWidth, mViewHeight;
     private Bitmap mBitmap;
@@ -61,8 +70,14 @@ 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");
-        lidar = new ZmqUpdater<>(PointScan.getDefaultInstance().getParserForType(), LIDAR_TOPIC,
-                                 host, port);
+        useGrpcStreams = prefs.getBoolean("use_grpc_streams", false);
+
+        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);
@@ -75,7 +90,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();
@@ -91,9 +115,7 @@ 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
@@ -135,6 +157,7 @@ public class LidarView extends SurfaceView
         try {
             lidarThread.join(1000);
         } catch (InterruptedException e) {
+            Log.d("LIDAR", "Lidar failed to join", e);
         }
     }
 
@@ -148,8 +171,18 @@ public class LidarView extends SurfaceView
                     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);
+                }
+
+                // TODO:
+                canvas.drawCircle((float) point.x, (float) point.y, 5,
+                                  Objects.requireNonNull(groupNumPaints
+                                                                 .get(point.groupNumber))); // Can't be null as we just added it.
             }
             canvas.restore();
             surfaceHolder.unlockCanvasAndPost(canvas);
@@ -157,17 +190,24 @@ public class LidarView extends SurfaceView
     }
 
     /**
-     *
      * @param groupNumber
      * @return
      */
-    private static int convertGroupNumberToHue(int groupNumber){
-        return 0;
+    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;
@@ -175,7 +215,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) {

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

@@ -10,7 +10,7 @@ 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;
     }