48 lines
1.3 KiB
Python
48 lines
1.3 KiB
Python
"""
|
|
Animates distances and angle of lidar
|
|
Uses model-free algorithms to track grouping of points (objects/groups)
|
|
"""
|
|
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
|
|
import tracking.algorithms as alg
|
|
|
|
|
|
PORT_NAME = '/dev/ttyUSB0'
|
|
DMAX = 4000
|
|
IMIN = 0
|
|
IMAX = 50
|
|
|
|
|
|
def update_line(num, iterator, line):
|
|
scan = next(iterator)
|
|
# Now update the groups, and then update the maps with different colours for different groups.
|
|
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()
|