Add recording functionality, did some other minor tweaks as well to protos

.idea/gradle.xml

@@ -1,5 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <project version="4">
+  <component name="GradleMigrationSettings" migrationVersion="1" />
   <component name="GradleSettings">
     <option name="linkedExternalProjectsSettings">
       <GradleProjectSettings>

.idea/vcs.xml

@@ -2,6 +2,5 @@
 <project version="4">
   <component name="VcsDirectoryMappings">
     <mapping directory="" vcs="Git" />
-    <mapping directory="$PROJECT_DIR$/car" vcs="Git" />
   </component>
 </project>

.vscode/launch.json

@@ -5,10 +5,14 @@
     "version": "0.2.0",
     "configurations": [
         {
-            "name": "Python: Module",
+            "name": "Car Module",
             "type": "python",
             "request": "launch",
-            "module": "car"
+            "module": "car",
+            "env": {
+                "CAR_LIDAR": "LIDAR_MOCK",
+                "CAR_VEHICLE": "CAR_MOCK"
+            }
         },
         {
             "name": "Python: Current File",

app/src/main/java/org/vato/carcontroller/LIDAR/LidarView.java

@@ -13,7 +13,8 @@ import android.view.SurfaceView;
 
 import androidx.preference.PreferenceManager;
 
-import org.vato.carcontroller.Empty;
+import com.google.protobuf.Empty;
+
 import org.vato.carcontroller.Int32Value;
 import org.vato.carcontroller.PersonTrackingGrpc;
 import org.vato.carcontroller.PointScan;
@@ -89,7 +90,7 @@ public class LidarView extends SurfaceView implements AbstractUpdater.MapChanged
             }
         };
         // use async grpc method, ZMQ doesn't need to connect straight away.
-        stub.startTracking(Int32Value.newBuilder().setValue(Integer.parseInt(port)).build(), response);
+        stub.startTracking(Empty.newBuilder().build(), response);
     }
 
     @Override

app/src/main/java/org/vato/carcontroller/SLAM/SlamView.java

@@ -13,8 +13,8 @@ import android.view.SurfaceView;
 import androidx.preference.PreferenceManager;
 
 import com.google.protobuf.ByteString;
+import com.google.protobuf.Empty;
 
-import org.vato.carcontroller.Empty;
 import org.vato.carcontroller.SlamControlGrpc;
 import org.vato.carcontroller.SlamDetails;
 import org.vato.carcontroller.SlamLocation;
@@ -57,7 +57,7 @@ public class SlamView extends SurfaceView implements AbstractUpdater.MapChangedL
     private void init() {
         SharedPreferences prefs = PreferenceManager.getDefaultSharedPreferences(getContext());
         String host = prefs.getString("host", "10.0.0.53");
-        port = prefs.getString("zmqPort", "5050");
+        port = prefs.getString("zmqPort", "50052");
         String gRPCPort = prefs.getString("port", "50051");
         mapSizePixels = Integer.parseInt(prefs.getString("MAPSIZEPIXELS", "540"));
         mapSizeMeters = Integer.parseInt(prefs.getString("MAPSIZEMETRES", "10"));
@@ -96,7 +96,6 @@ public class SlamView extends SurfaceView implements AbstractUpdater.MapChangedL
         stub.startMapStreaming(SlamDetails.newBuilder()
                 .setMapSizePixels(mapSizePixels)
                 .setMapSizeMeters(mapSizeMeters)
-                .setPort(Integer.parseInt(port))
                 .build(), response);
     }
 

car/src/car/Messaging/message_factory.py

@@ -22,7 +22,7 @@ class ZmqPubSubStreamer:
         self.send_message_topic("", message)
 
     def send_message_topic(self, topic, message):
-        self._socket.send_multipart([bytes(topic), message.serialise()])
+        self._socket.send_multipart([topic, message.serialise()])
 
 
 class BluetoothStreamer:

car/src/car/control/gpio/recording_vehicle_decorator.py

