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

Format files and add gRPC stream support.

Browse Source
This commit is contained in:
Piv
2020-05-03 16:10:40 +09:30
parent ff73d09855
commit 7fb20f9d86
14 changed files with 346 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
car/build.gradle
View File

@@ -12,7 +12,7 @@ dependencies {
task copyPythonCode(type: Copy, dependsOn: configurations.python){
// Copy python protobuf code from proto project.
from zipTree(configurations.python.asPath)
into './src'
into 'src'
}
task build(type: Exec, dependsOn: copyPythonCode) {

17
car/src/car/control/motor_servicer.py
View File

@@ -12,6 +12,7 @@ class MotorServicer(motorService_pb2_grpc.CarControlServicer):
def __init__(self, vehicle):
self.vehicle = VehicleRecordingDecorator(vehicle)
self._timer = None
self._timeout_elapsed = False
def set_throttle(self, request, context):
# gRPC streams currently don't work between python and android.
@@ -26,6 +27,21 @@ class MotorServicer(motorService_pb2_grpc.CarControlServicer):
self.vehicle.steering = request.steering
return motorService_pb2.SteeringResponse(steeringSet=True)
def stream_vehicle_2d(self, request_iterator, context):
print('Starting Vehicle Control Stream')
self._timeout_elapsed = False
for request_2d in request_iterator:
# End the stream if the user fails to respond in time.
if self._timeout_elapsed:
break
print('Setting 2d values to: ' + str((request_2d.throttle.throttle, request_2d.steering.steering)))
self.set_timeout(3)
# TODO: Make a vehicle method set 2d throttle/steering.
self.vehicle.throttle = request_2d.throttle.throttle
self.vehicle.steering = request_2d.steering.steering
return empty.Empty()
def set_timeout(self, min_timeout):
"""Stops the old timer and restarts it to the specified time.
@@ -39,6 +55,7 @@ class MotorServicer(motorService_pb2_grpc.CarControlServicer):
def timeout_elapsed(self):
"""Election or heartbeat timeout has elapsed."""
print("Node timeout elapsed")
self._timeout_elapsed = True
self.vehicle.stop()
def record(self, request, context):

87
car/src/car/slam/slam_processor.py Normal file
View File

@@ -0,0 +1,87 @@
import car.tracking.devices.factory as lidar_fact
from breezyslam.algorithms import RMHC_SLAM
from breezyslam.sensors import RPLidarA1 as LaserModel
class SlamProcessor:
def __init__(self, map_pixels=None, map_meters=None):
self._map_pixels = map_pixels
self._map_meters = map_meters
def do_scanning(self):
self.can_scan = True
# Adapted from BreezySLAM rpslam example.
# Connect to Lidar unit. For some reason it likes to be done twice, otherwise it errors out...
lidar = lidar_fact.get_lidar()
lidar = lidar_fact.get_lidar()
print('Initialised lidar')
# Create an RMHC SLAM object with a laser model and optional robot model
slam = RMHC_SLAM(LaserModel(), self._map_pixels, self._map_meters)
print('initialised slam')
# Initialize empty map
mapbytes = bytearray(self.map_pixels * self.map_pixels)
print('Initialised byte []')
# Create an iterator to collect scan data from the RPLidar
iterator = lidar.iter_scans()
print('Scanning')
while self.can_scan:
# Extract (quality, angle, distance) triples from current scan
# This can still sometimes fail, at least on macOS.
try:
items = [item for item in next(iterator)]
except Exception:
# Ignore errors for now.
print('Failed to retrieve some scans from lidar.')
continue
# Extract distances and angles from triples
distances = [item[2] 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)
print('Map updated')
slam.getmap(mapbytes)
# Make a generator that can be iterated on by this function.
yield (mapbytes, slam.getpos())
# Close the generator when user stops scanning.
raise StopIteration
def stop_scanning(self):
self.can_scan = False
# Properties
@property
def map_pixels(self):
return self._map_pixels
@map_pixels.setter
def map_pixels(self, value):
self._map_pixels = value
@property
def map_meters(self):
return self._map_meters
@map_meters.setter
def map_meters(self, value):
self._map_meters = value
@property
def lidar_connection(self):
return self._lidar_connection
@lidar_connection.setter
def lidar_connection(self, value):
self._lidar_connection = value

21
car/src/car/slam/slam_servicer.py
View File

@@ -2,8 +2,11 @@ import car.slam.SlamController_pb2_grpc as grpc
import car.slam.SlamController_pb2 as proto
import car.empty_pb2 as empty
import car.slam.slam_streamer as slam
from .slam_processor import SlamProcessor
from multiprocessing import Process
import os
from car.slam.SlamController_pb2 import SlamLocation, SlamScan
class SlamServicer(grpc.SlamControlServicer):
@@ -11,24 +14,34 @@ class SlamServicer(grpc.SlamControlServicer):
def __init__(self):
print('Servicer initialised')
self.slam = slam.SlamStreamer()
self.slam = SlamProcessor()
def start_map_streaming(self, request, context):
print('Received Map Start Streaming Request')
slam_streamer = slam.SlamStreamer(self.slam)
if self.slam_thread is None:
print('initialising slam_thread')
# Don't bother creating and starting slam more than once.
self.slam.port = 50052 if "CAR_ZMQ_PORT" not in os.environ else os.environ[
slam_streamer.port = 50052 if "CAR_ZMQ_PORT" not in os.environ else os.environ[
'CAR_ZMQ_PORT']
self.slam.map_pixels = request.map_size_pixels
self.slam.map_meters = request.map_size_meters
self.slam_thread = Process(target=self.slam.start)
self.slam_thread = Process(target=slam_streamer.start)
self.slam_thread.start()
return empty.Empty()
def map_stream(self, request, context):
print('Received Map gRPC Stream Start Request')
self.slam.map_meters = request.map_size_meters
self.slam.map_pixels = request.map_size_pixels
# If expression doesn't work, then just do yields in a for loop.
return (SlamScan(map=bytes(m), location=SlamLocation(x=pos[0], y=pos[1], theta=pos[2])) for m, pos in self.slam.do_scanning())
def stop_streaming(self, request, context):
# Need to adapt this to also be able to stop gRPC streamer.
self.slam.stop_scanning()
if self.slam_thread is not None:
self.slam.stop_scanning()
self.slam_thread.join()
self.slam = None
return empty.Empty()

79
car/src/car/slam/slam_streamer.py
View File

@@ -1,10 +1,9 @@
import zmq
from breezyslam.algorithms import RMHC_SLAM
from breezyslam.sensors import RPLidarA1 as LaserModel
from car.slam.SlamController_pb2 import SlamScan, SlamLocation
import car.messaging.message_factory as mf
import car.messaging.messages as messages
import car.tracking.devices.factory as lidar_fact
from .slam_processor import SlamProcessor
# Left here as was used in the example, configure as necessary.
@@ -15,10 +14,12 @@ import car.tracking.devices.factory as lidar_fact
class SlamStreamer:
can_scan = False
def __init__(self, map_pixels=None, map_meters=None, port=None):
self._map_pixels = map_pixels
self._map_meters = map_meters
def __init__(self, processor, port=None):
"""
ZMQ implementation to stream slam map.
"""
self._port = port
self.processor = processor
def start(self):
'''
@@ -31,47 +32,12 @@ class SlamStreamer:
to calling this method, and changing those values after
calling this method will have no effect.
'''
self.can_scan = True
print('Starting to stream')
# TODO: Don't get the messenger here, pass it in/specify another way.
self._mFactory = mf.getZmqPubSubStreamer(self._port)
print('Started and bound zmq socket.')
# Adapted from BreezySLAM rpslam example.
# Connect to Lidar unit. For some reason it likes to be done twice, otherwise it errors out...
lidar = lidar_fact.get_lidar()
lidar = lidar_fact.get_lidar()
print('Initialised lidar')
# Create an RMHC SLAM object with a laser model and optional robot model
slam = RMHC_SLAM(LaserModel(), self._map_pixels, self._map_meters)
print('initialised slam')
# Initialize empty map
mapbytes = bytearray(self.map_pixels * self.map_pixels)
print('Initialised byte []')
# Create an iterator to collect scan data from the RPLidar
iterator = lidar.iter_scans()
print('Scanning')
while self.can_scan:
# Extract (quality, angle, distance) triples from current scan
items = [item for item in next(iterator)]
# Extract distances and angles from triples
distances = [item[2] 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)
print('Map updated')
slam.getmap(mapbytes)
self._push_map(mapbytes, slam.getpos())
for map, pos in self.processor.do_scanning():
self._push_map(map, pos)
def _push_map(self, mapbytes, location):
'''
@@ -86,32 +52,7 @@ class SlamStreamer:
b'slam_map', protoScan)
def stop_scanning(self):
self.can_scan = False
# Properties
@property
def map_pixels(self):
return self._map_pixels
@map_pixels.setter
def map_pixels(self, value):
self._map_pixels = value
@property
def map_meters(self):
return self._map_meters
@map_meters.setter
def map_meters(self, value):
self._map_meters = value
@property
def lidar_connection(self):
return self._lidar_connection
@lidar_connection.setter
def lidar_connection(self, value):
self._lidar_connection = value
self.processor.stop_scanning()
@property
def port(self):