vato007/picar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add SlamController stuff

Browse Source
This commit is contained in:
Piv
2020-02-03 19:00:50 +10:30
parent 03f8485bd5
commit 35f35622cf
6 changed files with 522 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

281
SlamController/SlamController_pb2.py Normal file
View File

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

63
SlamController/SlamController_pb2_grpc.py Normal file
View File

@@ -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,))

32
SlamController/proto/SlamController.proto Normal file
View File

@@ -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) {}
}

21
SlamController/slam_servicer.py Normal file
View File

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

117
SlamController/slam_streamer.py Normal file
View File

@@ -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