Autonomy and new streams-based networking (#12)

Co-authored-by: Binghamton Rover <[email protected]>

.github/workflows/analyzer.yml

@@ -16,12 +16,12 @@ jobs:
       # You can specify other versions if desired, see documentation here:
       # https://github.com/dart-lang/setup-dart/blob/main/README.md
       - uses: dart-lang/setup-dart@v1
-        with: 
-          sdk: 3.2.2
+        with:
+          sdk: 3.5.2
 
       - name: Install dependencies
         run: dart pub get
-        
+
       - name: Analyze project source
         run: dart analyze --fatal-infos
 

bin/data.dart

@@ -1,7 +1,6 @@
 import "dart:math";
 
 import "package:burt_network/burt_network.dart";
-import "package:subsystems/subsystems.dart";
 
 class SwingingIterator implements Iterator<double> {
   final double min;
@@ -26,7 +25,7 @@ class SwingingIterator implements Iterator<double> {
 }
 
 Future<void> main() async {
-  final server = SubsystemsServer(port: 8001);
+  final server = RoverSocket(port: 8001, device: Device.SUBSYSTEMS);
   await server.init();
   final throttle = SwingingIterator(0, 1, 0.01);
   final voltage = SwingingIterator(24, 30, 0.1);
@@ -36,6 +35,8 @@ Future<void> main() async {
   final roll = SwingingIterator(-45, 45, 1);
   final pitch = SwingingIterator(-45, 45, 1);
   final yaw = SwingingIterator(-45, 45, 1);
+  final color = SwingingIterator(0, 1, 0.01);
+  final gps = SwingingIterator(-1, 1, 0.01);
   while (true) {
     throttle.moveNext();
     voltage.moveNext();
@@ -45,33 +46,43 @@ Future<void> main() async {
     roll.moveNext();
     pitch.moveNext();
     yaw.moveNext();
+    color.moveNext();
+    gps.moveNext();
     final data = DriveData(
-      left: 1, 
-      setLeft: true, 
-      right: -1, 
-      setRight: true, 
-      throttle: throttle.current, 
-      setThrottle: true, 
-      batteryVoltage: voltage.current, 
+      left: 1,
+      setLeft: true,
+      right: -1,
+      setRight: true,
+      throttle: throttle.current,
+      setThrottle: true,
+      batteryVoltage: voltage.current,
       batteryCurrent: current.current,
       version: Version(major: 1),
-    ); 
+      color: color.current < 0.25 ? ProtoColor.UNLIT
+        : color.current < 0.5 ? ProtoColor.BLUE
+        : color.current < 0.75 ? ProtoColor.RED
+        : ProtoColor.GREEN,
+    );
     server.sendMessage(data);
     final data2 = ArmData(
-      base: MotorData(angle: motor2.current), 
-      shoulder: MotorData(angle: motor.current), 
+      base: MotorData(angle: motor2.current),
+      shoulder: MotorData(angle: motor.current),
       elbow: MotorData(angle: motor.current),
       version: Version(major: 1),
     );
     server.sendMessage(data2);
-    final data3 = GripperData(lift: MotorData(angle: motor.current), version: Version(major: 1));
+    final data3 = GripperData(lift: MotorData(angle: pi + -1 * 2 * motor.current), version: Version(major: 1));
     server.sendMessage(data3);
     final data4 = RoverPosition(
       orientation: Orientation(
         x: roll.current,
-        y: pitch.current, 
+        y: pitch.current,
         z: yaw.current,
       ),
+      gps: GpsCoordinates(
+        latitude: gps.current,
+        longitude: gps.current,
+      ),
       version: Version(major: 1),
     );
     server.sendMessage(data4);

bin/imu.dart

@@ -0,0 +1,6 @@
+import "package:subsystems/subsystems.dart";
+
+void main() async {
+  final reader = ImuReader();
+  await reader.init();
+}

bin/motor.dart

@@ -4,10 +4,11 @@ import "package:burt_network/logging.dart";
 const speed = [0, 0, 0x9, 0xc4];
 
 void main() async {
+  final can = CanSocket.forPlatform();
   Logger.level = LogLevel.info;
-  await collection.init();
+  if (!await can.init()) logger.critical("Could not initialize CAN");
   while (true) {
-    collection.can.can.sendMessage(id: 0x304, data: speed);
+    can.sendMessage(id: 0x304, data: speed);
     await Future<void>.delayed(const Duration(milliseconds: 500));
   }
 }

bin/serial.dart

@@ -1,7 +1,6 @@
 import "dart:io";
 import "dart:typed_data";
 import "package:burt_network/burt_network.dart";
-import "package:collection/collection.dart";
 import "package:libserialport/libserialport.dart";
 
 final logger = BurtLogger();
@@ -11,7 +10,7 @@ bool ascii = false;
 Future<void> listenToDevice(String port) async {
   logger.info("Connecting to $port...");
   final device = SerialDevice(
-    portName: port, 
+    portName: port,
     readInterval: const Duration(milliseconds: 100),
     logger: logger,
   );
@@ -20,22 +19,29 @@ Future<void> listenToDevice(String port) async {
     return;
   }
   logger.info("Connected. Listening...");
-  device.stream.listen(process);
+  device.stream.listen(processAscii);
   device.startListening();
 }
 
 Future<void> listenToFirmware(String port) async {
   logger.info("Connecting to $port...");
   final device = BurtFirmwareSerial(
-    port: port, 
+    port: port,
     logger: logger,
   );
   if (!await device.init()) {
     logger.critical("Could not connect to $port");
+    await device.dispose();
     return;
   }
   logger.info("Connected? ${device.isReady}. Listening...");
-  device.stream?.listen(process);
+  constructor = getDataConstructor(device.device);
+  if (constructor == null) {
+    logger.error("Unsupported serial device: ${device.device.name}");
+    await device.dispose();
+    return;
+  }
+  device.rawStream.listen(processFirmware);
 }
 
 typedef ProtoConstructor = Message Function(List<int> data);
@@ -55,18 +61,7 @@ void main(List<String> args) async {
     logger.info("Ports: ${SerialPort.availablePorts}");
     return;
   } else if (args.contains("-h") || args.contains("--help")) {
-    logger.info("Usage: dart run -r :serial [-a | --ascii] [port device]");
-    return;
-  }
-  final deviceName = args.last;
-  final device = Device.values.firstWhereOrNull((d) => d.name.toLowerCase() == deviceName.toLowerCase());
-  if (device == null) {
-    logger.error("Enter a device name as the last argument. Unrecognized device: $deviceName");
-    return;
-  }
-  constructor = getDataConstructor(device);
-  if (constructor == null) {
-    logger.error("Unsupported serial device: $deviceName");
+    logger.info("Usage: dart run -r :serial [-a | --ascii] [port]");
     return;
   }
   var port = args.first;
@@ -82,16 +77,16 @@ void main(List<String> args) async {
   }
 }
 
-void process(Uint8List buffer) {
-  if (ascii) {
-    final s = String.fromCharCodes(buffer).trim();
-    logger.debug("Got string: $s");	
-  } else {
-    try {
-      final data = constructor!(buffer);
-      logger.debug("Got data: ${data.toProto3Json()}");
-    } catch (error) {
-      logger.error("Could not decode DriveData: $error\n  Buffer: $buffer");
-    }
+void processFirmware(Uint8List buffer) {
+  try {
+    final data = constructor!(buffer);
+    logger.debug("Got data: ${data.toProto3Json()}");
+  } catch (error) {
+    logger.error("Could not decode DriveData: $error\n  Buffer: $buffer");
   }
 }
+
+void processAscii(Uint8List buffer) {
+  final s = String.fromCharCodes(buffer).trim();
+  logger.debug("Got string: $s");
+}

ffigen-can.yaml

@@ -1,15 +1,15 @@
 # Run with `dart run ffigen --config can-ffigen.yaml -v severe`.
 name: CanBindings
-description: |
+description: |-
   Bindings for `src/burt_can`.
 
   Regenerate bindings with `dart run ffigen --config ffigen-can.yaml -v severe`.
-output: "./lib/src/generated/can_ffi_bindings.dart"
+output: lib/src/generated/can_ffi_bindings.dart
 headers:
   entry-points:
     - 'src/burt_can/burt_can.h'
   include-directives:
-    - 'src/burt_can/burt_can.h'
+    - '**/src/burt_can/burt_can.h'
 comments:
   style: any
   length: full
@@ -22,6 +22,6 @@ type-map:
       'dart-type': 'Utf8'
 
 functions:
-  symbol-address: 
-    include: 
+  symbol-address:
+    include:
       - ".+_free"

lib/src/can/ffi.dart

@@ -6,22 +6,24 @@ export "package:ffi/ffi.dart";
 export "package:subsystems/src/generated/can_ffi_bindings.dart";
 
 /// The native SocketCAN-based library.
-/// 
+///
 /// See `src/can.h` in this repository. Only supported on Linux.
 final nativeLib = CanBindings(DynamicLibrary.open("burt_can.so"));
 
-/// These values come from the [BurtCanStatus] enum.
-String? getCanError(int value) => switch (value) {
-  1 => null,
-  2 => "Could not create socket",
-  3 => "Could not parse interface",
-  4 => "Could not bind to socket",
-  5 => "Could not close socket",
-  6 => "Invalid MTU",
-  7 => "CAN FD is not supported",
-  8 => "Could not switch to CAN FD",
-  9 => "Could not write data",
-  10 => "Could not read data",
-  11 => "Frame was not fully read",
-  _ => throw ArgumentError.value(value, "CanStatus", "Unknown CAN status"),
-};
+/// Helpful methods on [BurtCanStatus].
+extension BurtCanStatusUtils on BurtCanStatus {
+  /// A human-readable string representing this error, if any.
+  String? get stringError => switch (this) {
+    BurtCanStatus.OK => null,
+    BurtCanStatus.SOCKET_CREATE_ERROR => "Could not create socket",
+    BurtCanStatus.INTERFACE_PARSE_ERROR => "Could not parse interface",
+    BurtCanStatus.BIND_ERROR => "Could not bind to socket",
+    BurtCanStatus.CLOSE_ERROR => "Could not close socket",
+    BurtCanStatus.MTU_ERROR => "Invalid MTU",
+    BurtCanStatus.CANFD_NOT_SUPPORTED => "CAN FD is not supported",
+    BurtCanStatus.FD_MISC_ERROR => "Could not switch to CAN FD",
+    BurtCanStatus.WRITE_ERROR => "Could not write data",
+    BurtCanStatus.READ_ERROR => "Could not read data",
+    BurtCanStatus.FRAME_NOT_FULLY_READ => "Frame was not fully read",
+  };
+}

lib/src/can/socket_ffi.dart

@@ -14,17 +14,17 @@ const canType = BurtCanType.CAN;
 const canTimeout = 1;
 
 /// The CAN interface, backed by the native SocketCAN library on Linux.
-/// 
+///
 /// - Access [incomingMessages] to handle messages as they are received
 /// - Call [sendMessage] to send a new [CanMessage]
 /// - Be sure to call [dispose] when you're done to avoid memory leaks
-/// 
+///
 /// Note that [CanMessage]s are natively allocated and need to be manually disposed of. Since this
 /// class sends them through the [incomingMessages] stream, you are responsible for disposing them
 /// if you listen to it. The stream gives you pointers so you can call [CanMessage.dispose].
-class CanFFI implements CanSocket {
+class CanFFI extends CanSocket {
   /// How often to poll CAN messages.
-  /// 
+  ///
   /// This should be small enough to catch incoming messages but large enough to
   /// not block other code from running.
   static const readInterval = Duration(milliseconds: 100);
@@ -51,7 +51,7 @@ class CanFFI implements CanSocket {
   Timer? _timer;
 
   @override
-  Future<bool> init() async { 
+  Future<bool> init() async {
     _can = nativeLib.BurtCan_create(canInterface.toNativeUtf8(), canTimeout, canType);
     await Process.run("sudo", ["ip", "link", "set", "can0", "down"]);
     final result = await Process.run("sudo", ["ip", "link", "set", "can0", "up", "type", "can", "bitrate", "500000"]);
@@ -60,13 +60,13 @@ class CanFFI implements CanSocket {
       hasError = true;
       return false;
     }
-    final error = getCanError(nativeLib.BurtCan_open(_can!));
+    final error = nativeLib.BurtCan_open(_can!).stringError;
     if (error != null) {
       hasError = true;
       logger.critical("Could not start the CAN bus", body: error);
       return false;
     }
-    _startListening(); 
+    _startListening();
     logger.info("Listening on CAN interface $canInterface");
     return true;
   }
@@ -86,7 +86,7 @@ class CanFFI implements CanSocket {
   void sendMessage({required int id, required List<int> data}) {
     if (hasError || _can == null) return;
     final message = CanMessage(id: id, data: data);
-    final error = getCanError(nativeLib.BurtCan_send(_can!, message.pointer));
+    final error = nativeLib.BurtCan_send(_can!, message.pointer).stringError;
     if (error != null) logger.warning("Could not send CAN message", body: "ID=$id, Data=$data, Error: $error");
     message.dispose();
   }
@@ -97,7 +97,7 @@ class CanFFI implements CanSocket {
     int count = 0;
     while (true) {
       final pointer = nativeLib.NativeCanMessage_create();
-      final error = getCanError(nativeLib.BurtCan_receive(_can!, pointer));
+      final error = nativeLib.BurtCan_receive(_can!, pointer).stringError;
       if (error != null) logger.warning("Could not read the CAN bus", body: error);
       if (pointer.ref.length == 0) break;
       count++;