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 c6142b2..aad1dc4 100644 --- a/app/src/main/java/org/vato/carcontroller/LIDAR/LidarView.java +++ b/app/src/main/java/org/vato/carcontroller/LIDAR/LidarView.java @@ -15,7 +15,6 @@ import androidx.preference.PreferenceManager; import com.google.protobuf.Empty; -import org.vato.carcontroller.Int32Value; import org.vato.carcontroller.PersonTrackingGrpc; import org.vato.carcontroller.PointScan; import org.vato.carcontroller.Updaters.AbstractUpdater; @@ -27,7 +26,8 @@ import io.grpc.ManagedChannel; import io.grpc.ManagedChannelBuilder; import io.grpc.stub.StreamObserver; -public class LidarView extends SurfaceView implements AbstractUpdater.MapChangedListener { +public class LidarView extends SurfaceView + implements AbstractUpdater.MapChangedListener { private static final String LIDAR_TOPIC = "lidar_map"; @@ -60,11 +60,13 @@ public class LidarView extends SurfaceView implements AbstractUpdater.MapChanged 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); + lidar = new ZmqUpdater<>(PointScan.getDefaultInstance().getParserForType(), LIDAR_TOPIC, + host, port); lidar.addMapChangedListener(this); surfaceHolder = getHolder(); lidarThread = new Thread(lidar); - ManagedChannel channel = ManagedChannelBuilder.forAddress(host, Integer.parseInt(gRPCPort)).usePlaintext().build(); + ManagedChannel channel = ManagedChannelBuilder.forAddress(host, Integer.parseInt(gRPCPort)) + .usePlaintext().build(); stub = PersonTrackingGrpc.newStub(channel); } @@ -127,7 +129,8 @@ public class LidarView extends SurfaceView implements AbstractUpdater.MapChanged Canvas canvas = surfaceHolder.lockCanvas(); canvas.save(); canvas.drawColor(Color.WHITE); - for (Point point : points.getPointsList().stream().map(Point::fromProtoPoint).collect(Collectors.toList())) { + for (Point point : points.getPointsList().stream().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. @@ -156,7 +159,8 @@ public class LidarView extends SurfaceView implements AbstractUpdater.MapChanged } 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(angle) + offset.x, + distance * Math.cos(angle) + offset.y); } } diff --git a/app/src/main/java/org/vato/carcontroller/PiLoader.java b/app/src/main/java/org/vato/carcontroller/PiLoader.java index 484064d..3531a87 100644 --- a/app/src/main/java/org/vato/carcontroller/PiLoader.java +++ b/app/src/main/java/org/vato/carcontroller/PiLoader.java @@ -1,29 +1,35 @@ package org.vato.carcontroller; +import android.util.Log; + import com.google.protobuf.Empty; +import java.util.concurrent.CountDownLatch; import java.util.concurrent.TimeUnit; import java.util.concurrent.atomic.AtomicBoolean; import io.grpc.ManagedChannel; import io.grpc.ManagedChannelBuilder; +import io.grpc.stub.StreamObserver; public class PiLoader implements Runnable { - Integer steeringValue = 50; - Integer throttleValue = 50; - + private Integer steeringValue = 50; + private Integer throttleValue = 50; private ManagedChannel mChannel; + private CarControlGrpc.CarControlBlockingStub stub; private AtomicBoolean stop = new AtomicBoolean(false); - Thread piUpdaterThread; + private Thread piUpdaterThread; + private boolean useGrpcStream; - public PiLoader(String host, Integer port) { + public PiLoader(String host, Integer port, boolean useGrpcStream) { mChannel = ManagedChannelBuilder.forAddress(host, port).usePlaintext().build(); // Create an async stub. stub = CarControlGrpc.newBlockingStub(mChannel); + this.useGrpcStream = useGrpcStream; } private void createAndStart() { @@ -53,10 +59,23 @@ public class PiLoader implements Runnable { @Override public void run() { + if (useGrpcStream) { + doStreamUpdates(); + } else { + doAtomicUpdates(); + } + } + + private void doAtomicUpdates() { + // Do atomic if the user hasn't check the option to use_grpc_streams. This method tends to be more compatible. while (!stop.get() && !Thread.interrupted()) { try { - SteeringResponse steeringResponse = stub.setSteering(SteeringRequest.newBuilder().setSteering((float) steeringValue / 50f - 1).build()); - ThrottleResponse throttleResponse = stub.setThrottle(ThrottleRequest.newBuilder().setThrottle((float) throttleValue / 50f - 1).build()); + SteeringResponse steeringResponse = stub.setSteering( + SteeringRequest.newBuilder().setSteering((float) steeringValue / 50f - 1) + .build()); + ThrottleResponse throttleResponse = stub.setThrottle( + ThrottleRequest.newBuilder().setThrottle((float) throttleValue / 50f - 1) + .build()); } catch (Exception e) { System.out.println("Error"); stop(); @@ -71,6 +90,55 @@ 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 responseObserver = new StreamObserver() { + @Override + public void onNext(Empty value) { + finishLatch.countDown(); + Log.d("PiLoader", "Finished streaming"); + } + + @Override + public void onError(Throwable t) { + finishLatch.countDown(); + Log.d("PiLoader", "Failed to do gRPC Stream.", t); + } + + @Override + public void onCompleted() { + finishLatch.countDown(); + Log.d("PiLoader", "Finished streaming"); + } + }; + StreamObserver requestStreamObserver = CarControlGrpc.newStub(mChannel) + .streamVehicle2d( + responseObserver); + while (!stop.get() && !Thread.interrupted() && finishLatch.getCount() > 0) { + requestStreamObserver.onNext(Vehicle2DRequest.newBuilder() + .setThrottle(ThrottleRequest.newBuilder() + .setThrottle( + (float) throttleValue / + 50f - + 1) + .build()) + .setSteering(SteeringRequest.newBuilder() + .setSteering( + (float) steeringValue / + 50f - + 1) + .build()) + .build()); + try { + // Use the same update rate as a typical screen refresh rate. + TimeUnit.MILLISECONDS.sleep(200); + } catch (InterruptedException e) { + // TODO: Handle when interrupted and sleeping. + } + } + } + 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()); diff --git a/app/src/main/java/org/vato/carcontroller/SLAM/SlamController.java b/app/src/main/java/org/vato/carcontroller/SLAM/SlamController.java index 710acc2..8483840 100644 --- a/app/src/main/java/org/vato/carcontroller/SLAM/SlamController.java +++ b/app/src/main/java/org/vato/carcontroller/SLAM/SlamController.java @@ -69,10 +69,13 @@ public class SlamController extends AppCompatActivity implements SeekBar.OnSeekB @Override protected void onResume() { super.onResume(); - SharedPreferences prefs = PreferenceManager.getDefaultSharedPreferences(getApplicationContext()); + SharedPreferences prefs = PreferenceManager.getDefaultSharedPreferences( + getApplicationContext()); if (grpcController == null) { - grpcController = new PiLoader(prefs.getString("host", "10.0.0.53"), Integer.parseInt(prefs.getString("port", "50051"))); + grpcController = new PiLoader(prefs.getString("host", "10.0.0.53"), + Integer.parseInt(prefs.getString("port", "50051")), + prefs.getBoolean("use_grpc_streams", false)); } // Should call the equivalent of the load method either here or in the loader. // Test without the grpc for steering. diff --git a/app/src/main/java/org/vato/carcontroller/SLAM/SlamView.java b/app/src/main/java/org/vato/carcontroller/SLAM/SlamView.java index 91a9263..55635b6 100644 --- a/app/src/main/java/org/vato/carcontroller/SLAM/SlamView.java +++ b/app/src/main/java/org/vato/carcontroller/SLAM/SlamView.java @@ -20,13 +20,15 @@ import org.vato.carcontroller.SlamDetails; import org.vato.carcontroller.SlamLocation; import org.vato.carcontroller.SlamScan; import org.vato.carcontroller.Updaters.AbstractUpdater; +import org.vato.carcontroller.Updaters.GrpcUpdater; import org.vato.carcontroller.Updaters.ZmqUpdater; import io.grpc.ManagedChannel; import io.grpc.ManagedChannelBuilder; import io.grpc.stub.StreamObserver; -public class SlamView extends SurfaceView implements AbstractUpdater.MapChangedListener { +public class SlamView extends SurfaceView implements AbstractUpdater.MapChangedListener, + GrpcUpdater.GrpcUpdateBootstrapper { private static final String SLAM_TOPIC = "slam_map"; @@ -38,6 +40,7 @@ public class SlamView extends SurfaceView implements AbstractUpdater.MapChangedL private int mapSizePixels; private int mapSizeMeters; private String port; + private boolean useGrpcStreams; public SlamView(Context context) { super(context); @@ -61,13 +64,24 @@ public class SlamView extends SurfaceView implements AbstractUpdater.MapChangedL String gRPCPort = prefs.getString("port", "50051"); mapSizePixels = Integer.parseInt(prefs.getString("MAPSIZEPIXELS", "540")); mapSizeMeters = Integer.parseInt(prefs.getString("MAPSIZEMETRES", "10")); - slam = new ZmqUpdater<>(SlamScan.getDefaultInstance().getParserForType(), SLAM_TOPIC, host, port); + useGrpcStreams = prefs.getBoolean("use_grpc_streams", false); + + // TODO: Generify the Updaters to support the strategy pattern. + if (useGrpcStreams) { + slam = new GrpcUpdater<>(SlamScan.getDefaultInstance().getParserForType(), + this); + } else { + slam = new ZmqUpdater<>(SlamScan.getDefaultInstance().getParserForType(), SLAM_TOPIC, + host, + port); + } slam.addMapChangedListener(this); surfaceHolder = getHolder(); paint = new Paint(); paint.setColor(Color.BLUE); mapThread = new Thread(slam); - ManagedChannel channel = ManagedChannelBuilder.forAddress(host, Integer.parseInt(gRPCPort)).usePlaintext().build(); + ManagedChannel channel = ManagedChannelBuilder.forAddress(host, Integer.parseInt(gRPCPort)) + .usePlaintext().build(); stub = SlamControlGrpc.newStub(channel); } @@ -75,6 +89,18 @@ public class SlamView extends SurfaceView implements AbstractUpdater.MapChangedL * Called by MainActivity.onResume() to start a thread. */ public void resume() { + if (useGrpcStreams) { + // TODO: Make a gRPC updater that is a generic , one for response proto (like normal) and + // the other for a request proto. onNext() will do the normal map updating. + // GrpcMapThread can just be started for now. + mapThread.start(); + } else { + doZmqSlamStream(); + } + } + + private void doZmqSlamStream() { + // TODO: See if this bootstrapping can be integrated into updaters, as grpc requires a similar thing. StreamObserver response = new StreamObserver() { @Override public void onNext(Empty value) { @@ -94,9 +120,9 @@ public class SlamView extends SurfaceView implements AbstractUpdater.MapChangedL }; // use async grpc method, ZMQ doesn't need to connect straight away. stub.startMapStreaming(SlamDetails.newBuilder() - .setMapSizePixels(mapSizePixels) - .setMapSizeMeters(mapSizeMeters) - .build(), response); + .setMapSizePixels(mapSizePixels) + .setMapSizeMeters(mapSizeMeters) + .build(), response); } public void stop() { @@ -119,17 +145,30 @@ public class SlamView extends SurfaceView implements AbstractUpdater.MapChangedL canvas.save(); canvas.drawColor(Color.WHITE); // Using width as we want square. - Bitmap bitmap = Bitmap.createBitmap(mapSizePixels, mapSizePixels, Bitmap.Config.ALPHA_8); + Bitmap bitmap = Bitmap.createBitmap(mapSizePixels, mapSizePixels, + Bitmap.Config.ALPHA_8); for (int i = 0; i < mapSizePixels; i++) { for (int j = 0; j < mapSizePixels; j++) { // 0-255 is appropriate for the config used. // Take away from 255 to invert the colours, so walls are the correct colour. - bitmap.setPixel(i, j, 255 - map.byteAt(i * mapSizePixels + j)); + bitmap.setPixel(i, j, map.byteAt(i * mapSizePixels + j)); } } + + // TODO: Place the car in the right spot. canvas.drawBitmap(bitmap, 0, 0, paint); canvas.restore(); surfaceHolder.unlockCanvasAndPost(canvas); } } + + @Override + public void bootstrap(StreamObserver responseObserver) { + stub.mapStream(SlamDetails.newBuilder() + .setMapSizePixels( + mapSizePixels) + .setMapSizeMeters( + mapSizeMeters) + .build(), responseObserver); + } } diff --git a/app/src/main/java/org/vato/carcontroller/SimpleController.java b/app/src/main/java/org/vato/carcontroller/SimpleController.java index 8c6f814..46a97f9 100644 --- a/app/src/main/java/org/vato/carcontroller/SimpleController.java +++ b/app/src/main/java/org/vato/carcontroller/SimpleController.java @@ -12,10 +12,6 @@ import androidx.preference.PreferenceManager; import com.google.protobuf.Empty; -import org.vato.carcontroller.LIDAR.LidarTrackingController; - -import java.util.stream.Stream; - import io.grpc.stub.StreamObserver; public class SimpleController extends AppCompatActivity implements SeekBar.OnSeekBarChangeListener { @@ -49,10 +45,13 @@ public class SimpleController extends AppCompatActivity implements SeekBar.OnSee @Override protected void onResume() { super.onResume(); - SharedPreferences prefs = PreferenceManager.getDefaultSharedPreferences(getApplicationContext()); + SharedPreferences prefs = PreferenceManager.getDefaultSharedPreferences( + getApplicationContext()); if (grpcController == null) { - grpcController = new PiLoader(prefs.getString("host", "10.0.0.53"), Integer.parseInt(prefs.getString("port", "50051"))); + grpcController = new PiLoader(prefs.getString("host", "10.0.0.53"), + Integer.parseInt(prefs.getString("port", "50051")), + prefs.getBoolean("use_grpc_streams", false)); } // Should call the equivalent of the load method either here or in the loader. grpcController.start(); @@ -115,12 +114,14 @@ public class SimpleController extends AppCompatActivity implements SeekBar.OnSee StreamObserver response = new StreamObserver() { @Override public void onNext(Empty value) { - Toast.makeText(getApplicationContext(), "Started Recording Lidar", Toast.LENGTH_SHORT).show(); + Toast.makeText(getApplicationContext(), "Started Recording Lidar", + Toast.LENGTH_SHORT).show(); } @Override public void onError(Throwable t) { - Toast.makeText(getApplicationContext(), "Failed to set lidar recording", Toast.LENGTH_SHORT).show(); + Toast.makeText(getApplicationContext(), "Failed to set lidar recording", + Toast.LENGTH_SHORT).show(); } @Override diff --git a/app/src/main/java/org/vato/carcontroller/Updaters/GrpcUpdater.java b/app/src/main/java/org/vato/carcontroller/Updaters/GrpcUpdater.java new file mode 100644 index 0000000..2ac8231 --- /dev/null +++ b/app/src/main/java/org/vato/carcontroller/Updaters/GrpcUpdater.java @@ -0,0 +1,56 @@ +package org.vato.carcontroller.Updaters; + +import android.util.Log; + +import com.google.protobuf.MessageLite; +import com.google.protobuf.Parser; + +import io.grpc.stub.StreamObserver; + +public class GrpcUpdater extends AbstractUpdater { + GrpcUpdateBootstrapper bootstrapper; + + public GrpcUpdater(Parser parser, GrpcUpdateBootstrapper bootstrapper) { + super(parser); + this.bootstrapper = bootstrapper; + } + + @Override + public void stop() { + + } + + @Override + public void run() { + StreamObserver responseObserver = new StreamObserver() { + @Override + public void onNext(T value) { + fireMapChanged(value); + } + + @Override + public void onError(Throwable t) { + Log.d("GRPC_UPDATER", "Failed to get a response", t); + } + + @Override + public void onCompleted() { + + } + }; + + bootstrapper.bootstrap(responseObserver); + } + + // TODO: Find if there's a way to clean this up so other updaters can easily substitute. + public interface GrpcUpdateBootstrapper { + + /** + * A responseObserver is passed into this method so that it can be called/initialised by + * the client. + * + * @param responseObserver + */ + void bootstrap(StreamObserver responseObserver); + } +} diff --git a/app/src/main/res/xml/root_preferences.xml b/app/src/main/res/xml/root_preferences.xml index 1c49e0c..5b0612d 100644 --- a/app/src/main/res/xml/root_preferences.xml +++ b/app/src/main/res/xml/root_preferences.xml @@ -28,6 +28,10 @@ android:title="Port number" app:key="port" app:useSimpleSummaryProvider="true" /> + diff --git a/car/build.gradle b/car/build.gradle index 9fdce20..d0455dc 100644 --- a/car/build.gradle +++ b/car/build.gradle @@ -12,7 +12,7 @@ dependencies { task copyPythonCode(type: Copy, dependsOn: configurations.python){ // Copy python protobuf code from proto project. from zipTree(configurations.python.asPath) - into './src' + into 'src' } task build(type: Exec, dependsOn: copyPythonCode) { diff --git a/car/src/car/control/motor_servicer.py b/car/src/car/control/motor_servicer.py index 673681e..2e1c270 100644 --- a/car/src/car/control/motor_servicer.py +++ b/car/src/car/control/motor_servicer.py @@ -12,6 +12,7 @@ class MotorServicer(motorService_pb2_grpc.CarControlServicer): def __init__(self, vehicle): self.vehicle = VehicleRecordingDecorator(vehicle) self._timer = None + self._timeout_elapsed = False def set_throttle(self, request, context): # gRPC streams currently don't work between python and android. @@ -26,6 +27,21 @@ class MotorServicer(motorService_pb2_grpc.CarControlServicer): self.vehicle.steering = request.steering return motorService_pb2.SteeringResponse(steeringSet=True) + def stream_vehicle_2d(self, request_iterator, context): + print('Starting Vehicle Control Stream') + self._timeout_elapsed = False + for request_2d in request_iterator: + # End the stream if the user fails to respond in time. + if self._timeout_elapsed: + break + print('Setting 2d values to: ' + str((request_2d.throttle.throttle, request_2d.steering.steering))) + self.set_timeout(3) + # TODO: Make a vehicle method set 2d throttle/steering. + self.vehicle.throttle = request_2d.throttle.throttle + self.vehicle.steering = request_2d.steering.steering + + return empty.Empty() + def set_timeout(self, min_timeout): """Stops the old timer and restarts it to the specified time. @@ -39,6 +55,7 @@ class MotorServicer(motorService_pb2_grpc.CarControlServicer): def timeout_elapsed(self): """Election or heartbeat timeout has elapsed.""" print("Node timeout elapsed") + self._timeout_elapsed = True self.vehicle.stop() def record(self, request, context): diff --git a/car/src/car/slam/slam_processor.py b/car/src/car/slam/slam_processor.py new file mode 100644 index 0000000..b900b62 --- /dev/null +++ b/car/src/car/slam/slam_processor.py @@ -0,0 +1,87 @@ +import car.tracking.devices.factory as lidar_fact +from breezyslam.algorithms import RMHC_SLAM +from breezyslam.sensors import RPLidarA1 as LaserModel + + +class SlamProcessor: + def __init__(self, map_pixels=None, map_meters=None): + self._map_pixels = map_pixels + self._map_meters = map_meters + + def do_scanning(self): + self.can_scan = True + + # Adapted from BreezySLAM rpslam example. + # Connect to Lidar unit. For some reason it likes to be done twice, otherwise it errors out... + lidar = lidar_fact.get_lidar() + lidar = lidar_fact.get_lidar() + + print('Initialised lidar') + + # Create an RMHC SLAM object with a laser model and optional robot model + slam = RMHC_SLAM(LaserModel(), self._map_pixels, self._map_meters) + + print('initialised slam') + + # Initialize empty map + mapbytes = bytearray(self.map_pixels * self.map_pixels) + + print('Initialised byte []') + + # Create an iterator to collect scan data from the RPLidar + iterator = lidar.iter_scans() + + print('Scanning') + + while self.can_scan: + # Extract (quality, angle, distance) triples from current scan + # This can still sometimes fail, at least on macOS. + try: + items = [item for item in next(iterator)] + except Exception: + # Ignore errors for now. + print('Failed to retrieve some scans from lidar.') + continue + + # Extract distances and angles from triples + distances = [item[2] for item in items] + angles = [item[1] for item in items] + print('Updating map') + # Update SLAM with current Lidar scan and scan angles + slam.update(distances, scan_angles_degrees=angles) + print('Map updated') + slam.getmap(mapbytes) + + # Make a generator that can be iterated on by this function. + yield (mapbytes, slam.getpos()) + + # Close the generator when user stops scanning. + raise StopIteration + + def stop_scanning(self): + self.can_scan = False + + # Properties + @property + def map_pixels(self): + return self._map_pixels + + @map_pixels.setter + def map_pixels(self, value): + self._map_pixels = value + + @property + def map_meters(self): + return self._map_meters + + @map_meters.setter + def map_meters(self, value): + self._map_meters = value + + @property + def lidar_connection(self): + return self._lidar_connection + + @lidar_connection.setter + def lidar_connection(self, value): + self._lidar_connection = value diff --git a/car/src/car/slam/slam_servicer.py b/car/src/car/slam/slam_servicer.py index 54847cd..d4843be 100644 --- a/car/src/car/slam/slam_servicer.py +++ b/car/src/car/slam/slam_servicer.py @@ -2,8 +2,11 @@ import car.slam.SlamController_pb2_grpc as grpc import car.slam.SlamController_pb2 as proto import car.empty_pb2 as empty import car.slam.slam_streamer as slam +from .slam_processor import SlamProcessor + from multiprocessing import Process import os +from car.slam.SlamController_pb2 import SlamLocation, SlamScan class SlamServicer(grpc.SlamControlServicer): @@ -11,24 +14,34 @@ class SlamServicer(grpc.SlamControlServicer): def __init__(self): print('Servicer initialised') - self.slam = slam.SlamStreamer() + self.slam = SlamProcessor() def start_map_streaming(self, request, context): print('Received Map Start Streaming Request') + slam_streamer = slam.SlamStreamer(self.slam) if self.slam_thread is None: print('initialising slam_thread') # Don't bother creating and starting slam more than once. - self.slam.port = 50052 if "CAR_ZMQ_PORT" not in os.environ else os.environ[ + slam_streamer.port = 50052 if "CAR_ZMQ_PORT" not in os.environ else os.environ[ 'CAR_ZMQ_PORT'] self.slam.map_pixels = request.map_size_pixels self.slam.map_meters = request.map_size_meters - self.slam_thread = Process(target=self.slam.start) + self.slam_thread = Process(target=slam_streamer.start) self.slam_thread.start() return empty.Empty() + def map_stream(self, request, context): + print('Received Map gRPC Stream Start Request') + self.slam.map_meters = request.map_size_meters + self.slam.map_pixels = request.map_size_pixels + + # If expression doesn't work, then just do yields in a for loop. + return (SlamScan(map=bytes(m), location=SlamLocation(x=pos[0], y=pos[1], theta=pos[2])) for m, pos in self.slam.do_scanning()) + def stop_streaming(self, request, context): + # Need to adapt this to also be able to stop gRPC streamer. + self.slam.stop_scanning() if self.slam_thread is not None: - self.slam.stop_scanning() self.slam_thread.join() self.slam = None return empty.Empty() diff --git a/car/src/car/slam/slam_streamer.py b/car/src/car/slam/slam_streamer.py index 117a37b..b1ea6f7 100644 --- a/car/src/car/slam/slam_streamer.py +++ b/car/src/car/slam/slam_streamer.py @@ -1,10 +1,9 @@ import zmq -from breezyslam.algorithms import RMHC_SLAM -from breezyslam.sensors import RPLidarA1 as LaserModel from car.slam.SlamController_pb2 import SlamScan, SlamLocation import car.messaging.message_factory as mf import car.messaging.messages as messages import car.tracking.devices.factory as lidar_fact +from .slam_processor import SlamProcessor # Left here as was used in the example, configure as necessary. @@ -15,10 +14,12 @@ import car.tracking.devices.factory as lidar_fact class SlamStreamer: can_scan = False - def __init__(self, map_pixels=None, map_meters=None, port=None): - self._map_pixels = map_pixels - self._map_meters = map_meters + def __init__(self, processor, port=None): + """ + ZMQ implementation to stream slam map. + """ self._port = port + self.processor = processor def start(self): ''' @@ -31,47 +32,12 @@ class SlamStreamer: to calling this method, and changing those values after calling this method will have no effect. ''' - self.can_scan = True print('Starting to stream') + # TODO: Don't get the messenger here, pass it in/specify another way. self._mFactory = mf.getZmqPubSubStreamer(self._port) - print('Started and bound zmq socket.') - - # Adapted from BreezySLAM rpslam example. - # Connect to Lidar unit. For some reason it likes to be done twice, otherwise it errors out... - lidar = lidar_fact.get_lidar() - lidar = lidar_fact.get_lidar() - - print('Initialised lidar') - - # Create an RMHC SLAM object with a laser model and optional robot model - slam = RMHC_SLAM(LaserModel(), self._map_pixels, self._map_meters) - - print('initialised slam') - - # Initialize empty map - mapbytes = bytearray(self.map_pixels * self.map_pixels) - - print('Initialised byte []') - - # Create an iterator to collect scan data from the RPLidar - iterator = lidar.iter_scans() - - print('Scanning') - - while self.can_scan: - # Extract (quality, angle, distance) triples from current scan - items = [item for item in next(iterator)] - - # Extract distances and angles from triples - distances = [item[2] for item in items] - angles = [item[1] for item in items] - print('Updating map') - # Update SLAM with current Lidar scan and scan angles - slam.update(distances, scan_angles_degrees=angles) - print('Map updated') - slam.getmap(mapbytes) - self._push_map(mapbytes, slam.getpos()) + for map, pos in self.processor.do_scanning(): + self._push_map(map, pos) def _push_map(self, mapbytes, location): ''' @@ -86,32 +52,7 @@ class SlamStreamer: b'slam_map', protoScan) def stop_scanning(self): - self.can_scan = False - - # Properties - @property - def map_pixels(self): - return self._map_pixels - - @map_pixels.setter - def map_pixels(self, value): - self._map_pixels = value - - @property - def map_meters(self): - return self._map_meters - - @map_meters.setter - def map_meters(self, value): - self._map_meters = value - - @property - def lidar_connection(self): - return self._lidar_connection - - @lidar_connection.setter - def lidar_connection(self, value): - self._lidar_connection = value + self.processor.stop_scanning() @property def port(self): diff --git a/protobuf/src/main/proto/car/control/motorService.proto b/protobuf/src/main/proto/car/control/motorService.proto index dd15142..ed75f8d 100644 --- a/protobuf/src/main/proto/car/control/motorService.proto +++ b/protobuf/src/main/proto/car/control/motorService.proto @@ -25,6 +25,11 @@ message SteeringResponse{ bool steeringSet = 1; } +message Vehicle2DRequest{ + ThrottleRequest throttle = 1; + SteeringRequest steering = 2; +} + message RecordingReqeust{ bool record = 1; } @@ -36,6 +41,7 @@ message SaveRequest{ 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 record(RecordingReqeust) returns (google.protobuf.Empty) {} rpc save_recorded_data(SaveRequest) returns (google.protobuf.Empty) {} } diff --git a/protobuf/src/main/proto/car/slam/SlamController.proto b/protobuf/src/main/proto/car/slam/SlamController.proto index d47b7a3..6888096 100644 --- a/protobuf/src/main/proto/car/slam/SlamController.proto +++ b/protobuf/src/main/proto/car/slam/SlamController.proto @@ -29,5 +29,7 @@ message SlamScan{ service SlamControl { rpc start_map_streaming(SlamDetails) returns (google.protobuf.Empty) {} + rpc map_stream(SlamDetails) returns (stream SlamScan) {} + rpc stop_streaming(google.protobuf.Empty) returns (google.protobuf.Empty) {} } \ No newline at end of file