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