Refactor python module structure
This commit is contained in:
Piv
288
slam/SlamController_pb2.py
Normal file
288
slam/SlamController_pb2.py
Normal file
@@ -0,0 +1,288 @@
|
||||
# -*- 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\"M\n\x0bSlamDetails\x12\x17\n\x0fmap_size_pixels\x18\x01 \x01(\x05\x12\x17\n\x0fmap_size_meters\x18\x02 \x01(\x05\x12\x0c\n\x04port\x18\x03 \x01(\x05\"\x19\n\x07SlamRow\x12\x0e\n\x06points\x18\x01 \x03(\x05\"3\n\x0cSlamLocation\x12\t\n\x01x\x18\x01 \x01(\x02\x12\t\n\x01y\x18\x02 \x01(\x02\x12\r\n\x05theta\x18\x03 \x01(\x02\"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),
|
||||
_descriptor.FieldDescriptor(
|
||||
name='port', full_name='SlamDetails.port', 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=24,
|
||||
serialized_end=101,
|
||||
)
|
||||
|
||||
|
||||
_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=103,
|
||||
serialized_end=128,
|
||||
)
|
||||
|
||||
|
||||
_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=2, cpp_type=6, label=1,
|
||||
has_default_value=False, default_value=float(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=2, cpp_type=6, label=1,
|
||||
has_default_value=False, default_value=float(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=2, cpp_type=6, label=1,
|
||||
has_default_value=False, default_value=float(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=130,
|
||||
serialized_end=181,
|
||||
)
|
||||
|
||||
|
||||
_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=183,
|
||||
serialized_end=239,
|
||||
)
|
||||
|
||||
|
||||
_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=241,
|
||||
serialized_end=248,
|
||||
)
|
||||
|
||||
_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=250,
|
||||
serialized_end=346,
|
||||
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)
|
||||
63
slam/SlamController_pb2_grpc.py
Normal file
63
slam/SlamController_pb2_grpc.py
Normal file
@@ -0,0 +1,63 @@
|
||||
# Generated by the gRPC Python protocol compiler plugin. DO NOT EDIT!
|
||||
import grpc
|
||||
|
||||
from . 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,))
|
||||
0
slam/__init__.py
Normal file
0
slam/__init__.py
Normal file
31
slam/slam_servicer.py
Normal file
31
slam/slam_servicer.py
Normal file
@@ -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, lidar_connection):
|
||||
print('Servicer initialised')
|
||||
self.slam = slam.SlamStreamer(lidar_connection=lidar_connection)
|
||||
|
||||
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()
|
||||
123
slam/slam_streamer.py
Normal file
123
slam/slam_streamer.py
Normal file
@@ -0,0 +1,123 @@
|
||||
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
|
||||
import messaging.message_factory as mf
|
||||
import messaging.messages as messages
|
||||
|
||||
|
||||
# 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._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.
|
||||
'''
|
||||
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(self._lidar_connection)
|
||||
lidar = Lidar(self._lidar_connection)
|
||||
|
||||
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
|
||||
28
slam/zmq_pair_testing/pair.py
Normal file
28
slam/zmq_pair_testing/pair.py
Normal file
@@ -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()
|
||||
Reference in New Issue
Block a user