Move packages to org.vato

This commit is contained in:
Piv
2020-03-22 21:04:28 +10:30
parent bd735fd6b0
commit faffc97043
16 changed files with 45 additions and 48 deletions

View File

@@ -0,0 +1,38 @@
package org.vato.carcontroller.LIDAR;
import android.os.Bundle;
import androidx.appcompat.app.AppCompatActivity;
import org.vato.carcontroller.R;
public class LidarTrackingController extends AppCompatActivity {
private LidarView lidarMap;
@Override
protected void onCreate(Bundle savedInstanceState) {
super.onCreate(savedInstanceState);
setContentView(R.layout.activity_lidar_tracking_controller);
lidarMap = findViewById(R.id.lidarMap);
}
@Override
protected void onResume() {
super.onResume();
if (lidarMap != null) {
lidarMap.resume();
}
}
@Override
protected void onPause() {
super.onPause();
if (lidarMap != null) {
lidarMap.stop();
}
}
}

View File

@@ -0,0 +1,162 @@
package org.vato.carcontroller.LIDAR;
import android.content.Context;
import android.content.SharedPreferences;
import android.graphics.Bitmap;
import android.graphics.Canvas;
import android.graphics.Color;
import android.graphics.Paint;
import android.util.AttributeSet;
import android.view.MotionEvent;
import android.view.SurfaceHolder;
import android.view.SurfaceView;
import androidx.preference.PreferenceManager;
import org.vato.carcontroller.Empty;
import org.vato.carcontroller.Int32Value;
import org.vato.carcontroller.PersonTrackingGrpc;
import org.vato.carcontroller.PointScan;
import org.vato.carcontroller.Updaters.AbstractUpdater;
import org.vato.carcontroller.Updaters.ZmqUpdater;
import java.util.stream.Collectors;
import io.grpc.ManagedChannel;
import io.grpc.ManagedChannelBuilder;
import io.grpc.stub.StreamObserver;
public class LidarView extends SurfaceView implements AbstractUpdater.MapChangedListener<PointScan> {
private static final String LIDAR_TOPIC = "lidar_map";
private AbstractUpdater<PointScan> lidar;
private Thread lidarThread;
private String port;
private SurfaceHolder surfaceHolder;
PersonTrackingGrpc.PersonTrackingStub stub;
private int mBitmapX, mBitmapY, mViewWidth, mViewHeight;
private Bitmap mBitmap;
public LidarView(Context context) {
super(context);
init();
}
public LidarView(Context context, AttributeSet attrs) {
super(context, attrs);
init();
}
public LidarView(Context context, AttributeSet attrs, int defStyleAttr) {
super(context, attrs, defStyleAttr);
init();
}
private void init() {
SharedPreferences prefs = PreferenceManager.getDefaultSharedPreferences(getContext());
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.addMapChangedListener(this);
surfaceHolder = getHolder();
lidarThread = new Thread(lidar);
ManagedChannel channel = ManagedChannelBuilder.forAddress(host, Integer.parseInt(gRPCPort)).usePlaintext().build();
stub = PersonTrackingGrpc.newStub(channel);
}
/**
* Called by MainActivity.onResume() to start a thread.
*/
public void resume() {
StreamObserver<Empty> response = new StreamObserver<Empty>() {
@Override
public void onNext(Empty value) {
lidarThread.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.startTracking(Int32Value.newBuilder().setValue(Integer.parseInt(port)).build(), response);
}
@Override
protected void onSizeChanged(int w, int h, int oldw, int oldh) {
super.onSizeChanged(w, h, oldw, oldh);
}
@Override
public boolean onTouchEvent(MotionEvent event) {
float x = event.getX();
float y = event.getY();
// TODO: Get the group that was selected and select on the grpc controller.
switch (event.getAction()) {
case MotionEvent.ACTION_DOWN:
break;
}
return true;
}
public void stop() {
// TODO: Use grpc to tell zmq to stop.
lidar.stop();
try {
lidarThread.join(1000);
} catch (InterruptedException e) {
}
}
@Override
public void mapChanged(PointScan points) {
if (surfaceHolder.getSurface().isValid()) {
Canvas canvas = surfaceHolder.lockCanvas();
canvas.save();
canvas.drawColor(Color.WHITE);
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.
canvas.drawCircle((float) point.x, (float) point.y, 5, new Paint());
}
canvas.restore();
surfaceHolder.unlockCanvasAndPost(canvas);
}
}
private static class Point {
private double x;
private double y;
private Point(double x, double y) {
this.x = x;
this.y = y;
}
static Point fromProtoPoint(org.vato.carcontroller.Point point) {
return fromHist(point.getDistance(), point.getAngle());
}
static Point fromHist(double distance, double angle) {
return fromHist(distance, angle, new Point(0, 0));
}
static Point fromHist(double distance, double angle, Point offset) {
return new Point(distance * Math.sin(angle) + offset.x, distance * Math.cos(angle) + offset.y);
}
}
}

View File

@@ -0,0 +1,64 @@
package org.vato.carcontroller;
import android.content.Intent;
import android.os.Bundle;
import android.view.Menu;
import android.view.MenuItem;
import android.view.View;
import androidx.appcompat.app.AppCompatActivity;
import androidx.appcompat.widget.Toolbar;
import org.vato.carcontroller.LIDAR.LidarTrackingController;
import org.vato.carcontroller.SLAM.SlamController;
public class MainActivity extends AppCompatActivity {
@Override
protected void onCreate(Bundle savedInstanceState) {
super.onCreate(savedInstanceState);
setContentView(R.layout.activity_main);
Toolbar toolbar = findViewById(R.id.toolbar);
setSupportActionBar(toolbar);
}
@Override
public boolean onCreateOptionsMenu(Menu menu) {
getMenuInflater().inflate(R.menu.menu_main, menu);
return true;
}
@Override
public boolean onOptionsItemSelected(MenuItem item) {
int id = item.getItemId();
if (id == R.id.action_settings) {
Intent intent = new Intent(this, SettingsActivity.class);
startActivity(intent);
return true;
}
return super.onOptionsItemSelected(item);
}
public void loadSimpleController(View view) {
Intent intent = new Intent(getApplicationContext(), SimpleController.class);
startActivity(intent);
}
public void loadLidarController(View view) {
Intent intent = new Intent(getApplicationContext(), LidarTrackingController.class);
startActivity(intent);
}
public void loadCameraController(View view) {
}
public void loadSlamController(View view) {
Intent intent = new Intent(getApplicationContext(), SlamController.class);
startActivity(intent);
}
}

View File

@@ -0,0 +1,71 @@
package org.vato.carcontroller;
import java.util.concurrent.TimeUnit;
import java.util.concurrent.atomic.AtomicBoolean;
import io.grpc.ManagedChannel;
import io.grpc.ManagedChannelBuilder;
public class PiLoader implements Runnable {
Integer steeringValue = 50;
Integer throttleValue = 50;
private ManagedChannel mChannel;
private CarControlGrpc.CarControlBlockingStub stub;
private AtomicBoolean stop = new AtomicBoolean(false);
Thread piUpdaterThread;
public PiLoader(String host, Integer port) {
mChannel = ManagedChannelBuilder.forAddress(host, port).usePlaintext().build();
// Create an async stub.
stub = CarControlGrpc.newBlockingStub(mChannel);
}
private void createAndStart() {
piUpdaterThread = new Thread(this);
piUpdaterThread.start();
}
public void updateSteering(int value) {
steeringValue = value;
}
public void updateThrottle(int value) {
throttleValue = value;
}
public void stop() {
stop.lazySet(true);
if (!stop.get()) {
piUpdaterThread.interrupt();
}
}
public void start() {
stop.lazySet(false);
createAndStart();
}
@Override
public void run() {
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());
} catch (Exception e) {
System.out.println("Error");
stop();
}
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.
}
}
}
}

