From 136c96a92647f3e88a7e5c89f2c4873b705a654c Mon Sep 17 00:00:00 2001
From: Piv <18462828+Piv200@users.noreply.github.com>
Date: Wed, 26 Feb 2020 21:58:42 +1030
Subject: [PATCH] Start adding alternate message streamers. Make algorithms
 less bad.

---
 Messaging/message_factory.py  | 46 +++++++++++++++++++++++++++++++++++
 persontracking/algorithms.py  | 32 +++++++++++++++---------
 persontracking/lidar_cache.py |  4 +--
 3 files changed, 69 insertions(+), 13 deletions(-)
 create mode 100644 Messaging/message_factory.py

diff --git a/Messaging/message_factory.py b/Messaging/message_factory.py
new file mode 100644
index 0000000..a509e70
--- /dev/null
+++ b/Messaging/message_factory.py
@@ -0,0 +1,46 @@
+import zmq
+
+
+class MessageFactory():
+
+    def getZmqPubSubStreamer(self, topic):
+        '''
+        Not thread-safe. Always get this inside the thread/process where you intend
+        to use it. 
+        '''
+        return ZmqPubSubStreamer(topic)
+
+
+class Streamer():
+    def send_message(self, message_bytes):
+        raise NotImplementedError
+
+    def send_message_topic(self, topic, message_bytes):
+        raise NotImplementedError
+
+
+class ZmqPubSubStreamer(Streamer):
+    '''
+    Not thread-safe. Always get this inside the thread/process where you intend
+    to use it. 
+    '''
+
+    def __init__(self, port):
+        # Should create the socket here always, since zmq is not thread safe.
+        # Hopefully whoever uses this is not stupid enough to create it then
+        # pass it into a thread.
+        pass
+
+    def send_message(self, message_bytes):
+        pass
+
+    def send_message_topic(self, topic, message_bytes):
+        pass
+
+
+class BluetoothStreamer(Streamer):
+    def __init__(self):
+        pass
+
+    def send_message(self, message_bytes):
+        pass
diff --git a/persontracking/algorithms.py b/persontracking/algorithms.py
index 8c1461a..884ade8 100644
--- a/persontracking/algorithms.py
+++ b/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
\ No newline at end of file
+
+    pass
diff --git a/persontracking/lidar_cache.py b/persontracking/lidar_cache.py
index 4b25e90..2a87b50 100644
--- a/persontracking/lidar_cache.py
+++ b/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