Start adding alternate message streamers. Make algorithms less bad.

persontracking/algorithms.py

@@ -1,7 +1,13 @@
 import math
 
+
 class Group:
 
+    _minX = None
+    _maxX = None
+    _minY = None
+    _maxY = None
+
     def __init__(self, number, points=[]):
         self._points = points
 
@@ -27,23 +33,22 @@ class Group:
         '''
         converted_point = self.convert_lidar_to_cartesian(new_point)
 
-        if not hasattr(self, '_minX') or self._minX > converted_point[0]:
+        if self._minX is None or self._minX > converted_point[0]:
             self._minX = converted_point[0]
 
-        if not hasattr(self, '_maxX') or self._maxX < converted_point[0]:
+        if self._maxX is None or self._maxX < converted_point[0]:
             self._maxX = converted_point[0]
 
-        if not hasattr(self, '_miny') or self._miny > converted_point[1]:
+        if self._minY is None or self._minY > converted_point[1]:
             self._minY = converted_point[1]
 
-        if not hasattr(self, '_maxY') or self._maxY < converted_point[1]:
-            self._maxY = converted_point[0]
-        
+        if self._maxY is None or self._maxY < converted_point[1]:
+            self._maxY = converted_point[1]
 
     def convert_lidar_to_cartesian(self, new_point):
         x = new_point[2] * math.sin(new_point[1])
         y = new_point[2] * math.cos(new_point[1])
-        return (x,y)
+        return (x, y)
 
     def get_minX(self):
         return self._minY
@@ -57,6 +62,7 @@ class Group:
     def get_maxY(self):
         return self._maxY
 
+
 def calc_groups(scan):
     '''
     Calculates groups of points from a lidar scan. The scan should
@@ -74,8 +80,9 @@ def calc_groups(scan):
             prevPoint = point
             continue
 
-        # Distances are in mm. 
-        if (point[2] - prevPoint[2]) ** 2 < 10 ** 2: # within 1cm makes a group. Will need to play around with this. 
+        # Distances are in mm.
+        # within 1cm makes a group. Will need to play around with this.
+        if (point[2] - prevPoint[2]) ** 2 < 10 ** 2:
             if currentGroup is None:
                 currentGroup = Group(currentGroupNumber)
                 allGroups.append(currentGroup)
@@ -89,9 +96,11 @@ def calc_groups(scan):
 
     return allGroups
 
+
 def find_centre(group):
     return ((group.get_maxX() + group.get_minX()) / 2, (group.get_maxY() + group.get_minY()) / 2)
 
+
 def assign_groups(prev_groups, new_groups):
     '''
     Assigns group numbers to a new scan based on the groups of an old scan. 
@@ -106,10 +115,11 @@ def assign_groups(prev_groups, new_groups):
 
     return new_groups
 
+
 def updateCarVelocity(oldGroup, newGroup):
     '''
     Return a tuple (throttleChange, steeringChange) that should be
     applied given the change in the centre of the groups. 
     '''
-    
-    pass
+
+    pass

persontracking/lidar_cache.py

@@ -2,6 +2,7 @@ import rplidar
 from rplidar import RPLidar
 from threading import Thread
 from persontracking import algorithms
+import persontracking.lidar_tracker_pb2 as tracker_pb
 import zmq
 
 
@@ -52,8 +53,7 @@ class LidarCache():
 
     def fireGroupsChanged(self):
         # Send the updated groups to 0MQ socket.
-        # self._socket.send_multipart(["lidar_map", ...])
-        pass
+        self._socket.send_multipart(["lidar_map", tracker_pb.PointScan(points=[]).SerializeToString()])
 
     def stop_scanning(self):
         self.run = False