34 lines
1.3 KiB
Python
34 lines
1.3 KiB
Python
import tracking.lidar_tracker_pb2 as lidar_tracker_pb2
|
|
from tracking.lidar_tracker_pb2_grpc import PersonTrackingServicer
|
|
from tracking.lidar_cache import LidarCache
|
|
from multiprocessing import Process
|
|
import messaging.message_factory as mf
|
|
from rplidar import RPLidar
|
|
from Messaging import messages
|
|
|
|
|
|
class LidarServicer(PersonTrackingServicer):
|
|
|
|
def __init__(self):
|
|
# TODO: Put the rplidar creation in a factory or something, to make it possible to test this servicer.
|
|
self.cache = LidarCache(RPLidar('/dev/ttyUSB0'), measurements=100)
|
|
self._mFactory = None
|
|
self._port = None
|
|
|
|
def set_tracking_group(self, request, context):
|
|
pass
|
|
|
|
def stop_tracking(self, request, context):
|
|
self.cache.stop_scanning()
|
|
|
|
def start_tracking(self, request, context):
|
|
"""Starts the lidar cache, streaming on the provided port."""
|
|
self._port = request.value
|
|
self.cache.start_cache(self)
|
|
|
|
def onGroupsChanged(self, message):
|
|
if self._mFactory is None:
|
|
# Create the zmq socket in the thread that it will be used, just to be safe.
|
|
self._mFactory = mf.getZmqPubSubStreamer(self._port)
|
|
self._mFactory.send_message_topic("lidar_map", messages.ProtoMessage(message=message.SerializeToString()))
|