From 571e2098a5d7cdd20f70d2e98da861123f4184d6 Mon Sep 17 00:00:00 2001 From: Levi Lesches Date: Tue, 10 Sep 2024 01:36:14 -0700 Subject: [PATCH] Autonomy and new streams-based networking (#12) Co-authored-by: Binghamton Rover --- .github/workflows/analyzer.yml | 6 +- bin/data.dart | 39 +++++---- bin/imu.dart | 6 ++ bin/motor.dart | 5 +- bin/serial.dart | 51 ++++++------ ffigen-can.yaml | 10 +-- lib/src/can/ffi.dart | 34 ++++---- lib/src/can/socket_ffi.dart | 18 ++-- lib/src/can/socket_interface.dart | 27 ++---- lib/src/can/socket_stub.dart | 2 +- lib/src/devices/firmware.dart | 78 ++++++++++++++++++ lib/src/devices/gps.dart | 17 ++-- lib/src/devices/imu.dart | 56 ++++++++++--- lib/src/devices/serial_utils.dart | 26 ++++++ lib/src/generated/can_ffi_bindings.dart | 103 ++++++++++++++--------- lib/src/messages/can.dart | 82 ------------------ lib/src/messages/serial.dart | 105 ------------------------ lib/src/messages/service.dart | 10 --- lib/src/server.dart | 33 -------- lib/subsystems.dart | 47 ++++------- pubspec.yaml | 14 ++-- src/burt_can/burt_can.h | 8 +- 22 files changed, 357 insertions(+), 420 deletions(-) create mode 100644 bin/imu.dart create mode 100644 lib/src/devices/firmware.dart create mode 100644 lib/src/devices/serial_utils.dart delete mode 100644 lib/src/messages/can.dart delete mode 100644 lib/src/messages/serial.dart delete mode 100644 lib/src/messages/service.dart delete mode 100644 lib/src/server.dart diff --git a/.github/workflows/analyzer.yml b/.github/workflows/analyzer.yml index 07f1ab0..2d2c9fe 100644 --- a/.github/workflows/analyzer.yml +++ b/.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 diff --git a/bin/data.dart b/bin/data.dart index b6fe718..ca76a20 100644 --- a/bin/data.dart +++ b/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 { final double min; @@ -26,7 +25,7 @@ class SwingingIterator implements Iterator { } Future 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 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 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); diff --git a/bin/imu.dart b/bin/imu.dart new file mode 100644 index 0000000..d6ccafe --- /dev/null +++ b/bin/imu.dart @@ -0,0 +1,6 @@ +import "package:subsystems/subsystems.dart"; + +void main() async { + final reader = ImuReader(); + await reader.init(); +} diff --git a/bin/motor.dart b/bin/motor.dart index 5cd8907..6f51e04 100644 --- a/bin/motor.dart +++ b/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.delayed(const Duration(milliseconds: 500)); } } diff --git a/bin/serial.dart b/bin/serial.dart index b9df8ab..ae9d7eb 100644 --- a/bin/serial.dart +++ b/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 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 listenToDevice(String port) async { return; } logger.info("Connected. Listening..."); - device.stream.listen(process); + device.stream.listen(processAscii); device.startListening(); } Future 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 data); @@ -55,18 +61,7 @@ void main(List 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 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"); +} diff --git a/ffigen-can.yaml b/ffigen-can.yaml index 9cdd03b..0d56668 100644 --- a/ffigen-can.yaml +++ b/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" diff --git a/lib/src/can/ffi.dart b/lib/src/can/ffi.dart index 7c35a05..bb01213 100644 --- a/lib/src/can/ffi.dart +++ b/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", + }; +} diff --git a/lib/src/can/socket_ffi.dart b/lib/src/can/socket_ffi.dart index 623f298..cc252c0 100644 --- a/lib/src/can/socket_ffi.dart +++ b/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 init() async { + Future 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 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++; diff --git a/lib/src/can/socket_interface.dart b/lib/src/can/socket_interface.dart index 981dd04..27b3225 100644 --- a/lib/src/can/socket_interface.dart +++ b/lib/src/can/socket_interface.dart @@ -2,44 +2,35 @@ import "dart:io"; import "package:burt_network/burt_network.dart"; -import "ffi.dart"; import "message.dart"; import "socket_stub.dart"; import "socket_ffi.dart"; -/// An exception that occurred while working with the CAN bus -- see [BurtCanStatus]. -class CanException implements Exception { - /// The error that occurred, using [getCanError]. - final String message; - /// A const constructor - const CanException(this.message); - - @override - String toString() => "CAN error: $message"; -} - /// A CAN socket that supports reading and writing CAN messages. -/// +/// /// This class uses the [CAN bus](https://en.wikipedia.org/wiki/CAN_bus) protocol to interface with -/// other devices on the CAN bus. Each message should have an ID and a payload. The ID is used to +/// other devices on the CAN bus. Each message should have an ID and a payload. The ID is used to /// identify the purpose of the message, and the receiving device can filter incoming messages /// based on its ID. The payload is limited to 8 bytes, or 64 bytes if using CAN FD. -/// +/// /// - Use [sendMessage] to send a message to all devices on the bus /// - Listen to [incomingMessages] to receive messages from other devices on the bus abstract class CanSocket extends Service { + /// A default constructor for a [CanSocket]. + CanSocket(); + /// Chooses the right implementation for the platform. Uses a stub on non-Linux platforms. - factory CanSocket() => Platform.isLinux ? CanFFI() : CanStub(); + factory CanSocket.forPlatform() => Platform.isLinux ? CanFFI() : CanStub(); /// Sends a CAN message with the given ID and data. void sendMessage({required int id, required List data}) { } /// A stream of incoming CAN messages. Use [Stream.listen] to handle them. - /// + /// /// This stream returns [CanMessage] objects, which are wrappers around native structs, which /// needs to be freed after use. Be sure to call [CanMessage.dispose] when you're done using it. Stream get incomingMessages; - /// Resets the CAN bus. + /// Resets the CAN bus. Future reset(); } diff --git a/lib/src/can/socket_stub.dart b/lib/src/can/socket_stub.dart index f5c9966..afe05c9 100644 --- a/lib/src/can/socket_stub.dart +++ b/lib/src/can/socket_stub.dart @@ -1,7 +1,7 @@ import "package:subsystems/subsystems.dart"; /// An implementation of the CAN interface that does nothing for platforms that don't support it. -class CanStub implements CanSocket { +class CanStub extends CanSocket { /// Creates a mock CAN interface that does nothing and receives no messages. CanStub(); diff --git a/lib/src/devices/firmware.dart b/lib/src/devices/firmware.dart new file mode 100644 index 0000000..38ff442 --- /dev/null +++ b/lib/src/devices/firmware.dart @@ -0,0 +1,78 @@ +import "dart:async"; + +import "package:collection/collection.dart"; + +import "package:subsystems/subsystems.dart"; +import "package:burt_network/burt_network.dart"; + +import "serial_utils.dart"; + +/// Maps command names to [Device]s. +final nameToDevice = { + ArmCommand().messageName: Device.ARM, + GripperCommand().messageName: Device.GRIPPER, + DriveCommand().messageName: Device.DRIVE, + ScienceCommand().messageName: Device.SCIENCE, +}; + +/// A service to manage all the connected firmware. +/// +/// Firmware means any device using the [Firmware-Utilities](https://github.com/BinghamtonRover/Firmware-Utilities) +/// library. In our case, all such devices are Teensy boards running on the Arduino platform, +/// connected via USB (serial). +/// +/// This service relies on the [BurtFirmwareSerial] class defined in `package:burt_network`. That +/// class takes care of connecting to, identifying, and streaming from a firmware device. This +/// service is responsible for routing incoming UDP messages to the correct firmware device +/// ([_sendToSerial]), and forwarding serial messages to the Dashboard ([RoverSocket.sendWrapper]). +class FirmwareManager extends Service { + /// Subscriptions to each of the firmware devices. + final List> _subscriptions = []; + + /// A list of firmware devices attached to the rover. + List devices = []; + + @override + Future init() async { + devices = await getFirmwareDevices(); + collection.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(collection.server.sendWrapper); + if (subscription == null) continue; + _subscriptions.add(subscription); + } + return result; + } + + @override + Future dispose() async { + for (final subscription in _subscriptions) { + await subscription.cancel(); + } + for (final device in devices) { + await device.dispose(); + } + } + + /// Sends a [WrappedMessage] to the correct Serial device. + /// + /// The notes on [sendMessage] apply here as well. + void _sendToSerial(WrappedMessage wrapper) { + final device = nameToDevice[wrapper.name]; + if (device == null) return; + final serial = devices.firstWhereOrNull((s) => s.device == device); + if (serial == null) return; + serial.sendBytes(wrapper.data); + } + + /// 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()); +} diff --git a/lib/src/devices/gps.dart b/lib/src/devices/gps.dart index 9e08b25..e287553 100644 --- a/lib/src/devices/gps.dart +++ b/lib/src/devices/gps.dart @@ -1,5 +1,6 @@ import "dart:async"; import "dart:convert"; +import "dart:io"; import "package:burt_network/burt_network.dart"; import "package:subsystems/subsystems.dart"; @@ -7,25 +8,28 @@ import "package:subsystems/subsystems.dart"; /// The port/device file to listen to the GPS on. const gpsPort = "/dev/rover-gps"; -/// Listens to the GPS and sends its output to the Dashboard. -/// +/// The UDP socket on the Autonomy program. +final autonomySocket = SocketInfo(address: InternetAddress("192.168.1.30"), port: 8001); + +/// Listens to the GPS and sends its output to the Dashboard. +/// /// Call [init] to start listening and [dispose] to stop. class GpsReader extends Service { /// Parses an NMEA sentence into a [GpsCoordinates] object. - /// + /// /// See https://shadyelectronics.com/gps-nmea-sentence-structure. static GpsCoordinates? parseNMEA(String nmeaSentence) { final parts = nmeaSentence.split(","); final tag = parts.first; if (tag.endsWith("GGA")) { return GpsCoordinates( - latitude: _nmeaToDecimal(double.tryParse(parts[2]) ?? 0.0), + latitude: _nmeaToDecimal(double.tryParse(parts[2]) ?? 0.0), longitude: _nmeaToDecimal(double.tryParse(parts[4]) ?? 0.0), altitude: double.tryParse(parts[9]) ?? 0.0, ); } else if (tag.endsWith("RMC")) { return GpsCoordinates( - latitude: _nmeaToDecimal(double.tryParse(parts[3]) ?? 0.0), + latitude: _nmeaToDecimal(double.tryParse(parts[3]) ?? 0.0), longitude: _nmeaToDecimal(double.tryParse(parts[5]) ?? 0.0), ); } else if (tag.endsWith("GLL")) { @@ -46,7 +50,7 @@ class GpsReader extends Service { /// The serial device representing the GPS. final SerialDevice device = SerialDevice( - portName: gpsPort, + portName: gpsPort, readInterval: const Duration(seconds: 1), logger: logger, ); @@ -64,6 +68,7 @@ class GpsReader extends Service { } final roverPosition = RoverPosition(gps: coordinates); collection.server.sendMessage(roverPosition); + collection.server.sendMessage(roverPosition, destination: autonomySocket); } /// Parses a packet into several NMEA sentences and handles them. diff --git a/lib/src/devices/imu.dart b/lib/src/devices/imu.dart index 818fdc0..1a65225 100644 --- a/lib/src/devices/imu.dart +++ b/lib/src/devices/imu.dart @@ -1,4 +1,5 @@ import "dart:async"; +import "dart:typed_data"; import "package:osc/osc.dart"; @@ -6,17 +7,17 @@ import "package:subsystems/subsystems.dart"; import "package:burt_network/burt_network.dart"; /// The serial port that the IMU is connected to. -const imuPort = "/dev/rover-imu"; +const imuPort = "COM5"; +// const imuPort = "/dev/rover-imu"; -extension on double { - bool isZero([double epsilon = 0.01]) => abs() < epsilon; -} +/// The version that we are using for [RoverPosition] data. +final positionVersion = Version(major: 1, minor: 0); /// A service to read orientation data from the connected IMU. class ImuReader extends Service { - /// The device that reads from the serial port. + /// The device that reads from the serial port. final serial = SerialDevice( - portName: imuPort, + portName: imuPort, readInterval: const Duration(milliseconds: 10), logger: logger, ); @@ -25,20 +26,49 @@ class ImuReader extends Service { StreamSubscription>? subscription; /// Parses an OSC bundle from a list of bytes. - void _handleOsc(List data) { + void handleOsc(List data) { try { - final message = OSCMessage.fromBytes(data.sublist(20)); + // 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, ); - if (orientation.x.isZero() || orientation.y.isZero() || orientation.z.isZero()) return; - if (orientation.x.abs() > 360 || orientation.y.abs() > 360 || orientation.z.abs() > 360) return; - final position = RoverPosition(orientation: orientation, version: Version(major: 1, minor: 0)); + final position = RoverPosition(orientation: orientation, version: positionVersion); collection.server.sendMessage(position); - } catch (error) { /* Ignore corrupt data */ } + collection.server.sendMessage(position, destination: autonomySocket); + } catch (error) { + /* Ignore corrupt data */ + } + } + + /// 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 data) { + const end = 192; + const esc = 219; + const escEnd = 220; + const escEsc = 221; + final newPacket = []; + 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); + } + prevElement = element; + } + if (newPacket.last == end) newPacket.removeLast(); + return Uint8List.fromList(newPacket); } @override @@ -48,7 +78,7 @@ class ImuReader extends Service { logger.critical("Could not open IMU on port $imuPort"); return false; } - subscription = serial.stream.listen(_handleOsc); + subscription = serial.stream.map(processSlip).listen(handleOsc); serial.startListening(); logger.info("Reading IMU on port $imuPort"); return true; diff --git a/lib/src/devices/serial_utils.dart b/lib/src/devices/serial_utils.dart new file mode 100644 index 0000000..b444707 --- /dev/null +++ b/lib/src/devices/serial_utils.dart @@ -0,0 +1,26 @@ +import "dart:io"; + +import "package:burt_network/burt_network.dart"; +import "package:subsystems/subsystems.dart"; + +/// Gets the real path to a Linux symlink path. +/// +/// Relies on the `realpath` command line tool, and must be run on Linux. +Future getRealPath(String symlink) async => + (await Process.run("realpath", [symlink])).stdout.trim(); + +/// Gets all the names of all the ports. +Future> getPortNames() async { + final allPorts = DelegateSerialPort.allPorts.toSet(); + if (!Platform.isLinux) return allPorts; + final imuPort = await getRealPath("/dev/rover-imu"); + final gpsPort = await getRealPath("/dev/rover-gps"); + final forbiddenPorts = {imuPort, gpsPort}; + return allPorts.toSet().difference(forbiddenPorts); +} + +/// Gets all firmware devices attached to the device, ignoring the GPS and IMU ports. +Future> getFirmwareDevices() async => [ + for (final port in await getPortNames()) + BurtFirmwareSerial(port: port, logger: logger), +]; diff --git a/lib/src/generated/can_ffi_bindings.dart b/lib/src/generated/can_ffi_bindings.dart index 9dfc19b..54367d4 100644 --- a/lib/src/generated/can_ffi_bindings.dart +++ b/lib/src/generated/can_ffi_bindings.dart @@ -8,7 +8,6 @@ import 'package:ffi/ffi.dart' as pkg_ffi; /// Bindings for `src/burt_can`. /// /// Regenerate bindings with `dart run ffigen --config ffigen-can.yaml -v severe`. -/// class CanBindings { /// Holds the symbol lookup function. final ffi.Pointer Function(String symbolName) @@ -27,19 +26,19 @@ class CanBindings { ffi.Pointer BurtCan_create( ffi.Pointer interface1, int readTimeout, - int type, + BurtCanType type, ) { return _BurtCan_create( interface1, readTimeout, - type, + type.value, ); } late final _BurtCan_createPtr = _lookup< ffi.NativeFunction< ffi.Pointer Function(ffi.Pointer, ffi.Int32, - ffi.Int32)>>('BurtCan_create'); + ffi.UnsignedInt)>>('BurtCan_create'); late final _BurtCan_create = _BurtCan_createPtr.asFunction< ffi.Pointer Function(ffi.Pointer, int, int)>(); @@ -57,65 +56,65 @@ class CanBindings { late final _BurtCan_free = _BurtCan_freePtr.asFunction)>(); - int BurtCan_open( + BurtCanStatus BurtCan_open( ffi.Pointer pointer, ) { - return _BurtCan_open( + return BurtCanStatus.fromValue(_BurtCan_open( pointer, - ); + )); } - late final _BurtCan_openPtr = - _lookup)>>( - 'BurtCan_open'); + late final _BurtCan_openPtr = _lookup< + ffi.NativeFunction)>>( + 'BurtCan_open'); late final _BurtCan_open = _BurtCan_openPtr.asFunction)>(); - int BurtCan_send( + BurtCanStatus BurtCan_send( ffi.Pointer pointer, ffi.Pointer message, ) { - return _BurtCan_send( + return BurtCanStatus.fromValue(_BurtCan_send( pointer, message, - ); + )); } late final _BurtCan_sendPtr = _lookup< ffi.NativeFunction< - ffi.Int32 Function(ffi.Pointer, + ffi.UnsignedInt Function(ffi.Pointer, ffi.Pointer)>>('BurtCan_send'); late final _BurtCan_send = _BurtCan_sendPtr.asFunction< int Function(ffi.Pointer, ffi.Pointer)>(); - int BurtCan_receive( + BurtCanStatus BurtCan_receive( ffi.Pointer pointer, ffi.Pointer message, ) { - return _BurtCan_receive( + return BurtCanStatus.fromValue(_BurtCan_receive( pointer, message, - ); + )); } late final _BurtCan_receivePtr = _lookup< ffi.NativeFunction< - ffi.Int32 Function(ffi.Pointer, + ffi.UnsignedInt Function(ffi.Pointer, ffi.Pointer)>>('BurtCan_receive'); late final _BurtCan_receive = _BurtCan_receivePtr.asFunction< int Function(ffi.Pointer, ffi.Pointer)>(); - int BurtCan_close( + BurtCanStatus BurtCan_close( ffi.Pointer pointer, ) { - return _BurtCan_close( + return BurtCanStatus.fromValue(_BurtCan_close( pointer, - ); + )); } - late final _BurtCan_closePtr = - _lookup)>>( - 'BurtCan_close'); + late final _BurtCan_closePtr = _lookup< + ffi.NativeFunction)>>( + 'BurtCan_close'); late final _BurtCan_close = _BurtCan_closePtr.asFunction)>(); @@ -156,29 +155,57 @@ class _SymbolAddresses { get NativeCanMessage_free => _library._NativeCanMessage_freePtr; } -abstract class BurtCanType { - static const int CAN = 0; - static const int CANFD = 1; +enum BurtCanType { + CAN(0), + CANFD(1); + + final int value; + const BurtCanType(this.value); + + static BurtCanType fromValue(int value) => switch (value) { + 0 => CAN, + 1 => CANFD, + _ => throw ArgumentError("Unknown value for BurtCanType: $value"), + }; } /// No 0 value to ensure we always set a status -abstract class BurtCanStatus { - static const int OK = 1; +enum BurtCanStatus { + OK(1), /// Errors when opening and closing - static const int SOCKET_CREATE_ERROR = 2; - static const int INTERFACE_PARSE_ERROR = 3; - static const int BIND_ERROR = 4; - static const int CLOSE_ERROR = 5; + SOCKET_CREATE_ERROR(2), + INTERFACE_PARSE_ERROR(3), + BIND_ERROR(4), + CLOSE_ERROR(5), /// CANFD errors - static const int MTU_ERROR = 6; - static const int CANFD_NOT_SUPPORTED = 7; - static const int FD_MISC_ERROR = 8; + MTU_ERROR(6), + CANFD_NOT_SUPPORTED(7), + FD_MISC_ERROR(8), /// IO errors - static const int WRITE_ERROR = 9; - static const int READ_ERROR = 10; + WRITE_ERROR(9), + READ_ERROR(10), + FRAME_NOT_FULLY_READ(11); + + final int value; + const BurtCanStatus(this.value); + + static BurtCanStatus fromValue(int value) => switch (value) { + 1 => OK, + 2 => SOCKET_CREATE_ERROR, + 3 => INTERFACE_PARSE_ERROR, + 4 => BIND_ERROR, + 5 => CLOSE_ERROR, + 6 => MTU_ERROR, + 7 => CANFD_NOT_SUPPORTED, + 8 => FD_MISC_ERROR, + 9 => WRITE_ERROR, + 10 => READ_ERROR, + 11 => FRAME_NOT_FULLY_READ, + _ => throw ArgumentError("Unknown value for BurtCanStatus: $value"), + }; } final class NativeCanMessage extends ffi.Struct { diff --git a/lib/src/messages/can.dart b/lib/src/messages/can.dart deleted file mode 100644 index cb9a850..0000000 --- a/lib/src/messages/can.dart +++ /dev/null @@ -1,82 +0,0 @@ -/// Uses Dart's FFI to interop with native C code to use Linux's SocketCan. -/// -/// - See [CanSocket] for usage. -/// - See [this page](https://bing-rover.gitbook.io/software-docs/overview/network#firmware-to-onboard-computers-can-bus) for a broad overview of CAN. -/// - See [this page](https://bing-rover.gitbook.io/software-docs/network-details/can-bus) for an in-depth look into how we use CAN on the rover. -/// - See also: the [Wikipedia](https://en.wikipedia.org/wiki/CAN_bus) page for CAN bus. -library; - -import "dart:async"; - -import "package:burt_network/burt_network.dart"; -import "package:subsystems/subsystems.dart"; - -import "service.dart"; - -/// Maps CAN IDs to [WrappedMessage.name] for data messages. -final Map dataCanIDs = { - 0x14: DriveData().messageName, - 0x15: ArmData().messageName, - 0x16: GripperData().messageName, - 0x17: ScienceData().messageName, -}; - -/// Maps [WrappedMessage.name] to CAN IDs for command messages. -final Map commandCanIDs = { - ArmCommand().messageName: 0x23, - GripperCommand().messageName: 0x33, - ScienceCommand().messageName: 0x43, - DriveCommand().messageName: 0x53, -}; - -/// Manages a CAN socket on the subsystems program. -/// -/// When a new message is received, its ID is looked up in [dataCanIDs] and sent over UDP. -/// When a UDP message is received, its ID is looked up in [commandCanIDs] and sent over CAN. -class CanService extends MessageService { - /// The native CAN library. On non-Linux platforms, this will be a stub that does nothing. - final can = CanSocket(); - - StreamSubscription? _subscription; - - /// Initializes the CAN library. - @override - Future init() async { - if (!await can.init()) return false; - _subscription = can.incomingMessages.listen(onMessage); - return true; - } - - /// Disposes the native CAN library and any resources it holds. - @override - Future dispose() async { - await _subscription?.cancel(); - await can.dispose(); - } - - /// Handles an incoming CAN message. - void onMessage(CanMessage message) { - final name = dataCanIDs[message.id]; -// logger.debug("Received CAN message (0x${message.id.toRadixString(16)}): ${message.data}. Name=${name ?? 'None'}"); - if (name == null) { - logger.warning("Received CAN message with unknown ID", body: "ID=0x${message.id.toRadixString(16)}"); - return; - } - // We must copy the data since we'll be disposing the pointer. - final copy = List.from(message.data); - final wrapper = WrappedMessage(name: name, data: copy); - collection.server.sendWrapper(wrapper); - message.dispose(); - } - - /// Sends a [WrappedMessage] to the appropriate subsystem, using [commandCanIDs]. - @override - void sendWrapper(WrappedMessage wrapper) { - final id = commandCanIDs[wrapper.name]; - if (id == null) { - logger.warning("Received unknown WrappedMessage: ${wrapper.name}", body: "Data: ${wrapper.data}"); - return; - } - can.sendMessage(id: id, data: wrapper.data); - } -} diff --git a/lib/src/messages/serial.dart b/lib/src/messages/serial.dart deleted file mode 100644 index 871b4ad..0000000 --- a/lib/src/messages/serial.dart +++ /dev/null @@ -1,105 +0,0 @@ -import "dart:async"; -import "dart:io"; -import "dart:typed_data"; - -import "package:collection/collection.dart"; - -import "package:burt_network/burt_network.dart"; -import "package:subsystems/subsystems.dart"; - -import "service.dart"; - -/// Maps command names to [Device]s. -final nameToDevice = { - ArmCommand().messageName: Device.ARM, - GripperCommand().messageName: Device.GRIPPER, - DriveCommand().messageName: Device.DRIVE, - ScienceCommand().messageName: Device.SCIENCE, -}; - -/// A service to send and receive messages to the firmware over serial. -class SerialService extends MessageService { - /// Gets all the names of all the ports. - static Future> getPortNames() async { - final allPorts = DelegateSerialPort.allPorts; - if (!Platform.isLinux) return allPorts; - final imuCommand = await Process.run("realpath", ["/dev/rover-imu"]); - final imuPort = imuCommand.stdout.trim(); - logger.trace("IMU is on: $imuPort"); - final gpsCommand = await Process.run("realpath", ["/dev/rover-gps"]); - final gpsPort = gpsCommand.stdout.trim(); - const piPort = "/dev/ttyAMA0"; - logger.trace("GPS is on: $gpsPort"); - logger.trace("All ports: $allPorts"); - final forbiddenPorts = {imuPort, gpsPort, piPort}; - return [ - for (final port in allPorts) - if (!forbiddenPorts.contains(port)) - port, - ]; - } - - /// Gets all firmware devices attached to the device, ignoring the GPS and IMU ports. - static Future> getFirmware() async => [ - for (final port in await getPortNames()) - BurtFirmwareSerial(port: port, logger: logger), - ]; - - final List> _subscriptions = []; - - /// All the connected devices. - List devices = []; - - @override - Future init() async { - devices = await getFirmware(); - for (final device in devices) { - logger.debug("Initializing device: ${device.port}"); - await device.init(); - if (!device.isReady) continue; - final subscription = device.stream?.listen((data) => _onMessage(data, device)); - if (subscription == null) continue; - _subscriptions.add(subscription); - } - // This service is a backup service for [CanService] - // If something went wrong here, the messages will be sent over CAN instead. - // In addition, 1 or 2 subsystems may be connected at a time, not all 3. - // Therefore, it is not as useful to return false here on an error condition. - return true; - } - - @override - Future dispose() async { - for (final subscription in _subscriptions) { - await subscription.cancel(); - } - for (final device in devices) { - await device.dispose(); - } - } - - void _onMessage(Uint8List data, BurtFirmwareSerial serial) { - final name = switch (serial.device) { - Device.ARM => ArmData().messageName, - Device.DRIVE => DriveData().messageName, - Device.GRIPPER => GripperData().messageName, - Device.SCIENCE => ScienceData().messageName, - _ => null, - }; - if (name == null) { - logger.warning("Unrecognized Serial device", body: "Port: ${serial.port}, name: ${serial.device}"); - return; - } - collection.server.sendWrapper(WrappedMessage(data: data, name: name)); - } - - @override - bool sendWrapper(WrappedMessage wrapper) { - final device = nameToDevice[wrapper.name]; - if (device == null) return false; - final serial = devices.firstWhereOrNull((s) => s.device == device); - if (serial == null) return false; - serial.sendBytes(wrapper.data); - return true; - } -} diff --git a/lib/src/messages/service.dart b/lib/src/messages/service.dart deleted file mode 100644 index 347ae0c..0000000 --- a/lib/src/messages/service.dart +++ /dev/null @@ -1,10 +0,0 @@ -import "package:burt_network/burt_network.dart"; - -/// A service to send commands and receive data from a device. -abstract class MessageService extends Service { - /// Unwraps a [WrappedMessage] and sends it to the device, - void sendWrapper(WrappedMessage wrapper); - - /// Wraps a message and sends it using [sendWrapper]. - void sendMessage(Message message) => sendWrapper(message.wrap()); -} diff --git a/lib/src/server.dart b/lib/src/server.dart deleted file mode 100644 index f82c4a0..0000000 --- a/lib/src/server.dart +++ /dev/null @@ -1,33 +0,0 @@ -import "package:burt_network/burt_network.dart"; - -import "package:subsystems/subsystems.dart"; - -/// A UDP server to connect to the dashboard. -/// -/// This server should collect all commands that come in and forward them to the -/// appropriate CAN device. All CAN messages should be forwarded to this server. -class SubsystemsServer extends RoverServer { - /// Creates a Subsystems server on the given port. - SubsystemsServer({required super.port}) : super(device: Device.SUBSYSTEMS); - - @override - void onMessage(WrappedMessage wrapper) { - if (wrapper.name == RoverPosition().messageName) return; - collection.sendWrapper(wrapper); - } - - @override - Future restart() async { - await collection.dispose(); - await collection.init(); - } - - @override - void onDisconnect() { - super.onDisconnect(); - collection.stopHardware(); - } - - @override - Future onShutdown() => collection.dispose(); -} diff --git a/lib/subsystems.dart b/lib/subsystems.dart index 745bd81..2deeacd 100644 --- a/lib/subsystems.dart +++ b/lib/subsystems.dart @@ -1,14 +1,10 @@ import "package:burt_network/burt_network.dart"; -import "src/server.dart"; import "src/devices/gps.dart"; import "src/devices/imu.dart"; -import "src/messages/can.dart"; -import "src/messages/serial.dart"; -import "src/messages/service.dart"; - -export "src/server.dart"; +import "src/devices/firmware.dart"; +export "src/devices/firmware.dart"; export "src/devices/imu.dart"; export "src/devices/gps.dart"; @@ -19,18 +15,19 @@ export "src/can/socket_interface.dart"; export "src/can/socket_stub.dart"; /// Contains all the resources needed by the subsystems program. -class SubsystemsCollection extends MessageService { +class SubsystemsCollection extends Service { /// Whether the subsystems is fully initialized. bool isReady = false; - - /// The CAN bus socket. - final can = CanService(); + /// The Serial service. - final serial = SerialService(); + final firmware = FirmwareManager(); + /// The UDP server. - final server = SubsystemsServer(port: 8001); + late final server = RoverSocket(port: 8001, collection: this, device: Device.SUBSYSTEMS); + /// The GPS reader. final gps = GpsReader(); + /// The IMU reader. final imu = ImuReader(); @@ -42,8 +39,7 @@ class SubsystemsCollection extends MessageService { await server.init(); var result = true; try { - result &= await can.init(); - result &= await serial.init(); + result &= await firmware.init(); result &= await gps.init(); result &= await imu.init(); logger.info("Subsystems initialized"); @@ -61,10 +57,9 @@ class SubsystemsCollection extends MessageService { @override Future dispose() async { logger.info("Shutting down..."); - stopHardware(); + await onDisconnect(); isReady = false; - await can.dispose(); - await serial.dispose(); + await firmware.dispose(); await server.dispose(); await imu.dispose(); await gps.dispose(); @@ -72,23 +67,17 @@ class SubsystemsCollection extends MessageService { } @override - void sendWrapper(WrappedMessage wrapper) { - if (!isReady) return; - if (collection.serial.sendWrapper(wrapper)) return; -// collection.can.sendWrapper(wrapper); - } - - /// Stops all the hardware from moving. - void stopHardware() { + Future onDisconnect() async { + await super.onDisconnect(); logger.info("Stopping all hardware"); final stopDrive = DriveCommand(throttle: 0, setThrottle: true); final stopArm = ArmCommand(stop: true); final stopGripper = GripperCommand(stop: true); final stopScience = ScienceCommand(stop: true); - sendMessage(stopDrive); - sendMessage(stopArm); - sendMessage(stopGripper); - sendMessage(stopScience); + firmware.sendMessage(stopDrive); + firmware.sendMessage(stopArm); + firmware.sendMessage(stopGripper); + firmware.sendMessage(stopScience); } } diff --git a/pubspec.yaml b/pubspec.yaml index 8b0d7c3..9c8c449 100644 --- a/pubspec.yaml +++ b/pubspec.yaml @@ -4,21 +4,21 @@ version: 1.0.0 publish_to: none environment: - sdk: 3.2.2 + sdk: ^3.5.0 # Add regular dependencies here. dependencies: - burt_network: + burt_network: git: https://github.com/BinghamtonRover/Networking.git - collection: ^1.18.0 + collection: ^1.19.0 csv: ^6.0.0 - ffi: 2.1.0 + ffi: ^2.1.3 libserialport: ^0.3.0+1 meta: ^1.14.0 osc: ^1.0.0 protobuf: ^3.1.0 dev_dependencies: - ffigen: ^11.0.0 - test: ^1.21.0 - very_good_analysis: ^5.0.0+1 + ffigen: 14.0.0 + test: ^1.25.8 + very_good_analysis: ^6.0.0 diff --git a/src/burt_can/burt_can.h b/src/burt_can/burt_can.h index 8c5efc5..548dde7 100644 --- a/src/burt_can/burt_can.h +++ b/src/burt_can/burt_can.h @@ -3,7 +3,10 @@ #include +#ifdef __cplusplus extern "C" { +#endif + typedef enum BurtCanType { CAN = 0, CANFD = 1, @@ -47,6 +50,9 @@ BurtCanStatus BurtCan_close(BurtCan* pointer); NativeCanMessage* NativeCanMessage_create(); void NativeCanMessage_free(NativeCanMessage* pointer); + +#ifdef __cplusplus } +#endif -#endif +#endif