Use message factory for slam streamer
This commit is contained in:
@@ -1,25 +1,7 @@
|
|||||||
import zmq
|
import zmq
|
||||||
|
|
||||||
|
|
||||||
class MessageFactory():
|
class ZmqPubSubStreamer:
|
||||||
|
|
||||||
def getZmqPubSubStreamer(self, topic):
|
|
||||||
'''
|
|
||||||
Not thread-safe. Always get this inside the thread/process where you intend
|
|
||||||
to use it.
|
|
||||||
'''
|
|
||||||
return ZmqPubSubStreamer(topic)
|
|
||||||
|
|
||||||
|
|
||||||
class Streamer():
|
|
||||||
def send_message(self, message_bytes):
|
|
||||||
raise NotImplementedError
|
|
||||||
|
|
||||||
def send_message_topic(self, topic, message_bytes):
|
|
||||||
raise NotImplementedError
|
|
||||||
|
|
||||||
|
|
||||||
class ZmqPubSubStreamer(Streamer):
|
|
||||||
'''
|
'''
|
||||||
Not thread-safe. Always get this inside the thread/process where you intend
|
Not thread-safe. Always get this inside the thread/process where you intend
|
||||||
to use it.
|
to use it.
|
||||||
@@ -29,7 +11,9 @@ class ZmqPubSubStreamer(Streamer):
|
|||||||
# Should create the socket here always, since zmq is not thread safe.
|
# Should create the socket here always, since zmq is not thread safe.
|
||||||
# Hopefully whoever uses this is not stupid enough to create it then
|
# Hopefully whoever uses this is not stupid enough to create it then
|
||||||
# pass it into a thread.
|
# pass it into a thread.
|
||||||
pass
|
self._socket = zmq.Context.instance().socket(zmq.PUB)
|
||||||
|
print('Starting socket with address: ' + 'tcp://*:' + str(port))
|
||||||
|
self._socket.bind("tcp://*:" + str(port))
|
||||||
|
|
||||||
def send_message(self, message):
|
def send_message(self, message):
|
||||||
'''
|
'''
|
||||||
@@ -37,15 +21,23 @@ class ZmqPubSubStreamer(Streamer):
|
|||||||
----
|
----
|
||||||
message: A message type that has the serialise() method.
|
message: A message type that has the serialise() method.
|
||||||
'''
|
'''
|
||||||
pass
|
self.send_message_topic("", message)
|
||||||
|
|
||||||
def send_message_topic(self, topic, message):
|
def send_message_topic(self, topic, message):
|
||||||
pass
|
self._socket.send_multipart([bytes(topic), message.serialise()])
|
||||||
|
|
||||||
|
|
||||||
class BluetoothStreamer(Streamer):
|
class BluetoothStreamer:
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
pass
|
pass
|
||||||
|
|
||||||
def send_message(self, message_bytes):
|
def send_message(self, message_bytes):
|
||||||
pass
|
pass
|
||||||
|
|
||||||
|
|
||||||
|
def getZmqPubSubStreamer(port):
|
||||||
|
'''
|
||||||
|
Not thread-safe. Always get this inside the thread/process where you intend
|
||||||
|
to use it.
|
||||||
|
'''
|
||||||
|
return ZmqPubSubStreamer(port)
|
||||||
|
|||||||
@@ -23,8 +23,8 @@ class PackMessage(Message):
|
|||||||
|
|
||||||
class ProtoMessage(Message):
|
class ProtoMessage(Message):
|
||||||
|
|
||||||
def __init__(self, proto_type, message=None):
|
def __init__(self, proto_type=None, message=None):
|
||||||
Message(message)
|
super().__init__(message)
|
||||||
self._type = proto_type
|
self._type = proto_type
|
||||||
|
|
||||||
def serialise(self):
|
def serialise(self):
|
||||||
|
|||||||
@@ -3,6 +3,8 @@ from breezyslam.algorithms import RMHC_SLAM
|
|||||||
from breezyslam.sensors import RPLidarA1 as LaserModel
|
from breezyslam.sensors import RPLidarA1 as LaserModel
|
||||||
from rplidar import RPLidar as Lidar
|
from rplidar import RPLidar as Lidar
|
||||||
from .SlamController_pb2 import SlamScan, SlamLocation
|
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.
|
# Left here as was used in the example, configure as necessary.
|
||||||
@@ -33,8 +35,7 @@ class SlamStreamer:
|
|||||||
'''
|
'''
|
||||||
self.can_scan = True
|
self.can_scan = True
|
||||||
print('Starting to stream')
|
print('Starting to stream')
|
||||||
self._socket = self._start_socket(
|
self._mFactory = mf.getZmqPubSubStreamer(self._port)
|
||||||
self._create_socket(self._zmq_context), self._port)
|
|
||||||
|
|
||||||
print('Started and bound zmq socket.')
|
print('Started and bound zmq socket.')
|
||||||
|
|
||||||
@@ -52,7 +53,7 @@ class SlamStreamer:
|
|||||||
|
|
||||||
# Initialize empty map
|
# Initialize empty map
|
||||||
mapbytes = bytearray(self.map_pixels * self.map_pixels)
|
mapbytes = bytearray(self.map_pixels * self.map_pixels)
|
||||||
|
|
||||||
print('Initialised byte []')
|
print('Initialised byte []')
|
||||||
|
|
||||||
# Create an iterator to collect scan data from the RPLidar
|
# Create an iterator to collect scan data from the RPLidar
|
||||||
@@ -80,25 +81,15 @@ class SlamStreamer:
|
|||||||
map should be the result of slam.getmap.
|
map should be the result of slam.getmap.
|
||||||
location should be a tuple, the result of slam.getpos()
|
location should be a tuple, the result of slam.getpos()
|
||||||
'''
|
'''
|
||||||
protoScan = SlamScan(map=bytes(mapbytes),
|
protoScan = messages.ProtoMessage(message=SlamScan(map=bytes(mapbytes),
|
||||||
location=SlamLocation(x=location[0], y=location[1], theta=location[2]))
|
location=SlamLocation(x=location[0], y=location[1], theta=location[2])))
|
||||||
print('Sending map')
|
print('Sending map')
|
||||||
self._socket.send_multipart([b'slam_map', protoScan.SerializeToString()])
|
self._mFactory.send_message_topic(
|
||||||
|
'slam_map', protoScan)
|
||||||
|
|
||||||
def stop_scanning(self):
|
def stop_scanning(self):
|
||||||
self.can_scan = False
|
self.can_scan = False
|
||||||
|
|
||||||
def _create_context(self):
|
|
||||||
return zmq.Context.instance()
|
|
||||||
|
|
||||||
def _create_socket(self, context):
|
|
||||||
return context.socket(zmq.PUB)
|
|
||||||
|
|
||||||
def _start_socket(self, socket, port):
|
|
||||||
print('Starting socket at with address: ' + 'tcp://*:' + str(self._port))
|
|
||||||
socket.bind('tcp://*:' + str(self._port))
|
|
||||||
return socket
|
|
||||||
|
|
||||||
# Properties
|
# Properties
|
||||||
@property
|
@property
|
||||||
def map_pixels(self):
|
def map_pixels(self):
|
||||||
|
|||||||
Reference in New Issue
Block a user