vato007/picar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Provide a generic way to receive zmq updates for lidar and slam.

Browse Source
This commit is contained in:
Piv
2020-03-22 19:01:40 +10:30
parent dc832d8a5d
commit 03f6c2b9f7
12 changed files with 97 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
.idea/misc.xml generated
View File

@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectRootManager" version="2" languageLevel="JDK_1_8" project-jdk-name="1.8" project-jdk-type="JavaSDK">
<component name="ProjectRootManager" version="2" languageLevel="JDK_1_9" project-jdk-name="1.8" project-jdk-type="JavaSDK">
<output url="file://$PROJECT_DIR$/build/classes" />
</component>
<component name="ProjectType">

4
app/build.gradle
View File

@@ -19,8 +19,8 @@ android {
}
}
compileOptions {
sourceCompatibility = 1.8
targetCompatibility = 1.8
sourceCompatibility = 9.8
targetCompatibility = 9.8
}
}

2
app/src/main/AndroidManifest.xml
View File

@@ -13,7 +13,7 @@
android:supportsRtl="true"
android:theme="@style/AppTheme">
<activity android:name=".SLAM.SlamController"></activity>
<activity android:name=".LidarTrackingController" />
<activity android:name=".LIDAR.LidarTrackingController" />
<activity
android:name=".SettingsActivity"
android:label="@string/title_activity_settings"

6
app/src/main/java/com/example/carcontroller/LidarTrackingController.java → app/src/main/java/com/example/carcontroller/LIDAR/LidarTrackingController.java
View File

@@ -1,8 +1,10 @@
package com.example.carcontroller;
package com.example.carcontroller.LIDAR;
import android.os.Bundle;
import androidx.appcompat.app.AppCompatActivity;
import android.os.Bundle;
import com.example.carcontroller.R;
public class LidarTrackingController extends AppCompatActivity {

4
app/src/main/java/com/example/carcontroller/LidarView.java → app/src/main/java/com/example/carcontroller/LIDAR/LidarView.java
View File

@@ -1,4 +1,4 @@
package com.example.carcontroller;
package com.example.carcontroller.LIDAR;
import android.content.Context;
import android.graphics.Bitmap;
@@ -8,6 +8,8 @@ import android.view.MotionEvent;
import android.view.SurfaceHolder;
import android.view.SurfaceView;
import com.example.carcontroller.PersonTrackingGrpc;
public class LidarView extends SurfaceView implements Runnable {
private boolean running;

13
app/src/main/java/com/example/carcontroller/MainActivity.java
View File

@@ -1,22 +1,17 @@
package com.example.carcontroller;
import androidx.appcompat.app.AppCompatActivity;
import androidx.appcompat.widget.Toolbar;
import android.content.Intent;
import android.os.Bundle;
import android.view.Menu;
import android.view.MenuInflater;
import android.view.MenuItem;
import android.view.View;
import android.widget.SeekBar;
import androidx.appcompat.app.AppCompatActivity;
import androidx.appcompat.widget.Toolbar;
import com.example.carcontroller.LIDAR.LidarTrackingController;
import com.example.carcontroller.SLAM.SlamController;
import java.util.concurrent.CountDownLatch;
import io.grpc.stub.StreamObserver;
public class MainActivity extends AppCompatActivity {

36
app/src/main/java/com/example/carcontroller/SLAM/SlamUpdater.java
View File

@@ -1,36 +0,0 @@
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<MapChangedListener> 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);
}
}

22
app/src/main/java/com/example/carcontroller/SLAM/SlamView.java
View File

