Add SlamController stuff

MotorControl/MotorServer.py

@@ -10,8 +10,9 @@ import grpc
 import MotorControl.motorService_pb2 as motorService_pb2
 import MotorControl.motorService_pb2_grpc as motorService_pb2_grpc
 from MotorControl.gpiozero.motor_session import Motor
+from SlamController.slam_streamer import SlamStreamer
+import SlamController.SlamController_pb2_grpc as SlamController_pb2_grpc
 
-servo_pin = 18
 
 class MotorServicer(motorService_pb2_grpc.CarControlServicer):
     def __init__(self, motor, servo):
@@ -53,13 +54,17 @@ class MotorServicer(motorService_pb2_grpc.CarControlServicer):
     def start_server(self):
         server = grpc.server(futures.ThreadPoolExecutor(max_workers=8))
         motorService_pb2_grpc.add_CarControlServicer_to_server(self, server)
-        # Disable security for local testing.
+        SlamController_pb2_grpc.add_SlamControlServicer_to_server(self.create_slam_servicer(), server)
+        # Disable tls for local testing.
         # server.add_secure_port('[::]:50051', self.create_credentials())
         server.add_insecure_port('[::]:50051')
         server.start()
         while True:
             time.sleep(60*60)
 
+    def create_slam_servicer(self):
+        return SlamStreamer()
+
     def create_credentials(self):
         pvtKeyPath = '/home/pi/tls/device.key'
         pvtCertPath = '/home/pi/tls/device.crt'
@@ -70,7 +75,7 @@ class MotorServicer(motorService_pb2_grpc.CarControlServicer):
         return grpc.ssl_server_credentials([[pvtKeyBytes, pvtCertBytes]])
 
 motor = Motor()
-servo = Servo(servo_pin)
+servo = Servo(18)
 servicer = MotorServicer(motor, servo)
 
 service_thread = Thread(target=servicer.start_server)

SlamController/SlamController_pb2.py

