From 35f35622cfb6945a4faad8882ce3b5d844de0b94 Mon Sep 17 00:00:00 2001 From: Piv <18462828+Piv200@users.noreply.github.com> Date: Mon, 3 Feb 2020 19:00:50 +1030 Subject: [PATCH] Add SlamController stuff --- MotorControl/MotorServer.py | 11 +- SlamController/SlamController_pb2.py | 281 ++++++++++++++++++++++ SlamController/SlamController_pb2_grpc.py | 63 +++++ SlamController/proto/SlamController.proto | 32 +++ SlamController/slam_servicer.py | 21 ++ SlamController/slam_streamer.py | 117 +++++++++ 6 files changed, 522 insertions(+), 3 deletions(-) create mode 100644 SlamController/SlamController_pb2.py create mode 100644 SlamController/SlamController_pb2_grpc.py create mode 100644 SlamController/proto/SlamController.proto create mode 100644 SlamController/slam_servicer.py create mode 100644 SlamController/slam_streamer.py diff --git a/MotorControl/MotorServer.py b/MotorControl/MotorServer.py index 10713c1..f175a37 100755 --- a/MotorControl/MotorServer.py +++ b/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) diff --git a/SlamController/SlamController_pb2.py b/SlamController/SlamController_pb2.py new file mode 100644 index 0000000..e2e86d6 --- /dev/null +++ b/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) diff --git a/SlamController/SlamController_pb2_grpc.py b/SlamController/SlamController_pb2_grpc.py new file mode 100644 index 0000000..16c5743 --- /dev/null +++ b/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,)) diff --git a/SlamController/proto/SlamController.proto b/SlamController/proto/SlamController.proto new file mode 100644 index 0000000..c69711c --- /dev/null +++ b/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) {} +} \ No newline at end of file diff --git a/SlamController/slam_servicer.py b/SlamController/slam_servicer.py new file mode 100644 index 0000000..da43184 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/SlamController/slam_streamer.py b/SlamController/slam_streamer.py new file mode 100644 index 0000000..a250cb2 --- /dev/null +++ b/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 \ No newline at end of file