View File

@@ -0,0 +1,102 @@
package org.vato.carcontroller.SLAM;
import android.content.SharedPreferences;
import android.os.Bundle;
import android.widget.SeekBar;
import androidx.appcompat.app.AppCompatActivity;
import androidx.preference.PreferenceManager;
import org.vato.carcontroller.PiLoader;
import org.vato.carcontroller.R;
public class SlamController extends AppCompatActivity implements SeekBar.OnSeekBarChangeListener {
private SeekBar steeringSlider;
private SeekBar throttleSlider;
private static PiLoader grpcController;
private SlamView slamView;
@Override
protected void onCreate(Bundle savedInstanceState) {
super.onCreate(savedInstanceState);
setContentView(R.layout.activity_slam_controller);
steeringSlider = findViewById(R.id.steeringBar);
if (steeringSlider != null) {
steeringSlider.setOnSeekBarChangeListener(this);
}
throttleSlider = findViewById(R.id.throttleBar);
if (throttleSlider != null) {
throttleSlider.setOnSeekBarChangeListener(this);
}
slamView = findViewById(R.id.slamView);
}
@Override
public void onProgressChanged(SeekBar seekBar, int i, boolean b) {
// Call the gRPC method to change the throttle.
switch (seekBar.getId()) {
case R.id.steeringBar:
if (grpcController != null) {
grpcController.updateSteering(i);
}
break;
case R.id.throttleBar:
if (grpcController != null) {
grpcController.updateThrottle(i);
}
break;
}
}
@Override
public void onStartTrackingTouch(SeekBar seekBar) {
}
@Override
public void onStopTrackingTouch(SeekBar seekBar) {
// Reset back to the middle, as this is how the controller will normally work.
// Check if this will actually still call the onProgressChanged listener method.
// Otherwise update the steering/throttle here as well.
steeringSlider.setProgress(50);
throttleSlider.setProgress(50);
}
@Override
protected void onResume() {
super.onResume();
SharedPreferences prefs = PreferenceManager.getDefaultSharedPreferences(getApplicationContext());
if (grpcController == null) {
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();
if (slamView != null) {
slamView.resume();
}
}
@Override
protected void onPause() {
super.onPause();
grpcController.stop();
if (slamView != null) {
slamView.stop();
}
}
@Override
protected void onStop() {
super.onStop();
grpcController.stop();
if (slamView != null) {
slamView.stop();
}
}
}

View File

@@ -0,0 +1,136 @@
package org.vato.carcontroller.SLAM;
import android.content.Context;
import android.content.SharedPreferences;
import android.graphics.Bitmap;
import android.graphics.Canvas;
import android.graphics.Color;
import android.graphics.Paint;
import android.util.AttributeSet;
import android.view.SurfaceHolder;
import android.view.SurfaceView;
import androidx.preference.PreferenceManager;
import com.google.protobuf.ByteString;
import org.vato.carcontroller.Empty;
import org.vato.carcontroller.SlamControlGrpc;
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.ZmqUpdater;
import io.grpc.ManagedChannel;
import io.grpc.ManagedChannelBuilder;
import io.grpc.stub.StreamObserver;
public class SlamView extends SurfaceView implements AbstractUpdater.MapChangedListener<SlamScan> {
private static final String SLAM_TOPIC = "slam_map";
private AbstractUpdater<SlamScan> slam;
private Thread mapThread;
private SurfaceHolder surfaceHolder;
private Paint paint;
private SlamControlGrpc.SlamControlStub stub;
private int mapSizePixels;
private int mapSizeMeters;
private String port;
public SlamView(Context context) {
super(context);
init();
}
public SlamView(Context context, AttributeSet attrs) {
super(context, attrs);
init();
}
public SlamView(Context context, AttributeSet attrs, int defStyleAttr) {
super(context, attrs, defStyleAttr);
init();
}
private void init() {
SharedPreferences prefs = PreferenceManager.getDefaultSharedPreferences(getContext());
String host = prefs.getString("host", "10.0.0.53");
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 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();
stub = SlamControlGrpc.newStub(channel);
}
/**
* Called by MainActivity.onResume() to start a thread.
*/
public void resume() {
StreamObserver<Empty> response = new StreamObserver<Empty>() {
@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)
.setPort(Integer.parseInt(port))
.build(), response);
}
public void stop() {
// TODO: Use grpc to tell zmq to stop.
slam.stop();
try {
mapThread.join(1000);
} catch (InterruptedException e) {
}
}
@Override
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();
canvas.drawColor(Color.WHITE);
// Using width as we want square.
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));
}
}
canvas.drawBitmap(bitmap, 0, 0, paint);
canvas.restore();
surfaceHolder.unlockCanvasAndPost(canvas);
}
}
}

