diff --git a/app/src/main/java/org/vato/carcontroller/LIDAR/LidarView.java b/app/src/main/java/org/vato/carcontroller/LIDAR/LidarView.java index 12bd7ae..6da157a 100644 --- a/app/src/main/java/org/vato/carcontroller/LIDAR/LidarView.java +++ b/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 { + implements AbstractUpdater.MapChangedListener, + GrpcUpdater.GrpcUpdateBootstrapper { 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 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 response = new StreamObserver() { + 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() { @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 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) { diff --git a/app/src/main/java/org/vato/carcontroller/Updaters/GrpcUpdater.java b/app/src/main/java/org/vato/carcontroller/Updaters/GrpcUpdater.java index 8e53a8f..ab48e8d 100644 --- a/app/src/main/java/org/vato/carcontroller/Updaters/GrpcUpdater.java +++ b/app/src/main/java/org/vato/carcontroller/Updaters/GrpcUpdater.java @@ -10,7 +10,7 @@ import io.grpc.stub.StreamObserver; public class GrpcUpdater extends AbstractUpdater { GrpcUpdateBootstrapper bootstrapper; - public GrpcUpdater(Parser parser, GrpcUpdateBootstrapper bootstrapper) { + public GrpcUpdater(Parser parser, GrpcUpdateBootstrapper bootstrapper) { super(parser); this.bootstrapper = bootstrapper; } diff --git a/protobuf/src/main/proto/car/tracking/lidar_tracker.proto b/protobuf/src/main/proto/car/tracking/lidar_tracker.proto index 860211f..b4806b7 100644 --- a/protobuf/src/main/proto/car/tracking/lidar_tracker.proto +++ b/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) {} + } \ No newline at end of file diff --git a/pycar/src/car/tracking/lidar_cache.py b/pycar/src/car/tracking/lidar_cache.py index f7551b3..ede6f69 100644 --- a/pycar/src/car/tracking/lidar_cache.py +++ b/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): - # Send the updated groups to 0MQ socket. - # 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)) + def _fireGroupsChanged(self): + pointScan = self.current_scan 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. """ - self._group_listeners.append(listener) + 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() diff --git a/pycar/src/car/tracking/lidar_servicer.py b/pycar/src/car/tracking/lidar_servicer.py index bb41106..51c4ccc 100644 --- a/pycar/src/car/tracking/lidar_servicer.py +++ b/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