Arm auxillary

.github/workflows/analyze.yaml

@@ -25,6 +25,9 @@ jobs:
       - name: Analyze code
         run: dart analyze --fatal-infos
 
+      - name: Check arm auxillary formatting
+        run: dart format --output=none --set-exit-if-changed arm_auxillary/bin/* arm_auxillary/lib/*
+
       - name: Check autonomy formatting
         run: dart format --output=none --set-exit-if-changed autonomy/bin/* autonomy/lib/* autonomy/test/*
 

arm_auxillary/.gitignore

@@ -0,0 +1,4 @@
+# https://dart.dev/guides/libraries/private-files
+# Created by `dart pub`
+.dart_tool/
+pubspec_overrides.yaml

arm_auxillary/README.md

@@ -0,0 +1 @@
+Program for the auxillary board on the arm.

arm_auxillary/bin/arm_auxillary.dart

@@ -0,0 +1,5 @@
+import "package:arm_auxillary/arm_auxillary.dart";
+
+void main() async {
+  await collection.init();
+}

arm_auxillary/lib/arm_auxillary.dart

@@ -0,0 +1,82 @@
+import "dart:io";
+
+import "package:burt_network/burt_network.dart";
+
+import "src/firmware.dart";
+import "src/arm_camera_manager.dart";
+
+/// Logger for the arm auxillary program
+final logger = BurtLogger();
+
+/// The socket destination for the subsystems program
+final subsystemsSocket = SocketInfo(
+  address: InternetAddress("192.168.1.20"),
+  port: 8001,
+);
+
+/// The resouces needed to run the arm auxillary program
+class ArmAuxillary extends Service {
+  /// Whether the arm auxillary code is fully initialized.
+  bool isReady = false;
+
+  /// The Serial service.
+  late final firmware = FirmwareManager(
+    getServer: () => server,
+    logger: logger,
+  );
+
+  /// The camera manager for arm cameras.
+  final cameras = ArmCameraManager();
+
+  /// The server for the arm auxillary program
+  late final RoverSocket server = RoverSocket(
+    device: Device.ARM,
+    port: 8010,
+    collection: this,
+    destination: subsystemsSocket,
+    keepDestination: true,
+  );
+
+  @override
+  Future<bool> init() async {
+    var result = true;
+    logger.socket = server;
+    result &= await server.init();
+    // TODO(arm): Initialize the rest of the arm auxillary's resources, such as
+    // TODO(arm): arm and EA board communication
+    try {
+      result &= await firmware.init();
+      result &= await cameras.init();
+      cameras.setServer(server);
+      if (result) {
+        logger.info("Arm Auxillary software initialized");
+      } else {
+        logger.warning("The arm auxillary software did not start properly");
+      }
+      isReady = true;
+
+      // The arm auxillary software should keep running even when something goes wrong.
+      return true;
+    } catch (error) {
+      logger.critical(
+        "Unexpected error when initializing Arm Auxillary",
+        body: error.toString(),
+      );
+      return false;
+    }
+  }
+
+  @override
+  Future<void> dispose() async {
+    logger.info("Arm Auxillary software shutting down...");
+    isReady = false;
+    await cameras.dispose();
+    await firmware.dispose();
+    await server.dispose();
+    logger.socket = null;
+    logger.info("Subsystems disposed");
+  }
+}
+
+/// The collection of all the arm auxillary's resources
+final collection = ArmAuxillary();

arm_auxillary/lib/src/arm_camera_manager.dart

@@ -0,0 +1,203 @@
+import "dart:async";
+import "dart:io";
+
+import "package:typed_isolate/typed_isolate.dart";
+import "package:burt_network/burt_network.dart";
+
+import "package:video/video.dart";
+
+/// Socket destination for the main video program on the Jetson
+final videoSocket = SocketInfo(
+  address: InternetAddress("192.168.1.30"),
+  port: 8004, // Video program port
+);
+
+/// Manages arm cameras and streams video data to the main video program.
+class ArmCameraManager extends Service {
+  /// The parent isolate that spawns the arm camera isolates.
+  final parent = IsolateParent<VideoCommand, IsolatePayload>();
+
+  /// Stream subscriptions for cleanup
+  StreamSubscription<VideoCommand>? _commands;
+  StreamSubscription<IsolatePayload>? _data;
+
+  /// Reference to the server for sending messages
+  RoverSocket? _server;
+
+  @override
+  Future<bool> init() async {
+    logger.info("Initializing arm camera manager");
+
+    parent.init();
+    _data = parent.stream.listen(onData);
+
+    await _spawnArmCameras();
+
+    logger.info("Arm camera manager initialized");
+    return true;
+  }
+
+  @override
+  Future<void> dispose() async {
+    logger.info("Disposing arm camera manager");
+
+    stopAll();
+
+    // Wait a bit after sending the stop command so the messages are received properly
+    await Future<void>.delayed(const Duration(milliseconds: 750));
+
+    await _commands?.cancel();
+    // Dispose the parent isolate and kill all children before canceling
+    // data subscription, just in case if one last native frame is received
+    await parent.dispose();
+
+    // Wait just a little bit to ensure any remaining messages get sent
+    // otherwise, if a message contained native memory, it will never
+    // be disposed
+    await Future<void>.delayed(const Duration(milliseconds: 50));
+
+    await _data?.cancel();
+
+    logger.info("Arm camera manager disposed");
+  }
+
+  /// Sets the server reference for message handling
+  void setServer(RoverSocket server) {
+    _server = server;
+
+    // Set up command subscription now that server is available
+    _commands = server.messages.onMessage<VideoCommand>(
+      name: VideoCommand().messageName,
+      constructor: VideoCommand.fromBuffer,
+      callback: _handleCommand,
+    );
+
+    logger.info("Arm camera manager connected to server");
+  }
+
+  /// Spawns camera isolates for detected arm cameras
+  Future<void> _spawnArmCameras() async {
+    logger.info("Detecting arm cameras...");
+
+    final armCameraNames = <CameraName>[
+      CameraName.ARM_LEFT,
+      CameraName.ARM_RIGHT,
+      CameraName.GAP_CAM,
+    ];
+
+    for (final cameraName in armCameraNames) {
+      try {
+        final details = _createArmCameraDetails(cameraName);
+        final isolate = OpenCVCameraIsolate(details: details);
+        await parent.spawn(isolate);
+
+        logger.info("Spawned camera isolate for $cameraName");
+      } catch (error) {
+        logger.error(
+          "Failed to spawn camera isolate for $cameraName",
+          body: error.toString(),
+        );
+      }
+    }
+  }
+
+  /// Creates camera details for arm cameras
+  CameraDetails _createArmCameraDetails(CameraName name) => CameraDetails(
+    name: name,
+    resolutionWidth: 640,
+    resolutionHeight: 480,
+    fps: 15,
+    quality: 80,
+    status: CameraStatus.CAMERA_LOADING,
+  );
+
+  /// Handles data coming from the arm camera isolates
+  void onData(IsolatePayload data) {
+    switch (data) {
+      case FramePayload(:final details, :final screenshotPath):
+        final image = data.image?.toU8List();
+        data.dispose();
+
+        if (_server != null && image != null) {
+          _server!.sendMessage(
+            VideoData(
+              frame: image,
+              details: details,
+              imagePath: screenshotPath,
+            ),
+            destination: videoSocket,
+          );
+        }
+
+      case LogPayload():
+        switch (data.level) {
+          case LogLevel.all:
+            logger.info("Camera isolate: ${data.message}", body: data.body);
+          // ignore: deprecated_member_use
+          case LogLevel.verbose:
+            logger.trace("Camera isolate: ${data.message}", body: data.body);
+          case LogLevel.trace:
+            logger.trace("Camera isolate: ${data.message}", body: data.body);
+          case LogLevel.debug:
+            logger.debug("Camera isolate: ${data.message}", body: data.body);
+          case LogLevel.info:
+            logger.info("Camera isolate: ${data.message}", body: data.body);
+          case LogLevel.warning:
+            logger.warning("Camera isolate: ${data.message}", body: data.body);
+          case LogLevel.error:
+            logger.error("Camera isolate: ${data.message}", body: data.body);
+          case LogLevel.fatal:
+            logger.critical("Camera isolate: ${data.message}", body: data.body);
+          case LogLevel.off:
+            logger.info("Camera isolate: ${data.message}", body: data.body);
+          // ignore: deprecated_member_use
+          case LogLevel.wtf:
+            logger.critical("Camera isolate: ${data.message}", body: data.body);
+          // ignore: deprecated_member_use
+          case LogLevel.nothing:
+            break;
+        }
+
+      case ObjectDetectionPayload(:final details, :final tags):
+        if (_server != null) {
+          final visionResult = VideoData(
+            details: details,
+            detectedObjects: tags,
+            version: Version(major: 1, minor: 2),
+          );
+          _server!.sendMessage(visionResult, destination: videoSocket);
+        }
+
+      default:
+        logger.warning("Unknown payload type from camera isolate");
+    }
+  }
+
+  /// Handles commands from the video program
+  void _handleCommand(VideoCommand command) {
+    logger.debug("Received camera command for ${command.details.name}");
+
+    // Route command to correct camera isolate
+    parent.sendToChild(data: command, id: command.details.name);
+  }
+
+  /// Stops all arm cameras
+  void stopAll() {
+    final stopCommand = VideoCommand(
+      details: CameraDetails(status: CameraStatus.CAMERA_DISABLED),
+    );
+
+    // Send stop command to all arm camera isolates
+    final armCameraNames = [
+      CameraName.ARM_LEFT,
+      CameraName.ARM_RIGHT,
+      CameraName.GAP_CAM,
+    ];
+
+    for (final name in armCameraNames) {
+      parent.sendToChild(data: stopCommand, id: name);
+    }
+
+    logger.info("Stopping all arm cameras");
+  }
+}

arm_auxillary/lib/src/firmware.dart

@@ -0,0 +1,76 @@
+import "dart:async";
+
+import "package:collection/collection.dart";
+
+import "package:burt_network/burt_network.dart";
+import "package:subsystems/subsystems.dart";
+
+/// Maps command names to [Device]s.
+final nameToDevice = <String, Device>{
+  ArmCommand().messageName: Device.ARM,
+  DriveCommand().messageName: Device.DRIVE,
+  ScienceCommand().messageName: Device.SCIENCE,
+  RelaysCommand().messageName: Device.RELAY,
+};
+
+/// Service to manage communication from the arm auxillary board to EA and HREI devices
+class FirmwareManager extends Service {
+  /// Reference to the server for routing messages
+  final RoverSocket? Function() getServer;
+
+  /// Logger instance
+  final BurtLogger logger;
+
+  /// Subscriptions to each of the firmware devices.
+  final List<StreamSubscription<WrappedMessage>> _subscriptions = [];
+
+  /// A list of firmware devices attached to the rover.
+  List<BurtFirmwareSerial> devices = [];
+
+  /// Creates a new FirmwareManager instance
+  FirmwareManager({required this.getServer, required this.logger});
+
+  @override
+  Future<bool> init() async {
+    devices = await getFirmwareDevices();
+    final server = getServer();
+    server?.messages.listen(_sendToSerial);
+    var result = true;
+    for (final device in devices) {
+      logger.debug("Initializing device: ${device.port}");
+      result &= await device.init();
+      if (!device.isReady) continue;
+      final subscription = device.messages.listen(
+        server?.sendWrapper ?? (_) {},
+      );
+      _subscriptions.add(subscription);
+    }
+    return result;
+  }
+
+  /// Sends messages from the server to the appropriate serial device
+  void _sendToSerial(WrappedMessage message) {
+    final device = nameToDevice[message.name];
+    if (device == null) return;
+    final serial = devices.firstWhereOrNull((s) => s.device == device);
+    if (serial == null) return;
+    serial.sendBytes(message.data);
+  }
+
+  @override
+  Future<void> dispose() async {
+    for (final subscription in _subscriptions) {
+      await subscription.cancel();
+    }
+    for (final device in devices) {
+      await device.dispose();
+    }
+  }
+
+  /// Sends a [Message] to the appropriate firmware device.
+  ///
+  /// This does nothing if the appropriate device is not connected. Specifically, this is not an
+  /// error because the Dashboard may be used during testing, when the hardware devices may not be
+  /// assembled, connected, or functional yet.
+  void sendMessage(Message message) => _sendToSerial(message.wrap());
+}