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

Use multiprocessing for slam, add more debug prints

Browse Source
This commit is contained in:
Piv
2020-02-12 22:03:25 +10:30
parent 3a9906f7da
commit 65d5e94c0d
2 changed files with 7 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
SlamController/slam_servicer.py
View File

@@ -1,7 +1,7 @@
from . import SlamController_pb2_grpc from . import SlamController_pb2_grpc
from . import SlamController_pb2 from . import SlamController_pb2
from . import slam_streamer as slam from . import slam_streamer as slam
from threading import Thread from multiprocessing import Process
class SlamServicer(SlamController_pb2_grpc.SlamControlServicer): class SlamServicer(SlamController_pb2_grpc.SlamControlServicer):
@@ -19,12 +19,13 @@ class SlamServicer(SlamController_pb2_grpc.SlamControlServicer):
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
self.slam_thread = Thread(target=self.slam.start) self.slam_thread = Process(target=self.slam.start)
self.slam_thread.start() self.slam_thread.start()
return SlamController_pb2.Empty() return SlamController_pb2.Empty()
def stop_streaming(self, request, context): def stop_streaming(self, request, context):
if self.slam_thread is not None: if self.slam_thread is not None:
self.slam.stop_scanning() self.slam.stop_scanning()
self.slam_thread.join()
self.slam = None self.slam = None
return SlamController_pb2.Empty() return SlamController_pb2.Empty()

6
SlamController/slam_streamer.py
View File

@@ -32,6 +32,7 @@ class SlamStreamer:
calling this method will have no effect. calling this method will have no effect.
''' '''
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. # Block until user opens zmq.
@@ -60,10 +61,10 @@ 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')
# 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)
print('Map updated')
self._push_map(slam.getmap(mapbytes), slam.getpos()) self._push_map(slam.getmap(mapbytes), slam.getpos())
def _push_map(self, mapbytes, location): def _push_map(self, mapbytes, location):
@@ -74,6 +75,7 @@ class SlamStreamer:
''' '''
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')
self._socket.send(protoScan.SerializeToString()) self._socket.send(protoScan.SerializeToString())
def stop_scanning(self): def stop_scanning(self):