vato007/picar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Use change listener rather than direct message factory for lidar.

Browse Source
This commit is contained in:
michaelpivato
2020-03-30 12:32:31 +10:30
parent 3fd6830f6b
commit 685f2ad35b
2 changed files with 18 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
tracking/lidar_cache.py
View File

@@ -2,7 +2,6 @@ from threading import Thread
from tracking import algorithms from tracking import algorithms
import tracking.lidar_tracker_pb2 as tracker_pb import tracking.lidar_tracker_pb2 as tracker_pb
import zmq import zmq
import messaging.messages as messages
class LidarCache(): class LidarCache():
@@ -25,17 +24,17 @@ class LidarCache():
self.thread = Thread(target=self.do_scanning, args=[sender]) self.thread = Thread(target=self.do_scanning, args=[sender])
self.thread.start() self.thread.start()
def do_scanning(self, sender): def do_scanning(self, listener):
"""Performs scans whilst cache is running, and will pass calculated groups data to the sender. """Performs scans whilst cache is running, and will pass calculated groups data to the sender.
Parameters Parameters
---------- ----------
sender: listener:
Any class given in messaging.message_factory. This acts as a listener. Any object that includes the onGroupsChanged method.
""" """
# Create the 0MQ socket first. This should not be passed between threads. # Create the 0MQ socket first. This should not be passed between threads.
self._mFactory = sender self._mFactory = listener
# Batch over scans, so we don't need to do our own batching to determine groups # Batch over scans, so we don't need to do our own batching to determine groups
# TODO: Implement custom batching, as iter_scans can be unreliable # TODO: Implement custom batching, as iter_scans can be unreliable
@@ -66,8 +65,7 @@ class LidarCache():
pointScan.points.append(tracker_pb.Point( pointScan.points.append(tracker_pb.Point(
angle=point[1], distance=point[2], group_number=group.number)) angle=point[1], distance=point[2], group_number=group.number))
self._mFactory.send_message_topic( self._mFactory.onGroupsChanged(pointScan)
"lidar_map", messages.ProtoMessage(message=pointScan.SerializeToString()))
def stop_scanning(self): def stop_scanning(self):
self.run = False self.run = False

13
tracking/lidar_servicer.py
View File

@@ -4,12 +4,16 @@ from tracking.lidar_cache import LidarCache
from multiprocessing import Process from multiprocessing import Process
import messaging.message_factory as mf import messaging.message_factory as mf
from rplidar import RPLidar from rplidar import RPLidar
from Messaging import messages
class LidarServicer(PersonTrackingServicer): class LidarServicer(PersonTrackingServicer):
def __init__(self): def __init__(self):
# TODO: Put the rplidar creation in a factory or something, to make it possible to test this servicer. # 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.cache = LidarCache(RPLidar('/dev/ttyUSB0'), measurements=100)
self._mFactory = None
self._port = None
def set_tracking_group(self, request, context): def set_tracking_group(self, request, context):
pass pass
@@ -19,4 +23,11 @@ class LidarServicer(PersonTrackingServicer):
def start_tracking(self, request, context): def start_tracking(self, request, context):
"""Starts the lidar cache, streaming on the provided port.""" """Starts the lidar cache, streaming on the provided port."""
self.cache.start_cache(mf.getZmqPubSubStreamer(request.value)) 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()))