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

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.