@@ -0,0 +1,82 @@
+import datetime
+
+
+class VehicleRecordingDecorator:
+    def __init__(self, vehicle):
+        """
+        A decorator for a vehicle object to record the changes in steering/throttle.
+        This will be recorded to memory, and will save to the given file when save is called.
+
+        Parameters
+        ----------
+
+        vehicle
+            Base vehicle to decorate.
+
+        outfile: str
+            Filename to write to. Will create/overwrite existing file. You must call save to save the 
+            data to the file.
+        """
+        self._vehicle = vehicle
+        self._recording = False
+        self._records = []
+
+    @property
+    def vehicle(self):
+        return self._vehicle
+
+    def save_data(self, outfile):
+        """
+        Flushes all current records to disk.
+        """
+        with open(outfile, 'w') as f:
+            for line in self._records:
+                f.write('%s\n' % line)
+        self._records = []
+
+    @property
+    def record(self):
+        return self._recording
+
+    @record.setter
+    def record(self, value):
+        self._recording = True
+
+    @property
+    def throttle(self):
+        return self._vehicle.throttle
+
+    @throttle.setter
+    def throttle(self, value):
+        if self._recording:
+            self._records.append(('t', value, str(datetime.datetime.now())))
+        self._vehicle.throttle = value
+
+    @property
+    def steering(self):
+        return self._vehicle.steering
+
+    @steering.setter
+    def steering(self, value):
+        if self._recording:
+            self._records.append(('s', value, str(datetime.datetime.now())))
+        self._vehicle.steering = value
+
+    @property
+    def motor_pin(self):
+        return self._vehicle.motor_pin
+
+    @motor_pin.setter
+    def motor_pin(self, value):
+        self._vehicle.motor_pin = value
+
+    @property
+    def steering_pin(self):
+        return self._vehicle.steering_pin
+
+    @steering_pin.setter
+    def steering_pin(self, value):
+        self._vehicle.steering_pin = value
+
+    def stop(self):
+        self.throttle = 0

car/src/car/control/motor_servicer.py

@@ -4,10 +4,12 @@ import time
 
 import car.control.motorService_pb2 as motorService_pb2
 import car.control.motorService_pb2_grpc as motorService_pb2_grpc
+from car.control.gpio.recording_vehicle_decorator import VehicleRecordingDecorator
+
 
 class MotorServicer(motorService_pb2_grpc.CarControlServicer):
     def __init__(self, vehicle):
-        self.vehicle = vehicle
+        self.vehicle = VehicleRecordingDecorator(vehicle)
         self._timer = None
 
     def SetThrottle(self, request, context):
@@ -38,3 +40,9 @@ class MotorServicer(motorService_pb2_grpc.CarControlServicer):
         print("Node timeout elapsed")
         self.vehicle.stop()
 
+    def Record(self, request, context):
+        """Indicate whether the vehicle data should be recorded."""
+        self.vehicle.record = request.record
+
+    def SaveRecordedData(self, request, context):
+        self.vehicle.save_data(request.file)

car/src/car/slam/slam_streamer.py

@@ -83,7 +83,7 @@ class SlamStreamer:
                                                            location=SlamLocation(x=location[0], y=location[1], theta=location[2])))
         print('Sending map')
         self._mFactory.send_message_topic(
-            'slam_map', protoScan)
+            b'slam_map', protoScan)
 
     def stop_scanning(self):
         self.can_scan = False

car/src/car/tracking/devices/recording_lidar.py

@@ -0,0 +1,52 @@
+import datetime
+
+
+class RecordingLidarDecorator:
+
+    def __init__(self, lidar):
+        self._lidar = lidar
+        self._scans = []
+        self._record = False
+
+    @property
+    def record(self):
+        return self._record
+
+    @record.setter
+    def record(self, value):
+        self.record = value
+
+    def save_data(self, filename):
+        with open(filename, 'w') as f:
+            for scan in self._scans:
+                f.write("%s\n" % scan)
+
+    def iter_scans(self, min_len=100):
+        # Need to customise the iterable.
+        return RecordingIterator(self._lidar.iter_scans(min_len), self._scans)
+
+    def get_health(self):
+        return self._lidar.get_health()
+
+    def get_info(self):
+        return self._lidar.get_info()
+
+    def stop(self):
+        return self._lidar.stop()
+
+    def disconnect(self):
+        return self._lidar.disconnect()
+
+
+class RecordingIterator:
+    def __init__(self, iterator, scan_list):
+        self._iterator = iterator
+        self._scans = scan_list
+
+    def __iter__(self):
+        return self
+
+    def __next__(self):
+        nextIter = next(self._iterator)
+        self._scans.append((nextIter, str(datetime.datetime.now())))
+        return nextIter

car/src/car/tracking/lidar_servicer.py

