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

Rework lidar cache to support grpc streaming

Browse Source
This commit is contained in:
Piv
2020-05-31 16:01:43 +09:30
parent 7750fa80d7
commit 31d6bed897
5 changed files with 105 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

62
app/src/main/java/org/vato/carcontroller/LIDAR/LidarView.java
View File

@@ -18,9 +18,14 @@ import com.google.protobuf.Empty;
import org.vato.carcontroller.PersonTrackingGrpc;
import org.vato.carcontroller.PointScan;
import org.vato.carcontroller.StreamMessage;
import org.vato.carcontroller.Updaters.AbstractUpdater;
import org.vato.carcontroller.Updaters.GrpcUpdater;
import org.vato.carcontroller.Updaters.ZmqUpdater;
import java.util.HashMap;
import java.util.Map;
import java.util.Objects;
import java.util.stream.Collectors;
import io.grpc.ManagedChannel;
@@ -28,7 +33,8 @@ import io.grpc.ManagedChannelBuilder;
import io.grpc.stub.StreamObserver;
public class LidarView extends SurfaceView
implements AbstractUpdater.MapChangedListener<PointScan> {
implements AbstractUpdater.MapChangedListener<PointScan>,
GrpcUpdater.GrpcUpdateBootstrapper<PointScan> {
private static final String LIDAR_TOPIC = "lidar_map";
@@ -36,7 +42,10 @@ public class LidarView extends SurfaceView
private Thread lidarThread;
private String port;
private SurfaceHolder surfaceHolder;
private boolean useGrpcStreams;
PersonTrackingGrpc.PersonTrackingStub stub;
private float timeBetweenMessages = 0.01f;
private Map<Integer, Paint> groupNumPaints = new HashMap<>();
private int mBitmapX, mBitmapY, mViewWidth, mViewHeight;
private Bitmap mBitmap;
@@ -61,8 +70,14 @@ public class LidarView extends SurfaceView
String host = prefs.getString("host", "10.0.0.53");
port = prefs.getString("zmqPort", "5050");
String gRPCPort = prefs.getString("port", "50051");
useGrpcStreams = prefs.getBoolean("use_grpc_streams", false);
if (useGrpcStreams) {
lidar = new GrpcUpdater<>(PointScan.getDefaultInstance().getParserForType(), this);
} else {
lidar = new ZmqUpdater<>(PointScan.getDefaultInstance().getParserForType(), LIDAR_TOPIC,
host, port);
}
lidar.addMapChangedListener(this);
surfaceHolder = getHolder();
lidarThread = new Thread(lidar);
@@ -75,7 +90,16 @@ public class LidarView extends SurfaceView
* Called by MainActivity.onResume() to start a thread.
*/
public void resume() {
StreamObserver<Empty> response = new StreamObserver<Empty>() {
if (useGrpcStreams) {
lidarThread.start();
} else {
doZmqLidarStream();
}
}
private void doZmqLidarStream() {
// use async grpc method, ZMQ doesn't need to connect straight away.
stub.startTracking(Empty.newBuilder().build(), new StreamObserver<Empty>() {
@Override
public void onNext(Empty value) {
lidarThread.start();
@@ -91,9 +115,7 @@ public class LidarView extends SurfaceView
public void onCompleted() {
// Don't care.
}
};
// use async grpc method, ZMQ doesn't need to connect straight away.
stub.startTracking(Empty.newBuilder().build(), response);
});
}
@Override
@@ -135,6 +157,7 @@ public class LidarView extends SurfaceView
try {
lidarThread.join(1000);
} catch (InterruptedException e) {
Log.d("LIDAR", "Lidar failed to join", e);
}
}
@@ -148,8 +171,18 @@ public class LidarView extends SurfaceView
Collectors.toList())) {
// Now for each point, draw a circle for the point (so it's big enough) in the correct spot,
// and create a colour for that point to paint it correctly.
// TODO: Dynamically change the colour of the paint object based on the point group number.
canvas.drawCircle((float) point.x, (float) point.y, 5, new Paint());
if (!groupNumPaints.containsKey(point.groupNumber)) {
Paint paint = new Paint();
paint.setColor(
Color.HSVToColor(new float[]{convertGroupNumberToHue(
point.groupNumber), 1f, 1f}));
groupNumPaints.put(point.groupNumber, paint);
}
// TODO:
canvas.drawCircle((float) point.x, (float) point.y, 5,
Objects.requireNonNull(groupNumPaints
.get(point.groupNumber))); // Can't be null as we just added it.
}
canvas.restore();
surfaceHolder.unlockCanvasAndPost(canvas);
@@ -157,17 +190,24 @@ public class LidarView extends SurfaceView
}
/**
*
* @param groupNumber
* @return
*/
private static int convertGroupNumberToHue(int groupNumber) {
return 0;
return (43 * groupNumber) % 360;
}
@Override
public void bootstrap(StreamObserver<PointScan> responseObserver) {
stub.lidarStream(
StreamMessage.newBuilder().setTimeBetweenMessages(timeBetweenMessages).build(),
responseObserver);
}
private static class Point {
private double x;
private double y;
private int groupNumber;
private Point(double x, double y) {
this.x = x;
@@ -175,7 +215,9 @@ public class LidarView extends SurfaceView
}
static Point fromProtoPoint(org.vato.carcontroller.Point point) {
return fromHist(point.getDistance(), point.getAngle());
Point p = fromHist(point.getDistance(), point.getAngle());
p.groupNumber = point.getGroupNumber();
return p;
}
static Point fromHist(double distance, double angle) {

2
app/src/main/java/org/vato/carcontroller/Updaters/GrpcUpdater.java
View File

@@ -10,7 +10,7 @@ import io.grpc.stub.StreamObserver;
public class GrpcUpdater<T extends MessageLite> extends AbstractUpdater<T> {
GrpcUpdateBootstrapper<T> bootstrapper;
public GrpcUpdater(Parser parser, GrpcUpdateBootstrapper bootstrapper) {
public GrpcUpdater(Parser<T> parser, GrpcUpdateBootstrapper<T> bootstrapper) {
super(parser);
this.bootstrapper = bootstrapper;
}

5
protobuf/src/main/proto/car/tracking/lidar_tracker.proto
View File

@@ -24,6 +24,9 @@ message PointScan{
repeated Point points = 1;
}
message StreamMessage{
float time_between_messages = 1;
}
service PersonTracking{
rpc set_tracking_group(Int32Value) returns (google.protobuf.Empty) {}
@@ -36,4 +39,6 @@ service PersonTracking{
rpc save_lidar(MotorControl.SaveRequest) returns (google.protobuf.Empty) {}
rpc lidar_stream(StreamMessage) returns (stream PointScan) {}
}

34
pycar/src/car/tracking/lidar_cache.py
View File

@@ -10,7 +10,7 @@ import time
class LidarCache():
"""
A class that retrieves scans from the lidar,
runs grouping algorithms between scans and
runs grouping algorithms on the scans and
keeps a copy of the group data.
"""
@@ -56,16 +56,10 @@ class LidarCache():
else:
self.currentGroups = algorithms.calc_groups(scan)
self.fireGroupsChanged()
self._fireGroupsChanged()
def fireGroupsChanged(self):
# Send the updated groups to 0MQ socket.
# Rename this to be a generic listener method, rather than an explicit 'send' (even though it can be treated as such already)
pointScan = tracker_pb.PointScan()
for group in self.currentGroups:
for point in group.get_points():
pointScan.points.append(tracker_pb.Point(
angle=point[1], distance=point[2], group_number=group.number))
def _fireGroupsChanged(self):
pointScan = self.current_scan
for listener in self._group_listeners:
listener(pointScan)
@@ -81,13 +75,31 @@ class LidarCache():
listener
An function that takes a PointScan proto object as its argument.
"""
if(listener not in self._group_listeners):
self._group_listeners.append(listener)
def clear_listeners(self):
"""
Clear all group change listeners.
"""
self._group_listeners = []
@property
def current_scan(self):
pointScan = tracker_pb.PointScan()
for group in self.currentGroups:
for point in group.get_points():
pointScan.points.append(tracker_pb.Point(
angle=point[1], distance=point[2], group_number=group.number))
return pointScan
def stop_scanning(self):
self.run = False
if __name__ == '__main__':
lidar = MockLidar(iter(lidar_loader.load_scans_bytes_file('car/src/car/tracking/out.pickle')))
lidar = MockLidar(iter(lidar_loader.load_scans_bytes_file(
'car/src/car/tracking/out.pickle')))
cache = LidarCache(lidar)
cache.add_groups_changed_listener(lambda a: print(a))
cache.start_cache()

22
pycar/src/car/tracking/lidar_servicer.py
View File

@@ -10,6 +10,7 @@ from car.messaging import messages
import car.tracking.algorithms as alg
import os
import google.protobuf.empty_pb2 as empty
import time
class LidarServicer(PersonTrackingServicer):
@@ -18,7 +19,6 @@ class LidarServicer(PersonTrackingServicer):
self._lidar = RecordingLidarDecorator(
lidar_factory.get_lidar())
self.cache = LidarCache(self._lidar, measurements=100)
self.cache.add_groups_changed_listener(self.onGroupsChanged)
self._mFactory = None
self._port = 50052 if 'CAR_ZMQ_PORT' not in os.environ else os.environ[
'CAR_ZMQ_PORT']
@@ -32,18 +32,22 @@ class LidarServicer(PersonTrackingServicer):
return empty.Empty()
def stop_tracking(self, request, context):
print('Stopping tracking')
self._should_stream = False
self.cache.stop_scanning()
self.cache.clear_listeners()
return empty.Empty()
def start_tracking(self, request, context):
"""Starts the lidar cache, streaming on the provided port."""
self._should_stream = True
# Want to rework this, like the sleam streamer/processer rework.
self.cache.add_groups_changed_listener(self.zmq_stream_listener)
self.cache.start_cache()
return empty.Empty()
def record(self, request, context):
# TODO: Fix this to not require
# TODO: Fix this to not require actually running the cache, just recording the lidar.
if request.value:
self.cache.start_cache()
else:
@@ -55,7 +59,17 @@ class LidarServicer(PersonTrackingServicer):
self._lidar.save_data(request.file)
return empty.Empty()
def onGroupsChanged(self, message):
def lidar_stream(self, request, context):
# Streams the state of the lidar cache.
print('Received Lidar gRPC Stream Start Request')
self._should_stream = True
self.cache.start_cache()
sleep_time = request.time_between_messages if request.time_between_messages > 0.01 else 0.01
while self._should_stream:
time.sleep(sleep_time)
yield self.cache.current_scan
def zmq_stream_listener(self, message):
if self._mFactory is None:
# Create the zmq socket in the thread that it will be used, just to be safe.
self._mFactory = mf.getZmqPubSubStreamer(self._port)
@@ -64,8 +78,10 @@ class LidarServicer(PersonTrackingServicer):
self._mFactory.send_message_topic(
"lidar_map", messages.ProtoMessage(message=message.SerializeToString()))
def on_groups_changed(self, message):
if self._tracked_group is not None and self._vehicle is not None:
# Update vehicle to correctly follow the tracked group.
# Leave for now, need to work out exactly how this will change.
# alg.dualServoChange(alg.find_centre())
# Put this in a separate module I think, shouldn't control the can autonomously from the servicer.
pass