Make the lidar streamer ready to work with main controller

Messaging/message_factory.py

@@ -59,3 +59,6 @@ def getZmqPubSubStreamer(port):
 
 def getTestingStreamer():
     return TestStreamer()
+
+# TODO: Create a general get method that will get the streamer based on an 
+# environment variable that is set. 

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()

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')

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')
 

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.

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:

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

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