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_PIXELS = 500
# MAP_SIZE_METERS = 10 # MAP_SIZE_METERS = 10
# LIDAR_DEVICE = '/dev/ttyUSB0' # LIDAR_DEVICE = '/dev/ttyUSB0'
class SlamStreamer: class SlamStreamer:
can_scan = False 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_pixels = map_pixels
self._map_meters = map_meters self._map_meters = map_meters
self._lidar_connection = lidar_connection self._lidar_connection = lidar_connection
@@ -33,12 +33,10 @@ class SlamStreamer:
''' '''
print('Starting to stream') print('Starting to stream')
self._socket = self._start_socket(self._create_socket(self._zmq_context), self._port) self._socket = self._start_socket(
self._create_socket(self._zmq_context), self._port)
# Block until user opens zmq.
self._socket.recv()
print('Received message from ZMQ') print('Started and bound zmq socket.')
# Adapted from BreezySLAM rpslam example. # Adapted from BreezySLAM rpslam example.
# Connect to Lidar unit # Connect to Lidar unit
@@ -62,7 +60,7 @@ class SlamStreamer:
# Extract distances and angles from triples # Extract distances and angles from triples
distances = [item[2] for item in items] distances = [item[2] for item in items]
angles = [item[1] for item in items] angles = [item[1] for item in items]
print('Updating map') print('Updating map')
# Update SLAM with current Lidar scan and scan angles # Update SLAM with current Lidar scan and scan angles
slam.update(distances, scan_angles_degrees=angles) slam.update(distances, scan_angles_degrees=angles)
@@ -74,11 +72,11 @@ class SlamStreamer:
Pushes a scan over zmq using protocol buffers. Pushes a scan over zmq using protocol buffers.
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 = SlamScan(map=bytes(mapbytes),
location = SlamLocation(x = location[0], y = location[1], theta = location[3])) location=SlamLocation(x=location[0], y=location[1], theta=location[3]))
print('Sending map') print('Sending map')
self._socket.send(protoScan.SerializeToString()) self._socket.send_multipart(b'slam_map', protoScan.SerializeToString())
def stop_scanning(self): def stop_scanning(self):
self.can_scan = False self.can_scan = False
@@ -87,7 +85,7 @@ class SlamStreamer:
return zmq.Context.instance() return zmq.Context.instance()
def _create_socket(self, context): def _create_socket(self, context):
return context.socket(zmq.PAIR) return context.socket(zmq.PUB)
def _start_socket(self, socket, port): def _start_socket(self, socket, port):
socket.bind('tcp://*:' + str(self._port)) socket.bind('tcp://*:' + str(self._port))
@@ -124,4 +122,4 @@ class SlamStreamer:
@port.setter @port.setter
def port(self, value): def port(self, value):
self._port = value self._port = value