Add lidar tracking

persontracking/lidar_cache.py

@@ -0,0 +1,46 @@
+import rplidar
+from rplidar import RPLidar
+from threading import Thread
+import algorithms
+
+class LidarCache():
+    '''
+    A class that retrieves scans from the lidar,
+    runs grouping algorithms between scans and 
+    keeps a copy of the group data.
+    '''
+    run = True
+    tracking_group_number = -1
+    currentGroups = None
+    groupsChanged = []
+    
+    def __init__(self, measurements=100):
+        self.lidar = RPLidar('/dev/ttyUSB0')
+        self.measurements = measurements
+        print('Info: ' + self.lidar.get_info())
+        print('Health: ' + self.lidar.get_health())
+
+    def start_cache(self):
+        self.thread = Thread(target=self.do_scanning)
+        self.thread.start()
+
+    def do_scanning(self):
+        '''
+        Performs a scan for the given number of iterations.
+        '''
+        for i, scan in enumerate(self.lidar.iter_scans(min_len=self.measurements)):
+            print('%d: Got %d measurments' % (i, len(scan)))
+            if(not self.run):
+                break
+
+            # Now process the groups. 
+            if self.currentGroups is not None:
+                self.currentGroups = algorithms.assign_groups(self.currentGroups, algorithms.calc_groups(scan))
+            else:
+                self.currentGroups = algorithms.calc_groups(scan)
+
+    def fireGroupsChanged(self):
+        pass
+
+    def stop_scanning(self):
+        self.run = False

persontracking/lidar_servicer.py

@@ -0,0 +1,25 @@
+import lidar_tracker_pb2
+import lidar_tracker_pb2_grpc
+from .lidar_cache import LidarCache
+class LidarServicer(lidar_tracker_pb2_grpc.PersonTrackingServicer):
+
+    def __init__(self):
+        self.cache = LidarCache(measurements=100)
+        self.cache.do_scanning()
+
+    def set_tracking_group(self, request, context):
+        pass
+
+    def stop_tracking(self, request, context):
+        self.cache.stop_scanning()
+
+    def get_scan_data(self, request, context):
+        pass
+
+    def start_tracking(self, request, context):
+        '''
+        Starts the lidar cache. 
+        '''
+        pass
+
+

persontracking/protos/lidar_tracker.proto

@@ -30,4 +30,6 @@ service PersonTracking{
     rpc stop_tracking(Empty) returns (Empty) {}
 
     rpc get_scan_data(Empty) returns (PointScan) {}
+
+    rpc start_tracking(Empty) returns (Empty) {}
 }