From ed371d3a2f8d79711677915470b905385ec59993 Mon Sep 17 00:00:00 2001 From: Piv <18462828+Piv200@users.noreply.github.com> Date: Thu, 16 Apr 2020 21:12:12 +0930 Subject: [PATCH] Make the lidar streamer ready to work with main controller --- Messaging/message_factory.py | 5 ++++- controller.py | 2 +- slam/slam_servicer.py | 4 ++-- slam/slam_streamer.py | 7 +++---- tracking/algorithms.py | 6 ++++-- tracking/devices/__init__.py | 0 tracking/devices/factory.py | 5 ++++- tracking/lidar_cache.py | 25 +++++++++++++++++++------ tracking/lidar_servicer.py | 22 ++++++++++++++++------ 9 files changed, 53 insertions(+), 23 deletions(-) create mode 100644 tracking/devices/__init__.py diff --git a/Messaging/message_factory.py b/Messaging/message_factory.py index 814e26a..1888468 100644 --- a/Messaging/message_factory.py +++ b/Messaging/message_factory.py @@ -58,4 +58,7 @@ def getZmqPubSubStreamer(port): return ZmqPubSubStreamer(port) def getTestingStreamer(): - return TestStreamer() \ No newline at end of file + return TestStreamer() + +# TODO: Create a general get method that will get the streamer based on an +# environment variable that is set. \ No newline at end of file diff --git a/controller.py b/controller.py index d56bcbb..b49d8d5 100755 --- a/controller.py +++ b/controller.py @@ -38,7 +38,7 @@ class CarServer(): return MotorServicer(self.vehicle) def create_slam_servicer(self): - return SlamServicer('/dev/ttyUSB0') + return SlamServicer() def create_lidar_servicer(self): return LidarServicer() diff --git a/slam/slam_servicer.py b/slam/slam_servicer.py index 3ee7bf8..5209d34 100644 --- a/slam/slam_servicer.py +++ b/slam/slam_servicer.py @@ -7,9 +7,9 @@ from multiprocessing import Process class SlamServicer(grpc.SlamControlServicer): slam_thread = None - def __init__(self, lidar_connection): + def __init__(self): print('Servicer initialised') - self.slam = slam.SlamStreamer(lidar_connection=lidar_connection) + self.slam = slam.SlamStreamer() def start_map_streaming(self, request, context): print('Received Map Start Streaming Request') diff --git a/slam/slam_streamer.py b/slam/slam_streamer.py index 3e598a0..601a186 100644 --- a/slam/slam_streamer.py +++ b/slam/slam_streamer.py @@ -15,10 +15,9 @@ import tracking.devices.factory as lidar_fact class SlamStreamer: can_scan = False - def __init__(self, map_pixels=None, map_meters=None, lidar_connection=None, port=None): + def __init__(self, map_pixels=None, map_meters=None, port=None): self._map_pixels = map_pixels self._map_meters = map_meters - self._lidar_connection = lidar_connection self._port = port def start(self): @@ -40,8 +39,8 @@ class SlamStreamer: # Adapted from BreezySLAM rpslam example. # Connect to Lidar unit. For some reason it likes to be done twice, otherwise it errors out... - lidar = lidar_fact.get_lidar(self._lidar_connection) - lidar = lidar_fact.get_lidar(self._lidar_connection) + lidar = lidar_fact.get_lidar() + lidar = lidar_fact.get_lidar() print('Initialised lidar') diff --git a/tracking/algorithms.py b/tracking/algorithms.py index 6efeff5..f61741c 100644 --- a/tracking/algorithms.py +++ b/tracking/algorithms.py @@ -186,7 +186,9 @@ def updateCarVelocity(oldGroup, newGroup): tuple (DistanceChange, AngleChange) A tuple containing how the groups' centres changed in the form (distance,angle) """ - return (find_centre(newGroup)) + old_polar = convert_cartesian_to_lidar(*find_centre(oldGroup)) + new_centre = convert_cartesian_to_lidar(*find_centre(newGroup)) + return (new_centre[0] - old_polar[0], new_centre[1] - old_polar[1]) def dualServoChange(newCentre, changeTuple): @@ -197,7 +199,7 @@ def dualServoChange(newCentre, changeTuple): Parameters --------- newCentre - Tuple (distance,angle) of the new centre of the tracked group. + Tuple (distance, angle) of the new centre of the tracked group. changeTuple Tuple (distanceChange, angleChange) from the old centre to the new centre. diff --git a/tracking/devices/__init__.py b/tracking/devices/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/tracking/devices/factory.py b/tracking/devices/factory.py index af80f17..dfcb704 100644 --- a/tracking/devices/factory.py +++ b/tracking/devices/factory.py @@ -2,8 +2,11 @@ from tracking.devices.mock_lidar import MockLidar from rplidar import RPLidar import tracking.lidar_loader as loader +connection = "TEST" +# connection = '/dev/ttyUSB0' -def get_lidar(connection: str): +def get_lidar(): + # Need a way to configure this, maybe with environment variables if connection == 'TEST': return MockLidar(loader.load_scans_bytes_file("tracking/out.pickle")) else: diff --git a/tracking/lidar_cache.py b/tracking/lidar_cache.py index fd94253..f74b6f1 100644 --- a/tracking/lidar_cache.py +++ b/tracking/lidar_cache.py @@ -19,12 +19,13 @@ class LidarCache(): self.run = True self.tracking_group_number = -1 self.currentGroups = None + self._group_listeners = [] - def start_cache(self, sender): - self.thread = Thread(target=self.do_scanning, args=[sender]) + def start_cache(self): + self.thread = Thread(target=self.do_scanning) self.thread.start() - def do_scanning(self, listener): + def do_scanning(self): """Performs scans whilst cache is running, and will pass calculated groups data to the sender. Parameters @@ -33,8 +34,6 @@ class LidarCache(): Any object that includes the onGroupsChanged method. """ - # Create the 0MQ socket first. This should not be passed between threads. - self._mFactory = listener # 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 @@ -65,7 +64,21 @@ class LidarCache(): pointScan.points.append(tracker_pb.Point( angle=point[1], distance=point[2], group_number=group.number)) - self._mFactory.onGroupsChanged(pointScan) + for listener in self._group_listeners: + listener.onGroupsChanged(pointScan) + + def add_groups_changed_listener(self, listener): + """ + Add a listener for a change in scans. THis will provide a tuple with the new group + scans, which can then be sent off to a network listener for display, or to update the + vehicle with a new velocity. + + Parameters + ---------- + listener + An object that implements the onGroupsChanged(message) method. + """ + self._group_listeners.append(listener) def stop_scanning(self): self.run = False diff --git a/tracking/lidar_servicer.py b/tracking/lidar_servicer.py index 441b141..5386fc3 100644 --- a/tracking/lidar_servicer.py +++ b/tracking/lidar_servicer.py @@ -3,21 +3,25 @@ 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 +import tracking.devices.factory as lidar_factory +from messaging import messages +import tracking.algorithms as alg class LidarServicer(PersonTrackingServicer): - def __init__(self): + 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(RPLidar('/dev/ttyUSB0'), measurements=100) + 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): - pass + self._tracked_group = request.value def stop_tracking(self, request, context): self.cache.stop_scanning() @@ -25,10 +29,16 @@ class LidarServicer(PersonTrackingServicer): def start_tracking(self, request, context): """Starts the lidar cache, streaming on the provided port.""" self._port = request.value - self.cache.start_cache(self) + 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