Use message factory for slam streamer
This commit is contained in:
@@ -3,6 +3,8 @@ 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.
|
||||
@@ -33,8 +35,7 @@ class SlamStreamer:
|
||||
'''
|
||||
self.can_scan = True
|
||||
print('Starting to stream')
|
||||
self._socket = self._start_socket(
|
||||
self._create_socket(self._zmq_context), self._port)
|
||||
self._mFactory = mf.getZmqPubSubStreamer(self._port)
|
||||
|
||||
print('Started and bound zmq socket.')
|
||||
|
||||
@@ -52,7 +53,7 @@ class SlamStreamer:
|
||||
|
||||
# Initialize empty map
|
||||
mapbytes = bytearray(self.map_pixels * self.map_pixels)
|
||||
|
||||
|
||||
print('Initialised byte []')
|
||||
|
||||
# Create an iterator to collect scan data from the RPLidar
|
||||
@@ -80,25 +81,15 @@ class SlamStreamer:
|
||||
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[2]))
|
||||
protoScan = messages.ProtoMessage(message=SlamScan(map=bytes(mapbytes),
|
||||
location=SlamLocation(x=location[0], y=location[1], theta=location[2])))
|
||||
print('Sending map')
|
||||
self._socket.send_multipart([b'slam_map', protoScan.SerializeToString()])
|
||||
self._mFactory.send_message_topic(
|
||||
'slam_map', protoScan)
|
||||
|
||||
def stop_scanning(self):
|
||||
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
|
||||
@property
|
||||
def map_pixels(self):
|
||||
|
||||
Reference in New Issue
Block a user