Add 'car/' from commit 'eee0e8dc445691e600680f4abc77f2814b20b054'

git-subtree-dir: car
git-subtree-mainline: 1d29a5526c
git-subtree-split: eee0e8dc44

car/slam/slam_servicer.py

@@ -0,0 +1,31 @@
+import slam.SlamController_pb2_grpc as grpc
+import slam.SlamController_pb2 as proto
+import slam.slam_streamer as slam
+from multiprocessing import Process
+
+
+class SlamServicer(grpc.SlamControlServicer):
+    slam_thread = None
+
+    def __init__(self):
+        print('Servicer initialised')
+        self.slam = slam.SlamStreamer()
+
+    def start_map_streaming(self, request, context):
+        print('Received Map Start Streaming Request')
+        if self.slam_thread is None:
+            print('initialising slam_thread')
+            # Don't bother creating and starting slam more than once.
+            self.slam.port = request.port
+            self.slam.map_pixels = request.map_size_pixels
+            self.slam.map_meters = request.map_size_meters
+            self.slam_thread = Process(target=self.slam.start)
+            self.slam_thread.start()
+        return proto.Empty()
+
+    def stop_streaming(self, request, context):
+        if self.slam_thread is not None:
+            self.slam.stop_scanning()
+            self.slam_thread.join()
+            self.slam = None
+        return proto.Empty()

car/slam/slam_streamer.py

@@ -0,0 +1,122 @@
+import zmq
+from breezyslam.algorithms import RMHC_SLAM
+from breezyslam.sensors import RPLidarA1 as LaserModel
+from slam.SlamController_pb2 import SlamScan, SlamLocation
+import messaging.message_factory as mf
+import messaging.messages as messages
+import tracking.devices.factory as lidar_fact
+
+
+# Left here as was used in the example, configure as necessary.
+# MAP_SIZE_PIXELS         = 500
+# MAP_SIZE_METERS         = 10
+# LIDAR_DEVICE            = '/dev/ttyUSB0'
+
+class SlamStreamer:
+    can_scan = False
+
+    def __init__(self, map_pixels=None, map_meters=None, port=None):
+        self._map_pixels = map_pixels
+        self._map_meters = map_meters
+        self._port = port
+
+    def start(self):
+        '''
+        Does scanning and constructs the slam map,
+        and pushes to subscribers through a zmq pub socket.
+        This is done on the main thread, so you'll need
+        to run this method on a separate thread yourself.
+
+        All constructor parameters must be set prior
+        to calling this method, and changing those values after
+        calling this method will have no effect.
+        '''
+        self.can_scan = True
+        print('Starting to stream')
+        self._mFactory = mf.getZmqPubSubStreamer(self._port)
+
+        print('Started and bound zmq socket.')
+
+        # 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()
+        lidar = lidar_fact.get_lidar()
+
+        print('Initialised lidar')
+
+        # Create an RMHC SLAM object with a laser model and optional robot model
+        slam = RMHC_SLAM(LaserModel(), self._map_pixels, self._map_meters)
+
+        print('initialised slam')
+
+        # Initialize empty map
+        mapbytes = bytearray(self.map_pixels * self.map_pixels)
+
+        print('Initialised byte []')
+
+        # Create an iterator to collect scan data from the RPLidar
+        iterator = lidar.iter_scans()
+
+        print('Scanning')
+
+        while self.can_scan:
+            # Extract (quality, angle, distance) triples from current scan
+            items = [item for item in next(iterator)]
+
+            # Extract distances and angles from triples
+            distances = [item[2] for item in items]
+            angles = [item[1] for item in items]
+            print('Updating map')
+            # Update SLAM with current Lidar scan and scan angles
+            slam.update(distances, scan_angles_degrees=angles)
+            print('Map updated')
+            slam.getmap(mapbytes)
+            self._push_map(mapbytes, slam.getpos())
+
+    def _push_map(self, mapbytes, location):
+        '''
+        Pushes a scan over zmq using protocol buffers.
+        map should be the result of slam.getmap.
+        location should be a tuple, the result of slam.getpos()
+        '''
+        protoScan = messages.ProtoMessage(message=SlamScan(map=bytes(mapbytes),
+                                                           location=SlamLocation(x=location[0], y=location[1], theta=location[2])))
+        print('Sending map')
+        self._mFactory.send_message_topic(
+            'slam_map', protoScan)
+
+    def stop_scanning(self):
+        self.can_scan = False
+
+    # Properties
+    @property
+    def map_pixels(self):
+        return self._map_pixels
+
+    @map_pixels.setter
+    def map_pixels(self, value):
+        self._map_pixels = value
+
+    @property
+    def map_meters(self):
+        return self._map_meters
+
+    @map_meters.setter
+    def map_meters(self, value):
+        self._map_meters = value
+
+    @property
+    def lidar_connection(self):
+        return self._lidar_connection
+
+    @lidar_connection.setter
+    def lidar_connection(self, value):
+        self._lidar_connection = value
+
+    @property
+    def port(self):
+        return self._port
+
+    @port.setter
+    def port(self, value):
+        self._port = value

car/slam/zmq_pair_testing/pair.py

@@ -0,0 +1,28 @@
+import zmq
+from threading import Thread
+import time
+
+context = zmq.Context.instance()
+
+def client(context):
+    print('in thread')
+    socket = context.socket(zmq.SUB)
+    print('created socket')
+    socket.connect('tcp://localhost:5050')
+    socket.subscribe(b'slam_map')
+    while True:
+        print(socket.recv())
+
+def server(context):
+    print('in thread')
+    socket = context.socket(zmq.PUB)
+    print('created socket')
+    socket.bind('tcp://*:5050')
+    while True:
+        socket.send_multipart([b'slam_map', b'Hi'])
+        time.sleep(1)
+
+# client_thread = Thread(target=client, args=[context])
+server_thread = Thread(target=server, args=[context])
+server_thread.start()
+# client_thread.start()