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

Use PAIR socket for slam, tidy up python stuff.

Browse Source
This commit is contained in:
Piv
2020-02-06 21:21:33 +10:30
parent 24bd93043e
commit 3b6df222eb
2 changed files with 11 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
SlamController/slam_servicer.py
View File

@@ -3,19 +3,21 @@ import slam_streamer as slam
from threading import Thread from threading import Thread
class SlamServicer(SlamController_pb2_grpc.SlamControlServicer): class SlamServicer(SlamController_pb2_grpc.SlamControlServicer):
slam_thread = None
def __init__(self, lidar_connection): def __init__(self, lidar_connection):
print('Servicer initialised') print('Servicer initialised')
self.slam = slam.SlamStreamer(lidar_connection=lidar_connection) self.slam = slam.SlamStreamer(lidar_connection=lidar_connection)
def start_map_streaming(self, request, context): def start_map_streaming(self, request, context):
if not hasattr(self, 'slamThread'): if self.slam_thread is None:
# Don't bother creating and starting slam more than once. # Don't bother creating and starting slam more than once.
self.slam.port = request.port self.slam.port = request.port
self.slam.map_pixels = request.map_size_pixels self.slam.map_pixels = request.map_size_pixels
self.slam.map_meters = request.map_size_meters self.slam.map_meters = request.map_size_meters
slamThread = Thread(target=self.slam.start) self.slam_thread = Thread(target=self.slam.start)
slamThread.start() self.slam_thread.start()
def stop_streaming(self, request, context): def stop_streaming(self, request, context):
if hasattr(self, 'slamThread'): if self.slam_thread is not None:
self.slam.stop_scanning() self.slam.stop_scanning()

6
SlamController/slam_streamer.py
View File

@@ -31,6 +31,10 @@ class SlamStreamer:
to calling this method, and changing those values after to calling this method, and changing those values after
calling this method will have no effect. calling this method will have no effect.
''' '''
# Block until user opens zmq.
self._socket.recv()
# Adapted from BreezySLAM rpslam example. # Adapted from BreezySLAM rpslam example.
# Connect to Lidar unit # Connect to Lidar unit
lidar = Lidar(self._lidar_connection) lidar = Lidar(self._lidar_connection)
@@ -77,7 +81,7 @@ class SlamStreamer:
return zmq.Context.instance() return zmq.Context.instance()
def _create_tcp_pub_socket(self): def _create_tcp_pub_socket(self):
return self._create_context().socket(zmq.PUB) return self._create_context().socket(zmq.PAIR)
def _start_socket(self, socket, port): def _start_socket(self, socket, port):
socket.bind('tcp://*:' + str(self._port)) socket.bind('tcp://*:' + str(self._port))