@@ -16,21 +16,25 @@ import com.example.carcontroller.Empty;
import com.example.carcontroller.SlamControlGrpc;
import com.example.carcontroller.SlamDetails;
import com.example.carcontroller.SlamLocation;
import com.example.carcontroller.SlamScan;
import com.example.carcontroller.Updaters.AbstractUpdater;
import com.example.carcontroller.Updaters.ZmqUpdater;
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 {
public class SlamView extends SurfaceView implements AbstractUpdater.MapChangedListener<SlamScan> {
private SlamUpdater slam;
private static final String SLAM_TOPIC = "slam_map";
private AbstractUpdater<SlamScan> slam;
private Thread mapThread;
private Context context;
private SurfaceHolder surfaceHolder;
private Paint paint;
private SlamControlGrpc.SlamControlStub stub;
private ManagedChannel channel;
private int mapSizePixels;
private int mapSizeMeters;
private String port;
@@ -60,15 +64,14 @@ public class SlamView extends SurfaceView implements SlamUpdater.MapChangedListe
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 = 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);
channel = ManagedChannelBuilder.forAddress(host, Integer.parseInt(gRPCPort)).usePlaintext().build();
ManagedChannel channel = ManagedChannelBuilder.forAddress(host, Integer.parseInt(gRPCPort)).usePlaintext().build();
stub = SlamControlGrpc.newStub(channel);
}
/**
@@ -106,12 +109,15 @@ public class SlamView extends SurfaceView implements SlamUpdater.MapChangedListe
try {
mapThread.join();
} catch (InterruptedException e) {
}
}
@Override
public void mapChanged(ByteString map, SlamLocation location) {
public void mapChanged(SlamScan scan) {
updateView(scan.getMap(), scan.getLocation());
}
private void updateView(ByteString map, SlamLocation location) {
if (surfaceHolder.getSurface().isValid()) {
Canvas canvas = surfaceHolder.lockCanvas();
canvas.save();

51
app/src/main/java/com/example/carcontroller/Updaters/AbstractUpdater.java Normal file
View File

@@ -0,0 +1,51 @@
package com.example.carcontroller.Updaters;
import com.google.protobuf.InvalidProtocolBufferException;
import com.google.protobuf.MessageLite;
import com.google.protobuf.Parser;
import java.util.HashSet;
import java.util.Set;
/**
* Provides an abstract way to receive updates from a remote host (i.e. the rc car).
* Subclasses of this implement the approriate connection mechanisms to continuously receive
* new updates of messages.
* Specifically, protobuf serialisation support is provided
*
* @param <T> The message type that will continuously be received
*/
public abstract class AbstractUpdater<T extends MessageLite> implements Runnable {
private Set<MapChangedListener<T>> listeners;
private Parser<T> parser;
public AbstractUpdater(Parser<T> parser) {
this.parser = parser;
init();
}
protected void init() {
listeners = new HashSet<>();
}
public void addMapChangedListener(MapChangedListener<T> listener) {
listeners.add(listener);
}
protected void fireMapChanged(T scan) {
listeners.forEach(listener -> listener.mapChanged(scan));
}
public abstract void stop();
public interface MapChangedListener<T extends MessageLite> {
void mapChanged(T points);
}
protected T parseMessage(byte[] messageBytes) throws InvalidProtocolBufferException {
return parser.parseFrom(messageBytes);
}
}

26
app/src/main/java/com/example/carcontroller/SLAM/ZmqSlamUpdater.java → app/src/main/java/com/example/carcontroller/Updaters/ZmqUpdater.java
View File

@@ -1,7 +1,8 @@
package com.example.carcontroller.SLAM;
package com.example.carcontroller.Updaters;
import com.example.carcontroller.SlamScan;
import com.google.protobuf.InvalidProtocolBufferException;
import com.google.protobuf.MessageLite;
import com.google.protobuf.Parser;
import org.zeromq.SocketType;
import org.zeromq.ZContext;
@@ -10,20 +11,21 @@ import org.zeromq.ZMQ;
import java.util.Arrays;
/**
* 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.
* Provides a way to easily deal with zeromq sub sockets that use protobuf
*
* @param <T>
*/
public class ZmqSlamUpdater extends SlamUpdater {
public class ZmqUpdater<T extends MessageLite> extends AbstractUpdater<T> {
private ZContext context;
private String host;
private String port;
private boolean running = false;
private static final byte[] SLAM_SUBSCRIPTION = "slam_map".getBytes();
private final byte[] SUBSCRIPTION;
public ZmqSlamUpdater(String host, String port) {
super();
public ZmqUpdater(Parser<T> parser, String topic, String host, String port) {
super(parser);
this.SUBSCRIPTION = topic.getBytes();
this.host = host;
this.port = port;
init();
@@ -42,12 +44,12 @@ public class ZmqSlamUpdater extends SlamUpdater {
// Receive map from zmq and update appropriately.
try (ZMQ.Socket socket = context.createSocket(SocketType.SUB)) {
socket.connect("tcp://" + host + ":" + port);
socket.subscribe(SLAM_SUBSCRIPTION);
socket.subscribe(SUBSCRIPTION);
while (running) {
byte[] map = socket.recv();
// Don't want to do the event when we just receive the header.
if (!Arrays.equals(map, SLAM_SUBSCRIPTION)) {
fireMapChanged(SlamScan.parseFrom(map));
if (!Arrays.equals(map, SUBSCRIPTION)) {
fireMapChanged(parseMessage(map));
}
}
} catch (InvalidProtocolBufferException e) {

4
app/src/main/res/layout-land/activity_lidar_tracking_controller.xml
View File

@@ -4,9 +4,9 @@
xmlns:tools="http://schemas.android.com/tools"
android:layout_width="match_parent"
android:layout_height="match_parent"
tools:context=".LidarTrackingController">
tools:context=".LIDAR.LidarTrackingController">
<com.example.carcontroller.LidarView
<com.example.carcontroller.LIDAR.LidarView
android:layout_width="match_parent"
android:layout_height="match_parent"
android:id="@+id/lidarMap"/>

2
app/src/main/res/layout/activity_lidar_tracking_controller.xml
View File

@@ -4,7 +4,7 @@
xmlns:tools="http://schemas.android.com/tools"
android:layout_width="match_parent"
android:layout_height="match_parent"
tools:context=".LidarTrackingController">
tools:context=".LIDAR.LidarTrackingController">
<TextView
android:layout_width="wrap_content"