Add command to zero IMU and reworked OSC handling (#16)

Co-authored-by: Levi Lesches <[email protected]>

.github/pubspec_overrides.yaml

@@ -4,4 +4,4 @@ dependency_overrides:
   burt_network:
     git:
       url: https://github.com/BinghamtonRover/Networking
-      ref: 2.3.1
+      ref: 2.4.0

lib/src/devices/imu.dart

@@ -1,11 +1,13 @@
 import "dart:async";
-import "dart:typed_data";
 
+import "package:collection/collection.dart";
 import "package:osc/osc.dart";
 
 import "package:subsystems/subsystems.dart";
 import "package:burt_network/burt_network.dart";
 
+import "../osc.dart";
+
 /// The serial port that the IMU is connected to.
 const imuPort = "/dev/rover-imu";
 
@@ -23,51 +25,46 @@ class ImuReader extends Service {
 
   /// The subscription that will be notified when a new serial packet arrives.
   StreamSubscription<List<int>>? subscription;
+  StreamSubscription<SubsystemsCommand>? _commandSubscription;
 
-  /// Parses an OSC bundle from a list of bytes.
-  void handleOsc(List<int> data) {
-    try {
-      // skip 8 byte "#bundle" + 8 byte timestamp + 4 byte data length
-      final buffer = data.sublist(20);
-      final message = OSCMessage.fromBytes(buffer);
-      if (message.address != "/euler") return;
-      final orientation = Orientation(
-        x: message.arguments[0] as double,
-        y: message.arguments[1] as double,
-        z: message.arguments[2] as double,
-      );
-      final position = RoverPosition(orientation: orientation, version: positionVersion);
-      collection.server.sendMessage(position);
-      collection.server.sendMessage(position, destination: autonomySocket);
-    } catch (error) {
-      /* Ignore corrupt data */
+  /// Handles an incoming [SubsystemsCommand]
+  void handleCommand(SubsystemsCommand command) {
+    if (command.zeroIMU && serial.isOpen) {
+      final message = OSCMessage("/ahrs/zero", arguments: []).toBytes();
+      serial.write(slip.encode(message).toUint8List());
     }
   }
 
-  /// Removes bytes inserted by the SLIP protocol.
-  ///
-  /// This function is here until `package:osc` supports SLIP, mandated by the OSC v1.1 spec.
-  /// See this issue: https://github.com/pq/osc/issues/24
-  /// See: https://en.wikipedia.org/wiki/Serial_Line_Internet_Protocol
-  Uint8List processSlip(List<int> data) {
-    const end = 192;
-    const esc = 219;
-    const escEnd = 220;
-    const escEsc = 221;
-    final newPacket = <int>[];
-    var prevElement = 0;
-    for (final element in data) {
-      if (prevElement == esc && element == escEnd) {
-        newPacket.last = end;  // ESC + ESC_END -> END
-      } else if (prevElement == esc && element == escEsc) {
-        newPacket.last = esc;  // ESC + ESC_ESC -> ESC
-      } else {
-        newPacket.add(element);
+  /// Handles incoming serial bytes
+  void handleSerial(List<int> bytes) {
+    for (final packet in bytes.splitAfter((element) => element == end)) {
+      final message = parseOsc(slip.decode(packet));
+      if (message == null) {
+        continue;
+      }
+      if (message.address == "/button") {
+        handleCommand(SubsystemsCommand(zeroIMU: true));
+      }
+      if (message.address == "/ahrs/zero") {
+        // signal that the zero was received and processed
+        if (serial.isOpen) {
+          final command = OSCMessage("/identify", arguments: []);
+          serial.write(slip.encode(command.toBytes()).toUint8List());
+        }
+        // send a duplicate of a subsystems command as a "handshake"
+        collection.server.sendMessage(SubsystemsCommand(zeroIMU: true));
+      }
+      if (message.address == "/euler") {
+        final orientation = Orientation(
+          x: message.arguments[0] as double,
+          y: message.arguments[1] as double,
+          z: message.arguments[2] as double,
+        );
+        final position = RoverPosition(orientation: orientation, version: positionVersion);
+        collection.server.sendMessage(position);
+        collection.server.sendMessage(position, destination: autonomySocket);
       }
-      prevElement = element;
     }
-    if (newPacket.last == end) newPacket.removeLast();
-    return Uint8List.fromList(newPacket);
   }
 
   @override
@@ -77,7 +74,12 @@ class ImuReader extends Service {
         logger.critical("Could not open IMU on port $imuPort");
         return false;
       }
-      subscription = serial.stream.map(processSlip).listen(handleOsc);
+      subscription = serial.stream.listen(handleSerial);
+      _commandSubscription = collection.server.messages.onMessage(
+        name: SubsystemsCommand().messageName,
+        constructor: SubsystemsCommand.fromBuffer,
+        callback: handleCommand,
+      );
       serial.startListening();
       logger.info("Reading IMU on port $imuPort");
       return true;
@@ -90,6 +92,7 @@ class ImuReader extends Service {
   @override
   Future<void> dispose() async {
     await subscription?.cancel();
+    await _commandSubscription?.cancel();
     await serial.dispose();
   }
 }

lib/src/osc.dart

@@ -0,0 +1,101 @@
+import "dart:convert";
+import "dart:typed_data";
+
+import "package:collection/collection.dart";
+import "package:osc/osc.dart";
+
+/// Frame end
+const end = 192;
+
+/// Frame esc
+const esc = 219;
+
+/// Transposed frame end
+const escEnd = 220;
+
+/// Transposed frame escape
+const escEsc = 221;
+
+/// The bytes of the OSC message #bundle header
+final bundleHeader = const Utf8Encoder().convert("#bundle");
+
+/// Parses an OSC bundle from a list of bytes.
+OSCMessage? parseOsc(List<int> data) {
+  try {
+    List<int> buffer;
+    // If multiple messages are sent at once, it won't have the #bundle header
+    final hasHeader = data.length > bundleHeader.length
+      && data.sublist(0, bundleHeader.length).equals(bundleHeader);
+    if (hasHeader) {
+      // skip 8 byte "#bundle" + 8 byte timestamp + 4 byte data length
+      buffer = data.sublist(20);
+    } else {
+      buffer = data;
+    }
+    return OSCMessage.fromBytes(buffer);
+  } catch (error) {
+    /* Ignore corrupt data */
+    return null;
+  }
+}
+
+/// This [SLIP](https://en.wikipedia.org/wiki/Serial_Line_Internet_Protocol) codec.
+///
+/// This converter is here until `package:osc` supports SLIP, mandated by the OSC v1.1 spec.
+/// See this issue: https://github.com/pq/osc/issues/24
+class SlipCodec extends Codec<List<int>, List<int>> {
+  @override
+  SlipEncoder get encoder => SlipEncoder();
+
+  @override
+  SlipDecoder get decoder => SlipDecoder();
+}
+
+/// Adds bytes as specified the SLIP protocol.
+class SlipEncoder extends Converter<List<int>, List<int>> {
+  @override
+  List<int> convert(List<int> input) {
+    final newPacket = <int>[];
+    for (final element in input) {
+      if (element == end) {
+        newPacket.addAll([esc, escEnd]);
+      } else if (element == esc) {
+        newPacket.addAll([esc, escEsc]);
+      } else {
+        newPacket.add(element);
+      }
+    }
+    newPacket.add(end);
+    return newPacket;
+  }
+}
+
+/// Removes bytes inserted by the SLIP protocol.
+class SlipDecoder extends Converter<List<int>, List<int>> {
+  @override
+  List<int> convert(List<int> input) {
+    final newPacket = <int>[];
+    var prevElement = 0;
+    for (final element in input) {
+      if (prevElement == esc && element == escEnd) {
+        newPacket.last = end;  // ESC + ESC_END -> END
+      } else if (prevElement == esc && element == escEsc) {
+        newPacket.last = esc;  // ESC + ESC_ESC -> ESC
+      } else {
+        newPacket.add(element);
+      }
+      prevElement = element;
+    }
+    if (newPacket.last == end) newPacket.removeLast();
+    return newPacket;
+  }
+}
+
+/// Helpful methods on lists of bytes.
+extension ListToUint8List on List<int> {
+  /// Converts to a more efficient [Uint8List].
+  Uint8List toUint8List() => Uint8List.fromList(this);
+}
+
+/// The global SLIP codec.
+final slip = SlipCodec();