View File

@@ -0,0 +1,34 @@
package org.vato.carcontroller;
import android.os.Bundle;
import androidx.appcompat.app.ActionBar;
import androidx.appcompat.app.AppCompatActivity;
import androidx.preference.PreferenceFragmentCompat;
public class SettingsActivity extends AppCompatActivity {
public final static String HOST = "host";
public final static String PORT = "port";
@Override
protected void onCreate(Bundle savedInstanceState) {
super.onCreate(savedInstanceState);
setContentView(R.layout.settings_activity);
getSupportFragmentManager()
.beginTransaction()
.replace(R.id.settings, new SettingsFragment())
.commit();
ActionBar actionBar = getSupportActionBar();
if (actionBar != null) {
actionBar.setDisplayHomeAsUpEnabled(true);
}
}
public static class SettingsFragment extends PreferenceFragmentCompat {
@Override
public void onCreatePreferences(Bundle savedInstanceState, String rootKey) {
setPreferencesFromResource(R.xml.root_preferences, rootKey);
}
}
}

View File

@@ -0,0 +1,86 @@
package org.vato.carcontroller;
import android.content.SharedPreferences;
import android.os.Bundle;
import android.widget.SeekBar;
import androidx.appcompat.app.AppCompatActivity;
import androidx.preference.PreferenceManager;
public class SimpleController extends AppCompatActivity implements SeekBar.OnSeekBarChangeListener {
SeekBar steeringSlider;
SeekBar throttleSlider;
private static PiLoader grpcController;
@Override
protected void onCreate(Bundle savedInstanceState) {
super.onCreate(savedInstanceState);
setContentView(R.layout.activity_simple_controller);
steeringSlider = findViewById(R.id.steeringBar);
if (steeringSlider != null) {
steeringSlider.setOnSeekBarChangeListener(this);
}
throttleSlider = findViewById(R.id.throttleBar);
if (throttleSlider != null) {
throttleSlider.setOnSeekBarChangeListener(this);
}
}
@Override
protected void onResume() {
super.onResume();
SharedPreferences prefs = PreferenceManager.getDefaultSharedPreferences(getApplicationContext());
if (grpcController == null) {
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.
grpcController.start();
}
@Override
protected void onPause() {
super.onPause();
grpcController.stop();
}
@Override
protected void onStop() {
super.onStop();
grpcController.stop();
}
@Override
public void onProgressChanged(SeekBar seekBar, int i, boolean b) {
// Call the gRPC method to change the throttle.
switch (seekBar.getId()) {
case R.id.steeringBar:
if (grpcController != null) {
grpcController.updateSteering(i);
}
break;
case R.id.throttleBar:
if (grpcController != null) {
grpcController.updateThrottle(i);
}
break;
}
}
@Override
public void onStartTrackingTouch(SeekBar seekBar) {
}
@Override
public void onStopTrackingTouch(SeekBar seekBar) {
// Reset back to the middle, as this is how the controller will normally work.
// Check if this will actually still call the onProgressChanged listener method.
// Otherwise update the steering/throttle here as well.
steeringSlider.setProgress(50);
throttleSlider.setProgress(50);
}
}

View File

@@ -0,0 +1,51 @@
package org.vato.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);
}
}

