Remove debug stuff, lidar streaming working.

Tracking still not working correctly, due to assign groups not working propery.

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<Integer, Paint> 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);
         }
 
     }

app/src/main/res/xml/root_preferences.xml

@@ -46,6 +46,14 @@
 
     </PreferenceCategory>
 
+    <PreferenceCategory>
+        <EditTextPreference
+            android:defaultValue="0.1"
+            android:title="LiDAR time between scan fetches."
+            app:key="lidar_timeout"
+            app:useSimpleSummaryProvider="true" />
+    </PreferenceCategory>
+
     <PreferenceCategory android:title="0MQ SLAM Connection">
         <EditTextPreference
             android:key="zmqPort"

pycar/src/car/tracking/algorithms.py

@@ -112,10 +112,8 @@ def calc_groups(scan):
     currentGroup = Group(0)
     allGroups = [currentGroup]
     currentGroupNumber = 0
-    num_iters = 0
     # assume the list is already sorted.
     for point in scan:
-        num_iters += 1
         if prevPoint is None:
             prevPoint = point
             currentGroup.add_point(point)
@@ -132,11 +130,6 @@ def calc_groups(scan):
             allGroups.append(currentGroup)
 
         prevPoint = point
-    print(num_iters)
-    print(len(allGroups))
-    for group in allGroups:
-        print(len(group.get_points()))
-
     return allGroups
 
 
@@ -182,7 +175,7 @@ def assign_groups(prev_groups, new_groups):
         for new_group in new_groups:
             new_centre = find_centre(new_group)
             # They are considered the same if the new group and old group centres are within 10cm.
-            if ((new_centre[0] - old_centre[0]) ** 2 + (new_centre[1] - old_centre[1]) ** 2) < 100 ** 2:
+            if ((new_centre[0] - old_centre[0]) ** 2 + (new_centre[1] - old_centre[1]) ** 2) < 50 ** 2:
                 new_group.number = group.number
                 if group.number > max_group_number:
                     max_group_number = group.number

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],

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):