diff --git a/app/src/main/java/org/vato/carcontroller/LIDAR/LidarView.java b/app/src/main/java/org/vato/carcontroller/LIDAR/LidarView.java index 6da157a..0d9c545 100644 --- a/app/src/main/java/org/vato/carcontroller/LIDAR/LidarView.java +++ b/app/src/main/java/org/vato/carcontroller/LIDAR/LidarView.java @@ -2,7 +2,6 @@ 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; @@ -44,11 +43,10 @@ public class LidarView extends SurfaceView private SurfaceHolder surfaceHolder; private boolean useGrpcStreams; PersonTrackingGrpc.PersonTrackingStub stub; - private float timeBetweenMessages = 0.01f; + private float timeBetweenMessages; private Map groupNumPaints = new HashMap<>(); - private int mBitmapX, mBitmapY, mViewWidth, mViewHeight; - private Bitmap mBitmap; + private int mViewWidth, mViewHeight, centreX, centreY; public LidarView(Context context) { super(context); @@ -71,6 +69,7 @@ public class LidarView extends SurfaceView port = prefs.getString("zmqPort", "5050"); String gRPCPort = prefs.getString("port", "50051"); useGrpcStreams = prefs.getBoolean("use_grpc_streams", false); + timeBetweenMessages = prefs.getFloat("lidar_timeout", 0.1f); if (useGrpcStreams) { lidar = new GrpcUpdater<>(PointScan.getDefaultInstance().getParserForType(), this); @@ -121,6 +120,10 @@ public class LidarView extends SurfaceView @Override protected void onSizeChanged(int w, int h, int oldw, int oldh) { super.onSizeChanged(w, h, oldw, oldh); + mViewWidth = w; + mViewHeight = h; + centreX = w / 2; + centreY = h / 2; } @Override @@ -167,8 +170,23 @@ public class LidarView extends SurfaceView Canvas canvas = surfaceHolder.lockCanvas(); canvas.save(); canvas.drawColor(Color.WHITE); - for (Point point : points.getPointsList().stream().map(Point::fromProtoPoint).collect( - Collectors.toList())) { + // TODO: Do an initial pass to find the max distance, which will be a scale factor for the other points. + double maxDistance = 0; + for (org.vato.carcontroller.Point point : points.getPointsList()) { + if (point.getDistance() > maxDistance) { + maxDistance = point.getDistance(); + } + } + + final double maxDistanceFinal = maxDistance; + + // Apply scaling factor from max distance. + for (Point point : points.getPointsList().stream().map( + point -> org.vato.carcontroller.Point.newBuilder(point).setDistance( + point.getDistance() / maxDistanceFinal * mViewHeight).build()).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. if (!groupNumPaints.containsKey(point.groupNumber)) { @@ -178,9 +196,8 @@ public class LidarView extends SurfaceView point.groupNumber), 1f, 1f})); groupNumPaints.put(point.groupNumber, paint); } - - // TODO: - canvas.drawCircle((float) point.x, (float) point.y, 5, + canvas.drawCircle((float) point.x + centreX, + (float) point.y + centreY, 5, Objects.requireNonNull(groupNumPaints .get(point.groupNumber))); // Can't be null as we just added it. } @@ -189,6 +206,7 @@ public class LidarView extends SurfaceView } } + /** * @param groupNumber * @return @@ -225,8 +243,8 @@ public class LidarView extends SurfaceView } 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(Math.toRadians(angle)) + offset.x, + distance * Math.cos(Math.toRadians(angle)) + offset.y); } } diff --git a/app/src/main/res/xml/root_preferences.xml b/app/src/main/res/xml/root_preferences.xml index 5b0612d..c72974a 100644 --- a/app/src/main/res/xml/root_preferences.xml +++ b/app/src/main/res/xml/root_preferences.xml @@ -46,6 +46,14 @@ + + + + max_group_number: max_group_number = group.number diff --git a/pycar/src/car/tracking/animate_alg.py b/pycar/src/car/tracking/animate_alg.py index e076542..44271c1 100644 --- a/pycar/src/car/tracking/animate_alg.py +++ b/pycar/src/car/tracking/animate_alg.py @@ -2,7 +2,7 @@ Animates distances and angle of lidar Uses model-free algorithms to track grouping of points (objects/groups) """ -from tracking.mock_lidar import MockLidar +from car.tracking.devices.mock_lidar import MockLidar import matplotlib.pyplot as plt import numpy as np import matplotlib.animation as animation @@ -35,7 +35,7 @@ class Bunch: def run(): - lidar = MockLidar(loader.load_scans_bytes_file("tracking/out.pickle")) + lidar = MockLidar(loader.load_scans_bytes_file("pycar/src/car/tracking/out.pickle")) fig = plt.figure() ax = plt.subplot(111, projection='polar') line = ax.scatter([0, 0], [0, 0], s=5, c=[IMIN, IMAX], diff --git a/pycar/src/car/tracking/lidar_cache.py b/pycar/src/car/tracking/lidar_cache.py index 102f3fb..673ea66 100644 --- a/pycar/src/car/tracking/lidar_cache.py +++ b/pycar/src/car/tracking/lidar_cache.py @@ -4,8 +4,6 @@ import car.tracking.lidar_tracker_pb2 as tracker_pb import zmq from car.tracking.devices.mock_lidar import MockLidar import car.tracking.lidar_loader as lidar_loader -import time -import timeit class LidarCache(): @@ -50,7 +48,6 @@ class LidarCache(): if not self.run: break - start_time = time.time() # Now process the groups. if self.currentGroups is not None: self.currentGroups = algorithms.assign_groups( @@ -58,7 +55,6 @@ class LidarCache(): else: self.currentGroups = algorithms.calc_groups(scan) - print("total time: " + (str)(time.time() - start_time)) self._fireGroupsChanged() def _fireGroupsChanged(self):