From 110facb8a36882e5befd936b31121d83cea63384 Mon Sep 17 00:00:00 2001 From: Piv <18462828+Piv200@users.noreply.github.com> Date: Fri, 7 Feb 2020 20:36:51 +1030 Subject: [PATCH] Make SLAM view easier to implement in multiple protocols. --- .../carcontroller/SLAM/SlamUpdater.java | 36 ++++++++++ .../example/carcontroller/SLAM/SlamView.java | 66 ++++++++++--------- .../carcontroller/SLAM/ZmqSlamUpdater.java | 59 +++++++++++++++++ 3 files changed, 129 insertions(+), 32 deletions(-) create mode 100644 app/src/main/java/com/example/carcontroller/SLAM/SlamUpdater.java create mode 100644 app/src/main/java/com/example/carcontroller/SLAM/ZmqSlamUpdater.java diff --git a/app/src/main/java/com/example/carcontroller/SLAM/SlamUpdater.java b/app/src/main/java/com/example/carcontroller/SLAM/SlamUpdater.java new file mode 100644 index 0000000..ce4170d --- /dev/null +++ b/app/src/main/java/com/example/carcontroller/SLAM/SlamUpdater.java @@ -0,0 +1,36 @@ +package com.example.carcontroller.SLAM; + +import com.example.carcontroller.SlamLocation; +import com.example.carcontroller.SlamScan; +import com.google.protobuf.ByteString; + +import java.util.HashSet; +import java.util.Set; + +public abstract class SlamUpdater implements Runnable { + + private Set listeners; + + public SlamUpdater() { + init(); + } + + protected void init() { + listeners = new HashSet<>(); + } + + public void addMapChangedListener(MapChangedListener listener) { + listeners.add(listener); + } + + + protected void fireMapChanged(SlamScan scan) { + listeners.forEach(listener -> listener.mapChanged(scan.getMap(), scan.getLocation())); + } + + public abstract void stop(); + + public interface MapChangedListener { + void mapChanged(ByteString map, SlamLocation location); + } +} 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 b9beb89..80580a0 100644 --- a/app/src/main/java/com/example/carcontroller/SLAM/SlamView.java +++ b/app/src/main/java/com/example/carcontroller/SLAM/SlamView.java @@ -3,67 +3,69 @@ package com.example.carcontroller.SLAM; import android.content.Context; import android.content.SharedPreferences; import android.util.AttributeSet; +import android.view.SurfaceHolder; import android.view.SurfaceView; import androidx.preference.PreferenceManager; -import com.example.carcontroller.SlamScan; -import com.google.protobuf.InvalidProtocolBufferException; +import com.example.carcontroller.SlamLocation; +import com.google.protobuf.ByteString; -import org.zeromq.SocketType; -import org.zeromq.ZContext; -import org.zeromq.ZMQ; +public class SlamView extends SurfaceView implements SlamUpdater.MapChangedListener { -public class SlamView extends SurfaceView implements Runnable { - - ZContext context; - String host; - String port; + SlamUpdater slam; Thread mapThread; + Context context; + private int width; + private int height; + private SurfaceHolder surfaceHolder; public SlamView(Context context) { super(context); + this.context = context; init(); } public SlamView(Context context, AttributeSet attrs) { super(context, attrs); + this.context = context; init(); } public SlamView(Context context, AttributeSet attrs, int defStyleAttr) { super(context, attrs, defStyleAttr); + this.context = context; init(); } private void init() { - context = new ZContext(); - SharedPreferences prefs = PreferenceManager.getDefaultSharedPreferences(getContext()); - host = prefs.getString("host", "10.0.0.53"); - port = prefs.getString("zmqPort", "5050"); + SharedPreferences prefs = PreferenceManager.getDefaultSharedPreferences(context); + String host = prefs.getString("host", "10.0.0.53"); + String port = prefs.getString("zmqPort", "5050"); + slam = new ZmqSlamUpdater(host, port); + slam.addMapChangedListener(this); } - @Override - public void run() { - // Receive map from zmq and update appropriately. - try (ZMQ.Socket socket = context.createSocket(SocketType.PAIR)) { - socket.connect("tcp://" + host + ":" + port); - socket.send("Hi"); - while (!Thread.currentThread().isInterrupted()) { - byte[] map = socket.recv(); - SlamScan scan = SlamScan.parseFrom(map); - for (Byte b : scan.getMap()) { - - } - } - } catch (InvalidProtocolBufferException e) { - System.out.println("Invalid map found"); - } - + /** + * Called by MainActivity.onResume() to start a thread. + */ + public void resume() { + mapThread = new Thread(slam); + mapThread.start(); } public void stop() { // Use grpc to tell the loader to stop. Interrupt the loader thread as well. - mapThread.interrupt(); + slam.stop(); + try { + mapThread.join(); + } catch (InterruptedException e) { + + } + } + + @Override + public void mapChanged(ByteString map, SlamLocation location) { + } } diff --git a/app/src/main/java/com/example/carcontroller/SLAM/ZmqSlamUpdater.java b/app/src/main/java/com/example/carcontroller/SLAM/ZmqSlamUpdater.java new file mode 100644 index 0000000..19593b0 --- /dev/null +++ b/app/src/main/java/com/example/carcontroller/SLAM/ZmqSlamUpdater.java @@ -0,0 +1,59 @@ +package com.example.carcontroller.SLAM; + +import com.example.carcontroller.SlamScan; +import com.google.protobuf.InvalidProtocolBufferException; + +import org.zeromq.SocketType; +import org.zeromq.ZContext; +import org.zeromq.ZMQ; + +/** + * Connects to Pi and retrieves updates of the SLAM map, + * notifying listeners of changes to the map when they arrive. + * Uses 0MQ for the transport. + */ +public class ZmqSlamUpdater extends SlamUpdater { + + private ZContext context; + private String host; + private String port; + private boolean running = false; + + public ZmqSlamUpdater(String host, String port) { + super(); + this.host = host; + this.port = port; + init(); + } + + @Override + protected void init() { + context = new ZContext(); + } + + + @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)) { + socket.connect("tcp://" + host + ":" + port); + socket.send("Hi"); + while (running) { + byte[] map = socket.recv(); + fireMapChanged(SlamScan.parseFrom(map)); + } + } catch (InvalidProtocolBufferException e) { + System.out.println("Invalid map found"); + running = false; + } + } + + + public void stop() { + running = false; + } +}