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

20
SlamController/slam_streamer.py
View File

@@ -13,7 +13,7 @@ from .SlamController_pb2 import SlamScan, SlamLocation
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. print('Started and bound zmq socket.')
self._socket.recv()
print('Received message from ZMQ')
# 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)
@@ -75,10 +73,10 @@ 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 = 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))