@@ -4,6 +4,7 @@ from car.tracking.lidar_cache import LidarCache
 from multiprocessing import Process
 import car.messaging.message_factory as mf
 import car.tracking.devices.factory as lidar_factory
+from car.tracking.devices.recording_lidar import RecordingLidarDecorator
 
 from car.messaging import messages
 import car.tracking.algorithms as alg
@@ -15,7 +16,8 @@ class LidarServicer(PersonTrackingServicer):
     def __init__(self, vehicle=None):
         # TODO: Put the rplidar creation in a factory or something, to make it possible to test this servicer.
         # Also, it would allow creating the service without the lidar being connected.
-        self.cache = LidarCache(lidar_factory.get_lidar(), measurements=100)
+        self.cache = LidarCache(RecordingLidarDecorator(
+            lidar_factory.get_lidar()), measurements=100)
         self.cache.add_groups_changed_listener(self)
         self._mFactory = None
         self._port = 50052 if 'CAR_ZMQ_PORT' not in os.environ else os.environ[
@@ -34,6 +36,10 @@ class LidarServicer(PersonTrackingServicer):
         """Starts the lidar cache, streaming on the provided port."""
         self.cache.start_cache()
 
+    def setRecordData(self, request, context):
+        # Set the vehicles to record their changes.
+        pass
+
     def onGroupsChanged(self, message):
         if self._mFactory is None:
             # Create the zmq socket in the thread that it will be used, just to be safe.

protobuf/src/main/proto/car/control/motorService.proto

@@ -7,6 +7,8 @@ option java_multiple_files = true;
 option java_package = "org.vato.carcontroller";
 option java_outer_classname = "MotorServiceProto";
 
+import "google/protobuf/empty.proto";
+
 message ThrottleRequest{
     float throttle = 1;
 }
@@ -23,7 +25,17 @@ message SteeringResponse{
     bool steeringSet = 1;
 }
 
+message RecordingReqeust{
+    bool record = 1;
+}
+
+message SaveRequest{
+    string file = 1;
+}
+
 service CarControl{
     rpc SetThrottle(ThrottleRequest) returns (ThrottleResponse){}
     rpc SetSteering(SteeringRequest) returns (SteeringResponse){}
+    rpc Record(RecordingReqeust) returns (google.protobuf.Empty) {}
+    rpc SaveRecordedData(SaveRequest) returns (google.protobuf.Empty) {}
 }

protobuf/src/main/proto/car/empty.proto

@@ -1,9 +0,0 @@
-syntax = "proto3";
-
-option java_multiple_files = true;
-option java_package = "org.vato.carcontroller";
-option java_outer_classname = "EmptyProto";
-
-message Empty{
-
-}

protobuf/src/main/proto/car/slam/SlamController.proto

@@ -4,7 +4,7 @@ option java_multiple_files = true;
 option java_package = "org.vato.carcontroller";
 option java_outer_classname = "SlamControllerProto";
 
-import "car/empty.proto";
+import "google/protobuf/empty.proto";
 
 message SlamDetails {
     int32 map_size_pixels = 1;
@@ -27,7 +27,7 @@ message SlamScan{
 }
 
 service SlamControl {
-    rpc start_map_streaming(SlamDetails) returns (Empty) {}
+    rpc start_map_streaming(SlamDetails) returns (google.protobuf.Empty) {}
 
-    rpc stop_streaming(Empty) returns (Empty) {}
+    rpc stop_streaming(google.protobuf.Empty) returns (google.protobuf.Empty) {}
 }

protobuf/src/main/proto/car/tracking/lidar_tracker.proto

@@ -6,7 +6,7 @@ option java_multiple_files = true;
 option java_package = "org.vato.carcontroller";
 option java_outer_classname = "PersonTrackingProto";
 
-import "car/empty.proto";
+import "google/protobuf/empty.proto";
 
 message Int32Value{
     int32 value = 1;
@@ -23,9 +23,9 @@ message PointScan{
 }
 
 service PersonTracking{
-    rpc set_tracking_group(Int32Value) returns (Empty) {}
+    rpc set_tracking_group(Int32Value) returns (google.protobuf.Empty) {}
 
-    rpc stop_tracking(Empty) returns (Empty) {}
+    rpc stop_tracking(google.protobuf.Empty) returns (google.protobuf.Empty) {}
 
-    rpc start_tracking(Empty) returns (Empty) {}
+    rpc start_tracking(google.protobuf.Empty) returns (google.protobuf.Empty) {}
 }