61 lines
2.1 KiB
Python
61 lines
2.1 KiB
Python
from threading import Thread
|
|
from tracking import algorithms
|
|
import tracking.lidar_tracker_pb2 as tracker_pb
|
|
import zmq
|
|
import Messaging.messages as messages
|
|
|
|
|
|
class LidarCache():
|
|
"""
|
|
A class that retrieves scans from the lidar,
|
|
runs grouping algorithms between scans and
|
|
keeps a copy of the group data.
|
|
"""
|
|
|
|
def __init__(self, lidar, measurements=100):
|
|
self.lidar = lidar
|
|
self.measurements = measurements
|
|
print('Info: ' + self.lidar.get_info())
|
|
print('Health: ' + self.lidar.get_health())
|
|
self.run = True
|
|
self.tracking_group_number = -1
|
|
self.currentGroups = None
|
|
self.groupsChanged = []
|
|
|
|
def start_cache(self, sender):
|
|
self.thread = Thread(target=self.do_scanning, args=[sender])
|
|
self.thread.start()
|
|
|
|
def do_scanning(self, sender):
|
|
"""Performs scans whilst cache is running, and will pass calculated groups data to the sender.
|
|
|
|
Parameters
|
|
----------
|
|
sender:
|
|
Any class given in messaging.message_factory. This acts as a listener.
|
|
|
|
"""
|
|
# Create the 0MQ socket first. This should not be passed between threads.
|
|
self._mFactory = sender
|
|
|
|
for scan in self.lidar.iter_scans(min_len=self.measurements):
|
|
print('Got %d measurments' % (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):
|
|
# Send the updated groups to 0MQ socket.
|
|
# Rename this to be a generic listener method, rather than an explicit 'send' (even though it can be treated as such already)
|
|
self._mFactory.send_message_topic("lidar_map", messages.ProtoMessage(
|
|
message=tracker_pb.PointScan(points=[]).SerializeToString()))
|
|
|
|
def stop_scanning(self):
|
|
self.run = False
|