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

Use pub socket with a key for the sub in the envelope.

Browse Source
This commit is contained in:
Piv
2020-02-15 20:43:22 +10:30
parent 65f3438631
commit 065dcc437a
1 changed files with 12 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
SlamController/slam_streamer.py
View File

@@ -9,11 +9,11 @@ from .SlamController_pb2 import SlamScan, SlamLocation
# 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):
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
@@ -33,12 +33,10 @@ class SlamStreamer:
'''
print('Starting to stream')
self._socket = self._start_socket(self._create_socket(self._zmq_context), self._port)
# Block until user opens zmq.
self._socket.recv()
self._socket = self._start_socket(
self._create_socket(self._zmq_context), self._port)
print('Received message from ZMQ')
print('Started and bound zmq socket.')
# Adapted from BreezySLAM rpslam example.
# Connect to Lidar unit
@@ -62,7 +60,7 @@ class SlamStreamer:
# Extract distances and angles from triples
distances = [item[2] for item in items]
angles = [item[1] 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)
@@ -74,11 +72,11 @@ class SlamStreamer:
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]))
'''
protoScan = SlamScan(map=bytes(mapbytes),
location=SlamLocation(x=location[0], y=location[1], theta=location[3]))
print('Sending map')
self._socket.send(protoScan.SerializeToString())
self._socket.send_multipart(b'slam_map', protoScan.SerializeToString())
def stop_scanning(self):
self.can_scan = False
@@ -87,7 +85,7 @@ class SlamStreamer:
return zmq.Context.instance()
def _create_socket(self, context):
return context.socket(zmq.PAIR)
return context.socket(zmq.PUB)
def _start_socket(self, socket, port):
socket.bind('tcp://*:' + str(self._port))
@@ -124,4 +122,4 @@ class SlamStreamer:
@port.setter
def port(self, value):
self._port = value
self._port = value