From 40e3d283bfa45113667fcfacbd433d8ffc67bebd Mon Sep 17 00:00:00 2001
From: Piv <18462828+Piv200@users.noreply.github.com>
Date: Wed, 11 Mar 2020 22:26:42 +1030
Subject: [PATCH] Add lidar animate test, fix mock iterable.

---
 tracking/animate.py    | 43 ++++++++++++++++++++++++++++++++++++++++++
 tracking/mock_lidar.py |  9 ++++++++-
 2 files changed, 51 insertions(+), 1 deletion(-)
 create mode 100755 tracking/animate.py

diff --git a/tracking/animate.py b/tracking/animate.py
new file mode 100755
index 0000000..451ec7f
--- /dev/null
+++ b/tracking/animate.py
@@ -0,0 +1,43 @@
+#!/usr/bin/env python3
+'''Animates distances and measurment quality'''
+from tracking.mock_lidar import MockLidar
+import matplotlib.pyplot as plt
+import numpy as np
+import matplotlib.animation as animation
+import tracking.lidar_loader as loader
+
+
+PORT_NAME = '/dev/ttyUSB0'
+DMAX = 4000
+IMIN = 0
+IMAX = 50
+
+
+def update_line(num, iterator, line):
+    scan = next(iterator)
+    offsets = np.array([(np.radians(meas[1]), meas[2]) for meas in scan])
+    line.set_offsets(offsets)
+    intens = np.array([meas[0] for meas in scan])
+    line.set_array(intens)
+    return line,
+
+
+def run():
+    lidar = MockLidar(loader.load_scans_bytes_file("tracking/out.pickle"))
+    fig = plt.figure()
+    ax = plt.subplot(111, projection='polar')
+    line = ax.scatter([0, 0], [0, 0], s=5, c=[IMIN, IMAX],
+                      cmap=plt.cm.Greys_r, lw=0)
+    ax.set_rmax(DMAX)
+    ax.grid(True)
+
+    iterator = lidar.iter_scans()
+    ani = animation.FuncAnimation(fig, update_line,
+                                  fargs=(iterator, line), interval=50)
+    plt.show()
+    lidar.stop()
+    lidar.disconnect()
+
+
+if __name__ == '__main__':
+    run()
diff --git a/tracking/mock_lidar.py b/tracking/mock_lidar.py
index ef4e735..f5ccea6 100644
--- a/tracking/mock_lidar.py
+++ b/tracking/mock_lidar.py
@@ -6,6 +6,7 @@ in the lidar_cache for testing (or anywhere else the rplidar may be used)
 
 import tracking.lidar_loader as loader
 
+
 class MockLidar:
 
     def __init__(self, scan_iter=None):
@@ -27,10 +28,16 @@ class MockLidar:
         self._iter = scan_iter
 
     def iter_scans(self, min_len=100):
-        return self._iter
+        return iter(self._iter)
 
     def get_health(self):
         return "Mock Lidar has scans" if self._iter is not None else "Mock lidar won't work properly!"
 
     def get_info(self):
         return self.get_health()
+
+    def stop(self):
+        pass
+
+    def disconnect(self):
+        pass