@@ -0,0 +1,281 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler.  DO NOT EDIT!
+# source: SlamController.proto
+
+import sys
+_b=sys.version_info[0]<3 and (lambda x:x) or (lambda x:x.encode('latin1'))
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import message as _message
+from google.protobuf import reflection as _reflection
+from google.protobuf import symbol_database as _symbol_database
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+
+
+DESCRIPTOR = _descriptor.FileDescriptor(
+  name='SlamController.proto',
+  package='',
+  syntax='proto3',
+  serialized_options=None,
+  serialized_pb=_b('\n\x14SlamController.proto\"?\n\x0bSlamDetails\x12\x17\n\x0fmap_size_pixels\x18\x01 \x01(\x05\x12\x17\n\x0fmap_size_meters\x18\x02 \x01(\x05\"\x19\n\x07SlamRow\x12\x0e\n\x06points\x18\x01 \x03(\x05\"3\n\x0cSlamLocation\x12\t\n\x01x\x18\x01 \x01(\x05\x12\t\n\x01y\x18\x02 \x01(\x05\x12\r\n\x05theta\x18\x03 \x01(\x05\"8\n\x08SlamScan\x12\x0b\n\x03map\x18\x01 \x01(\x0c\x12\x1f\n\x08location\x18\x02 \x01(\x0b\x32\r.SlamLocation\"\x07\n\x05\x45mpty2`\n\x0bSlamControl\x12-\n\x13start_map_streaming\x12\x0c.SlamDetails\x1a\x06.Empty\"\x00\x12\"\n\x0estop_streaming\x12\x06.Empty\x1a\x06.Empty\"\x00\x62\x06proto3')
+)
+
+
+
+
+_SLAMDETAILS = _descriptor.Descriptor(
+  name='SlamDetails',
+  full_name='SlamDetails',
+  filename=None,
+  file=DESCRIPTOR,
+  containing_type=None,
+  fields=[
+    _descriptor.FieldDescriptor(
+      name='map_size_pixels', full_name='SlamDetails.map_size_pixels', index=0,
+      number=1, type=5, cpp_type=1, label=1,
+      has_default_value=False, default_value=0,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='map_size_meters', full_name='SlamDetails.map_size_meters', index=1,
+      number=2, type=5, cpp_type=1, label=1,
+      has_default_value=False, default_value=0,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+  ],
+  extensions=[
+  ],
+  nested_types=[],
+  enum_types=[
+  ],
+  serialized_options=None,
+  is_extendable=False,
+  syntax='proto3',
+  extension_ranges=[],
+  oneofs=[
+  ],
+  serialized_start=24,
+  serialized_end=87,
+)
+
+
+_SLAMROW = _descriptor.Descriptor(
+  name='SlamRow',
+  full_name='SlamRow',
+  filename=None,
+  file=DESCRIPTOR,
+  containing_type=None,
+  fields=[
+    _descriptor.FieldDescriptor(
+      name='points', full_name='SlamRow.points', index=0,
+      number=1, type=5, cpp_type=1, label=3,
+      has_default_value=False, default_value=[],
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+  ],
+  extensions=[
+  ],
+  nested_types=[],
+  enum_types=[
+  ],
+  serialized_options=None,
+  is_extendable=False,
+  syntax='proto3',
+  extension_ranges=[],
+  oneofs=[
+  ],
+  serialized_start=89,
+  serialized_end=114,
+)
+
+
+_SLAMLOCATION = _descriptor.Descriptor(
+  name='SlamLocation',
+  full_name='SlamLocation',
+  filename=None,
+  file=DESCRIPTOR,
+  containing_type=None,
+  fields=[
+    _descriptor.FieldDescriptor(
+      name='x', full_name='SlamLocation.x', index=0,
+      number=1, type=5, cpp_type=1, label=1,
+      has_default_value=False, default_value=0,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='y', full_name='SlamLocation.y', index=1,
+      number=2, type=5, cpp_type=1, label=1,
+      has_default_value=False, default_value=0,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='theta', full_name='SlamLocation.theta', index=2,
+      number=3, type=5, cpp_type=1, label=1,
+      has_default_value=False, default_value=0,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+  ],
+  extensions=[
+  ],
+  nested_types=[],
+  enum_types=[
+  ],
+  serialized_options=None,
+  is_extendable=False,
+  syntax='proto3',
+  extension_ranges=[],
+  oneofs=[
+  ],
+  serialized_start=116,
+  serialized_end=167,
+)
+
+
+_SLAMSCAN = _descriptor.Descriptor(
+  name='SlamScan',
+  full_name='SlamScan',
+  filename=None,
+  file=DESCRIPTOR,
+  containing_type=None,
+  fields=[
+    _descriptor.FieldDescriptor(
+      name='map', full_name='SlamScan.map', index=0,
+      number=1, type=12, cpp_type=9, label=1,
+      has_default_value=False, default_value=_b(""),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='location', full_name='SlamScan.location', index=1,
+      number=2, type=11, cpp_type=10, label=1,
+      has_default_value=False, default_value=None,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+  ],
+  extensions=[
+  ],
+  nested_types=[],
+  enum_types=[
+  ],
+  serialized_options=None,
+  is_extendable=False,
+  syntax='proto3',
+  extension_ranges=[],
+  oneofs=[
+  ],
+  serialized_start=169,
+  serialized_end=225,
+)
+
+
+_EMPTY = _descriptor.Descriptor(
+  name='Empty',
+  full_name='Empty',
+  filename=None,
+  file=DESCRIPTOR,
+  containing_type=None,
+  fields=[
+  ],
+  extensions=[
+  ],
+  nested_types=[],
+  enum_types=[
+  ],
+  serialized_options=None,
+  is_extendable=False,
+  syntax='proto3',
+  extension_ranges=[],
+  oneofs=[
+  ],
+  serialized_start=227,
+  serialized_end=234,
+)
+
+_SLAMSCAN.fields_by_name['location'].message_type = _SLAMLOCATION
+DESCRIPTOR.message_types_by_name['SlamDetails'] = _SLAMDETAILS
+DESCRIPTOR.message_types_by_name['SlamRow'] = _SLAMROW
+DESCRIPTOR.message_types_by_name['SlamLocation'] = _SLAMLOCATION
+DESCRIPTOR.message_types_by_name['SlamScan'] = _SLAMSCAN
+DESCRIPTOR.message_types_by_name['Empty'] = _EMPTY
+_sym_db.RegisterFileDescriptor(DESCRIPTOR)
+
+SlamDetails = _reflection.GeneratedProtocolMessageType('SlamDetails', (_message.Message,), {
+  'DESCRIPTOR' : _SLAMDETAILS,
+  '__module__' : 'SlamController_pb2'
+  # @@protoc_insertion_point(class_scope:SlamDetails)
+  })
+_sym_db.RegisterMessage(SlamDetails)
+
+SlamRow = _reflection.GeneratedProtocolMessageType('SlamRow', (_message.Message,), {
+  'DESCRIPTOR' : _SLAMROW,
+  '__module__' : 'SlamController_pb2'
+  # @@protoc_insertion_point(class_scope:SlamRow)
+  })
+_sym_db.RegisterMessage(SlamRow)
+
+SlamLocation = _reflection.GeneratedProtocolMessageType('SlamLocation', (_message.Message,), {
+  'DESCRIPTOR' : _SLAMLOCATION,
+  '__module__' : 'SlamController_pb2'
+  # @@protoc_insertion_point(class_scope:SlamLocation)
+  })
+_sym_db.RegisterMessage(SlamLocation)
+
+SlamScan = _reflection.GeneratedProtocolMessageType('SlamScan', (_message.Message,), {
+  'DESCRIPTOR' : _SLAMSCAN,
+  '__module__' : 'SlamController_pb2'
+  # @@protoc_insertion_point(class_scope:SlamScan)
+  })
+_sym_db.RegisterMessage(SlamScan)
+
+Empty = _reflection.GeneratedProtocolMessageType('Empty', (_message.Message,), {
+  'DESCRIPTOR' : _EMPTY,
+  '__module__' : 'SlamController_pb2'
+  # @@protoc_insertion_point(class_scope:Empty)
+  })
+_sym_db.RegisterMessage(Empty)
+
+
+
+_SLAMCONTROL = _descriptor.ServiceDescriptor(
+  name='SlamControl',
+  full_name='SlamControl',
+  file=DESCRIPTOR,
+  index=0,
+  serialized_options=None,
+  serialized_start=236,
+  serialized_end=332,
+  methods=[
+  _descriptor.MethodDescriptor(
+    name='start_map_streaming',
+    full_name='SlamControl.start_map_streaming',
+    index=0,
+    containing_service=None,
+    input_type=_SLAMDETAILS,
+    output_type=_EMPTY,
+    serialized_options=None,
+  ),
+  _descriptor.MethodDescriptor(
+    name='stop_streaming',
+    full_name='SlamControl.stop_streaming',
+    index=1,
+    containing_service=None,
+    input_type=_EMPTY,
+    output_type=_EMPTY,
+    serialized_options=None,
+  ),
+])
+_sym_db.RegisterServiceDescriptor(_SLAMCONTROL)
+
+DESCRIPTOR.services_by_name['SlamControl'] = _SLAMCONTROL
+
+# @@protoc_insertion_point(module_scope)

SlamController/SlamController_pb2_grpc.py

@@ -0,0 +1,63 @@
+# Generated by the gRPC Python protocol compiler plugin. DO NOT EDIT!
+import grpc
+
+import SlamController_pb2 as SlamController__pb2
+
+
+class SlamControlStub(object):
+  # missing associated documentation comment in .proto file
+  pass
+
+  def __init__(self, channel):
+    """Constructor.
+
+    Args:
+      channel: A grpc.Channel.
+    """
+    self.start_map_streaming = channel.unary_unary(
+        '/SlamControl/start_map_streaming',
+        request_serializer=SlamController__pb2.SlamDetails.SerializeToString,
+        response_deserializer=SlamController__pb2.Empty.FromString,
+        )
+    self.stop_streaming = channel.unary_unary(
+        '/SlamControl/stop_streaming',
+        request_serializer=SlamController__pb2.Empty.SerializeToString,
+        response_deserializer=SlamController__pb2.Empty.FromString,
+        )
+
+
+class SlamControlServicer(object):
+  # missing associated documentation comment in .proto file
+  pass
+
+  def start_map_streaming(self, request, context):
+    # missing associated documentation comment in .proto file
+    pass
+    context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+    context.set_details('Method not implemented!')
+    raise NotImplementedError('Method not implemented!')
+
+  def stop_streaming(self, request, context):
+    # missing associated documentation comment in .proto file
+    pass
+    context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+    context.set_details('Method not implemented!')
+    raise NotImplementedError('Method not implemented!')
+
+
+def add_SlamControlServicer_to_server(servicer, server):
+  rpc_method_handlers = {
+      'start_map_streaming': grpc.unary_unary_rpc_method_handler(
+          servicer.start_map_streaming,
+          request_deserializer=SlamController__pb2.SlamDetails.FromString,
+          response_serializer=SlamController__pb2.Empty.SerializeToString,
+      ),
+      'stop_streaming': grpc.unary_unary_rpc_method_handler(
+          servicer.stop_streaming,
+          request_deserializer=SlamController__pb2.Empty.FromString,
+          response_serializer=SlamController__pb2.Empty.SerializeToString,
+      ),
+  }
+  generic_handler = grpc.method_handlers_generic_handler(
+      'SlamControl', rpc_method_handlers)
+  server.add_generic_rpc_handlers((generic_handler,))

SlamController/proto/SlamController.proto

@@ -0,0 +1,32 @@
+syntax = "proto3";
+
+message SlamDetails {
+    int32 map_size_pixels = 1;
+    int32 map_size_meters = 2;
+    int32 port = 3;
+}
+
+message SlamRow{
+    repeated int32 points = 1;
+}
+
+message SlamLocation{
+    int32 x = 1;
+    int32 y = 2;
+    int32 theta = 3;
+}
+
+message SlamScan{
+    bytes map = 1;
+    SlamLocation location = 2;
+}
+
+message Empty{
+
+}
+
+service SlamControl {
+    rpc start_map_streaming(SlamDetails) returns (Empty) {}
+
+    rpc stop_streaming(Empty) returns (Empty) {}
+}

SlamController/slam_servicer.py

@@ -0,0 +1,21 @@
+import SlamController_pb2_grpc 
+import slam_streamer as slam
+from threading import Thread
+
+class SlamServicer(SlamController_pb2_grpc.SlamControlServicer):
+    def __init__(self, lidar_connection):
+        print('Servicer initialised')
+        self.slam = slam.SlamStreamer(lidar_connection=lidar_connection)
+
+    def start_map_streaming(self, request, context):
+        if not hasattr(self, 'slamThread'):
+            # 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
+            slamThread = Thread(target=self.slam.start)
+            slamThread.start()
+
+    def stop_streaming(self, request, context):
+        if hasattr(self, 'slamThread'):
+            self.slam.stop_scanning()

SlamController/slam_streamer.py

@@ -0,0 +1,117 @@
+import zmq
+from breezyslam.algorithms import RMHC_SLAM
+from breezyslam.sensors import RPLidarA1 as LaserModel
+from rplidar import RPLidar as Lidar
+from SlamController_pb2 import SlamScan, SlamLocation
+
+
+# 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, lidar_connection = None, port = None):
+        self._socket = self._start_socket(self._create_tcp_pub_socket(), port)
+        self._map_pixels = map_pixels
+        self._map_meters = map_meters
+        self._lidar_connection = lidar_connection
+        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.
+        '''
+        # Adapted from BreezySLAM rpslam example.
+        # Connect to Lidar unit
+        lidar = Lidar(self._lidar_connection)
+
+        # Create an RMHC SLAM object with a laser model and optional robot model
+        slam = RMHC_SLAM(LaserModel(), self._map_pixels, self._map_meters)
+
+        # Initialize empty map
+        mapbytes = bytearray(self.map_pixels * self.map_pixels)
+
+        # Create an iterator to collect scan data from the RPLidar
+        iterator = lidar.iter_scans()
+
+        # First scan is crap, so ignore it
+        next(iterator)
+
+        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]
+
+            # Update SLAM with current Lidar scan and scan angles
+            slam.update(distances, scan_angles_degrees=angles)
+
+            self._push_map(slam.getmap(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 = SlamScan(map = bytes(mapbytes), \
+            location = SlamLocation(x = location[0], y = location[1], theta = location[3]))
+        self._socket.send(protoScan.SerializeToString())
+
+    def stop_scanning(self):
+        self.can_scan = False
+
+    def _create_context(self):
+        return zmq.Context.instance()
+
+    def _create_tcp_pub_socket(self):
+        return self._create_context().socket(zmq.PUB)
+
+    def _start_socket(self, socket, port):
+        socket.bind('tcp://*:' + str(self._port))
+        return socket
+
+    # 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