45 lines
1.8 KiB
Python
45 lines
1.8 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
|
|
import tracking.devices.factory as lidar_factory
|
|
|
|
from messaging import messages
|
|
import tracking.algorithms as alg
|
|
|
|
class LidarServicer(PersonTrackingServicer):
|
|
|
|
def __init__(self, vehicle=None):
|
|
# TODO: Put the rplidar creation in a factory or something, to make it possible to test this servicer.
|
|
# Also, it would allow creating the service without the lidar being connected.
|
|
self.cache = LidarCache(lidar_factory.get_lidar(), measurements=100)
|
|
self.cache.add_groups_changed_listener(self)
|
|
self._mFactory = None
|
|
self._port = None
|
|
self._vehicle = vehicle
|
|
self._tracked_group = None
|
|
|
|
def set_tracking_group(self, request, context):
|
|
self._tracked_group = request.value
|
|
|
|
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()
|
|
|
|
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()))
|
|
|
|
if self._tracked_group is not None and self._vehicle is not None:
|
|
# Update vehicle to correctly follow the tracked group.
|
|
# Leave for now, need to work out exactly how this will change.
|
|
# alg.dualServoChange(alg.find_centre())
|
|
pass
|