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");
+        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.
+                if (!groupNumPaints.containsKey(point.groupNumber)) {
-                canvas.drawCircle((float) point.x, (float) point.y, 5, new Paint());
+                    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){
+    private static int convertGroupNumberToHue(int groupNumber) {
-        return 0;
+        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;
     }

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

@@ -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/src/car/tracking/lidar_cache.py

@@ -10,7 +10,7 @@ import time
 class LidarCache():
     """
     A class that retrieves scans from the lidar,
-    runs grouping algorithms between scans and 
+    runs grouping algorithms on the scans and 
     keeps a copy of the group data.
     """
 
@@ -56,16 +56,10 @@ class LidarCache():
             else:
                 self.currentGroups = algorithms.calc_groups(scan)
 
-            self.fireGroupsChanged()
+            self._fireGroupsChanged()
 
-    def fireGroupsChanged(self):
+    def _fireGroupsChanged(self):
-        # Send the updated groups to 0MQ socket.
+        pointScan = self.current_scan
-        # Rename this to be a generic listener method, rather than an explicit 'send' (even though it can be treated as such already)
-        pointScan = tracker_pb.PointScan()
-        for group in self.currentGroups:
-            for point in group.get_points():
-                pointScan.points.append(tracker_pb.Point(
-                    angle=point[1], distance=point[2], group_number=group.number))
 
         for listener in self._group_listeners:
             listener(pointScan)
@@ -81,15 +75,33 @@ class LidarCache():
         listener
             An function that takes a PointScan proto object as its argument.
         """
+        if(listener not in self._group_listeners):
             self._group_listeners.append(listener)
 
+    def clear_listeners(self):
+        """
+        Clear all group change listeners.
+        """
+        self._group_listeners = []
+
+    @property
+    def current_scan(self):
+        pointScan = tracker_pb.PointScan()
+        for group in self.currentGroups:
+            for point in group.get_points():
+                pointScan.points.append(tracker_pb.Point(
+                    angle=point[1], distance=point[2], group_number=group.number))
+        return pointScan
+
     def stop_scanning(self):
         self.run = False
 
+
 if __name__ == '__main__':
-    lidar = MockLidar(iter(lidar_loader.load_scans_bytes_file('car/src/car/tracking/out.pickle')))
+    lidar = MockLidar(iter(lidar_loader.load_scans_bytes_file(
+        'car/src/car/tracking/out.pickle')))
     cache = LidarCache(lidar)
-    cache.add_groups_changed_listener(lambda a : print(a))
+    cache.add_groups_changed_listener(lambda a: print(a))
     cache.start_cache()
     time.sleep(1)
     cache.stop_scanning()

pycar/src/car/tracking/lidar_servicer.py

@@ -10,6 +10,7 @@ from car.messaging import messages
 import car.tracking.algorithms as alg
 import os
 import google.protobuf.empty_pb2 as empty
+import time
 
 
 class LidarServicer(PersonTrackingServicer):
@@ -18,7 +19,6 @@ class LidarServicer(PersonTrackingServicer):
         self._lidar = RecordingLidarDecorator(
             lidar_factory.get_lidar())
         self.cache = LidarCache(self._lidar, measurements=100)
-        self.cache.add_groups_changed_listener(self.onGroupsChanged)
         self._mFactory = None
         self._port = 50052 if 'CAR_ZMQ_PORT' not in os.environ else os.environ[
             'CAR_ZMQ_PORT']
@@ -32,18 +32,22 @@ class LidarServicer(PersonTrackingServicer):
         return empty.Empty()
 
     def stop_tracking(self, request, context):
+        print('Stopping tracking')
         self._should_stream = False
         self.cache.stop_scanning()
+        self.cache.clear_listeners()
         return empty.Empty()
 
     def start_tracking(self, request, context):
         """Starts the lidar cache, streaming on the provided port."""
         self._should_stream = True
+        # Want to rework this, like the sleam streamer/processer rework.
+        self.cache.add_groups_changed_listener(self.zmq_stream_listener)
         self.cache.start_cache()
         return empty.Empty()
 
     def record(self, request, context):
-        # TODO: Fix this to not require 
+        # TODO: Fix this to not require actually running the cache, just recording the lidar.
         if request.value:
             self.cache.start_cache()
         else:
@@ -55,7 +59,17 @@ class LidarServicer(PersonTrackingServicer):
         self._lidar.save_data(request.file)
         return empty.Empty()
 
-    def onGroupsChanged(self, message):
+    def lidar_stream(self, request, context):
+        # Streams the state of the lidar cache.
+        print('Received Lidar gRPC Stream Start Request')
+        self._should_stream = True
+        self.cache.start_cache()
+        sleep_time = request.time_between_messages if request.time_between_messages > 0.01 else 0.01
+        while self._should_stream:
+            time.sleep(sleep_time)
+            yield self.cache.current_scan
+
+    def zmq_stream_listener(self, message):
         if self._mFactory is None:
             # Create the zmq socket in the thread that it will be used, just to be safe.
             self._mFactory = mf.getZmqPubSubStreamer(self._port)
@@ -64,8 +78,10 @@ class LidarServicer(PersonTrackingServicer):
             self._mFactory.send_message_topic(
                 "lidar_map", messages.ProtoMessage(message=message.SerializeToString()))
 
+    def on_groups_changed(self, message):
         if self._tracked_group is not None and self._vehicle is not None:
             # Update vehicle to correctly follow the tracked group.
             # Leave for now, need to work out exactly how this will change.
             # alg.dualServoChange(alg.find_centre())
+            # Put this in a separate module I think, shouldn't control the can autonomously from the servicer.
             pass