From f281829595b73b75d015c13133e1ce8f5b30c03b Mon Sep 17 00:00:00 2001 From: Piv <18462828+Piv200@users.noreply.github.com> Date: Sat, 15 Feb 2020 20:28:35 +1030 Subject: [PATCH] Fix SLAM controller, use PUB instead of PAIR sockets for zmq updater. --- .../carcontroller/SLAM/SlamController.java | 19 ++++++--- .../example/carcontroller/SLAM/SlamView.java | 39 ++++++++++++++++++- .../carcontroller/SLAM/ZmqSlamUpdater.java | 7 +--- app/src/main/proto/SlamController.proto | 1 + 4 files changed, 53 insertions(+), 13 deletions(-) diff --git a/app/src/main/java/com/example/carcontroller/SLAM/SlamController.java b/app/src/main/java/com/example/carcontroller/SLAM/SlamController.java index 9aebe39..7e9f549 100644 --- a/app/src/main/java/com/example/carcontroller/SLAM/SlamController.java +++ b/app/src/main/java/com/example/carcontroller/SLAM/SlamController.java @@ -1,12 +1,12 @@ package com.example.carcontroller.SLAM; -import androidx.appcompat.app.AppCompatActivity; -import androidx.preference.PreferenceManager; - import android.content.SharedPreferences; import android.os.Bundle; import android.widget.SeekBar; +import androidx.appcompat.app.AppCompatActivity; +import androidx.preference.PreferenceManager; + import com.example.carcontroller.PiLoader; import com.example.carcontroller.R; @@ -75,21 +75,28 @@ public class SlamController extends AppCompatActivity implements SeekBar.OnSeekB grpcController = new PiLoader(prefs.getString("host", "10.0.0.53"), Integer.parseInt(prefs.getString("port", "50051"))); } // Should call the equivalent of the load method either here or in the loader. + // Test without the grpc for steering. grpcController.start(); - slamView.resume(); + if (slamView != null) { + slamView.resume(); + } } @Override protected void onPause() { super.onPause(); grpcController.stop(); - slamView.stop(); + if (slamView != null) { + slamView.stop(); + } } @Override protected void onStop() { super.onStop(); grpcController.stop(); - slamView.stop(); + if (slamView != null) { + slamView.stop(); + } } } diff --git a/app/src/main/java/com/example/carcontroller/SLAM/SlamView.java b/app/src/main/java/com/example/carcontroller/SLAM/SlamView.java index 5a9bfd0..3479dd6 100644 --- a/app/src/main/java/com/example/carcontroller/SLAM/SlamView.java +++ b/app/src/main/java/com/example/carcontroller/SLAM/SlamView.java @@ -12,9 +12,16 @@ import android.view.SurfaceView; import androidx.preference.PreferenceManager; +import com.example.carcontroller.Empty; +import com.example.carcontroller.SlamControlGrpc; +import com.example.carcontroller.SlamDetails; import com.example.carcontroller.SlamLocation; import com.google.protobuf.ByteString; +import io.grpc.ManagedChannel; +import io.grpc.ManagedChannelBuilder; +import io.grpc.stub.StreamObserver; + public class SlamView extends SurfaceView implements SlamUpdater.MapChangedListener { private SlamUpdater slam; @@ -23,6 +30,10 @@ public class SlamView extends SurfaceView implements SlamUpdater.MapChangedListe private int width; private SurfaceHolder surfaceHolder; private Paint paint; + private SlamControlGrpc.SlamControlStub stub; + private ManagedChannel channel; + private int mapSizePixels; + private int mapSizeMeters; public SlamView(Context context) { super(context); @@ -46,19 +57,43 @@ public class SlamView extends SurfaceView implements SlamUpdater.MapChangedListe SharedPreferences prefs = PreferenceManager.getDefaultSharedPreferences(context); String host = prefs.getString("host", "10.0.0.53"); String port = prefs.getString("zmqPort", "5050"); + String gRPCPort = prefs.getString("port", "50051"); + mapSizePixels = Integer.parseInt(prefs.getString("MAPSIZEPIXELS", "540")); + mapSizeMeters = Integer.parseInt(prefs.getString("MAPSIZEMETRES", "10")); slam = new ZmqSlamUpdater(host, port); slam.addMapChangedListener(this); surfaceHolder = getHolder(); paint = new Paint(); paint.setColor(Color.BLUE); + mapThread = new Thread(slam); + channel = ManagedChannelBuilder.forAddress(host, Integer.parseInt(gRPCPort)).usePlaintext().build(); + stub = SlamControlGrpc.newStub(channel); + } /** * Called by MainActivity.onResume() to start a thread. */ public void resume() { - mapThread = new Thread(slam); - mapThread.start(); + StreamObserver response = new StreamObserver() { + @Override + public void onNext(Empty value) { + mapThread.start(); + } + + @Override + public void onError(Throwable t) { + // TODO: close the activity, + System.out.println(t.getMessage()); + } + + @Override + public void onCompleted() { + // Don't care. + } + }; + // use async grpc method, ZMQ doesn't need to connect straight away. + stub.startMapStreaming(SlamDetails.newBuilder().setMapSizePixels(mapSizePixels).setMapSizeMeters(mapSizeMeters).build(), response); } public void stop() { diff --git a/app/src/main/java/com/example/carcontroller/SLAM/ZmqSlamUpdater.java b/app/src/main/java/com/example/carcontroller/SLAM/ZmqSlamUpdater.java index 2e922d2..4bfddc4 100644 --- a/app/src/main/java/com/example/carcontroller/SLAM/ZmqSlamUpdater.java +++ b/app/src/main/java/com/example/carcontroller/SLAM/ZmqSlamUpdater.java @@ -35,14 +35,11 @@ public class ZmqSlamUpdater extends SlamUpdater { @Override public void run() { - // Should send a gRPC message to start listening... - - running = true; // Receive map from zmq and update appropriately. - try (ZMQ.Socket socket = context.createSocket(SocketType.PAIR)) { + try (ZMQ.Socket socket = context.createSocket(SocketType.SUB)) { socket.connect("tcp://" + host + ":" + port); - socket.send("Hi"); + socket.subscribe("slam_map"); while (running) { byte[] map = socket.recv(); fireMapChanged(SlamScan.parseFrom(map)); diff --git a/app/src/main/proto/SlamController.proto b/app/src/main/proto/SlamController.proto index bdc823a..065185f 100644 --- a/app/src/main/proto/SlamController.proto +++ b/app/src/main/proto/SlamController.proto @@ -9,6 +9,7 @@ import "empty.proto"; message SlamDetails { int32 map_size_pixels = 1; int32 map_size_meters = 2; + int32 port = 3; } message SlamRow{