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

Format files and add gRPC stream support.

Browse Source
This commit is contained in:
Piv
2020-05-03 16:10:40 +09:30
parent ff73d09855
commit 7fb20f9d86
14 changed files with 346 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
app/src/main/java/org/vato/carcontroller/LIDAR/LidarView.java
View File

@@ -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<PointScan> {
public class LidarView extends SurfaceView
implements AbstractUpdater.MapChangedListener<PointScan> {
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);
}
}

82
app/src/main/java/org/vato/carcontroller/PiLoader.java
View File

@@ -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<Empty> responseObserver = new StreamObserver<Empty>() {
@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<Vehicle2DRequest> 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());

7
app/src/main/java/org/vato/carcontroller/SLAM/SlamController.java
View File

@@ -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.

55
app/src/main/java/org/vato/carcontroller/SLAM/SlamView.java
View File

@@ -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<SlamScan> {
public class SlamView extends SurfaceView implements AbstractUpdater.MapChangedListener<SlamScan>,
GrpcUpdater.GrpcUpdateBootstrapper<SlamScan> {
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 <T,U>, 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<Empty> response = new StreamObserver<Empty>() {
@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<SlamScan> responseObserver) {
stub.mapStream(SlamDetails.newBuilder()
.setMapSizePixels(
mapSizePixels)
.setMapSizeMeters(
mapSizeMeters)
.build(), responseObserver);
}
}

17
app/src/main/java/org/vato/carcontroller/SimpleController.java
View File

@@ -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<Empty> response = new StreamObserver<Empty>() {
@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

56
app/src/main/java/org/vato/carcontroller/Updaters/GrpcUpdater.java Normal file
View File

@@ -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<T extends MessageLite> extends AbstractUpdater<T> {
GrpcUpdateBootstrapper<T> bootstrapper;
public GrpcUpdater(Parser parser, GrpcUpdateBootstrapper bootstrapper) {
super(parser);
this.bootstrapper = bootstrapper;
}
@Override
public void stop() {
}
@Override
public void run() {
StreamObserver<T> responseObserver = new StreamObserver<T>() {
@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<T extends MessageLite> {
/**
* A responseObserver is passed into this method so that it can be called/initialised by
* the client.
*
* @param responseObserver
*/
void bootstrap(StreamObserver<T> responseObserver);
}
}

4
app/src/main/res/xml/root_preferences.xml
View File

@@ -28,6 +28,10 @@
android:title="Port number"
app:key="port"
app:useSimpleSummaryProvider="true" />
<SwitchPreference
android:defaultValue="false"
android:key="use_grpc_streams"
android:title="Use gRPC Streams Where Possible" />
</PreferenceCategory>
<PreferenceCategory android:title="Car Calibration">