View File

@@ -0,0 +1,65 @@
package org.vato.carcontroller.Updaters;
import com.google.protobuf.InvalidProtocolBufferException;
import com.google.protobuf.MessageLite;
import com.google.protobuf.Parser;
import org.zeromq.SocketType;
import org.zeromq.ZContext;
import org.zeromq.ZMQ;
import java.util.Arrays;
/**
* Provides a way to easily deal with zeromq sub sockets that use protobuf
*
* @param <T>
*/
public class ZmqUpdater<T extends MessageLite> extends AbstractUpdater<T> {
private ZContext context;
private String host;
private String port;
private boolean running = false;
private final byte[] SUBSCRIPTION;
public ZmqUpdater(Parser<T> parser, String topic, String host, String port) {
super(parser);
this.SUBSCRIPTION = topic.getBytes();
this.host = host;
this.port = port;
init();
}
@Override
protected void init() {
super.init();
context = new ZContext();
}
@Override
public void run() {
running = true;
// Receive map from zmq and update appropriately.
try (ZMQ.Socket socket = context.createSocket(SocketType.SUB)) {
socket.connect("tcp://" + host + ":" + port);
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, SUBSCRIPTION)) {
fireMapChanged(parseMessage(map));
}
}
} catch (InvalidProtocolBufferException e) {
System.out.println("Invalid map found");
running = false;
}
}
public void stop() {
running = false;
}
}