From 5399146bc3772c5e92d7c28d8ce90ee7db9338de Mon Sep 17 00:00:00 2001 From: Gold87 <91761103+Gold872@users.noreply.github.com> Date: Wed, 12 Nov 2025 14:08:40 -0500 Subject: [PATCH 01/16] Add skeleton for arm auxillary program --- .github/workflows/analyze.yaml | 3 ++ arm_auxillary/.gitignore | 4 +++ arm_auxillary/README.md | 1 + arm_auxillary/bin/arm_auxillary.dart | 5 ++++ arm_auxillary/lib/arm_auxillary.dart | 43 ++++++++++++++++++++++++++++ arm_auxillary/lib/src/firmware.dart | 15 ++++++++++ arm_auxillary/pubspec.yaml | 16 +++++++++++ pubspec.yaml | 1 + 8 files changed, 88 insertions(+) create mode 100644 arm_auxillary/.gitignore create mode 100644 arm_auxillary/README.md create mode 100644 arm_auxillary/bin/arm_auxillary.dart create mode 100644 arm_auxillary/lib/arm_auxillary.dart create mode 100644 arm_auxillary/lib/src/firmware.dart create mode 100644 arm_auxillary/pubspec.yaml diff --git a/.github/workflows/analyze.yaml b/.github/workflows/analyze.yaml index 79808197..9c152959 100644 --- a/.github/workflows/analyze.yaml +++ b/.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/* diff --git a/arm_auxillary/.gitignore b/arm_auxillary/.gitignore new file mode 100644 index 00000000..2d9b0510 --- /dev/null +++ b/arm_auxillary/.gitignore @@ -0,0 +1,4 @@ +# https://dart.dev/guides/libraries/private-files +# Created by `dart pub` +.dart_tool/ +pubspec_overrides.yaml diff --git a/arm_auxillary/README.md b/arm_auxillary/README.md new file mode 100644 index 00000000..dfcd3131 --- /dev/null +++ b/arm_auxillary/README.md @@ -0,0 +1 @@ +Program for the auxillary board on the arm. diff --git a/arm_auxillary/bin/arm_auxillary.dart b/arm_auxillary/bin/arm_auxillary.dart new file mode 100644 index 00000000..0851a687 --- /dev/null +++ b/arm_auxillary/bin/arm_auxillary.dart @@ -0,0 +1,5 @@ +import "package:arm_auxillary/arm_auxillary.dart"; + +void main() async { + await collection.init(); +} diff --git a/arm_auxillary/lib/arm_auxillary.dart b/arm_auxillary/lib/arm_auxillary.dart new file mode 100644 index 00000000..88fe64c9 --- /dev/null +++ b/arm_auxillary/lib/arm_auxillary.dart @@ -0,0 +1,43 @@ +import "dart:io"; + +import "package:burt_network/burt_network.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 { + /// The server for the arm auxillary program + late final RoverSocket server = RoverSocket( + // change to ARM_AUXILLARY once protobuf message is added + device: Device.DEVICE_UNDEFINED, + port: 8010, + collection: this, + destination: subsystemsSocket, + keepDestination: true, + ); + + @override + Future init() async { + bool result = true; + logger.socket = server; + result &= await server.init(); + // TODO: initialize the rest of the arm auxillary's resources, such as + // arm and EA board communication + return result; + } + + @override + Future dispose() async { + // TODO: implement dispose + } +} + +/// The collection of all the arm auxillary's resources +final collection = ArmAuxillary(); diff --git a/arm_auxillary/lib/src/firmware.dart b/arm_auxillary/lib/src/firmware.dart new file mode 100644 index 00000000..f792303f --- /dev/null +++ b/arm_auxillary/lib/src/firmware.dart @@ -0,0 +1,15 @@ +import "package:burt_network/burt_network.dart"; + +/// Service to manage communication from the arm auxillary board to EA and HREI devices +class FirmwareManager extends Service { + @override + Future init() async { + // TODO: implement init, finding all serial devices and attempting to connect to them + return false; + } + + @override + Future dispose() async { + // TODO: implement dispose + } +} diff --git a/arm_auxillary/pubspec.yaml b/arm_auxillary/pubspec.yaml new file mode 100644 index 00000000..6643990a --- /dev/null +++ b/arm_auxillary/pubspec.yaml @@ -0,0 +1,16 @@ +name: arm_auxillary +description: A sample command-line application. +version: 1.0.0 +publish_to: none + +resolution: workspace + +environment: + sdk: ^3.9.0 + +# Add regular dependencies here. +dependencies: + burt_network: ^2.7.0 + +dev_dependencies: + very_good_analysis: ^6.0.0 diff --git a/pubspec.yaml b/pubspec.yaml index f58ebf22..eba815eb 100644 --- a/pubspec.yaml +++ b/pubspec.yaml @@ -4,6 +4,7 @@ environment: sdk: ^3.9.0 workspace: + - arm_auxillary - autonomy - burt_network - subsystems From e4c36cf36e80bd43013d2afa8b0387b9642d5ce6 Mon Sep 17 00:00:00 2001 From: preston Date: Sun, 23 Nov 2025 12:21:24 -0500 Subject: [PATCH 02/16] Implement Arm Auxillary Logic --- arm_auxillary/lib/arm_auxillary.dart | 30 ++++++++++++++-- arm_auxillary/lib/src/firmware.dart | 53 ++++++++++++++++++++++++++-- burt_network/Protobuf | 2 +- 3 files changed, 79 insertions(+), 6 deletions(-) diff --git a/arm_auxillary/lib/arm_auxillary.dart b/arm_auxillary/lib/arm_auxillary.dart index 88fe64c9..5a658ea6 100644 --- a/arm_auxillary/lib/arm_auxillary.dart +++ b/arm_auxillary/lib/arm_auxillary.dart @@ -1,6 +1,7 @@ import "dart:io"; import "package:burt_network/burt_network.dart"; +import "src/firmware.dart"; /// Logger for the arm auxillary program final logger = BurtLogger(); @@ -13,6 +14,12 @@ final subsystemsSocket = SocketInfo( /// 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. + final firmware = FirmwareManager(); + /// The server for the arm auxillary program late final RoverSocket server = RoverSocket( // change to ARM_AUXILLARY once protobuf message is added @@ -30,12 +37,31 @@ class ArmAuxillary extends Service { result &= await server.init(); // TODO: initialize the rest of the arm auxillary's resources, such as // arm and EA board communication - return result; + try { + result &= await firmware.init(); + 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 dispose() async { - // TODO: implement dispose + logger.info("Arm Auxillary software shutting down..."); + isReady = false; + await firmware.dispose(); + await server.dispose(); + logger.socket = null; + logger.info("Subsystems disposed"); } } diff --git a/arm_auxillary/lib/src/firmware.dart b/arm_auxillary/lib/src/firmware.dart index f792303f..480425f6 100644 --- a/arm_auxillary/lib/src/firmware.dart +++ b/arm_auxillary/lib/src/firmware.dart @@ -1,15 +1,62 @@ import "package:burt_network/burt_network.dart"; +import "package:subsystems/lib/src/devices/serial_utils.dart"; +import "package:collection/collection.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, + RelaysCommand().messageName: Device.RELAY, +}; /// Service to manage communication from the arm auxillary board to EA and HREI devices 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 { - // TODO: implement init, finding all serial devices and attempting to connect to them - return false; + 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); + _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 dispose() async { - // TODO: implement dispose + 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()); } diff --git a/burt_network/Protobuf b/burt_network/Protobuf index 3a009905..7e77da8a 160000 --- a/burt_network/Protobuf +++ b/burt_network/Protobuf @@ -1 +1 @@ -Subproject commit 3a0099055d805fe6db85d532e89dcde2dbfc4fe0 +Subproject commit 7e77da8a2c9d9ce2cd6e6751a6aa777af5e257ca From 13b3a500ccf078c17aecec67ceef3c94a20adb53 Mon Sep 17 00:00:00 2001 From: Felforge Date: Mon, 26 Jan 2026 20:03:46 -0500 Subject: [PATCH 03/16] Fix Stuff --- arm_auxillary/lib/arm_auxillary.dart | 14 ++++++++------ arm_auxillary/lib/src/firmware.dart | 18 ++++++++++++++---- arm_auxillary/pubspec.yaml | 2 ++ 3 files changed, 24 insertions(+), 10 deletions(-) diff --git a/arm_auxillary/lib/arm_auxillary.dart b/arm_auxillary/lib/arm_auxillary.dart index 5a658ea6..f427197c 100644 --- a/arm_auxillary/lib/arm_auxillary.dart +++ b/arm_auxillary/lib/arm_auxillary.dart @@ -18,12 +18,14 @@ class ArmAuxillary extends Service { bool isReady = false; /// The Serial service. - final firmware = FirmwareManager(); + late final firmware = FirmwareManager( + getServer: () => server, + logger: logger, + ); /// The server for the arm auxillary program late final RoverSocket server = RoverSocket( - // change to ARM_AUXILLARY once protobuf message is added - device: Device.DEVICE_UNDEFINED, + device: Device.ARM, port: 8010, collection: this, destination: subsystemsSocket, @@ -32,11 +34,11 @@ class ArmAuxillary extends Service { @override Future init() async { - bool result = true; + var result = true; logger.socket = server; result &= await server.init(); - // TODO: initialize the rest of the arm auxillary's resources, such as - // arm and EA board communication + // 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(); if (result) { diff --git a/arm_auxillary/lib/src/firmware.dart b/arm_auxillary/lib/src/firmware.dart index 480425f6..d9661fce 100644 --- a/arm_auxillary/lib/src/firmware.dart +++ b/arm_auxillary/lib/src/firmware.dart @@ -1,11 +1,11 @@ +import "dart:async"; import "package:burt_network/burt_network.dart"; -import "package:subsystems/lib/src/devices/serial_utils.dart"; +import "package:subsystems/src/devices/serial_utils.dart"; import "package:collection/collection.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, RelaysCommand().messageName: Device.RELAY, @@ -13,22 +13,32 @@ final nameToDevice = { /// 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> _subscriptions = []; /// A list of firmware devices attached to the rover. List devices = []; + /// Creates a new FirmwareManager instance + FirmwareManager({required this.getServer, required this.logger}); + @override Future init() async { devices = await getFirmwareDevices(); - collection.server.messages.listen(_sendToSerial); + 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(collection.server.sendWrapper); + final subscription = device.messages.listen(server?.sendWrapper ?? (_) {}); _subscriptions.add(subscription); } return result; diff --git a/arm_auxillary/pubspec.yaml b/arm_auxillary/pubspec.yaml index 6643990a..c585a6a9 100644 --- a/arm_auxillary/pubspec.yaml +++ b/arm_auxillary/pubspec.yaml @@ -11,6 +11,8 @@ environment: # Add regular dependencies here. dependencies: burt_network: ^2.7.0 + collection: ^1.19.0 + subsystems: ^1.0.0 dev_dependencies: very_good_analysis: ^6.0.0 From 7e289692d2b87d3f450c65abbd547fca07d0bba9 Mon Sep 17 00:00:00 2001 From: Felforge Date: Mon, 26 Jan 2026 20:05:49 -0500 Subject: [PATCH 04/16] Fix some imports --- arm_auxillary/lib/src/firmware.dart | 2 +- subsystems/lib/subsystems.dart | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/arm_auxillary/lib/src/firmware.dart b/arm_auxillary/lib/src/firmware.dart index d9661fce..cde1e775 100644 --- a/arm_auxillary/lib/src/firmware.dart +++ b/arm_auxillary/lib/src/firmware.dart @@ -1,6 +1,6 @@ import "dart:async"; import "package:burt_network/burt_network.dart"; -import "package:subsystems/src/devices/serial_utils.dart"; +import "package:subsystems/subsystems.dart"; import "package:collection/collection.dart"; /// Maps command names to [Device]s. diff --git a/subsystems/lib/subsystems.dart b/subsystems/lib/subsystems.dart index ce023b35..1729d498 100644 --- a/subsystems/lib/subsystems.dart +++ b/subsystems/lib/subsystems.dart @@ -9,6 +9,7 @@ import "src/devices/firmware.dart"; export "src/devices/firmware.dart"; export "src/devices/imu.dart"; export "src/devices/gps.dart"; +export "src/devices/serial_utils.dart"; export "src/can/ffi.dart"; export "src/can/message.dart"; From 953d611c6164dbf7ac90608815c97c5644376aba Mon Sep 17 00:00:00 2001 From: Felforge Date: Mon, 26 Jan 2026 20:07:19 -0500 Subject: [PATCH 05/16] Remove unecessary import --- subsystems/lib/src/devices/firmware.dart | 2 -- 1 file changed, 2 deletions(-) diff --git a/subsystems/lib/src/devices/firmware.dart b/subsystems/lib/src/devices/firmware.dart index ee8acc9b..8e151285 100644 --- a/subsystems/lib/src/devices/firmware.dart +++ b/subsystems/lib/src/devices/firmware.dart @@ -5,8 +5,6 @@ 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, From 7e5ae337ecc81aa4363f41af9d38a45b513bd2e0 Mon Sep 17 00:00:00 2001 From: Felforge Date: Mon, 26 Jan 2026 20:09:36 -0500 Subject: [PATCH 06/16] Fix formatting issues? --- arm_auxillary/lib/arm_auxillary.dart | 1 + arm_auxillary/lib/src/firmware.dart | 4 +++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/arm_auxillary/lib/arm_auxillary.dart b/arm_auxillary/lib/arm_auxillary.dart index f427197c..b858f412 100644 --- a/arm_auxillary/lib/arm_auxillary.dart +++ b/arm_auxillary/lib/arm_auxillary.dart @@ -1,6 +1,7 @@ import "dart:io"; import "package:burt_network/burt_network.dart"; + import "src/firmware.dart"; /// Logger for the arm auxillary program diff --git a/arm_auxillary/lib/src/firmware.dart b/arm_auxillary/lib/src/firmware.dart index cde1e775..edaaa628 100644 --- a/arm_auxillary/lib/src/firmware.dart +++ b/arm_auxillary/lib/src/firmware.dart @@ -1,7 +1,9 @@ import "dart:async"; + +import "package:collection/collection.dart"; + import "package:burt_network/burt_network.dart"; import "package:subsystems/subsystems.dart"; -import "package:collection/collection.dart"; /// Maps command names to [Device]s. final nameToDevice = { From 84996894ca18cfbd316b1c2921872846805933fd Mon Sep 17 00:00:00 2001 From: Felforge Date: Mon, 26 Jan 2026 20:15:20 -0500 Subject: [PATCH 07/16] Actually fix formatting this time I think --- arm_auxillary/lib/arm_auxillary.dart | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/arm_auxillary/lib/arm_auxillary.dart b/arm_auxillary/lib/arm_auxillary.dart index b858f412..8d75f86b 100644 --- a/arm_auxillary/lib/arm_auxillary.dart +++ b/arm_auxillary/lib/arm_auxillary.dart @@ -52,7 +52,10 @@ class ArmAuxillary extends Service { // 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()); + logger.critical( + "Unexpected error when initializing Arm Auxillary", + body: error.toString(), + ); return false; } } From 4dd4e4eb3b723f82298ae1788bc49103338ee51b Mon Sep 17 00:00:00 2001 From: Felforge Date: Mon, 26 Jan 2026 20:17:53 -0500 Subject: [PATCH 08/16] Fix Dart formatting in firmware.dart --- arm_auxillary/lib/src/firmware.dart | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/arm_auxillary/lib/src/firmware.dart b/arm_auxillary/lib/src/firmware.dart index edaaa628..81234113 100644 --- a/arm_auxillary/lib/src/firmware.dart +++ b/arm_auxillary/lib/src/firmware.dart @@ -40,7 +40,9 @@ class FirmwareManager extends Service { logger.debug("Initializing device: ${device.port}"); result &= await device.init(); if (!device.isReady) continue; - final subscription = device.messages.listen(server?.sendWrapper ?? (_) {}); + final subscription = device.messages.listen( + server?.sendWrapper ?? (_) {}, + ); _subscriptions.add(subscription); } return result; From 2bd8b24da1b9006eabe2669f3d3c46fcdf3ab1a7 Mon Sep 17 00:00:00 2001 From: preston Date: Wed, 28 Jan 2026 09:55:36 -0500 Subject: [PATCH 09/16] Merge stuff? --- autonomy/.github/pubspec_overrides.yaml | 7 + autonomy/.github/workflows/analyze.yml | 41 +++ autonomy/.github/workflows/documentation.yml | 55 ++++ autonomy/bin/arcuo.dart | 16 ++ autonomy/bin/sensorless.dart | 63 +++++ autonomy/bin/test.dart | 23 ++ autonomy/lib/src/interfaces/a_star.dart | 66 +++++ autonomy/lib/src/interfaces/autonomy.dart | 66 +++++ autonomy/lib/src/interfaces/detector.dart | 10 + autonomy/lib/src/interfaces/drive.dart | 79 ++++++ autonomy/lib/src/interfaces/error.dart | 14 + autonomy/lib/src/interfaces/gps.dart | 27 ++ autonomy/lib/src/interfaces/gps_utils.dart | 62 +++++ autonomy/lib/src/interfaces/imu.dart | 19 ++ autonomy/lib/src/interfaces/imu_utils.dart | 27 ++ autonomy/lib/src/interfaces/orchestrator.dart | 61 +++++ autonomy/lib/src/interfaces/pathfinding.dart | 18 ++ autonomy/lib/src/interfaces/receiver.dart | 9 + autonomy/lib/src/interfaces/reporter.dart | 23 ++ autonomy/lib/src/interfaces/server.dart | 25 ++ autonomy/lib/src/interfaces/service.dart | 1 + autonomy/lib/src/interfaces/video.dart | 17 ++ autonomy/lib/src/rover/corrector.dart | 40 +++ autonomy/lib/src/rover/detector.dart | 21 ++ autonomy/lib/src/rover/drive.dart | 67 +++++ autonomy/lib/src/rover/drive/motors.dart | 26 ++ autonomy/lib/src/rover/drive/sensor.dart | 148 +++++++++++ autonomy/lib/src/rover/drive/timed.dart | 51 ++++ autonomy/lib/src/rover/gps.dart | 41 +++ autonomy/lib/src/rover/imu.dart | 43 +++ autonomy/lib/src/rover/orchestrator.dart | 113 ++++++++ autonomy/lib/src/rover/pathfinding.dart | 22 ++ autonomy/lib/src/rover/sensorless.dart | 64 +++++ autonomy/lib/src/rover/video.dart | 32 +++ autonomy/lib/src/simulator/detector.dart | 46 ++++ autonomy/lib/src/simulator/drive.dart | 54 ++++ autonomy/lib/src/simulator/gps.dart | 28 ++ autonomy/lib/src/simulator/imu.dart | 35 +++ autonomy/lib/src/simulator/orchestrator.dart | 32 +++ autonomy/lib/src/simulator/pathfinding.dart | 25 ++ autonomy/lib/src/simulator/realsense.dart | 22 ++ autonomy/test/sensor_test.dart | 246 ++++++++++++++++++ 42 files changed, 1885 insertions(+) create mode 100644 autonomy/.github/pubspec_overrides.yaml create mode 100644 autonomy/.github/workflows/analyze.yml create mode 100644 autonomy/.github/workflows/documentation.yml create mode 100644 autonomy/bin/arcuo.dart create mode 100644 autonomy/bin/sensorless.dart create mode 100644 autonomy/bin/test.dart create mode 100644 autonomy/lib/src/interfaces/a_star.dart create mode 100644 autonomy/lib/src/interfaces/autonomy.dart create mode 100644 autonomy/lib/src/interfaces/detector.dart create mode 100644 autonomy/lib/src/interfaces/drive.dart create mode 100644 autonomy/lib/src/interfaces/error.dart create mode 100644 autonomy/lib/src/interfaces/gps.dart create mode 100644 autonomy/lib/src/interfaces/gps_utils.dart create mode 100644 autonomy/lib/src/interfaces/imu.dart create mode 100644 autonomy/lib/src/interfaces/imu_utils.dart create mode 100644 autonomy/lib/src/interfaces/orchestrator.dart create mode 100644 autonomy/lib/src/interfaces/pathfinding.dart create mode 100644 autonomy/lib/src/interfaces/receiver.dart create mode 100644 autonomy/lib/src/interfaces/reporter.dart create mode 100644 autonomy/lib/src/interfaces/server.dart create mode 100644 autonomy/lib/src/interfaces/service.dart create mode 100644 autonomy/lib/src/interfaces/video.dart create mode 100644 autonomy/lib/src/rover/corrector.dart create mode 100644 autonomy/lib/src/rover/detector.dart create mode 100644 autonomy/lib/src/rover/drive.dart create mode 100644 autonomy/lib/src/rover/drive/motors.dart create mode 100644 autonomy/lib/src/rover/drive/sensor.dart create mode 100644 autonomy/lib/src/rover/drive/timed.dart create mode 100644 autonomy/lib/src/rover/gps.dart create mode 100644 autonomy/lib/src/rover/imu.dart create mode 100644 autonomy/lib/src/rover/orchestrator.dart create mode 100644 autonomy/lib/src/rover/pathfinding.dart create mode 100644 autonomy/lib/src/rover/sensorless.dart create mode 100644 autonomy/lib/src/rover/video.dart create mode 100644 autonomy/lib/src/simulator/detector.dart create mode 100644 autonomy/lib/src/simulator/drive.dart create mode 100644 autonomy/lib/src/simulator/gps.dart create mode 100644 autonomy/lib/src/simulator/imu.dart create mode 100644 autonomy/lib/src/simulator/orchestrator.dart create mode 100644 autonomy/lib/src/simulator/pathfinding.dart create mode 100644 autonomy/lib/src/simulator/realsense.dart create mode 100644 autonomy/test/sensor_test.dart diff --git a/autonomy/.github/pubspec_overrides.yaml b/autonomy/.github/pubspec_overrides.yaml new file mode 100644 index 00000000..b7c9d120 --- /dev/null +++ b/autonomy/.github/pubspec_overrides.yaml @@ -0,0 +1,7 @@ +resolution: + +dependency_overrides: + burt_network: + git: + url: https://github.com/BinghamtonRover/Networking + ref: 2.3.1 \ No newline at end of file diff --git a/autonomy/.github/workflows/analyze.yml b/autonomy/.github/workflows/analyze.yml new file mode 100644 index 00000000..b822d66a --- /dev/null +++ b/autonomy/.github/workflows/analyze.yml @@ -0,0 +1,41 @@ +name: Dart Analyzer + +on: + push: + branches: [ "main" ] + pull_request: + +jobs: + analyze: + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v3 + + # Note: This workflow uses the latest stable version of the Dart SDK. + # 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.6.0 + + # This package is part of a Pub Workspace. However, CI still needs to + # run on this repo by itself, so we want to override burt_network to use + # a Git dependency ONLY on GitHub Actions. + # + # To get around this, we commit the overrides to the .github folder where + # Dart can't find them, then copy them as part of the CI workflow. + - name: Install dependencies + run: | + mv .github/pubspec_overrides.yaml . + dart pub get + + - name: Analyze project source + run: dart analyze --fatal-infos + + # Your project will need to have tests in test/ and a dependency on + # package:test for this step to succeed. Note that Flutter projects will + # want to change this to 'flutter test'. + # FIXME: Disabling tests for now + # - name: Run tests + # run: dart test diff --git a/autonomy/.github/workflows/documentation.yml b/autonomy/.github/workflows/documentation.yml new file mode 100644 index 00000000..0267dfea --- /dev/null +++ b/autonomy/.github/workflows/documentation.yml @@ -0,0 +1,55 @@ +name: Generate Dart Documentation + +on: + push: + branches: [ "main" ] + +jobs: + build: + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v3 + with: + fetch-depth: 0 + + - name: Git Setup + run: | + git config user.name "GitHub Actions Bot" + git config user.email "<>" + git branch --all + git switch --track origin/documentation + git reset --hard origin/main + + - name: Install Dart + uses: dart-lang/setup-dart@v1 + with: + sdk: 3.5.2 + + - name: Install dependencies + run: dart pub get + + - name: Analyze Dart code + run: dart analyze --fatal-infos + + - name: Output error + if: failure() + run: echo "::error The code or is missing documentation. Run flutter analyze --dartdocs" + + - name: Generate documentation + run: dart doc --output=docs + + # Your project will need to have tests in test/ and a dependency on + # package:test for this step to succeed. Note that Flutter projects will + # want to change this to 'flutter test'. + - name: Run tests + run: dart test + + - name: Commit and push files + run: | + cd docs + cd .. + git status + git stage --force docs + git commit -a -m "Generated documentation" + git push --force diff --git a/autonomy/bin/arcuo.dart b/autonomy/bin/arcuo.dart new file mode 100644 index 00000000..ae9f7d8c --- /dev/null +++ b/autonomy/bin/arcuo.dart @@ -0,0 +1,16 @@ +import "package:autonomy/autonomy.dart"; + +void main() async { + final rover = RoverAutonomy(); + rover.drive = RoverDrive(collection: rover, useImu: false, useGps: false); + await rover.init(); + //await rover.waitForValue(); + final didSeeAruco = await rover.drive.spinForAruco(); + if (didSeeAruco) { + rover.logger.info("Found aruco"); + await rover.drive.approachAruco(); + } else { + rover.logger.warning("COuld not find aruco"); + } + rover.logger.info("Done"); +} diff --git a/autonomy/bin/sensorless.dart b/autonomy/bin/sensorless.dart new file mode 100644 index 00000000..0ddcd1f0 --- /dev/null +++ b/autonomy/bin/sensorless.dart @@ -0,0 +1,63 @@ +import "package:burt_network/logging.dart"; +import "package:burt_network/protobuf.dart"; + +import "package:autonomy/simulator.dart"; +import "package:autonomy/rover.dart"; + +void main() async { + Logger.level = LogLevel.debug; + final simulator = AutonomySimulator(); + simulator.drive = SensorlessDrive(collection: simulator, useGps: false, useImu: false); + await simulator.init(); + await simulator.server.waitForConnection(); + + final message = AutonomyData( + destination: GpsCoordinates(latitude: 0, longitude: 0), + state: AutonomyState.DRIVING, + task: AutonomyTask.GPS_ONLY, + obstacles: [], + path: [ + for (double x = 0; x < 3; x++) + for (double y = 0; y < 3; y++) + GpsCoordinates(latitude: y, longitude: x), + ], + ); + simulator.server.sendMessage(message); + + // "Snakes" around a 3x3 meter box. (0, 0), North + await simulator.drive.goForward(); // (1, 0), North + await simulator.drive.goForward(); // (2, 0), North + await simulator.drive.turnRight(); // (2, 0), East + await simulator.drive.goForward(); // (2, 1), East + await simulator.drive.turnRight(); // (2, 1), South + await simulator.drive.goForward(); // (1, 1), South + await simulator.drive.goForward(); // (0, 1), South + await simulator.drive.turnLeft(); // (0, 1), East + await simulator.drive.goForward(); // (0, 2), East + await simulator.drive.turnLeft(); // (0, 2), North + await simulator.drive.goForward(); // (1, 2), North + await simulator.drive.goForward(); // (2, 2), North + await simulator.drive.turnLeft(); // (2, 2), West + await simulator.drive.goForward(); // (2, 1), West + await simulator.drive.goForward(); // (2, 0), West + await simulator.drive.turnLeft(); // (2, 0), South + await simulator.drive.goForward(); // (1, 0), South + await simulator.drive.goForward(); // (0, 0), South + await simulator.drive.turnLeft(); // (0, 0), East + await simulator.drive.turnLeft(); // (0, 0), North + + final message2 = AutonomyData( + state: AutonomyState.AT_DESTINATION, + task: AutonomyTask.AUTONOMY_TASK_UNDEFINED, + obstacles: [], + path: [ + for (double x = 0; x < 3; x++) + for (double y = 0; y < 3; y++) + GpsCoordinates(latitude: y, longitude: x), + ], + ); + simulator.server.sendMessage(message2); + simulator.server.sendMessage(message2); + + await simulator.dispose(); +} diff --git a/autonomy/bin/test.dart b/autonomy/bin/test.dart new file mode 100644 index 00000000..12205b1d --- /dev/null +++ b/autonomy/bin/test.dart @@ -0,0 +1,23 @@ +import "package:autonomy/autonomy.dart"; +import "package:burt_network/logging.dart"; +import "package:autonomy/src/rover/gps.dart"; +import "package:autonomy/src/rover/imu.dart"; + +void main() async { + Logger.level = LogLevel.all; + final rover = RoverAutonomy(); + rover.gps = RoverGps(collection: rover); + rover.imu = RoverImu(collection: rover); + rover.drive = RoverDrive(collection: rover, useGps: false); + + await rover.init(); +// await rover.waitForReadings(); +// await rover.waitForConnection(); + + rover.logger.info("Starting"); + await rover.drive.turnLeft(); + await rover.drive.turnRight(); + + rover.logger.info("Done"); + await rover.dispose(); +} diff --git a/autonomy/lib/src/interfaces/a_star.dart b/autonomy/lib/src/interfaces/a_star.dart new file mode 100644 index 00000000..6c7f16ec --- /dev/null +++ b/autonomy/lib/src/interfaces/a_star.dart @@ -0,0 +1,66 @@ +import "package:a_star/a_star.dart"; + +import "package:burt_network/protobuf.dart"; +import "package:autonomy/interfaces.dart"; + +class AutonomyAStarState extends AStarState { + final DriveDirection direction; + final DriveOrientation orientation; + final GpsCoordinates position; + final GpsCoordinates goal; + final AutonomyInterface collection; + + AutonomyAStarState({ + required this.position, + required this.goal, + required this.collection, + required this.direction, + required this.orientation, + required super.depth, + }); + + factory AutonomyAStarState.start({ + required AutonomyInterface collection, + required GpsCoordinates goal, + }) => AutonomyAStarState( + position: collection.gps.coordinates, + goal: goal, + collection: collection, + direction: DriveDirection.stop, + orientation: collection.imu.orientation!, + depth: 0, + ); + + @override + String toString() => switch(direction) { + DriveDirection.forward => "Go forward to ${position.prettyPrint()}", + DriveDirection.left => "Turn left to face $direction", + DriveDirection.right => "Turn right to face $direction", + DriveDirection.stop => "Start/Stop at ${position.prettyPrint()}", + }; + + @override + double heuristic() => position.manhattanDistance(goal); + + @override + String hash() => "${position.prettyPrint()} ($orientation)"; + + @override + bool isGoal() => position.isNear(goal); + + AutonomyAStarState copyWith({required DriveDirection direction, required DriveOrientation orientation, required GpsCoordinates position}) => AutonomyAStarState( + collection: collection, + position: position, + orientation: orientation, + direction: direction, + goal: goal, + depth: direction == DriveDirection.forward ? depth + 1 : depth + 2, + ); + + @override + Iterable expand() => [ + copyWith(direction: DriveDirection.forward, orientation: orientation, position: position.goForward(orientation)), + copyWith(direction: DriveDirection.left, orientation: orientation.turnLeft(), position: position), + copyWith(direction: DriveDirection.right, orientation: orientation.turnRight(), position: position), + ].where((state) => !collection.pathfinder.isObstacle(state.position)); +} diff --git a/autonomy/lib/src/interfaces/autonomy.dart b/autonomy/lib/src/interfaces/autonomy.dart new file mode 100644 index 00000000..ae69aba6 --- /dev/null +++ b/autonomy/lib/src/interfaces/autonomy.dart @@ -0,0 +1,66 @@ +import "package:burt_network/burt_network.dart"; + +import "package:autonomy/interfaces.dart"; + +abstract class AutonomyInterface extends Service with Receiver { + BurtLogger get logger; + GpsInterface get gps; + ImuInterface get imu; + DriveInterface get drive; + RoverSocket get server; + PathfindingInterface get pathfinder; + DetectorInterface get detector; + VideoInterface get video; + OrchestratorInterface get orchestrator; + + @override + Future init() async { + var result = true; + result &= await gps.init(); + result &= await imu.init(); + result &= await drive.init(); + result &= await server.init(); + result &= await pathfinder.init(); + result &= await detector.init(); + result &= await video.init(); + logger.info("Init orchestrator"); + await orchestrator.init(); + logger.info("Init orchestrator done"); + if (result) { + logger.info("Autonomy initialized"); + } else { + logger.warning("Autonomy could not initialize"); + } + return result; + } + + @override + Future dispose() async { + await gps.dispose(); + await imu.dispose(); + await drive.dispose(); + await pathfinder.dispose(); + await detector.dispose(); + await video.dispose(); + await orchestrator.dispose(); + logger.info("Autonomy disposed"); + await server.dispose(); + } + + Future restart() async { + await dispose(); + await init(); + } + + @override + Future waitForValue() async { + logger.info("Waiting for readings..."); + await gps.waitForValue(); + await imu.waitForValue(); + await video.waitForValue(); + logger.info("Received GPS and IMU values"); + } + + @override + bool get hasValue => gps.hasValue && imu.hasValue && video.hasValue; +} diff --git a/autonomy/lib/src/interfaces/detector.dart b/autonomy/lib/src/interfaces/detector.dart new file mode 100644 index 00000000..c42a6943 --- /dev/null +++ b/autonomy/lib/src/interfaces/detector.dart @@ -0,0 +1,10 @@ +import "package:autonomy/interfaces.dart"; + +abstract class DetectorInterface extends Service { + AutonomyInterface collection; + DetectorInterface({required this.collection}); + + bool findObstacles(); + bool canSeeAruco(); + bool isOnSlope(); +} diff --git a/autonomy/lib/src/interfaces/drive.dart b/autonomy/lib/src/interfaces/drive.dart new file mode 100644 index 00000000..08b677ed --- /dev/null +++ b/autonomy/lib/src/interfaces/drive.dart @@ -0,0 +1,79 @@ +import "package:autonomy/interfaces.dart"; +import "package:burt_network/protobuf.dart"; + +enum DriveDirection { + forward, + left, + right, + stop, +} + +enum DriveOrientation { + north(0), + west(90), + south(180), + east(270); + + final int angle; + const DriveOrientation(this.angle); + + static DriveOrientation? fromRaw(Orientation orientation) { + // TODO: Make this more precise. + for (final value in values) { + if (orientation.isNear(value.angle.toDouble())) return value; + } + return null; + } + + DriveOrientation turnLeft() => switch (this) { + north => west, + west => south, + south => east, + east => north, + }; + + DriveOrientation turnRight() => switch (this) { + north => east, + west => north, + south => west, + east => south, + }; +} + +abstract class DriveInterface extends Service { + AutonomyInterface collection; + DriveOrientation orientation = DriveOrientation.north; + DriveInterface({required this.collection}); + + Future goDirection(DriveDirection direction) async => switch (direction) { + DriveDirection.forward => await goForward(), + DriveDirection.left => await turnLeft(), + DriveDirection.right => await turnRight(), + DriveDirection.stop => await stop(), + }; + + Future faceNorth(); + + Future goForward(); + Future turnLeft(); + Future turnRight(); + Future stop(); + + Future faceDirection(DriveOrientation orientation) async { + this.orientation = orientation; + } + + Future followPath(List path) async { + for (final state in path) { + await goDirection(state.direction); + } + } + + void setLedStrip(ProtoColor color, {bool blink = false}) { + final command = DriveCommand(color: color, blink: blink ? BoolState.YES : BoolState.NO); + collection.server.sendCommand(command); + } + + Future spinForAruco() async => false; + Future approachAruco() async { } +} diff --git a/autonomy/lib/src/interfaces/error.dart b/autonomy/lib/src/interfaces/error.dart new file mode 100644 index 00000000..9d0c9ed3 --- /dev/null +++ b/autonomy/lib/src/interfaces/error.dart @@ -0,0 +1,14 @@ +import "dart:math"; + +class RandomError { + final double maxError; + final _random = Random(); + + RandomError(this.maxError); + + double get value { + final sign = _random.nextBool() ? 1 : -1; + final value = _random.nextDouble() * maxError; + return sign * value; + } +} diff --git a/autonomy/lib/src/interfaces/gps.dart b/autonomy/lib/src/interfaces/gps.dart new file mode 100644 index 00000000..8ccef300 --- /dev/null +++ b/autonomy/lib/src/interfaces/gps.dart @@ -0,0 +1,27 @@ +import "package:burt_network/protobuf.dart"; +import "package:autonomy/interfaces.dart"; + +abstract class GpsInterface extends Service with Receiver { + static const gpsError = 0.00003; + static double currentLatitude = 0; + + final AutonomyInterface collection; + GpsInterface({required this.collection}); + + double get longitude => coordinates.longitude; + double get latitude => coordinates.latitude; + + void update(GpsCoordinates newValue); + GpsCoordinates get coordinates; + + bool isNear(GpsCoordinates other) => coordinates.isNear(other); + + @override + Future waitForValue() async { + await super.waitForValue(); + currentLatitude = coordinates.latitude; + } + + @override + Future init() async => true; +} diff --git a/autonomy/lib/src/interfaces/gps_utils.dart b/autonomy/lib/src/interfaces/gps_utils.dart new file mode 100644 index 00000000..dc4fc74c --- /dev/null +++ b/autonomy/lib/src/interfaces/gps_utils.dart @@ -0,0 +1,62 @@ + +import "dart:math"; + +import "package:autonomy/interfaces.dart"; +import "package:burt_network/protobuf.dart"; + +extension RecordToGps on (num, num) { + GpsCoordinates toGps() => GpsCoordinates(latitude: $1.toDouble() * GpsUtils.latitudePerMeter, longitude: $2.toDouble() * GpsUtils.longitudePerMeter); +} + +extension GpsUtils on GpsCoordinates { + static double maxErrorMeters = 1; + static double get epsilonLatitude => maxErrorMeters * latitudePerMeter; + static double get epsilonLongitude => maxErrorMeters * longitudePerMeter; + + static GpsCoordinates get east => + GpsCoordinates(longitude: -1 / metersPerLongitude); + static GpsCoordinates get west => + GpsCoordinates(longitude: 1 / metersPerLongitude); + static GpsCoordinates get north => + GpsCoordinates(latitude: 1 * latitudePerMeter); + static GpsCoordinates get south => + GpsCoordinates(latitude: -1 * latitudePerMeter); + + // Taken from https://stackoverflow.com/a/39540339/9392211 + // static const metersPerLatitude = 111.32 * 1000; // 111.32 km + static const metersPerLatitude = 1; + static const radiansPerDegree = pi / 180; + static double get metersPerLongitude => 1; +// 40075 * cos( GpsInterface.currentLatitude * radiansPerDegree ) / 360 * 1000; + + static double get latitudePerMeter => 1 / metersPerLatitude; + static double get longitudePerMeter => 1 / metersPerLongitude; + + double distanceTo(GpsCoordinates other) => sqrt( + pow(latitude - other.latitude, 2) + + pow(longitude - other.longitude, 2), + ); + + double manhattanDistance(GpsCoordinates other) => + (latitude - other.latitude).abs() * metersPerLatitude + + (longitude - other.longitude).abs() * metersPerLongitude; + + bool isNear(GpsCoordinates other) => + (latitude - other.latitude).abs() < epsilonLatitude && + (longitude - other.longitude).abs() < epsilonLongitude; + + GpsCoordinates operator +(GpsCoordinates other) => GpsCoordinates( + latitude: latitude + other.latitude, + longitude: longitude + other.longitude, + ); + +// String prettyPrint() => "(lat=${(latitude * GpsUtils.metersPerLatitude).toStringAsFixed(2)}, long=${(longitude * GpsUtils.metersPerLongitude).toStringAsFixed(2)})"; + String prettyPrint() => toProto3Json().toString(); + + GpsCoordinates goForward(DriveOrientation orientation) => this + switch(orientation) { + DriveOrientation.north => GpsUtils.north, + DriveOrientation.south => GpsUtils.south, + DriveOrientation.west => GpsUtils.west, + DriveOrientation.east => GpsUtils.east, + }; +} diff --git a/autonomy/lib/src/interfaces/imu.dart b/autonomy/lib/src/interfaces/imu.dart new file mode 100644 index 00000000..5bb6bba2 --- /dev/null +++ b/autonomy/lib/src/interfaces/imu.dart @@ -0,0 +1,19 @@ +import "package:autonomy/interfaces.dart"; +import "package:burt_network/burt_network.dart"; + +abstract class ImuInterface extends Service with Receiver { + final AutonomyInterface collection; + ImuInterface({required this.collection}); + + double get heading => raw.z; + Orientation get raw; + DriveOrientation? get orientation { + collection.logger.trace("Trying to find orientation at $heading"); + return DriveOrientation.fromRaw(raw); + } + void update(Orientation newValue); + bool isNear(double angle) => raw.isNear(angle); + + @override + Future init() async => true; +} diff --git a/autonomy/lib/src/interfaces/imu_utils.dart b/autonomy/lib/src/interfaces/imu_utils.dart new file mode 100644 index 00000000..588189d3 --- /dev/null +++ b/autonomy/lib/src/interfaces/imu_utils.dart @@ -0,0 +1,27 @@ +import "package:burt_network/protobuf.dart"; + +extension OrientationUtils on Orientation { + static const double epsilon = 10; + + static final north = Orientation(z: 0); + static final west = Orientation(z: 90); + static final south = Orientation(z: 180); + static final east = Orientation(z: 270); + + double get heading => z; + + bool get isEmpty => x == 0 && y == 0 && z == 0; + + Orientation clampHeading() { + var adjustedHeading = heading; + if (heading >= 360) adjustedHeading -= 360; + if (heading < 0) adjustedHeading = 360 + heading; + return Orientation(x: x, y: y, z: adjustedHeading); + } + + bool isNear(double value) => value > 270 && z < 90 + ? (z + 360 - value).abs() < epsilon + : value < 90 && z > 270 + ? (z - value - 360).abs() < epsilon + : (z - value).abs() < epsilon; +} diff --git a/autonomy/lib/src/interfaces/orchestrator.dart b/autonomy/lib/src/interfaces/orchestrator.dart new file mode 100644 index 00000000..13ee8ed6 --- /dev/null +++ b/autonomy/lib/src/interfaces/orchestrator.dart @@ -0,0 +1,61 @@ +import "dart:io"; + +import "package:autonomy/interfaces.dart"; +import "package:burt_network/burt_network.dart"; +import "package:meta/meta.dart"; + +abstract class OrchestratorInterface extends Service { + final AutonomyInterface collection; + OrchestratorInterface({required this.collection}); + + AutonomyCommand? currentCommand; + AutonomyState currentState = AutonomyState.AUTONOMY_STATE_UNDEFINED; + Future onCommand(AutonomyCommand command) async { + collection.server.sendMessage(command); + if (command.abort) return abort(); + if (currentCommand != null) { + collection.logger.error("Already executing a command", body: "Abort first if you want to switch tasks"); + return; + } + + if (!collection.hasValue) { + collection.logger.error("Sensors haven't gotten any readings yet!"); + currentState = AutonomyState.NO_SOLUTION; + return; + } + await collection.drive.faceNorth(); + currentCommand = command; + switch (command.task) { + case AutonomyTask.GPS_ONLY: await handleGpsTask(command); + case AutonomyTask.VISUAL_MARKER: await handleArucoTask(command); + // TODO: Add more tasks + default: collection.logger.error("Unrecognized task: ${command.task}"); // ignore: no_default_cases + } + } + + @override + Future init() async { + collection.server.messages.onMessage( + name: AutonomyCommand().messageName, + constructor: AutonomyCommand.fromBuffer, + callback: onCommand, + ); + return true; + } + + @mustCallSuper + Future abort() async { + currentCommand = null; + collection.logger.warning("Aborting task!"); + currentState = AutonomyState.ABORTING; + await collection.drive.stop(); + await collection.dispose(); + exit(1); + } + + Future handleGpsTask(AutonomyCommand command); + Future handleArucoTask(AutonomyCommand command); + Future handleHammerTask(AutonomyCommand command); + Future handleBottleTask(AutonomyCommand command); + AutonomyData get statusMessage; +} diff --git a/autonomy/lib/src/interfaces/pathfinding.dart b/autonomy/lib/src/interfaces/pathfinding.dart new file mode 100644 index 00000000..a3b4c73a --- /dev/null +++ b/autonomy/lib/src/interfaces/pathfinding.dart @@ -0,0 +1,18 @@ +import "package:autonomy/interfaces.dart"; +import "package:burt_network/protobuf.dart"; + +abstract class PathfindingInterface extends Service { + final AutonomyInterface collection; + PathfindingInterface({required this.collection}); + + List? getPath(GpsCoordinates destination, {bool verbose = false}); + + Set obstacles = {}; + void recordObstacle(GpsCoordinates coordinates) => obstacles.add(coordinates); + bool isObstacle(GpsCoordinates coordinates) => obstacles.any((obstacle) => obstacle.isNear(coordinates)); + + @override + Future dispose() async { + obstacles.clear(); + } +} diff --git a/autonomy/lib/src/interfaces/receiver.dart b/autonomy/lib/src/interfaces/receiver.dart new file mode 100644 index 00000000..59ec05bf --- /dev/null +++ b/autonomy/lib/src/interfaces/receiver.dart @@ -0,0 +1,9 @@ +mixin Receiver { + bool hasValue = false; + + Future waitForValue() async { + while (!hasValue) { + await Future.delayed(const Duration(seconds: 1)); + } + } +} diff --git a/autonomy/lib/src/interfaces/reporter.dart b/autonomy/lib/src/interfaces/reporter.dart new file mode 100644 index 00000000..4524b60d --- /dev/null +++ b/autonomy/lib/src/interfaces/reporter.dart @@ -0,0 +1,23 @@ +import "dart:async"; +import "package:burt_network/protobuf.dart"; + +import "package:autonomy/interfaces.dart"; + +mixin ValueReporter on Service { + AutonomyInterface get collection; + Message getMessage(); + + Timer? timer; + Duration get reportInterval => const Duration(milliseconds: 10); + + @override + Future init() async { + timer = Timer.periodic(reportInterval, (timer) => _reportValue()); + return super.init(); + } + + @override + Future dispose() async => timer?.cancel(); + + void _reportValue() => collection.server.sendMessage(getMessage()); +} diff --git a/autonomy/lib/src/interfaces/server.dart b/autonomy/lib/src/interfaces/server.dart new file mode 100644 index 00000000..cec6850e --- /dev/null +++ b/autonomy/lib/src/interfaces/server.dart @@ -0,0 +1,25 @@ +import "dart:io"; + +import "package:burt_network/burt_network.dart"; + +extension ServerUtils on RoverSocket { + static SocketInfo subsystemsDestination = SocketInfo( + address: InternetAddress("192.168.1.20"), + port: 8001, + ); + + void sendCommand(Message message) => sendMessage(message, destination: subsystemsDestination); + + Future waitForConnection() async { + logger.info("Waiting for connection..."); + while (!isConnected) { + await Future.delayed(const Duration(milliseconds: 100)); + } + return; + } + + void sendDone() { + final message = AutonomyData(state: AutonomyState.AT_DESTINATION, task: AutonomyTask.AUTONOMY_TASK_UNDEFINED); + sendMessage(message); + } +} diff --git a/autonomy/lib/src/interfaces/service.dart b/autonomy/lib/src/interfaces/service.dart new file mode 100644 index 00000000..19cbacaa --- /dev/null +++ b/autonomy/lib/src/interfaces/service.dart @@ -0,0 +1 @@ +export "package:burt_network/burt_network.dart" show Service; diff --git a/autonomy/lib/src/interfaces/video.dart b/autonomy/lib/src/interfaces/video.dart new file mode 100644 index 00000000..6f3fc32b --- /dev/null +++ b/autonomy/lib/src/interfaces/video.dart @@ -0,0 +1,17 @@ +import "package:autonomy/interfaces.dart"; +import "package:burt_network/protobuf.dart"; + +/// Handles obstacle detection data and ArUco data from video +abstract class VideoInterface extends Service with Receiver { + bool flag = false; + + final AutonomyInterface collection; + VideoInterface({required this.collection}); + + VideoData data = VideoData(); + + void updateFrame(VideoData newData); + + // double get arucoSize => data.arucoSize; + // double get arucoPosition => data.arucoPosition; +} diff --git a/autonomy/lib/src/rover/corrector.dart b/autonomy/lib/src/rover/corrector.dart new file mode 100644 index 00000000..46b59e1f --- /dev/null +++ b/autonomy/lib/src/rover/corrector.dart @@ -0,0 +1,40 @@ +import "dart:collection"; + +class ErrorCorrector { // non-nullable + final int maxSamples; + final double maxDeviation; + ErrorCorrector({required this.maxSamples, this.maxDeviation = double.infinity}); + + double calibratedValue = 0; + final Queue recentSamples = DoubleLinkedQueue(); + + void addValue(double value) { + if (recentSamples.isEmpty) { + recentSamples.add(value); + calibratedValue = value; + return; + } + final deviation = (calibratedValue - value).abs(); + if (deviation > maxDeviation) return; + if (recentSamples.length == maxSamples) recentSamples.removeLast(); + recentSamples.addFirst(value); + calibratedValue = recentSamples.weightedAverage(); + } + + void clear() { + calibratedValue = 0; + recentSamples.clear(); + } +} + +extension on Iterable { + double weightedAverage() { + num sum = 0; + var count = 0; + for (final element in this) { + sum += element; + count++; + } + return sum / count; + } +} diff --git a/autonomy/lib/src/rover/detector.dart b/autonomy/lib/src/rover/detector.dart new file mode 100644 index 00000000..f28c508b --- /dev/null +++ b/autonomy/lib/src/rover/detector.dart @@ -0,0 +1,21 @@ +import "package:autonomy/interfaces.dart"; + +class RoverDetector extends DetectorInterface { + RoverDetector({required super.collection}); + + @override + bool isOnSlope() => false; + + @override + bool findObstacles() => false; + + @override +// bool canSeeAruco() => collection.video.data.arucoDetected == BoolState.YES; + bool canSeeAruco() => collection.video.flag; + + @override + Future init() async => true; + + @override + Future dispose() async { } +} diff --git a/autonomy/lib/src/rover/drive.dart b/autonomy/lib/src/rover/drive.dart new file mode 100644 index 00000000..7cef09e4 --- /dev/null +++ b/autonomy/lib/src/rover/drive.dart @@ -0,0 +1,67 @@ +import "package:burt_network/burt_network.dart"; +import "package:autonomy/interfaces.dart"; + +import "drive/timed.dart"; +import "drive/sensor.dart"; + +/// A helper class to send drive commands to the rover with a simpler API. +class RoverDrive extends DriveInterface { + final bool useGps; + final bool useImu; + + final SensorDrive sensorDrive; + final TimedDrive timedDrive; + + // TODO: Calibrate these + RoverDrive({required super.collection, this.useGps = true, this.useImu = true}) : + sensorDrive = SensorDrive(collection: collection), + timedDrive = TimedDrive(collection: collection); + + /// Initializes the rover's drive subsystems. + @override + Future init() async => true; + + /// Stops the rover from driving. + @override + Future dispose() => stop(); + + /// Sets the angle of the front camera. + void setCameraAngle({required double swivel, required double tilt}) { + collection.logger.trace("Setting camera angles to $swivel (swivel) and $tilt (tilt)"); + final command = DriveCommand(frontSwivel: swivel, frontTilt: tilt); + collection.server.sendCommand(command); + } + + @override + Future stop() async { + await timedDrive.stop(); + } + + @override + Future faceNorth() => useImu ? sensorDrive.faceNorth() : timedDrive.faceNorth(); + + @override + Future spinForAruco() => sensorDrive.spinForAruco(); + @override + Future approachAruco() => sensorDrive.approachAruco(); + + + @override + Future faceDirection(DriveOrientation orientation) async { + if (useImu) { + await sensorDrive.faceDirection(orientation); + } else { + await timedDrive.faceDirection(orientation); + } + await super.faceDirection(orientation); + } + + @override + Future goForward() => useGps ? sensorDrive.goForward() : timedDrive.goForward(); + + @override + Future turnLeft() => useImu ? sensorDrive.turnLeft() : timedDrive.turnLeft(); + + @override + Future turnRight() => useImu ? sensorDrive.turnRight() : timedDrive.turnRight(); +} diff --git a/autonomy/lib/src/rover/drive/motors.dart b/autonomy/lib/src/rover/drive/motors.dart new file mode 100644 index 00000000..12ae541b --- /dev/null +++ b/autonomy/lib/src/rover/drive/motors.dart @@ -0,0 +1,26 @@ +import "package:autonomy/interfaces.dart"; +import "package:burt_network/protobuf.dart"; + +mixin RoverMotors { + AutonomyInterface get collection; + + /// Sets the max speed of the rover. + /// + /// [setSpeeds] takes the speeds of each side of wheels. These numbers are percentages of the + /// max speed allowed by the rover, which we call the throttle. This function adjusts the + /// throttle, as a percentage of the rover's top speed. + void setThrottle(double throttle) { + collection.logger.trace("Setting throttle to $throttle"); + collection.server.sendCommand(DriveCommand(throttle: throttle, setThrottle: true)); + } + + /// Sets the speeds of the left and right wheels, using differential steering. + /// + /// These values are percentages of the max speed allowed by the rover. See [setThrottle]. + void setSpeeds({required double left, required double right}) { + right *= -1; + collection.logger.trace("Setting speeds to $left and $right"); + collection.server.sendCommand(DriveCommand(left: left, setLeft: true)); + collection.server.sendCommand(DriveCommand(right: right, setRight: true)); + } +} diff --git a/autonomy/lib/src/rover/drive/sensor.dart b/autonomy/lib/src/rover/drive/sensor.dart new file mode 100644 index 00000000..207e4fc5 --- /dev/null +++ b/autonomy/lib/src/rover/drive/sensor.dart @@ -0,0 +1,148 @@ +import "package:autonomy/autonomy.dart"; +import "package:autonomy/interfaces.dart"; + +import "motors.dart"; + +class SensorDrive extends DriveInterface with RoverMotors { + static const double maxThrottle = 0.1; + static const double turnThrottle = 0.1; + static const predicateDelay = Duration(milliseconds: 100); + static const turnDelay = Duration(milliseconds: 1500); + + SensorDrive({required super.collection}); + + @override + Future stop() async { + setThrottle(0); + setSpeeds(left: 0, right: 0); + } + + Future waitFor(bool Function() predicate) async { + while (!predicate()) { + await Future.delayed(predicateDelay); + await collection.imu.waitForValue(); + } + } + + @override + Future init() async => true; + + @override + Future dispose() async { } + + @override + Future goForward() async { + final orientation = collection.imu.orientation; + final currentCoordinates = collection.gps.coordinates; + final destination = currentCoordinates.goForward(orientation!); + + setThrottle(maxThrottle); + setSpeeds(left: 1, right: 1); + await waitFor(() => collection.gps.coordinates.isNear(destination)); + await stop(); + } + + @override + Future faceNorth() async { + collection.logger.info("Turning to face north..."); + setThrottle(turnThrottle); + setSpeeds(left: -1, right: 1); + await waitFor(() { + collection.logger.trace("Current heading: ${collection.imu.heading}"); + return collection.imu.raw.isNear(0); + }); + await stop(); + } + + @override + Future faceDirection(DriveOrientation orientation) async { + collection.logger.info("Turning to face $orientation..."); + setThrottle(turnThrottle); + setSpeeds(left: -1, right: 1); + await waitFor(() { + collection.logger.trace("Current heading: ${collection.imu.heading}"); + return collection.imu.raw.isNear(orientation.angle.toDouble()); + }); + await stop(); + await super.faceDirection(orientation); + } + + @override + Future turnLeft() async { + await collection.imu.waitForValue(); + + if (collection.imu.orientation == null) { + await faceNorth(); + await faceDirection(this.orientation); + } + await collection.imu.waitForValue(); + + final orientation = collection.imu.orientation; + final destination = orientation!.turnLeft(); // do NOT clamp! + setThrottle(maxThrottle); + setSpeeds(left: -1, right: 1); + await waitFor(() => collection.imu.orientation == destination); + await stop(); + this.orientation = this.orientation.turnLeft(); + } + + @override + Future turnRight() async { + await collection.imu.waitForValue(); + if (collection.imu.orientation == null) { + await faceNorth(); + await faceDirection(this.orientation); + } + await collection.imu.waitForValue(); + final orientation = collection.imu.orientation; + final destination = orientation!.turnRight(); // do NOT clamp! + setThrottle(maxThrottle); + setSpeeds(left: 1, right: -1); + await waitFor(() => collection.imu.orientation == destination); + await stop(); + this.orientation = this.orientation.turnRight(); + } + + @override + Future spinForAruco() async { + for (var i = 0; i < 16; i++) { + setThrottle(turnThrottle); + setSpeeds(left: -1, right: 1); + await Future.delayed(turnDelay); + await stop(); + + for (var j = 0; j < 300; j++) { + await Future.delayed(const Duration(milliseconds: 10)); + collection.logger.trace("Can see aruco? ${collection.detector.canSeeAruco()}"); + + if (collection.detector.canSeeAruco()) { + // Spin a bit more to center it + // print("We found it!"); + // setThrottle(0.1); + // setSpeeds(left: -1, right: 1); + // await waitFor(() { + // final pos = collection.video.arucoPosition; + // collection.logger.debug("aruco is at $pos"); + // return pos > 0.2; + // }); + // await stop(); + return true; + }} + } + return false; + } + + @override + Future approachAruco() async { + setThrottle(maxThrottle); + setSpeeds(left: 1, right: 1); + // const threshold = 0.2; + // await waitFor(() { + // final pos = collection.video.arucoSize; + // collection.logger.debug("It is at $pos percent"); + // return (pos.abs() < 0.00001 && !collection.detector.canSeeAruco()) || pos >= threshold; + // }); + await Future.delayed(const Duration(seconds: 10)); + await stop(); + } +} diff --git a/autonomy/lib/src/rover/drive/timed.dart b/autonomy/lib/src/rover/drive/timed.dart new file mode 100644 index 00000000..142a1ff4 --- /dev/null +++ b/autonomy/lib/src/rover/drive/timed.dart @@ -0,0 +1,51 @@ +import "package:autonomy/interfaces.dart"; + +import "motors.dart"; + +class TimedDrive extends DriveInterface with RoverMotors { + static const maxThrottle = 0.1; + static const turnThrottle = 0.1; + static const oneMeterDelay = Duration(milliseconds: 5500); + static const turnDelay = Duration(milliseconds: 4500); + + TimedDrive({required super.collection}); + + @override + Future init() async => true; + + @override + Future dispose() async { } + + @override + Future stop() async { + setThrottle(0); + setSpeeds(left: 0, right: 0); + } + + @override + Future faceNorth() async { /* Assume already facing north */ } + + @override + Future goForward() async { + setThrottle(maxThrottle); + setSpeeds(left: 1, right: 1); + await Future.delayed(oneMeterDelay); + await stop(); + } + + @override + Future turnLeft() async { + setThrottle(turnThrottle); + setSpeeds(left: -1, right: 1); + await Future.delayed(turnDelay); + await stop(); + } + + @override + Future turnRight() async { + setThrottle(turnThrottle); + setSpeeds(left: 1, right: -1); + await Future.delayed(turnDelay); + await stop(); + } +} diff --git a/autonomy/lib/src/rover/gps.dart b/autonomy/lib/src/rover/gps.dart new file mode 100644 index 00000000..2704b853 --- /dev/null +++ b/autonomy/lib/src/rover/gps.dart @@ -0,0 +1,41 @@ +import "package:burt_network/burt_network.dart"; +import "package:autonomy/interfaces.dart"; + +import "corrector.dart"; + +class RoverGps extends GpsInterface { + final _latitudeCorrector = ErrorCorrector(maxSamples: 1, maxDeviation: GpsInterface.gpsError * 10); + final _longitudeCorrector = ErrorCorrector(maxSamples: 1, maxDeviation: GpsInterface.gpsError * 10); + RoverGps({required super.collection}); + + @override + Future init() async { + collection.server.messages.onMessage( + name: RoverPosition().messageName, + constructor: RoverPosition.fromBuffer, + callback: (pos) { + if (pos.hasGps()) update(pos.gps); + }, + ); + return super.init(); + } + + @override + Future dispose() async { + _latitudeCorrector.clear(); + _longitudeCorrector.clear(); + } + + @override + void update(GpsCoordinates newValue) { + _latitudeCorrector.addValue(newValue.latitude); + _longitudeCorrector.addValue(newValue.longitude); + hasValue = true; + } + + @override + GpsCoordinates get coordinates => GpsCoordinates( + latitude: _latitudeCorrector.calibratedValue, + longitude: _longitudeCorrector.calibratedValue, + ); +} diff --git a/autonomy/lib/src/rover/imu.dart b/autonomy/lib/src/rover/imu.dart new file mode 100644 index 00000000..48f66666 --- /dev/null +++ b/autonomy/lib/src/rover/imu.dart @@ -0,0 +1,43 @@ +import "package:burt_network/burt_network.dart"; +import "package:autonomy/interfaces.dart"; + +import "corrector.dart"; + +class RoverImu extends ImuInterface { + final _zCorrector = ErrorCorrector(maxSamples: 10, maxDeviation: 15); + RoverImu({required super.collection}); + + Orientation value = Orientation(); + + @override + Future init() async { + collection.server.messages.onMessage( + name: RoverPosition().messageName, + constructor: RoverPosition.fromBuffer, + callback: (pos) { + if (pos.hasOrientation()) update(pos.orientation); + }, + ); + return super.init(); + } + + @override + Future dispose() async { + _zCorrector.clear(); + } + + @override + void update(Orientation newValue) { + // _zCorrector.addValue(newValue.heading); + // collection.logger.trace("Got IMU value"); + hasValue = true; + value = newValue; + } + + @override + Orientation get raw => Orientation( + x: 0, + y: 0, + z: value.z, + ).clampHeading(); +} diff --git a/autonomy/lib/src/rover/orchestrator.dart b/autonomy/lib/src/rover/orchestrator.dart new file mode 100644 index 00000000..2fd9a2f8 --- /dev/null +++ b/autonomy/lib/src/rover/orchestrator.dart @@ -0,0 +1,113 @@ +import "package:autonomy/interfaces.dart"; +import "package:burt_network/protobuf.dart"; +import "dart:async"; + +class RoverOrchestrator extends OrchestratorInterface with ValueReporter { + final List traversed = []; + List? currentPath; + RoverOrchestrator({required super.collection}); + + @override + Future dispose() async { + currentPath = null; + currentCommand = null; + currentState = AutonomyState.AUTONOMY_STATE_UNDEFINED; + traversed.clear(); + await super.dispose(); + } + + @override + AutonomyData get statusMessage => AutonomyData( + destination: currentCommand?.destination, + state: currentState, + obstacles: collection.pathfinder.obstacles, + path: [ + for (final transition in currentPath ?? []) + transition.position, + ...traversed, + ], + task: currentCommand?.task, + crash: false, // TODO: Investigate if this is used and how to use it better + ); + + @override + Message getMessage() => statusMessage; + + @override + Future handleGpsTask(AutonomyCommand command) async { + final destination = command.destination; + collection.logger.info("Got GPS Task: Go to ${destination.prettyPrint()}"); + collection.logger.debug("Currently at ${collection.gps.coordinates.prettyPrint()}"); + collection.drive.setLedStrip(ProtoColor.RED); + while (!collection.gps.coordinates.isNear(destination)) { + // Calculate a path + collection.logger.debug("Finding a path"); + currentState = AutonomyState.PATHING; + await collection.drive.faceNorth(); + final path = collection.pathfinder.getPath(destination); + currentPath = path; // also use local variable path for promotion + if (path == null) { + final current = collection.gps.coordinates; + collection.logger.error("Could not find a path", body: "No path found from ${current.prettyPrint()} to ${destination.prettyPrint()}"); + currentState = AutonomyState.NO_SOLUTION; + currentCommand = null; + return; + } + // Try to take that path + final current = collection.gps.coordinates; + collection.logger.info("Found a path from ${current.prettyPrint()} to ${destination.prettyPrint()}: ${path.length} steps"); + collection.logger.debug("Here is a summary of the path"); + for (final step in path) { + collection.logger.debug(step.toString()); + } + currentState = AutonomyState.DRIVING; + var count = 0; + for (final state in path) { + collection.logger.debug(state.toString()); + await collection.drive.goDirection(state.direction); + traversed.add(state.position); + if (state.direction != DriveDirection.forward) continue; + if (count++ == 5) break; + final foundObstacle = collection.detector.findObstacles(); + if (foundObstacle) { + collection.logger.debug("Found an obstacle. Recalculating path..."); + break; // calculate a new path + } + } + } + collection.logger.info("Task complete"); + collection.drive.setLedStrip(ProtoColor.GREEN, blink: true); + currentState = AutonomyState.AT_DESTINATION; + currentCommand = null; + } + + @override + Future handleArucoTask(AutonomyCommand command) async { + collection.drive.setLedStrip(ProtoColor.RED); + + // Go to GPS coordinates + // await handleGpsTask(command); + collection.logger.info("Got ArUco Task"); + + currentState = AutonomyState.SEARCHING; + collection.logger.info("Searching for ArUco tag"); + final didSeeAruco = await collection.drive.spinForAruco(); + if (didSeeAruco) { + collection.logger.info("Found aruco"); + currentState = AutonomyState.APPROACHING; + await collection.drive.approachAruco(); + collection.drive.setLedStrip(ProtoColor.GREEN, blink: true); + currentState = AutonomyState.AT_DESTINATION; + } + } + + @override + Future handleHammerTask(AutonomyCommand command) async { + + } + + @override + Future handleBottleTask(AutonomyCommand command) async { + + } +} diff --git a/autonomy/lib/src/rover/pathfinding.dart b/autonomy/lib/src/rover/pathfinding.dart new file mode 100644 index 00000000..2eb06735 --- /dev/null +++ b/autonomy/lib/src/rover/pathfinding.dart @@ -0,0 +1,22 @@ +import "package:a_star/a_star.dart"; + +import "package:autonomy/interfaces.dart"; +import "package:burt_network/burt_network.dart"; +import "package:burt_network/protobuf.dart"; + +class RoverPathfinder extends PathfindingInterface { + RoverPathfinder({required super.collection}); + + @override + Future init() async => true; + + @override + List? getPath(GpsCoordinates destination, {bool verbose = false}) { + if (isObstacle(destination)) return null; + final state = AutonomyAStarState.start(collection: collection, goal: destination); + final result = aStar(state, verbose: verbose, limit: 50000); + if (result == null) return null; + final transitions = result.reconstructPath().toList(); + return transitions; + } +} diff --git a/autonomy/lib/src/rover/sensorless.dart b/autonomy/lib/src/rover/sensorless.dart new file mode 100644 index 00000000..2beab095 --- /dev/null +++ b/autonomy/lib/src/rover/sensorless.dart @@ -0,0 +1,64 @@ +import "package:autonomy/interfaces.dart"; +import "package:autonomy/rover.dart"; +import "package:autonomy/simulator.dart"; + +class SensorlessDrive extends DriveInterface { + final DriveInterface simulatedDrive; + final DriveInterface realDrive; + + SensorlessDrive({required super.collection, bool useGps = true, bool useImu = true}) : + simulatedDrive = DriveSimulator(collection: collection), + realDrive = RoverDrive(collection: collection, useGps: useGps, useImu: useImu); + + @override + Future init() async { + var result = true; + result &= await simulatedDrive.init(); + result &= await realDrive.init(); + return result; + } + + @override + Future dispose() async { + await simulatedDrive.dispose(); + await realDrive.dispose(); + } + + @override + Future goForward() async { + await simulatedDrive.goForward(); + await realDrive.goForward(); + } + + @override + Future stop() async { + await simulatedDrive.stop(); + await realDrive.stop(); + } + + @override + Future faceNorth() async { + await simulatedDrive.faceNorth(); + await realDrive.faceNorth(); + } + + + @override + Future faceDirection(DriveOrientation orientation) async { + await simulatedDrive.faceDirection(orientation); + await realDrive.faceDirection(orientation); + await super.faceDirection(orientation); + } + + @override + Future turnLeft() async { + await simulatedDrive.turnLeft(); + await realDrive.turnLeft(); + } + + @override + Future turnRight() async { + await simulatedDrive.turnRight(); + await realDrive.turnRight(); + } +} diff --git a/autonomy/lib/src/rover/video.dart b/autonomy/lib/src/rover/video.dart new file mode 100644 index 00000000..0f829acb --- /dev/null +++ b/autonomy/lib/src/rover/video.dart @@ -0,0 +1,32 @@ +import "dart:async"; + +import "package:autonomy/interfaces.dart"; +import "package:burt_network/burt_network.dart"; + +class RoverVideo extends VideoInterface { + RoverVideo({required super.collection}); + + @override + Future init() async { + collection.server.messages.onMessage( + name: VideoData().messageName, + constructor: VideoData.fromBuffer, + callback: updateFrame, + ); + return true; + } + + @override + Future dispose() async { } + + @override + void updateFrame(VideoData newData) { + data = newData; + // if (data.arucoDetected == BoolState.YES) { + // flag = true; + // Timer(const Duration(seconds: 3), () => flag = false); + // collection.logger.info("Is ArUco detected: ${data.arucoDetected}"); + // } + hasValue = true; + } +} diff --git a/autonomy/lib/src/simulator/detector.dart b/autonomy/lib/src/simulator/detector.dart new file mode 100644 index 00000000..82349221 --- /dev/null +++ b/autonomy/lib/src/simulator/detector.dart @@ -0,0 +1,46 @@ +import "package:autonomy/interfaces.dart"; +import "package:burt_network/protobuf.dart"; + +class SimulatedObstacle { + final GpsCoordinates coordinates; + final int radius; + SimulatedObstacle({required this.coordinates, required this.radius}); + + bool isNear(GpsCoordinates other) => + coordinates.distanceTo(other) <= radius; +} + +class DetectorSimulator extends DetectorInterface { + static const arucoPosition = (10, 10); + static const slopedLatitude = -5; + + final List obstacles; + final Set found = {}; + + DetectorSimulator({required super.collection, required this.obstacles}); + + @override + Future init() async => true; + + @override + Future dispose() async => obstacles.clear(); + + @override + bool findObstacles() { + final coordinates = collection.gps.coordinates; + var result = false; + for (final obstacle in obstacles) { + if (!obstacle.isNear(coordinates) || found.contains(obstacle.coordinates)) continue; + result = true; + found.add(obstacle.coordinates); + collection.pathfinder.recordObstacle(obstacle.coordinates); + } + return result; + } + + @override + bool canSeeAruco() => false; // if can see [arucoPosition] + + @override + bool isOnSlope() => false; // if on [slopedLatitude] +} diff --git a/autonomy/lib/src/simulator/drive.dart b/autonomy/lib/src/simulator/drive.dart new file mode 100644 index 00000000..16eecaae --- /dev/null +++ b/autonomy/lib/src/simulator/drive.dart @@ -0,0 +1,54 @@ +// ignore_for_file: cascade_invocations + +import "package:autonomy/interfaces.dart"; +import "package:burt_network/protobuf.dart"; + +class DriveSimulator extends DriveInterface { + final bool shouldDelay; + DriveSimulator({required super.collection, this.shouldDelay = false}); + + @override + Future init() async => true; + + @override + Future dispose() async { } + + @override + Future goForward() async { + final position = collection.gps.coordinates; + final orientation = collection.imu.orientation; + final newPosition = position.goForward(orientation!); + collection.logger.debug("Going forward"); + collection.logger.trace(" Old position: ${position.prettyPrint()}"); + collection.logger.trace(" Orientation: $orientation"); + collection.logger.trace(" New position: ${newPosition.prettyPrint()}"); + collection.gps.update(newPosition); + if (shouldDelay) await Future.delayed(const Duration(milliseconds: 500)); + } + + @override + Future faceNorth() async { + collection.imu.update(OrientationUtils.north); + } + + @override + Future turnLeft() async { + collection.logger.debug("Turning left"); + final heading = collection.imu.heading; + final orientation = Orientation(z: heading + 90); + collection.imu.update(orientation); + if (shouldDelay) await Future.delayed(const Duration(milliseconds: 500)); + } + + @override + Future turnRight() async { + collection.logger.debug("Turning right"); + final heading = collection.imu.heading; + final orientation = Orientation(z: heading - 90); + collection.imu.update(orientation); + if (shouldDelay) await Future.delayed(const Duration(milliseconds: 500)); + } + + @override + Future stop() async => collection.logger.debug("Stopping"); +} diff --git a/autonomy/lib/src/simulator/gps.dart b/autonomy/lib/src/simulator/gps.dart new file mode 100644 index 00000000..0062db25 --- /dev/null +++ b/autonomy/lib/src/simulator/gps.dart @@ -0,0 +1,28 @@ +import "package:burt_network/protobuf.dart"; +import "package:autonomy/interfaces.dart"; + +class GpsSimulator extends GpsInterface with ValueReporter { + final RandomError _error; + GpsSimulator({required super.collection, double maxError = 0}) : + _error = RandomError(maxError); + + @override + RoverPosition getMessage() => RoverPosition(gps: coordinates); + + GpsCoordinates _coordinates = GpsCoordinates(); + + @override + GpsCoordinates get coordinates => GpsCoordinates( + latitude: _coordinates.latitude + _error.value, + longitude: _coordinates.longitude + _error.value, + ); + + @override + void update(GpsCoordinates newValue) => _coordinates = newValue; + + @override + Future init() async { + hasValue = true; + return super.init(); + } +} diff --git a/autonomy/lib/src/simulator/imu.dart b/autonomy/lib/src/simulator/imu.dart new file mode 100644 index 00000000..3c4cbacc --- /dev/null +++ b/autonomy/lib/src/simulator/imu.dart @@ -0,0 +1,35 @@ +import "package:burt_network/protobuf.dart"; +import "package:autonomy/interfaces.dart"; + +class ImuSimulator extends ImuInterface with ValueReporter { + final RandomError _error; + ImuSimulator({required super.collection, double maxError = 0}) : + _error = RandomError(maxError); + + @override + RoverPosition getMessage() => RoverPosition(orientation: raw); + + Orientation _orientation = Orientation(); + + @override + Orientation get raw => Orientation( + x: _orientation.x + _error.value, + y: _orientation.y + _error.value, + z: _orientation.z + _error.value, + ); + + @override + void update(Orientation newValue) => _orientation = newValue.clampHeading(); + + @override + Future init() async { + hasValue = true; + return super.init(); + } + + @override + Future dispose() async { + _orientation = Orientation(); + await super.dispose(); + } +} diff --git a/autonomy/lib/src/simulator/orchestrator.dart b/autonomy/lib/src/simulator/orchestrator.dart new file mode 100644 index 00000000..56420d6b --- /dev/null +++ b/autonomy/lib/src/simulator/orchestrator.dart @@ -0,0 +1,32 @@ +import "package:autonomy/interfaces.dart"; +import "package:burt_network/protobuf.dart"; + +class OrchestratorSimulator extends OrchestratorInterface { + OrchestratorSimulator({required super.collection}); + + @override + Future dispose() async { } + + @override + AutonomyData get statusMessage => AutonomyData(); + + @override + Future handleGpsTask(AutonomyCommand command) async { + + } + + @override + Future handleArucoTask(AutonomyCommand command) async { + + } + + @override + Future handleHammerTask(AutonomyCommand command) async { + + } + + @override + Future handleBottleTask(AutonomyCommand command) async { + + } +} diff --git a/autonomy/lib/src/simulator/pathfinding.dart b/autonomy/lib/src/simulator/pathfinding.dart new file mode 100644 index 00000000..155f20e0 --- /dev/null +++ b/autonomy/lib/src/simulator/pathfinding.dart @@ -0,0 +1,25 @@ +import "package:autonomy/interfaces.dart"; +import "package:burt_network/protobuf.dart"; + +class PathfindingSimulator extends PathfindingInterface { + static int i = 0; + + PathfindingSimulator({required super.collection}); + + @override + Future init() async => true; + + @override + List getPath(GpsCoordinates destination, {bool verbose = false}) => [ + AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (0, 0).toGps(), orientation: DriveOrientation.north, direction: DriveDirection.right, collection: collection), + AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (0, 0).toGps(), orientation: DriveOrientation.east, direction: DriveDirection.forward, collection: collection), + AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (0, 1).toGps(), orientation: DriveOrientation.east, direction: DriveDirection.forward, collection: collection), + AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (0, 2).toGps(), orientation: DriveOrientation.east, direction: DriveDirection.left, collection: collection), + AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (0, 2).toGps(), orientation: DriveOrientation.north, direction: DriveDirection.forward, collection: collection), + AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (1, 2).toGps(), orientation: DriveOrientation.north, direction: DriveDirection.forward, collection: collection), + AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (2, 2).toGps(), orientation: DriveOrientation.north, direction: DriveDirection.left, collection: collection), + AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (2, 2).toGps(), orientation: DriveOrientation.west, direction: DriveDirection.forward, collection: collection), + AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (2, 1).toGps(), orientation: DriveOrientation.west, direction: DriveDirection.right, collection: collection), + AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (2, 1).toGps(), orientation: DriveOrientation.north, direction: DriveDirection.forward, collection: collection), + ]; +} diff --git a/autonomy/lib/src/simulator/realsense.dart b/autonomy/lib/src/simulator/realsense.dart new file mode 100644 index 00000000..f4251028 --- /dev/null +++ b/autonomy/lib/src/simulator/realsense.dart @@ -0,0 +1,22 @@ +import "dart:typed_data"; + +import "package:autonomy/interfaces.dart"; +import "package:burt_network/protobuf.dart"; + +class VideoSimulator extends VideoInterface { + VideoSimulator({required super.collection}); + + @override + Future init() async { + hasValue = true; + return true; + } + + @override + Future dispose() async => depthFrame = Uint16List.fromList([]); + + Uint16List depthFrame = Uint16List.fromList([]); + + @override + void updateFrame(VideoData newData) { } +} diff --git a/autonomy/test/sensor_test.dart b/autonomy/test/sensor_test.dart new file mode 100644 index 00000000..6375b9eb --- /dev/null +++ b/autonomy/test/sensor_test.dart @@ -0,0 +1,246 @@ +import "package:autonomy/src/rover/imu.dart"; +import "package:burt_network/logging.dart"; +import "package:test/test.dart"; + +import "package:burt_network/protobuf.dart"; + +import "package:autonomy/interfaces.dart"; +import "package:autonomy/simulator.dart"; +import "package:autonomy/src/rover/gps.dart"; + +const imuError = 2.5; +const gpsPrecision = 7; + +void main() => group("[Sensors]", tags: ["sensors"], () { + setUp(() => Logger.level = LogLevel.off); + tearDown(() => Logger.level = LogLevel.off); + + test("GpsUtils.isNear", () { + final origin = (0, 0).toGps(); + expect(GpsCoordinates(latitude: 0, longitude: 0), origin); + expect(origin.isNear(origin), isTrue); + + final a = (5, 5).toGps(); + final a2 = (5, 5).toGps(); + final b = (5, 6.5).toGps(); + + expect(a.isNear(a), isTrue); + expect(a.isNear(a2), isTrue); + expect(a.isNear(b), isFalse); + + final c = GpsCoordinates(latitude: 0, longitude: 0); + final d = GpsCoordinates(latitude: 45.123, longitude: 72.123); + final e = GpsCoordinates(latitude: 12.123, longitude: 24.123); + + expect(c.isNear(d), isFalse); + expect(c.isNear(e), isFalse); + expect(d.isNear(e), isFalse); + + final f = (12, 12).toGps(); + final g = (12.2, 12.2).toGps(); + expect(f.isNear(g), isTrue); + }); + + test("GPS noise when stationary", () async { + // Set up a simulated and real GPS, both starting at (0, 0) + final oldError = GpsUtils.maxErrorMeters; + GpsUtils.maxErrorMeters = 3; + Logger.level = LogLevel.off; + + final simulator = AutonomySimulator(); + final realGps = RoverGps(collection: simulator); + final simulatedGps = GpsSimulator(collection: simulator, maxError: GpsInterface.gpsError); + final origin = GpsCoordinates(); + simulatedGps.update(origin); + realGps.update(origin); + expect(realGps.isNear(origin), isTrue); + + // Feed many noisy signals to produce a cleaner signal. + var count = 0; + for (var step = 0; step < 1000; step++) { + realGps.update(simulatedGps.coordinates); + simulator.logger.trace("New coordinate: ${realGps.coordinates.latitude.toStringAsFixed(gpsPrecision)} vs real position: ${origin.latitude.toStringAsFixed(gpsPrecision)}"); + if (step < 10) continue; + if (realGps.isNear(origin)) count++; + } + + final percentage = count / 1000; + simulator.logger.info("Final coordinates: ${realGps.coordinates.latitude}"); + expect(percentage, greaterThan(0.75), reason: "GPS should be accurate >75% of the time: $percentage"); + + // Ensure that *very* noisy readings don't affect anything. + simulator.logger.debug("Adding 100, 100"); + simulator.gps.update(GpsCoordinates(latitude: 100, longitude: 100)); + expect(realGps.isNear(origin), isTrue); + + await simulator.dispose(); + GpsUtils.maxErrorMeters = oldError; + }); + + test("IMU noise when stationary", () async { + Logger.level = LogLevel.off; + final simulator = AutonomySimulator(); + final simulatedImu = ImuSimulator(collection: simulator, maxError: imuError); + final realImu = RoverImu(collection: simulator); + final north = OrientationUtils.north; + simulatedImu.update(north); + + var count = 0; + for (var i = 0; i < 1000; i++) { + final newData = simulatedImu.raw; + realImu.update(newData); + simulator.logger.trace("Got new value: ${newData.heading}"); + simulator.logger.trace(" New heading: ${realImu.heading}"); + simulator.logger.trace(" Real position: ${north.heading}"); + if (i < 10) continue; + simulator.logger.trace(" Values are similar: ${realImu.isNear(north.heading)}"); + if (realImu.isNear(north.heading)) count++; + } + + final percentage = count / 1000; + simulator.logger.info("Final orientation: ${realImu.heading}"); + expect(percentage, greaterThan(0.75), reason: "IMU should be accurate >75% of the time: $percentage"); + + + realImu.update(OrientationUtils.south); + expect(realImu.isNear(OrientationUtils.north.heading), isTrue); + await simulator.dispose(); + }); + + test("GPS noise when moving", () async { + // Set up a simulated and real GPS, both starting at (0, 0) + final oldError = GpsUtils.maxErrorMeters; + GpsUtils.maxErrorMeters = 3; + final simulator = AutonomySimulator(); + final realGps = RoverGps(collection: simulator); + final simulatedGps = GpsSimulator(collection: simulator, maxError: GpsInterface.gpsError); + var realCoordinates = GpsCoordinates(); + simulatedGps.update(realCoordinates); + realGps.update(realCoordinates); + expect(realGps.coordinates.isNear(realCoordinates), isTrue); + + // For each step forward, use the noisy GPS to update the real GPS. + var count = 0; + for (var step = 0; step < 1000; step++) { + realCoordinates += GpsUtils.north; + simulatedGps.update(realCoordinates); + realGps.update(simulatedGps.coordinates); + simulator.logger.trace("New coordinate: ${realGps.coordinates.latitude.toStringAsFixed(5)} vs real position: ${realCoordinates.latitude.toStringAsFixed(5)}"); + simulator.logger.trace(" Difference: ${(realGps.latitude - realCoordinates.latitude).abs().toStringAsFixed(5)} < ${GpsUtils.epsilonLatitude.toStringAsFixed(5)}"); + if (step < 10) continue; + if (realGps.isNear(realCoordinates)) count++; + } + + final percentage = count / 1000; + simulator.logger.info("Final coordinates: ${realGps.coordinates.prettyPrint()} vs actual: ${realCoordinates.prettyPrint()}"); + expect(percentage, greaterThan(0.75), reason: "GPS should be accurate >75% of the time: $percentage"); + + // Ensure that *very* noisy readings don't affect anything. + simulator.logger.debug("Adding 100, 100"); + simulator.gps.update(GpsCoordinates(latitude: 100, longitude: 100)); + expect(realGps.isNear(realCoordinates), isTrue); + GpsUtils.maxErrorMeters = oldError; + }); + + test("IMU noise when moving", () async { + Logger.level = LogLevel.off; + final simulator = AutonomySimulator(); + final simulatedImu = ImuSimulator(collection: simulator, maxError: imuError); + final realImu = RoverImu(collection: simulator); + final orientation = OrientationUtils.north; + simulatedImu.update(orientation); + + var count = 0; + for (var i = 0; i < 350; i++) { + orientation.z += 1; + simulatedImu.update(orientation); + final newData = simulatedImu.raw; + realImu.update(newData); + simulator.logger.trace("Got new value: ${newData.heading}"); + simulator.logger.trace(" New heading: ${realImu.heading}"); + simulator.logger.trace(" Real position: ${orientation.heading}"); + if (i < 10) continue; + simulator.logger.trace(" Values are similar: ${realImu.isNear(orientation.heading)}"); + if (realImu.isNear(orientation.heading)) count++; + } + + final percentage = count / 350; + expect(percentage, greaterThan(0.75), reason: "IMU should be accurate >75% of the time: $percentage"); + + final badData = Orientation(z: 10); + simulator.logger.info("Final orientation: ${realImu.heading}"); + simulator.logger.info("Bad orientation: ${badData.heading}"); + realImu.update(badData); + simulator.logger.info("Unaffected orientation: ${realImu.heading}"); + expect(realImu.isNear(orientation.heading), isTrue); + await simulator.dispose(); + }); + + test("GPS latitude is set properly", () async { + final simulator = AutonomySimulator(); + const utahLatitude = 38.406683; + final utah = GpsCoordinates(latitude: utahLatitude); + + simulator.gps.update(utah); + expect(simulator.hasValue, isFalse); + expect(GpsInterface.currentLatitude, 0); + + await simulator.init(); + await simulator.waitForValue(); + expect(simulator.hasValue, isTrue); + expect(GpsInterface.currentLatitude, utahLatitude); + + await simulator.dispose(); + GpsInterface.currentLatitude = 0; + }); + + test("GPS can start in a real location", () async { + final simulator = AutonomySimulator(); + final ocean = GpsCoordinates(latitude: 0, longitude: 0); + final newYork = GpsCoordinates(latitude: 45, longitude: 72); + simulator.gps = RoverGps(collection: simulator); + + await simulator.init(); + expect(simulator.gps.coordinates, ocean); + expect(simulator.gps.isNear(newYork), isFalse); + expect(ocean.isNear(newYork), isFalse); + + simulator.gps.update(newYork); + expect(simulator.gps.isNear(ocean), isFalse); + expect(simulator.gps.isNear(newYork), isTrue); + + await simulator.dispose(); + }); + + test("IMU can handle values on the edge", () async { + Logger.level = LogLevel.off; + final simulator = AutonomySimulator(); + final simulatedImu = ImuSimulator(collection: simulator, maxError: imuError); + final realImu = RoverImu(collection: simulator); + final orientation = Orientation(z: 360); + simulatedImu.update(orientation); + + var count = 0; + for (var i = 0; i < 1000; i++) { + final newData = simulatedImu.raw; + realImu.update(newData); + simulator.logger.trace("Got new value: ${newData.heading}"); + simulator.logger.trace(" New heading: ${realImu.heading}"); + simulator.logger.trace(" Real position: ${orientation.heading}"); + if (i < 10) continue; + simulator.logger.trace(" Values are similar: ${realImu.isNear(orientation.heading)}"); + if (realImu.isNear(orientation.heading)) count++; + } + + final percentage = count / 1000; + expect(percentage, greaterThan(0.75), reason: "IMU should be accurate >75% of the time: $percentage"); + + final badData = Orientation(z: 10); + simulator.logger.info("Final orientation: ${realImu.heading}"); + simulator.logger.info("Bad orientation: ${badData.heading}"); + realImu.update(badData); + simulator.logger.info("Unaffected orientation: ${realImu.heading}"); + expect(realImu.isNear(orientation.heading), isTrue); + await simulator.dispose(); + }); +}); From 4dc7a669d55dfe51841f6c40d4ea3de29a2060d0 Mon Sep 17 00:00:00 2001 From: preston Date: Wed, 28 Jan 2026 10:04:20 -0500 Subject: [PATCH 10/16] Update autonomy to main --- autonomy/.github/pubspec_overrides.yaml | 4 +- autonomy/.github/workflows/analyze.yml | 13 +- autonomy/.github/workflows/documentation.yml | 17 +- autonomy/bin/arcuo.dart | 16 -- autonomy/bin/sensorless.dart | 63 ----- autonomy/bin/test.dart | 23 -- autonomy/lib/src/interfaces/a_star.dart | 66 ----- autonomy/lib/src/interfaces/autonomy.dart | 66 ----- autonomy/lib/src/interfaces/detector.dart | 10 - autonomy/lib/src/interfaces/drive.dart | 79 ------ autonomy/lib/src/interfaces/error.dart | 14 - autonomy/lib/src/interfaces/gps.dart | 27 -- autonomy/lib/src/interfaces/gps_utils.dart | 62 ----- autonomy/lib/src/interfaces/imu.dart | 19 -- autonomy/lib/src/interfaces/imu_utils.dart | 27 -- autonomy/lib/src/interfaces/orchestrator.dart | 61 ----- autonomy/lib/src/interfaces/pathfinding.dart | 18 -- autonomy/lib/src/interfaces/receiver.dart | 9 - autonomy/lib/src/interfaces/reporter.dart | 23 -- autonomy/lib/src/interfaces/server.dart | 25 -- autonomy/lib/src/interfaces/service.dart | 1 - autonomy/lib/src/interfaces/video.dart | 17 -- autonomy/lib/src/rover/corrector.dart | 40 --- autonomy/lib/src/rover/detector.dart | 21 -- autonomy/lib/src/rover/drive.dart | 67 ----- autonomy/lib/src/rover/drive/motors.dart | 26 -- autonomy/lib/src/rover/drive/sensor.dart | 148 ----------- autonomy/lib/src/rover/drive/timed.dart | 51 ---- autonomy/lib/src/rover/gps.dart | 41 --- autonomy/lib/src/rover/imu.dart | 43 --- autonomy/lib/src/rover/orchestrator.dart | 113 -------- autonomy/lib/src/rover/pathfinding.dart | 22 -- autonomy/lib/src/rover/sensorless.dart | 64 ----- autonomy/lib/src/rover/video.dart | 32 --- autonomy/lib/src/simulator/detector.dart | 46 ---- autonomy/lib/src/simulator/drive.dart | 54 ---- autonomy/lib/src/simulator/gps.dart | 28 -- autonomy/lib/src/simulator/imu.dart | 35 --- autonomy/lib/src/simulator/orchestrator.dart | 32 --- autonomy/lib/src/simulator/pathfinding.dart | 25 -- autonomy/lib/src/simulator/realsense.dart | 22 -- autonomy/test/sensor_test.dart | 246 ------------------ 42 files changed, 11 insertions(+), 1805 deletions(-) delete mode 100644 autonomy/bin/arcuo.dart delete mode 100644 autonomy/bin/sensorless.dart delete mode 100644 autonomy/bin/test.dart delete mode 100644 autonomy/lib/src/interfaces/a_star.dart delete mode 100644 autonomy/lib/src/interfaces/autonomy.dart delete mode 100644 autonomy/lib/src/interfaces/detector.dart delete mode 100644 autonomy/lib/src/interfaces/drive.dart delete mode 100644 autonomy/lib/src/interfaces/error.dart delete mode 100644 autonomy/lib/src/interfaces/gps.dart delete mode 100644 autonomy/lib/src/interfaces/gps_utils.dart delete mode 100644 autonomy/lib/src/interfaces/imu.dart delete mode 100644 autonomy/lib/src/interfaces/imu_utils.dart delete mode 100644 autonomy/lib/src/interfaces/orchestrator.dart delete mode 100644 autonomy/lib/src/interfaces/pathfinding.dart delete mode 100644 autonomy/lib/src/interfaces/receiver.dart delete mode 100644 autonomy/lib/src/interfaces/reporter.dart delete mode 100644 autonomy/lib/src/interfaces/server.dart delete mode 100644 autonomy/lib/src/interfaces/service.dart delete mode 100644 autonomy/lib/src/interfaces/video.dart delete mode 100644 autonomy/lib/src/rover/corrector.dart delete mode 100644 autonomy/lib/src/rover/detector.dart delete mode 100644 autonomy/lib/src/rover/drive.dart delete mode 100644 autonomy/lib/src/rover/drive/motors.dart delete mode 100644 autonomy/lib/src/rover/drive/sensor.dart delete mode 100644 autonomy/lib/src/rover/drive/timed.dart delete mode 100644 autonomy/lib/src/rover/gps.dart delete mode 100644 autonomy/lib/src/rover/imu.dart delete mode 100644 autonomy/lib/src/rover/orchestrator.dart delete mode 100644 autonomy/lib/src/rover/pathfinding.dart delete mode 100644 autonomy/lib/src/rover/sensorless.dart delete mode 100644 autonomy/lib/src/rover/video.dart delete mode 100644 autonomy/lib/src/simulator/detector.dart delete mode 100644 autonomy/lib/src/simulator/drive.dart delete mode 100644 autonomy/lib/src/simulator/gps.dart delete mode 100644 autonomy/lib/src/simulator/imu.dart delete mode 100644 autonomy/lib/src/simulator/orchestrator.dart delete mode 100644 autonomy/lib/src/simulator/pathfinding.dart delete mode 100644 autonomy/lib/src/simulator/realsense.dart delete mode 100644 autonomy/test/sensor_test.dart diff --git a/autonomy/.github/pubspec_overrides.yaml b/autonomy/.github/pubspec_overrides.yaml index b7c9d120..e25d2156 100644 --- a/autonomy/.github/pubspec_overrides.yaml +++ b/autonomy/.github/pubspec_overrides.yaml @@ -3,5 +3,5 @@ resolution: dependency_overrides: burt_network: git: - url: https://github.com/BinghamtonRover/Networking - ref: 2.3.1 \ No newline at end of file + url: https://github.com/BinghamtonRover/Rover-Code.git + path: burt_network \ No newline at end of file diff --git a/autonomy/.github/workflows/analyze.yml b/autonomy/.github/workflows/analyze.yml index b822d66a..6761abc3 100644 --- a/autonomy/.github/workflows/analyze.yml +++ b/autonomy/.github/workflows/analyze.yml @@ -10,14 +10,16 @@ jobs: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - name: Clone repo + uses: actions/checkout@v4 # Note: This workflow uses the latest stable version of the Dart SDK. # 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 + - name: Setup dart + uses: dart-lang/setup-dart@v1 with: - sdk: 3.6.0 + sdk: 3.9.0 # This package is part of a Pub Workspace. However, CI still needs to # run on this repo by itself, so we want to override burt_network to use @@ -36,6 +38,5 @@ jobs: # Your project will need to have tests in test/ and a dependency on # package:test for this step to succeed. Note that Flutter projects will # want to change this to 'flutter test'. - # FIXME: Disabling tests for now - # - name: Run tests - # run: dart test + - name: Run tests + run: dart test diff --git a/autonomy/.github/workflows/documentation.yml b/autonomy/.github/workflows/documentation.yml index 0267dfea..4d214996 100644 --- a/autonomy/.github/workflows/documentation.yml +++ b/autonomy/.github/workflows/documentation.yml @@ -24,26 +24,13 @@ jobs: - name: Install Dart uses: dart-lang/setup-dart@v1 with: - sdk: 3.5.2 + sdk: 3.9.0 - name: Install dependencies run: dart pub get - - name: Analyze Dart code - run: dart analyze --fatal-infos - - - name: Output error - if: failure() - run: echo "::error The code or is missing documentation. Run flutter analyze --dartdocs" - - name: Generate documentation - run: dart doc --output=docs - - # Your project will need to have tests in test/ and a dependency on - # package:test for this step to succeed. Note that Flutter projects will - # want to change this to 'flutter test'. - - name: Run tests - run: dart test + run: dartdoc --ignore 'unresolved-doc-reference,not-implemented,no-documentable-libraries,ambiguous-reexport' --exclude 'dart:async,dart:collection,dart:convert,dart:core,dart:developer,dart:io,dart:isolate,dart:math,dart:typed_data,dart:ui,dart:html,dart:js,dart:ffi,dart:js_util' --quiet --json --output docs --no-validate-links --no-verbose-warnings --no-allow-non-local-warnings - name: Commit and push files run: | diff --git a/autonomy/bin/arcuo.dart b/autonomy/bin/arcuo.dart deleted file mode 100644 index ae9f7d8c..00000000 --- a/autonomy/bin/arcuo.dart +++ /dev/null @@ -1,16 +0,0 @@ -import "package:autonomy/autonomy.dart"; - -void main() async { - final rover = RoverAutonomy(); - rover.drive = RoverDrive(collection: rover, useImu: false, useGps: false); - await rover.init(); - //await rover.waitForValue(); - final didSeeAruco = await rover.drive.spinForAruco(); - if (didSeeAruco) { - rover.logger.info("Found aruco"); - await rover.drive.approachAruco(); - } else { - rover.logger.warning("COuld not find aruco"); - } - rover.logger.info("Done"); -} diff --git a/autonomy/bin/sensorless.dart b/autonomy/bin/sensorless.dart deleted file mode 100644 index 0ddcd1f0..00000000 --- a/autonomy/bin/sensorless.dart +++ /dev/null @@ -1,63 +0,0 @@ -import "package:burt_network/logging.dart"; -import "package:burt_network/protobuf.dart"; - -import "package:autonomy/simulator.dart"; -import "package:autonomy/rover.dart"; - -void main() async { - Logger.level = LogLevel.debug; - final simulator = AutonomySimulator(); - simulator.drive = SensorlessDrive(collection: simulator, useGps: false, useImu: false); - await simulator.init(); - await simulator.server.waitForConnection(); - - final message = AutonomyData( - destination: GpsCoordinates(latitude: 0, longitude: 0), - state: AutonomyState.DRIVING, - task: AutonomyTask.GPS_ONLY, - obstacles: [], - path: [ - for (double x = 0; x < 3; x++) - for (double y = 0; y < 3; y++) - GpsCoordinates(latitude: y, longitude: x), - ], - ); - simulator.server.sendMessage(message); - - // "Snakes" around a 3x3 meter box. (0, 0), North - await simulator.drive.goForward(); // (1, 0), North - await simulator.drive.goForward(); // (2, 0), North - await simulator.drive.turnRight(); // (2, 0), East - await simulator.drive.goForward(); // (2, 1), East - await simulator.drive.turnRight(); // (2, 1), South - await simulator.drive.goForward(); // (1, 1), South - await simulator.drive.goForward(); // (0, 1), South - await simulator.drive.turnLeft(); // (0, 1), East - await simulator.drive.goForward(); // (0, 2), East - await simulator.drive.turnLeft(); // (0, 2), North - await simulator.drive.goForward(); // (1, 2), North - await simulator.drive.goForward(); // (2, 2), North - await simulator.drive.turnLeft(); // (2, 2), West - await simulator.drive.goForward(); // (2, 1), West - await simulator.drive.goForward(); // (2, 0), West - await simulator.drive.turnLeft(); // (2, 0), South - await simulator.drive.goForward(); // (1, 0), South - await simulator.drive.goForward(); // (0, 0), South - await simulator.drive.turnLeft(); // (0, 0), East - await simulator.drive.turnLeft(); // (0, 0), North - - final message2 = AutonomyData( - state: AutonomyState.AT_DESTINATION, - task: AutonomyTask.AUTONOMY_TASK_UNDEFINED, - obstacles: [], - path: [ - for (double x = 0; x < 3; x++) - for (double y = 0; y < 3; y++) - GpsCoordinates(latitude: y, longitude: x), - ], - ); - simulator.server.sendMessage(message2); - simulator.server.sendMessage(message2); - - await simulator.dispose(); -} diff --git a/autonomy/bin/test.dart b/autonomy/bin/test.dart deleted file mode 100644 index 12205b1d..00000000 --- a/autonomy/bin/test.dart +++ /dev/null @@ -1,23 +0,0 @@ -import "package:autonomy/autonomy.dart"; -import "package:burt_network/logging.dart"; -import "package:autonomy/src/rover/gps.dart"; -import "package:autonomy/src/rover/imu.dart"; - -void main() async { - Logger.level = LogLevel.all; - final rover = RoverAutonomy(); - rover.gps = RoverGps(collection: rover); - rover.imu = RoverImu(collection: rover); - rover.drive = RoverDrive(collection: rover, useGps: false); - - await rover.init(); -// await rover.waitForReadings(); -// await rover.waitForConnection(); - - rover.logger.info("Starting"); - await rover.drive.turnLeft(); - await rover.drive.turnRight(); - - rover.logger.info("Done"); - await rover.dispose(); -} diff --git a/autonomy/lib/src/interfaces/a_star.dart b/autonomy/lib/src/interfaces/a_star.dart deleted file mode 100644 index 6c7f16ec..00000000 --- a/autonomy/lib/src/interfaces/a_star.dart +++ /dev/null @@ -1,66 +0,0 @@ -import "package:a_star/a_star.dart"; - -import "package:burt_network/protobuf.dart"; -import "package:autonomy/interfaces.dart"; - -class AutonomyAStarState extends AStarState { - final DriveDirection direction; - final DriveOrientation orientation; - final GpsCoordinates position; - final GpsCoordinates goal; - final AutonomyInterface collection; - - AutonomyAStarState({ - required this.position, - required this.goal, - required this.collection, - required this.direction, - required this.orientation, - required super.depth, - }); - - factory AutonomyAStarState.start({ - required AutonomyInterface collection, - required GpsCoordinates goal, - }) => AutonomyAStarState( - position: collection.gps.coordinates, - goal: goal, - collection: collection, - direction: DriveDirection.stop, - orientation: collection.imu.orientation!, - depth: 0, - ); - - @override - String toString() => switch(direction) { - DriveDirection.forward => "Go forward to ${position.prettyPrint()}", - DriveDirection.left => "Turn left to face $direction", - DriveDirection.right => "Turn right to face $direction", - DriveDirection.stop => "Start/Stop at ${position.prettyPrint()}", - }; - - @override - double heuristic() => position.manhattanDistance(goal); - - @override - String hash() => "${position.prettyPrint()} ($orientation)"; - - @override - bool isGoal() => position.isNear(goal); - - AutonomyAStarState copyWith({required DriveDirection direction, required DriveOrientation orientation, required GpsCoordinates position}) => AutonomyAStarState( - collection: collection, - position: position, - orientation: orientation, - direction: direction, - goal: goal, - depth: direction == DriveDirection.forward ? depth + 1 : depth + 2, - ); - - @override - Iterable expand() => [ - copyWith(direction: DriveDirection.forward, orientation: orientation, position: position.goForward(orientation)), - copyWith(direction: DriveDirection.left, orientation: orientation.turnLeft(), position: position), - copyWith(direction: DriveDirection.right, orientation: orientation.turnRight(), position: position), - ].where((state) => !collection.pathfinder.isObstacle(state.position)); -} diff --git a/autonomy/lib/src/interfaces/autonomy.dart b/autonomy/lib/src/interfaces/autonomy.dart deleted file mode 100644 index ae69aba6..00000000 --- a/autonomy/lib/src/interfaces/autonomy.dart +++ /dev/null @@ -1,66 +0,0 @@ -import "package:burt_network/burt_network.dart"; - -import "package:autonomy/interfaces.dart"; - -abstract class AutonomyInterface extends Service with Receiver { - BurtLogger get logger; - GpsInterface get gps; - ImuInterface get imu; - DriveInterface get drive; - RoverSocket get server; - PathfindingInterface get pathfinder; - DetectorInterface get detector; - VideoInterface get video; - OrchestratorInterface get orchestrator; - - @override - Future init() async { - var result = true; - result &= await gps.init(); - result &= await imu.init(); - result &= await drive.init(); - result &= await server.init(); - result &= await pathfinder.init(); - result &= await detector.init(); - result &= await video.init(); - logger.info("Init orchestrator"); - await orchestrator.init(); - logger.info("Init orchestrator done"); - if (result) { - logger.info("Autonomy initialized"); - } else { - logger.warning("Autonomy could not initialize"); - } - return result; - } - - @override - Future dispose() async { - await gps.dispose(); - await imu.dispose(); - await drive.dispose(); - await pathfinder.dispose(); - await detector.dispose(); - await video.dispose(); - await orchestrator.dispose(); - logger.info("Autonomy disposed"); - await server.dispose(); - } - - Future restart() async { - await dispose(); - await init(); - } - - @override - Future waitForValue() async { - logger.info("Waiting for readings..."); - await gps.waitForValue(); - await imu.waitForValue(); - await video.waitForValue(); - logger.info("Received GPS and IMU values"); - } - - @override - bool get hasValue => gps.hasValue && imu.hasValue && video.hasValue; -} diff --git a/autonomy/lib/src/interfaces/detector.dart b/autonomy/lib/src/interfaces/detector.dart deleted file mode 100644 index c42a6943..00000000 --- a/autonomy/lib/src/interfaces/detector.dart +++ /dev/null @@ -1,10 +0,0 @@ -import "package:autonomy/interfaces.dart"; - -abstract class DetectorInterface extends Service { - AutonomyInterface collection; - DetectorInterface({required this.collection}); - - bool findObstacles(); - bool canSeeAruco(); - bool isOnSlope(); -} diff --git a/autonomy/lib/src/interfaces/drive.dart b/autonomy/lib/src/interfaces/drive.dart deleted file mode 100644 index 08b677ed..00000000 --- a/autonomy/lib/src/interfaces/drive.dart +++ /dev/null @@ -1,79 +0,0 @@ -import "package:autonomy/interfaces.dart"; -import "package:burt_network/protobuf.dart"; - -enum DriveDirection { - forward, - left, - right, - stop, -} - -enum DriveOrientation { - north(0), - west(90), - south(180), - east(270); - - final int angle; - const DriveOrientation(this.angle); - - static DriveOrientation? fromRaw(Orientation orientation) { - // TODO: Make this more precise. - for (final value in values) { - if (orientation.isNear(value.angle.toDouble())) return value; - } - return null; - } - - DriveOrientation turnLeft() => switch (this) { - north => west, - west => south, - south => east, - east => north, - }; - - DriveOrientation turnRight() => switch (this) { - north => east, - west => north, - south => west, - east => south, - }; -} - -abstract class DriveInterface extends Service { - AutonomyInterface collection; - DriveOrientation orientation = DriveOrientation.north; - DriveInterface({required this.collection}); - - Future goDirection(DriveDirection direction) async => switch (direction) { - DriveDirection.forward => await goForward(), - DriveDirection.left => await turnLeft(), - DriveDirection.right => await turnRight(), - DriveDirection.stop => await stop(), - }; - - Future faceNorth(); - - Future goForward(); - Future turnLeft(); - Future turnRight(); - Future stop(); - - Future faceDirection(DriveOrientation orientation) async { - this.orientation = orientation; - } - - Future followPath(List path) async { - for (final state in path) { - await goDirection(state.direction); - } - } - - void setLedStrip(ProtoColor color, {bool blink = false}) { - final command = DriveCommand(color: color, blink: blink ? BoolState.YES : BoolState.NO); - collection.server.sendCommand(command); - } - - Future spinForAruco() async => false; - Future approachAruco() async { } -} diff --git a/autonomy/lib/src/interfaces/error.dart b/autonomy/lib/src/interfaces/error.dart deleted file mode 100644 index 9d0c9ed3..00000000 --- a/autonomy/lib/src/interfaces/error.dart +++ /dev/null @@ -1,14 +0,0 @@ -import "dart:math"; - -class RandomError { - final double maxError; - final _random = Random(); - - RandomError(this.maxError); - - double get value { - final sign = _random.nextBool() ? 1 : -1; - final value = _random.nextDouble() * maxError; - return sign * value; - } -} diff --git a/autonomy/lib/src/interfaces/gps.dart b/autonomy/lib/src/interfaces/gps.dart deleted file mode 100644 index 8ccef300..00000000 --- a/autonomy/lib/src/interfaces/gps.dart +++ /dev/null @@ -1,27 +0,0 @@ -import "package:burt_network/protobuf.dart"; -import "package:autonomy/interfaces.dart"; - -abstract class GpsInterface extends Service with Receiver { - static const gpsError = 0.00003; - static double currentLatitude = 0; - - final AutonomyInterface collection; - GpsInterface({required this.collection}); - - double get longitude => coordinates.longitude; - double get latitude => coordinates.latitude; - - void update(GpsCoordinates newValue); - GpsCoordinates get coordinates; - - bool isNear(GpsCoordinates other) => coordinates.isNear(other); - - @override - Future waitForValue() async { - await super.waitForValue(); - currentLatitude = coordinates.latitude; - } - - @override - Future init() async => true; -} diff --git a/autonomy/lib/src/interfaces/gps_utils.dart b/autonomy/lib/src/interfaces/gps_utils.dart deleted file mode 100644 index dc4fc74c..00000000 --- a/autonomy/lib/src/interfaces/gps_utils.dart +++ /dev/null @@ -1,62 +0,0 @@ - -import "dart:math"; - -import "package:autonomy/interfaces.dart"; -import "package:burt_network/protobuf.dart"; - -extension RecordToGps on (num, num) { - GpsCoordinates toGps() => GpsCoordinates(latitude: $1.toDouble() * GpsUtils.latitudePerMeter, longitude: $2.toDouble() * GpsUtils.longitudePerMeter); -} - -extension GpsUtils on GpsCoordinates { - static double maxErrorMeters = 1; - static double get epsilonLatitude => maxErrorMeters * latitudePerMeter; - static double get epsilonLongitude => maxErrorMeters * longitudePerMeter; - - static GpsCoordinates get east => - GpsCoordinates(longitude: -1 / metersPerLongitude); - static GpsCoordinates get west => - GpsCoordinates(longitude: 1 / metersPerLongitude); - static GpsCoordinates get north => - GpsCoordinates(latitude: 1 * latitudePerMeter); - static GpsCoordinates get south => - GpsCoordinates(latitude: -1 * latitudePerMeter); - - // Taken from https://stackoverflow.com/a/39540339/9392211 - // static const metersPerLatitude = 111.32 * 1000; // 111.32 km - static const metersPerLatitude = 1; - static const radiansPerDegree = pi / 180; - static double get metersPerLongitude => 1; -// 40075 * cos( GpsInterface.currentLatitude * radiansPerDegree ) / 360 * 1000; - - static double get latitudePerMeter => 1 / metersPerLatitude; - static double get longitudePerMeter => 1 / metersPerLongitude; - - double distanceTo(GpsCoordinates other) => sqrt( - pow(latitude - other.latitude, 2) + - pow(longitude - other.longitude, 2), - ); - - double manhattanDistance(GpsCoordinates other) => - (latitude - other.latitude).abs() * metersPerLatitude + - (longitude - other.longitude).abs() * metersPerLongitude; - - bool isNear(GpsCoordinates other) => - (latitude - other.latitude).abs() < epsilonLatitude && - (longitude - other.longitude).abs() < epsilonLongitude; - - GpsCoordinates operator +(GpsCoordinates other) => GpsCoordinates( - latitude: latitude + other.latitude, - longitude: longitude + other.longitude, - ); - -// String prettyPrint() => "(lat=${(latitude * GpsUtils.metersPerLatitude).toStringAsFixed(2)}, long=${(longitude * GpsUtils.metersPerLongitude).toStringAsFixed(2)})"; - String prettyPrint() => toProto3Json().toString(); - - GpsCoordinates goForward(DriveOrientation orientation) => this + switch(orientation) { - DriveOrientation.north => GpsUtils.north, - DriveOrientation.south => GpsUtils.south, - DriveOrientation.west => GpsUtils.west, - DriveOrientation.east => GpsUtils.east, - }; -} diff --git a/autonomy/lib/src/interfaces/imu.dart b/autonomy/lib/src/interfaces/imu.dart deleted file mode 100644 index 5bb6bba2..00000000 --- a/autonomy/lib/src/interfaces/imu.dart +++ /dev/null @@ -1,19 +0,0 @@ -import "package:autonomy/interfaces.dart"; -import "package:burt_network/burt_network.dart"; - -abstract class ImuInterface extends Service with Receiver { - final AutonomyInterface collection; - ImuInterface({required this.collection}); - - double get heading => raw.z; - Orientation get raw; - DriveOrientation? get orientation { - collection.logger.trace("Trying to find orientation at $heading"); - return DriveOrientation.fromRaw(raw); - } - void update(Orientation newValue); - bool isNear(double angle) => raw.isNear(angle); - - @override - Future init() async => true; -} diff --git a/autonomy/lib/src/interfaces/imu_utils.dart b/autonomy/lib/src/interfaces/imu_utils.dart deleted file mode 100644 index 588189d3..00000000 --- a/autonomy/lib/src/interfaces/imu_utils.dart +++ /dev/null @@ -1,27 +0,0 @@ -import "package:burt_network/protobuf.dart"; - -extension OrientationUtils on Orientation { - static const double epsilon = 10; - - static final north = Orientation(z: 0); - static final west = Orientation(z: 90); - static final south = Orientation(z: 180); - static final east = Orientation(z: 270); - - double get heading => z; - - bool get isEmpty => x == 0 && y == 0 && z == 0; - - Orientation clampHeading() { - var adjustedHeading = heading; - if (heading >= 360) adjustedHeading -= 360; - if (heading < 0) adjustedHeading = 360 + heading; - return Orientation(x: x, y: y, z: adjustedHeading); - } - - bool isNear(double value) => value > 270 && z < 90 - ? (z + 360 - value).abs() < epsilon - : value < 90 && z > 270 - ? (z - value - 360).abs() < epsilon - : (z - value).abs() < epsilon; -} diff --git a/autonomy/lib/src/interfaces/orchestrator.dart b/autonomy/lib/src/interfaces/orchestrator.dart deleted file mode 100644 index 13ee8ed6..00000000 --- a/autonomy/lib/src/interfaces/orchestrator.dart +++ /dev/null @@ -1,61 +0,0 @@ -import "dart:io"; - -import "package:autonomy/interfaces.dart"; -import "package:burt_network/burt_network.dart"; -import "package:meta/meta.dart"; - -abstract class OrchestratorInterface extends Service { - final AutonomyInterface collection; - OrchestratorInterface({required this.collection}); - - AutonomyCommand? currentCommand; - AutonomyState currentState = AutonomyState.AUTONOMY_STATE_UNDEFINED; - Future onCommand(AutonomyCommand command) async { - collection.server.sendMessage(command); - if (command.abort) return abort(); - if (currentCommand != null) { - collection.logger.error("Already executing a command", body: "Abort first if you want to switch tasks"); - return; - } - - if (!collection.hasValue) { - collection.logger.error("Sensors haven't gotten any readings yet!"); - currentState = AutonomyState.NO_SOLUTION; - return; - } - await collection.drive.faceNorth(); - currentCommand = command; - switch (command.task) { - case AutonomyTask.GPS_ONLY: await handleGpsTask(command); - case AutonomyTask.VISUAL_MARKER: await handleArucoTask(command); - // TODO: Add more tasks - default: collection.logger.error("Unrecognized task: ${command.task}"); // ignore: no_default_cases - } - } - - @override - Future init() async { - collection.server.messages.onMessage( - name: AutonomyCommand().messageName, - constructor: AutonomyCommand.fromBuffer, - callback: onCommand, - ); - return true; - } - - @mustCallSuper - Future abort() async { - currentCommand = null; - collection.logger.warning("Aborting task!"); - currentState = AutonomyState.ABORTING; - await collection.drive.stop(); - await collection.dispose(); - exit(1); - } - - Future handleGpsTask(AutonomyCommand command); - Future handleArucoTask(AutonomyCommand command); - Future handleHammerTask(AutonomyCommand command); - Future handleBottleTask(AutonomyCommand command); - AutonomyData get statusMessage; -} diff --git a/autonomy/lib/src/interfaces/pathfinding.dart b/autonomy/lib/src/interfaces/pathfinding.dart deleted file mode 100644 index a3b4c73a..00000000 --- a/autonomy/lib/src/interfaces/pathfinding.dart +++ /dev/null @@ -1,18 +0,0 @@ -import "package:autonomy/interfaces.dart"; -import "package:burt_network/protobuf.dart"; - -abstract class PathfindingInterface extends Service { - final AutonomyInterface collection; - PathfindingInterface({required this.collection}); - - List? getPath(GpsCoordinates destination, {bool verbose = false}); - - Set obstacles = {}; - void recordObstacle(GpsCoordinates coordinates) => obstacles.add(coordinates); - bool isObstacle(GpsCoordinates coordinates) => obstacles.any((obstacle) => obstacle.isNear(coordinates)); - - @override - Future dispose() async { - obstacles.clear(); - } -} diff --git a/autonomy/lib/src/interfaces/receiver.dart b/autonomy/lib/src/interfaces/receiver.dart deleted file mode 100644 index 59ec05bf..00000000 --- a/autonomy/lib/src/interfaces/receiver.dart +++ /dev/null @@ -1,9 +0,0 @@ -mixin Receiver { - bool hasValue = false; - - Future waitForValue() async { - while (!hasValue) { - await Future.delayed(const Duration(seconds: 1)); - } - } -} diff --git a/autonomy/lib/src/interfaces/reporter.dart b/autonomy/lib/src/interfaces/reporter.dart deleted file mode 100644 index 4524b60d..00000000 --- a/autonomy/lib/src/interfaces/reporter.dart +++ /dev/null @@ -1,23 +0,0 @@ -import "dart:async"; -import "package:burt_network/protobuf.dart"; - -import "package:autonomy/interfaces.dart"; - -mixin ValueReporter on Service { - AutonomyInterface get collection; - Message getMessage(); - - Timer? timer; - Duration get reportInterval => const Duration(milliseconds: 10); - - @override - Future init() async { - timer = Timer.periodic(reportInterval, (timer) => _reportValue()); - return super.init(); - } - - @override - Future dispose() async => timer?.cancel(); - - void _reportValue() => collection.server.sendMessage(getMessage()); -} diff --git a/autonomy/lib/src/interfaces/server.dart b/autonomy/lib/src/interfaces/server.dart deleted file mode 100644 index cec6850e..00000000 --- a/autonomy/lib/src/interfaces/server.dart +++ /dev/null @@ -1,25 +0,0 @@ -import "dart:io"; - -import "package:burt_network/burt_network.dart"; - -extension ServerUtils on RoverSocket { - static SocketInfo subsystemsDestination = SocketInfo( - address: InternetAddress("192.168.1.20"), - port: 8001, - ); - - void sendCommand(Message message) => sendMessage(message, destination: subsystemsDestination); - - Future waitForConnection() async { - logger.info("Waiting for connection..."); - while (!isConnected) { - await Future.delayed(const Duration(milliseconds: 100)); - } - return; - } - - void sendDone() { - final message = AutonomyData(state: AutonomyState.AT_DESTINATION, task: AutonomyTask.AUTONOMY_TASK_UNDEFINED); - sendMessage(message); - } -} diff --git a/autonomy/lib/src/interfaces/service.dart b/autonomy/lib/src/interfaces/service.dart deleted file mode 100644 index 19cbacaa..00000000 --- a/autonomy/lib/src/interfaces/service.dart +++ /dev/null @@ -1 +0,0 @@ -export "package:burt_network/burt_network.dart" show Service; diff --git a/autonomy/lib/src/interfaces/video.dart b/autonomy/lib/src/interfaces/video.dart deleted file mode 100644 index 6f3fc32b..00000000 --- a/autonomy/lib/src/interfaces/video.dart +++ /dev/null @@ -1,17 +0,0 @@ -import "package:autonomy/interfaces.dart"; -import "package:burt_network/protobuf.dart"; - -/// Handles obstacle detection data and ArUco data from video -abstract class VideoInterface extends Service with Receiver { - bool flag = false; - - final AutonomyInterface collection; - VideoInterface({required this.collection}); - - VideoData data = VideoData(); - - void updateFrame(VideoData newData); - - // double get arucoSize => data.arucoSize; - // double get arucoPosition => data.arucoPosition; -} diff --git a/autonomy/lib/src/rover/corrector.dart b/autonomy/lib/src/rover/corrector.dart deleted file mode 100644 index 46b59e1f..00000000 --- a/autonomy/lib/src/rover/corrector.dart +++ /dev/null @@ -1,40 +0,0 @@ -import "dart:collection"; - -class ErrorCorrector { // non-nullable - final int maxSamples; - final double maxDeviation; - ErrorCorrector({required this.maxSamples, this.maxDeviation = double.infinity}); - - double calibratedValue = 0; - final Queue recentSamples = DoubleLinkedQueue(); - - void addValue(double value) { - if (recentSamples.isEmpty) { - recentSamples.add(value); - calibratedValue = value; - return; - } - final deviation = (calibratedValue - value).abs(); - if (deviation > maxDeviation) return; - if (recentSamples.length == maxSamples) recentSamples.removeLast(); - recentSamples.addFirst(value); - calibratedValue = recentSamples.weightedAverage(); - } - - void clear() { - calibratedValue = 0; - recentSamples.clear(); - } -} - -extension on Iterable { - double weightedAverage() { - num sum = 0; - var count = 0; - for (final element in this) { - sum += element; - count++; - } - return sum / count; - } -} diff --git a/autonomy/lib/src/rover/detector.dart b/autonomy/lib/src/rover/detector.dart deleted file mode 100644 index f28c508b..00000000 --- a/autonomy/lib/src/rover/detector.dart +++ /dev/null @@ -1,21 +0,0 @@ -import "package:autonomy/interfaces.dart"; - -class RoverDetector extends DetectorInterface { - RoverDetector({required super.collection}); - - @override - bool isOnSlope() => false; - - @override - bool findObstacles() => false; - - @override -// bool canSeeAruco() => collection.video.data.arucoDetected == BoolState.YES; - bool canSeeAruco() => collection.video.flag; - - @override - Future init() async => true; - - @override - Future dispose() async { } -} diff --git a/autonomy/lib/src/rover/drive.dart b/autonomy/lib/src/rover/drive.dart deleted file mode 100644 index 7cef09e4..00000000 --- a/autonomy/lib/src/rover/drive.dart +++ /dev/null @@ -1,67 +0,0 @@ -import "package:burt_network/burt_network.dart"; -import "package:autonomy/interfaces.dart"; - -import "drive/timed.dart"; -import "drive/sensor.dart"; - -/// A helper class to send drive commands to the rover with a simpler API. -class RoverDrive extends DriveInterface { - final bool useGps; - final bool useImu; - - final SensorDrive sensorDrive; - final TimedDrive timedDrive; - - // TODO: Calibrate these - RoverDrive({required super.collection, this.useGps = true, this.useImu = true}) : - sensorDrive = SensorDrive(collection: collection), - timedDrive = TimedDrive(collection: collection); - - /// Initializes the rover's drive subsystems. - @override - Future init() async => true; - - /// Stops the rover from driving. - @override - Future dispose() => stop(); - - /// Sets the angle of the front camera. - void setCameraAngle({required double swivel, required double tilt}) { - collection.logger.trace("Setting camera angles to $swivel (swivel) and $tilt (tilt)"); - final command = DriveCommand(frontSwivel: swivel, frontTilt: tilt); - collection.server.sendCommand(command); - } - - @override - Future stop() async { - await timedDrive.stop(); - } - - @override - Future faceNorth() => useImu ? sensorDrive.faceNorth() : timedDrive.faceNorth(); - - @override - Future spinForAruco() => sensorDrive.spinForAruco(); - @override - Future approachAruco() => sensorDrive.approachAruco(); - - - @override - Future faceDirection(DriveOrientation orientation) async { - if (useImu) { - await sensorDrive.faceDirection(orientation); - } else { - await timedDrive.faceDirection(orientation); - } - await super.faceDirection(orientation); - } - - @override - Future goForward() => useGps ? sensorDrive.goForward() : timedDrive.goForward(); - - @override - Future turnLeft() => useImu ? sensorDrive.turnLeft() : timedDrive.turnLeft(); - - @override - Future turnRight() => useImu ? sensorDrive.turnRight() : timedDrive.turnRight(); -} diff --git a/autonomy/lib/src/rover/drive/motors.dart b/autonomy/lib/src/rover/drive/motors.dart deleted file mode 100644 index 12ae541b..00000000 --- a/autonomy/lib/src/rover/drive/motors.dart +++ /dev/null @@ -1,26 +0,0 @@ -import "package:autonomy/interfaces.dart"; -import "package:burt_network/protobuf.dart"; - -mixin RoverMotors { - AutonomyInterface get collection; - - /// Sets the max speed of the rover. - /// - /// [setSpeeds] takes the speeds of each side of wheels. These numbers are percentages of the - /// max speed allowed by the rover, which we call the throttle. This function adjusts the - /// throttle, as a percentage of the rover's top speed. - void setThrottle(double throttle) { - collection.logger.trace("Setting throttle to $throttle"); - collection.server.sendCommand(DriveCommand(throttle: throttle, setThrottle: true)); - } - - /// Sets the speeds of the left and right wheels, using differential steering. - /// - /// These values are percentages of the max speed allowed by the rover. See [setThrottle]. - void setSpeeds({required double left, required double right}) { - right *= -1; - collection.logger.trace("Setting speeds to $left and $right"); - collection.server.sendCommand(DriveCommand(left: left, setLeft: true)); - collection.server.sendCommand(DriveCommand(right: right, setRight: true)); - } -} diff --git a/autonomy/lib/src/rover/drive/sensor.dart b/autonomy/lib/src/rover/drive/sensor.dart deleted file mode 100644 index 207e4fc5..00000000 --- a/autonomy/lib/src/rover/drive/sensor.dart +++ /dev/null @@ -1,148 +0,0 @@ -import "package:autonomy/autonomy.dart"; -import "package:autonomy/interfaces.dart"; - -import "motors.dart"; - -class SensorDrive extends DriveInterface with RoverMotors { - static const double maxThrottle = 0.1; - static const double turnThrottle = 0.1; - static const predicateDelay = Duration(milliseconds: 100); - static const turnDelay = Duration(milliseconds: 1500); - - SensorDrive({required super.collection}); - - @override - Future stop() async { - setThrottle(0); - setSpeeds(left: 0, right: 0); - } - - Future waitFor(bool Function() predicate) async { - while (!predicate()) { - await Future.delayed(predicateDelay); - await collection.imu.waitForValue(); - } - } - - @override - Future init() async => true; - - @override - Future dispose() async { } - - @override - Future goForward() async { - final orientation = collection.imu.orientation; - final currentCoordinates = collection.gps.coordinates; - final destination = currentCoordinates.goForward(orientation!); - - setThrottle(maxThrottle); - setSpeeds(left: 1, right: 1); - await waitFor(() => collection.gps.coordinates.isNear(destination)); - await stop(); - } - - @override - Future faceNorth() async { - collection.logger.info("Turning to face north..."); - setThrottle(turnThrottle); - setSpeeds(left: -1, right: 1); - await waitFor(() { - collection.logger.trace("Current heading: ${collection.imu.heading}"); - return collection.imu.raw.isNear(0); - }); - await stop(); - } - - @override - Future faceDirection(DriveOrientation orientation) async { - collection.logger.info("Turning to face $orientation..."); - setThrottle(turnThrottle); - setSpeeds(left: -1, right: 1); - await waitFor(() { - collection.logger.trace("Current heading: ${collection.imu.heading}"); - return collection.imu.raw.isNear(orientation.angle.toDouble()); - }); - await stop(); - await super.faceDirection(orientation); - } - - @override - Future turnLeft() async { - await collection.imu.waitForValue(); - - if (collection.imu.orientation == null) { - await faceNorth(); - await faceDirection(this.orientation); - } - await collection.imu.waitForValue(); - - final orientation = collection.imu.orientation; - final destination = orientation!.turnLeft(); // do NOT clamp! - setThrottle(maxThrottle); - setSpeeds(left: -1, right: 1); - await waitFor(() => collection.imu.orientation == destination); - await stop(); - this.orientation = this.orientation.turnLeft(); - } - - @override - Future turnRight() async { - await collection.imu.waitForValue(); - if (collection.imu.orientation == null) { - await faceNorth(); - await faceDirection(this.orientation); - } - await collection.imu.waitForValue(); - final orientation = collection.imu.orientation; - final destination = orientation!.turnRight(); // do NOT clamp! - setThrottle(maxThrottle); - setSpeeds(left: 1, right: -1); - await waitFor(() => collection.imu.orientation == destination); - await stop(); - this.orientation = this.orientation.turnRight(); - } - - @override - Future spinForAruco() async { - for (var i = 0; i < 16; i++) { - setThrottle(turnThrottle); - setSpeeds(left: -1, right: 1); - await Future.delayed(turnDelay); - await stop(); - - for (var j = 0; j < 300; j++) { - await Future.delayed(const Duration(milliseconds: 10)); - collection.logger.trace("Can see aruco? ${collection.detector.canSeeAruco()}"); - - if (collection.detector.canSeeAruco()) { - // Spin a bit more to center it - // print("We found it!"); - // setThrottle(0.1); - // setSpeeds(left: -1, right: 1); - // await waitFor(() { - // final pos = collection.video.arucoPosition; - // collection.logger.debug("aruco is at $pos"); - // return pos > 0.2; - // }); - // await stop(); - return true; - }} - } - return false; - } - - @override - Future approachAruco() async { - setThrottle(maxThrottle); - setSpeeds(left: 1, right: 1); - // const threshold = 0.2; - // await waitFor(() { - // final pos = collection.video.arucoSize; - // collection.logger.debug("It is at $pos percent"); - // return (pos.abs() < 0.00001 && !collection.detector.canSeeAruco()) || pos >= threshold; - // }); - await Future.delayed(const Duration(seconds: 10)); - await stop(); - } -} diff --git a/autonomy/lib/src/rover/drive/timed.dart b/autonomy/lib/src/rover/drive/timed.dart deleted file mode 100644 index 142a1ff4..00000000 --- a/autonomy/lib/src/rover/drive/timed.dart +++ /dev/null @@ -1,51 +0,0 @@ -import "package:autonomy/interfaces.dart"; - -import "motors.dart"; - -class TimedDrive extends DriveInterface with RoverMotors { - static const maxThrottle = 0.1; - static const turnThrottle = 0.1; - static const oneMeterDelay = Duration(milliseconds: 5500); - static const turnDelay = Duration(milliseconds: 4500); - - TimedDrive({required super.collection}); - - @override - Future init() async => true; - - @override - Future dispose() async { } - - @override - Future stop() async { - setThrottle(0); - setSpeeds(left: 0, right: 0); - } - - @override - Future faceNorth() async { /* Assume already facing north */ } - - @override - Future goForward() async { - setThrottle(maxThrottle); - setSpeeds(left: 1, right: 1); - await Future.delayed(oneMeterDelay); - await stop(); - } - - @override - Future turnLeft() async { - setThrottle(turnThrottle); - setSpeeds(left: -1, right: 1); - await Future.delayed(turnDelay); - await stop(); - } - - @override - Future turnRight() async { - setThrottle(turnThrottle); - setSpeeds(left: 1, right: -1); - await Future.delayed(turnDelay); - await stop(); - } -} diff --git a/autonomy/lib/src/rover/gps.dart b/autonomy/lib/src/rover/gps.dart deleted file mode 100644 index 2704b853..00000000 --- a/autonomy/lib/src/rover/gps.dart +++ /dev/null @@ -1,41 +0,0 @@ -import "package:burt_network/burt_network.dart"; -import "package:autonomy/interfaces.dart"; - -import "corrector.dart"; - -class RoverGps extends GpsInterface { - final _latitudeCorrector = ErrorCorrector(maxSamples: 1, maxDeviation: GpsInterface.gpsError * 10); - final _longitudeCorrector = ErrorCorrector(maxSamples: 1, maxDeviation: GpsInterface.gpsError * 10); - RoverGps({required super.collection}); - - @override - Future init() async { - collection.server.messages.onMessage( - name: RoverPosition().messageName, - constructor: RoverPosition.fromBuffer, - callback: (pos) { - if (pos.hasGps()) update(pos.gps); - }, - ); - return super.init(); - } - - @override - Future dispose() async { - _latitudeCorrector.clear(); - _longitudeCorrector.clear(); - } - - @override - void update(GpsCoordinates newValue) { - _latitudeCorrector.addValue(newValue.latitude); - _longitudeCorrector.addValue(newValue.longitude); - hasValue = true; - } - - @override - GpsCoordinates get coordinates => GpsCoordinates( - latitude: _latitudeCorrector.calibratedValue, - longitude: _longitudeCorrector.calibratedValue, - ); -} diff --git a/autonomy/lib/src/rover/imu.dart b/autonomy/lib/src/rover/imu.dart deleted file mode 100644 index 48f66666..00000000 --- a/autonomy/lib/src/rover/imu.dart +++ /dev/null @@ -1,43 +0,0 @@ -import "package:burt_network/burt_network.dart"; -import "package:autonomy/interfaces.dart"; - -import "corrector.dart"; - -class RoverImu extends ImuInterface { - final _zCorrector = ErrorCorrector(maxSamples: 10, maxDeviation: 15); - RoverImu({required super.collection}); - - Orientation value = Orientation(); - - @override - Future init() async { - collection.server.messages.onMessage( - name: RoverPosition().messageName, - constructor: RoverPosition.fromBuffer, - callback: (pos) { - if (pos.hasOrientation()) update(pos.orientation); - }, - ); - return super.init(); - } - - @override - Future dispose() async { - _zCorrector.clear(); - } - - @override - void update(Orientation newValue) { - // _zCorrector.addValue(newValue.heading); - // collection.logger.trace("Got IMU value"); - hasValue = true; - value = newValue; - } - - @override - Orientation get raw => Orientation( - x: 0, - y: 0, - z: value.z, - ).clampHeading(); -} diff --git a/autonomy/lib/src/rover/orchestrator.dart b/autonomy/lib/src/rover/orchestrator.dart deleted file mode 100644 index 2fd9a2f8..00000000 --- a/autonomy/lib/src/rover/orchestrator.dart +++ /dev/null @@ -1,113 +0,0 @@ -import "package:autonomy/interfaces.dart"; -import "package:burt_network/protobuf.dart"; -import "dart:async"; - -class RoverOrchestrator extends OrchestratorInterface with ValueReporter { - final List traversed = []; - List? currentPath; - RoverOrchestrator({required super.collection}); - - @override - Future dispose() async { - currentPath = null; - currentCommand = null; - currentState = AutonomyState.AUTONOMY_STATE_UNDEFINED; - traversed.clear(); - await super.dispose(); - } - - @override - AutonomyData get statusMessage => AutonomyData( - destination: currentCommand?.destination, - state: currentState, - obstacles: collection.pathfinder.obstacles, - path: [ - for (final transition in currentPath ?? []) - transition.position, - ...traversed, - ], - task: currentCommand?.task, - crash: false, // TODO: Investigate if this is used and how to use it better - ); - - @override - Message getMessage() => statusMessage; - - @override - Future handleGpsTask(AutonomyCommand command) async { - final destination = command.destination; - collection.logger.info("Got GPS Task: Go to ${destination.prettyPrint()}"); - collection.logger.debug("Currently at ${collection.gps.coordinates.prettyPrint()}"); - collection.drive.setLedStrip(ProtoColor.RED); - while (!collection.gps.coordinates.isNear(destination)) { - // Calculate a path - collection.logger.debug("Finding a path"); - currentState = AutonomyState.PATHING; - await collection.drive.faceNorth(); - final path = collection.pathfinder.getPath(destination); - currentPath = path; // also use local variable path for promotion - if (path == null) { - final current = collection.gps.coordinates; - collection.logger.error("Could not find a path", body: "No path found from ${current.prettyPrint()} to ${destination.prettyPrint()}"); - currentState = AutonomyState.NO_SOLUTION; - currentCommand = null; - return; - } - // Try to take that path - final current = collection.gps.coordinates; - collection.logger.info("Found a path from ${current.prettyPrint()} to ${destination.prettyPrint()}: ${path.length} steps"); - collection.logger.debug("Here is a summary of the path"); - for (final step in path) { - collection.logger.debug(step.toString()); - } - currentState = AutonomyState.DRIVING; - var count = 0; - for (final state in path) { - collection.logger.debug(state.toString()); - await collection.drive.goDirection(state.direction); - traversed.add(state.position); - if (state.direction != DriveDirection.forward) continue; - if (count++ == 5) break; - final foundObstacle = collection.detector.findObstacles(); - if (foundObstacle) { - collection.logger.debug("Found an obstacle. Recalculating path..."); - break; // calculate a new path - } - } - } - collection.logger.info("Task complete"); - collection.drive.setLedStrip(ProtoColor.GREEN, blink: true); - currentState = AutonomyState.AT_DESTINATION; - currentCommand = null; - } - - @override - Future handleArucoTask(AutonomyCommand command) async { - collection.drive.setLedStrip(ProtoColor.RED); - - // Go to GPS coordinates - // await handleGpsTask(command); - collection.logger.info("Got ArUco Task"); - - currentState = AutonomyState.SEARCHING; - collection.logger.info("Searching for ArUco tag"); - final didSeeAruco = await collection.drive.spinForAruco(); - if (didSeeAruco) { - collection.logger.info("Found aruco"); - currentState = AutonomyState.APPROACHING; - await collection.drive.approachAruco(); - collection.drive.setLedStrip(ProtoColor.GREEN, blink: true); - currentState = AutonomyState.AT_DESTINATION; - } - } - - @override - Future handleHammerTask(AutonomyCommand command) async { - - } - - @override - Future handleBottleTask(AutonomyCommand command) async { - - } -} diff --git a/autonomy/lib/src/rover/pathfinding.dart b/autonomy/lib/src/rover/pathfinding.dart deleted file mode 100644 index 2eb06735..00000000 --- a/autonomy/lib/src/rover/pathfinding.dart +++ /dev/null @@ -1,22 +0,0 @@ -import "package:a_star/a_star.dart"; - -import "package:autonomy/interfaces.dart"; -import "package:burt_network/burt_network.dart"; -import "package:burt_network/protobuf.dart"; - -class RoverPathfinder extends PathfindingInterface { - RoverPathfinder({required super.collection}); - - @override - Future init() async => true; - - @override - List? getPath(GpsCoordinates destination, {bool verbose = false}) { - if (isObstacle(destination)) return null; - final state = AutonomyAStarState.start(collection: collection, goal: destination); - final result = aStar(state, verbose: verbose, limit: 50000); - if (result == null) return null; - final transitions = result.reconstructPath().toList(); - return transitions; - } -} diff --git a/autonomy/lib/src/rover/sensorless.dart b/autonomy/lib/src/rover/sensorless.dart deleted file mode 100644 index 2beab095..00000000 --- a/autonomy/lib/src/rover/sensorless.dart +++ /dev/null @@ -1,64 +0,0 @@ -import "package:autonomy/interfaces.dart"; -import "package:autonomy/rover.dart"; -import "package:autonomy/simulator.dart"; - -class SensorlessDrive extends DriveInterface { - final DriveInterface simulatedDrive; - final DriveInterface realDrive; - - SensorlessDrive({required super.collection, bool useGps = true, bool useImu = true}) : - simulatedDrive = DriveSimulator(collection: collection), - realDrive = RoverDrive(collection: collection, useGps: useGps, useImu: useImu); - - @override - Future init() async { - var result = true; - result &= await simulatedDrive.init(); - result &= await realDrive.init(); - return result; - } - - @override - Future dispose() async { - await simulatedDrive.dispose(); - await realDrive.dispose(); - } - - @override - Future goForward() async { - await simulatedDrive.goForward(); - await realDrive.goForward(); - } - - @override - Future stop() async { - await simulatedDrive.stop(); - await realDrive.stop(); - } - - @override - Future faceNorth() async { - await simulatedDrive.faceNorth(); - await realDrive.faceNorth(); - } - - - @override - Future faceDirection(DriveOrientation orientation) async { - await simulatedDrive.faceDirection(orientation); - await realDrive.faceDirection(orientation); - await super.faceDirection(orientation); - } - - @override - Future turnLeft() async { - await simulatedDrive.turnLeft(); - await realDrive.turnLeft(); - } - - @override - Future turnRight() async { - await simulatedDrive.turnRight(); - await realDrive.turnRight(); - } -} diff --git a/autonomy/lib/src/rover/video.dart b/autonomy/lib/src/rover/video.dart deleted file mode 100644 index 0f829acb..00000000 --- a/autonomy/lib/src/rover/video.dart +++ /dev/null @@ -1,32 +0,0 @@ -import "dart:async"; - -import "package:autonomy/interfaces.dart"; -import "package:burt_network/burt_network.dart"; - -class RoverVideo extends VideoInterface { - RoverVideo({required super.collection}); - - @override - Future init() async { - collection.server.messages.onMessage( - name: VideoData().messageName, - constructor: VideoData.fromBuffer, - callback: updateFrame, - ); - return true; - } - - @override - Future dispose() async { } - - @override - void updateFrame(VideoData newData) { - data = newData; - // if (data.arucoDetected == BoolState.YES) { - // flag = true; - // Timer(const Duration(seconds: 3), () => flag = false); - // collection.logger.info("Is ArUco detected: ${data.arucoDetected}"); - // } - hasValue = true; - } -} diff --git a/autonomy/lib/src/simulator/detector.dart b/autonomy/lib/src/simulator/detector.dart deleted file mode 100644 index 82349221..00000000 --- a/autonomy/lib/src/simulator/detector.dart +++ /dev/null @@ -1,46 +0,0 @@ -import "package:autonomy/interfaces.dart"; -import "package:burt_network/protobuf.dart"; - -class SimulatedObstacle { - final GpsCoordinates coordinates; - final int radius; - SimulatedObstacle({required this.coordinates, required this.radius}); - - bool isNear(GpsCoordinates other) => - coordinates.distanceTo(other) <= radius; -} - -class DetectorSimulator extends DetectorInterface { - static const arucoPosition = (10, 10); - static const slopedLatitude = -5; - - final List obstacles; - final Set found = {}; - - DetectorSimulator({required super.collection, required this.obstacles}); - - @override - Future init() async => true; - - @override - Future dispose() async => obstacles.clear(); - - @override - bool findObstacles() { - final coordinates = collection.gps.coordinates; - var result = false; - for (final obstacle in obstacles) { - if (!obstacle.isNear(coordinates) || found.contains(obstacle.coordinates)) continue; - result = true; - found.add(obstacle.coordinates); - collection.pathfinder.recordObstacle(obstacle.coordinates); - } - return result; - } - - @override - bool canSeeAruco() => false; // if can see [arucoPosition] - - @override - bool isOnSlope() => false; // if on [slopedLatitude] -} diff --git a/autonomy/lib/src/simulator/drive.dart b/autonomy/lib/src/simulator/drive.dart deleted file mode 100644 index 16eecaae..00000000 --- a/autonomy/lib/src/simulator/drive.dart +++ /dev/null @@ -1,54 +0,0 @@ -// ignore_for_file: cascade_invocations - -import "package:autonomy/interfaces.dart"; -import "package:burt_network/protobuf.dart"; - -class DriveSimulator extends DriveInterface { - final bool shouldDelay; - DriveSimulator({required super.collection, this.shouldDelay = false}); - - @override - Future init() async => true; - - @override - Future dispose() async { } - - @override - Future goForward() async { - final position = collection.gps.coordinates; - final orientation = collection.imu.orientation; - final newPosition = position.goForward(orientation!); - collection.logger.debug("Going forward"); - collection.logger.trace(" Old position: ${position.prettyPrint()}"); - collection.logger.trace(" Orientation: $orientation"); - collection.logger.trace(" New position: ${newPosition.prettyPrint()}"); - collection.gps.update(newPosition); - if (shouldDelay) await Future.delayed(const Duration(milliseconds: 500)); - } - - @override - Future faceNorth() async { - collection.imu.update(OrientationUtils.north); - } - - @override - Future turnLeft() async { - collection.logger.debug("Turning left"); - final heading = collection.imu.heading; - final orientation = Orientation(z: heading + 90); - collection.imu.update(orientation); - if (shouldDelay) await Future.delayed(const Duration(milliseconds: 500)); - } - - @override - Future turnRight() async { - collection.logger.debug("Turning right"); - final heading = collection.imu.heading; - final orientation = Orientation(z: heading - 90); - collection.imu.update(orientation); - if (shouldDelay) await Future.delayed(const Duration(milliseconds: 500)); - } - - @override - Future stop() async => collection.logger.debug("Stopping"); -} diff --git a/autonomy/lib/src/simulator/gps.dart b/autonomy/lib/src/simulator/gps.dart deleted file mode 100644 index 0062db25..00000000 --- a/autonomy/lib/src/simulator/gps.dart +++ /dev/null @@ -1,28 +0,0 @@ -import "package:burt_network/protobuf.dart"; -import "package:autonomy/interfaces.dart"; - -class GpsSimulator extends GpsInterface with ValueReporter { - final RandomError _error; - GpsSimulator({required super.collection, double maxError = 0}) : - _error = RandomError(maxError); - - @override - RoverPosition getMessage() => RoverPosition(gps: coordinates); - - GpsCoordinates _coordinates = GpsCoordinates(); - - @override - GpsCoordinates get coordinates => GpsCoordinates( - latitude: _coordinates.latitude + _error.value, - longitude: _coordinates.longitude + _error.value, - ); - - @override - void update(GpsCoordinates newValue) => _coordinates = newValue; - - @override - Future init() async { - hasValue = true; - return super.init(); - } -} diff --git a/autonomy/lib/src/simulator/imu.dart b/autonomy/lib/src/simulator/imu.dart deleted file mode 100644 index 3c4cbacc..00000000 --- a/autonomy/lib/src/simulator/imu.dart +++ /dev/null @@ -1,35 +0,0 @@ -import "package:burt_network/protobuf.dart"; -import "package:autonomy/interfaces.dart"; - -class ImuSimulator extends ImuInterface with ValueReporter { - final RandomError _error; - ImuSimulator({required super.collection, double maxError = 0}) : - _error = RandomError(maxError); - - @override - RoverPosition getMessage() => RoverPosition(orientation: raw); - - Orientation _orientation = Orientation(); - - @override - Orientation get raw => Orientation( - x: _orientation.x + _error.value, - y: _orientation.y + _error.value, - z: _orientation.z + _error.value, - ); - - @override - void update(Orientation newValue) => _orientation = newValue.clampHeading(); - - @override - Future init() async { - hasValue = true; - return super.init(); - } - - @override - Future dispose() async { - _orientation = Orientation(); - await super.dispose(); - } -} diff --git a/autonomy/lib/src/simulator/orchestrator.dart b/autonomy/lib/src/simulator/orchestrator.dart deleted file mode 100644 index 56420d6b..00000000 --- a/autonomy/lib/src/simulator/orchestrator.dart +++ /dev/null @@ -1,32 +0,0 @@ -import "package:autonomy/interfaces.dart"; -import "package:burt_network/protobuf.dart"; - -class OrchestratorSimulator extends OrchestratorInterface { - OrchestratorSimulator({required super.collection}); - - @override - Future dispose() async { } - - @override - AutonomyData get statusMessage => AutonomyData(); - - @override - Future handleGpsTask(AutonomyCommand command) async { - - } - - @override - Future handleArucoTask(AutonomyCommand command) async { - - } - - @override - Future handleHammerTask(AutonomyCommand command) async { - - } - - @override - Future handleBottleTask(AutonomyCommand command) async { - - } -} diff --git a/autonomy/lib/src/simulator/pathfinding.dart b/autonomy/lib/src/simulator/pathfinding.dart deleted file mode 100644 index 155f20e0..00000000 --- a/autonomy/lib/src/simulator/pathfinding.dart +++ /dev/null @@ -1,25 +0,0 @@ -import "package:autonomy/interfaces.dart"; -import "package:burt_network/protobuf.dart"; - -class PathfindingSimulator extends PathfindingInterface { - static int i = 0; - - PathfindingSimulator({required super.collection}); - - @override - Future init() async => true; - - @override - List getPath(GpsCoordinates destination, {bool verbose = false}) => [ - AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (0, 0).toGps(), orientation: DriveOrientation.north, direction: DriveDirection.right, collection: collection), - AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (0, 0).toGps(), orientation: DriveOrientation.east, direction: DriveDirection.forward, collection: collection), - AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (0, 1).toGps(), orientation: DriveOrientation.east, direction: DriveDirection.forward, collection: collection), - AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (0, 2).toGps(), orientation: DriveOrientation.east, direction: DriveDirection.left, collection: collection), - AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (0, 2).toGps(), orientation: DriveOrientation.north, direction: DriveDirection.forward, collection: collection), - AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (1, 2).toGps(), orientation: DriveOrientation.north, direction: DriveDirection.forward, collection: collection), - AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (2, 2).toGps(), orientation: DriveOrientation.north, direction: DriveDirection.left, collection: collection), - AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (2, 2).toGps(), orientation: DriveOrientation.west, direction: DriveDirection.forward, collection: collection), - AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (2, 1).toGps(), orientation: DriveOrientation.west, direction: DriveDirection.right, collection: collection), - AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (2, 1).toGps(), orientation: DriveOrientation.north, direction: DriveDirection.forward, collection: collection), - ]; -} diff --git a/autonomy/lib/src/simulator/realsense.dart b/autonomy/lib/src/simulator/realsense.dart deleted file mode 100644 index f4251028..00000000 --- a/autonomy/lib/src/simulator/realsense.dart +++ /dev/null @@ -1,22 +0,0 @@ -import "dart:typed_data"; - -import "package:autonomy/interfaces.dart"; -import "package:burt_network/protobuf.dart"; - -class VideoSimulator extends VideoInterface { - VideoSimulator({required super.collection}); - - @override - Future init() async { - hasValue = true; - return true; - } - - @override - Future dispose() async => depthFrame = Uint16List.fromList([]); - - Uint16List depthFrame = Uint16List.fromList([]); - - @override - void updateFrame(VideoData newData) { } -} diff --git a/autonomy/test/sensor_test.dart b/autonomy/test/sensor_test.dart deleted file mode 100644 index 6375b9eb..00000000 --- a/autonomy/test/sensor_test.dart +++ /dev/null @@ -1,246 +0,0 @@ -import "package:autonomy/src/rover/imu.dart"; -import "package:burt_network/logging.dart"; -import "package:test/test.dart"; - -import "package:burt_network/protobuf.dart"; - -import "package:autonomy/interfaces.dart"; -import "package:autonomy/simulator.dart"; -import "package:autonomy/src/rover/gps.dart"; - -const imuError = 2.5; -const gpsPrecision = 7; - -void main() => group("[Sensors]", tags: ["sensors"], () { - setUp(() => Logger.level = LogLevel.off); - tearDown(() => Logger.level = LogLevel.off); - - test("GpsUtils.isNear", () { - final origin = (0, 0).toGps(); - expect(GpsCoordinates(latitude: 0, longitude: 0), origin); - expect(origin.isNear(origin), isTrue); - - final a = (5, 5).toGps(); - final a2 = (5, 5).toGps(); - final b = (5, 6.5).toGps(); - - expect(a.isNear(a), isTrue); - expect(a.isNear(a2), isTrue); - expect(a.isNear(b), isFalse); - - final c = GpsCoordinates(latitude: 0, longitude: 0); - final d = GpsCoordinates(latitude: 45.123, longitude: 72.123); - final e = GpsCoordinates(latitude: 12.123, longitude: 24.123); - - expect(c.isNear(d), isFalse); - expect(c.isNear(e), isFalse); - expect(d.isNear(e), isFalse); - - final f = (12, 12).toGps(); - final g = (12.2, 12.2).toGps(); - expect(f.isNear(g), isTrue); - }); - - test("GPS noise when stationary", () async { - // Set up a simulated and real GPS, both starting at (0, 0) - final oldError = GpsUtils.maxErrorMeters; - GpsUtils.maxErrorMeters = 3; - Logger.level = LogLevel.off; - - final simulator = AutonomySimulator(); - final realGps = RoverGps(collection: simulator); - final simulatedGps = GpsSimulator(collection: simulator, maxError: GpsInterface.gpsError); - final origin = GpsCoordinates(); - simulatedGps.update(origin); - realGps.update(origin); - expect(realGps.isNear(origin), isTrue); - - // Feed many noisy signals to produce a cleaner signal. - var count = 0; - for (var step = 0; step < 1000; step++) { - realGps.update(simulatedGps.coordinates); - simulator.logger.trace("New coordinate: ${realGps.coordinates.latitude.toStringAsFixed(gpsPrecision)} vs real position: ${origin.latitude.toStringAsFixed(gpsPrecision)}"); - if (step < 10) continue; - if (realGps.isNear(origin)) count++; - } - - final percentage = count / 1000; - simulator.logger.info("Final coordinates: ${realGps.coordinates.latitude}"); - expect(percentage, greaterThan(0.75), reason: "GPS should be accurate >75% of the time: $percentage"); - - // Ensure that *very* noisy readings don't affect anything. - simulator.logger.debug("Adding 100, 100"); - simulator.gps.update(GpsCoordinates(latitude: 100, longitude: 100)); - expect(realGps.isNear(origin), isTrue); - - await simulator.dispose(); - GpsUtils.maxErrorMeters = oldError; - }); - - test("IMU noise when stationary", () async { - Logger.level = LogLevel.off; - final simulator = AutonomySimulator(); - final simulatedImu = ImuSimulator(collection: simulator, maxError: imuError); - final realImu = RoverImu(collection: simulator); - final north = OrientationUtils.north; - simulatedImu.update(north); - - var count = 0; - for (var i = 0; i < 1000; i++) { - final newData = simulatedImu.raw; - realImu.update(newData); - simulator.logger.trace("Got new value: ${newData.heading}"); - simulator.logger.trace(" New heading: ${realImu.heading}"); - simulator.logger.trace(" Real position: ${north.heading}"); - if (i < 10) continue; - simulator.logger.trace(" Values are similar: ${realImu.isNear(north.heading)}"); - if (realImu.isNear(north.heading)) count++; - } - - final percentage = count / 1000; - simulator.logger.info("Final orientation: ${realImu.heading}"); - expect(percentage, greaterThan(0.75), reason: "IMU should be accurate >75% of the time: $percentage"); - - - realImu.update(OrientationUtils.south); - expect(realImu.isNear(OrientationUtils.north.heading), isTrue); - await simulator.dispose(); - }); - - test("GPS noise when moving", () async { - // Set up a simulated and real GPS, both starting at (0, 0) - final oldError = GpsUtils.maxErrorMeters; - GpsUtils.maxErrorMeters = 3; - final simulator = AutonomySimulator(); - final realGps = RoverGps(collection: simulator); - final simulatedGps = GpsSimulator(collection: simulator, maxError: GpsInterface.gpsError); - var realCoordinates = GpsCoordinates(); - simulatedGps.update(realCoordinates); - realGps.update(realCoordinates); - expect(realGps.coordinates.isNear(realCoordinates), isTrue); - - // For each step forward, use the noisy GPS to update the real GPS. - var count = 0; - for (var step = 0; step < 1000; step++) { - realCoordinates += GpsUtils.north; - simulatedGps.update(realCoordinates); - realGps.update(simulatedGps.coordinates); - simulator.logger.trace("New coordinate: ${realGps.coordinates.latitude.toStringAsFixed(5)} vs real position: ${realCoordinates.latitude.toStringAsFixed(5)}"); - simulator.logger.trace(" Difference: ${(realGps.latitude - realCoordinates.latitude).abs().toStringAsFixed(5)} < ${GpsUtils.epsilonLatitude.toStringAsFixed(5)}"); - if (step < 10) continue; - if (realGps.isNear(realCoordinates)) count++; - } - - final percentage = count / 1000; - simulator.logger.info("Final coordinates: ${realGps.coordinates.prettyPrint()} vs actual: ${realCoordinates.prettyPrint()}"); - expect(percentage, greaterThan(0.75), reason: "GPS should be accurate >75% of the time: $percentage"); - - // Ensure that *very* noisy readings don't affect anything. - simulator.logger.debug("Adding 100, 100"); - simulator.gps.update(GpsCoordinates(latitude: 100, longitude: 100)); - expect(realGps.isNear(realCoordinates), isTrue); - GpsUtils.maxErrorMeters = oldError; - }); - - test("IMU noise when moving", () async { - Logger.level = LogLevel.off; - final simulator = AutonomySimulator(); - final simulatedImu = ImuSimulator(collection: simulator, maxError: imuError); - final realImu = RoverImu(collection: simulator); - final orientation = OrientationUtils.north; - simulatedImu.update(orientation); - - var count = 0; - for (var i = 0; i < 350; i++) { - orientation.z += 1; - simulatedImu.update(orientation); - final newData = simulatedImu.raw; - realImu.update(newData); - simulator.logger.trace("Got new value: ${newData.heading}"); - simulator.logger.trace(" New heading: ${realImu.heading}"); - simulator.logger.trace(" Real position: ${orientation.heading}"); - if (i < 10) continue; - simulator.logger.trace(" Values are similar: ${realImu.isNear(orientation.heading)}"); - if (realImu.isNear(orientation.heading)) count++; - } - - final percentage = count / 350; - expect(percentage, greaterThan(0.75), reason: "IMU should be accurate >75% of the time: $percentage"); - - final badData = Orientation(z: 10); - simulator.logger.info("Final orientation: ${realImu.heading}"); - simulator.logger.info("Bad orientation: ${badData.heading}"); - realImu.update(badData); - simulator.logger.info("Unaffected orientation: ${realImu.heading}"); - expect(realImu.isNear(orientation.heading), isTrue); - await simulator.dispose(); - }); - - test("GPS latitude is set properly", () async { - final simulator = AutonomySimulator(); - const utahLatitude = 38.406683; - final utah = GpsCoordinates(latitude: utahLatitude); - - simulator.gps.update(utah); - expect(simulator.hasValue, isFalse); - expect(GpsInterface.currentLatitude, 0); - - await simulator.init(); - await simulator.waitForValue(); - expect(simulator.hasValue, isTrue); - expect(GpsInterface.currentLatitude, utahLatitude); - - await simulator.dispose(); - GpsInterface.currentLatitude = 0; - }); - - test("GPS can start in a real location", () async { - final simulator = AutonomySimulator(); - final ocean = GpsCoordinates(latitude: 0, longitude: 0); - final newYork = GpsCoordinates(latitude: 45, longitude: 72); - simulator.gps = RoverGps(collection: simulator); - - await simulator.init(); - expect(simulator.gps.coordinates, ocean); - expect(simulator.gps.isNear(newYork), isFalse); - expect(ocean.isNear(newYork), isFalse); - - simulator.gps.update(newYork); - expect(simulator.gps.isNear(ocean), isFalse); - expect(simulator.gps.isNear(newYork), isTrue); - - await simulator.dispose(); - }); - - test("IMU can handle values on the edge", () async { - Logger.level = LogLevel.off; - final simulator = AutonomySimulator(); - final simulatedImu = ImuSimulator(collection: simulator, maxError: imuError); - final realImu = RoverImu(collection: simulator); - final orientation = Orientation(z: 360); - simulatedImu.update(orientation); - - var count = 0; - for (var i = 0; i < 1000; i++) { - final newData = simulatedImu.raw; - realImu.update(newData); - simulator.logger.trace("Got new value: ${newData.heading}"); - simulator.logger.trace(" New heading: ${realImu.heading}"); - simulator.logger.trace(" Real position: ${orientation.heading}"); - if (i < 10) continue; - simulator.logger.trace(" Values are similar: ${realImu.isNear(orientation.heading)}"); - if (realImu.isNear(orientation.heading)) count++; - } - - final percentage = count / 1000; - expect(percentage, greaterThan(0.75), reason: "IMU should be accurate >75% of the time: $percentage"); - - final badData = Orientation(z: 10); - simulator.logger.info("Final orientation: ${realImu.heading}"); - simulator.logger.info("Bad orientation: ${badData.heading}"); - realImu.update(badData); - simulator.logger.info("Unaffected orientation: ${realImu.heading}"); - expect(realImu.isNear(orientation.heading), isTrue); - await simulator.dispose(); - }); -}); From 364eaf4dc7908a7ae5a46572e22a52cd71b895dc Mon Sep 17 00:00:00 2001 From: preston Date: Sun, 1 Feb 2026 13:00:57 -0500 Subject: [PATCH 11/16] Create Camera Streaming Stuff or something --- arm_auxillary/lib/src/arm_camera_manager.dart | 191 +++++++ arm_auxillary/pubspec.yaml | 3 +- burt_network/Protobuf | 2 +- burt_network/lib/src/generated/arm.pb.dart | 515 +++++++++++------- .../lib/src/generated/arm.pbjson.dart | 104 ++-- .../google/protobuf/timestamp.pb.dart | 198 +++++++ .../google/protobuf/timestamp.pbenum.dart | 11 + .../google/protobuf/timestamp.pbjson.dart | 30 + burt_network/lib/src/generated/video.pb.dart | 14 + .../lib/src/generated/video.pbenum.dart | 11 +- .../lib/src/generated/video.pbjson.dart | 14 +- 11 files changed, 841 insertions(+), 252 deletions(-) create mode 100644 arm_auxillary/lib/src/arm_camera_manager.dart create mode 100644 burt_network/lib/src/generated/google/protobuf/timestamp.pb.dart create mode 100644 burt_network/lib/src/generated/google/protobuf/timestamp.pbenum.dart create mode 100644 burt_network/lib/src/generated/google/protobuf/timestamp.pbjson.dart diff --git a/arm_auxillary/lib/src/arm_camera_manager.dart b/arm_auxillary/lib/src/arm_camera_manager.dart new file mode 100644 index 00000000..9b1daebe --- /dev/null +++ b/arm_auxillary/lib/src/arm_camera_manager.dart @@ -0,0 +1,191 @@ +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(); + + /// Stream subscriptions for cleanup + StreamSubscription? _commands; + StreamSubscription? _data; + + /// Reference to the server for sending messages + RoverSocket? _server; + + @override + Future 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 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.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.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( + name: VideoCommand().messageName, + constructor: VideoCommand.fromBuffer, + callback: _handleCommand, + ); + + logger.info("Arm camera manager connected to server"); + } + + /// Spawns camera isolates for detected arm cameras + Future _spawnArmCameras() async { + logger.info("Detecting arm cameras..."); + + final armCameraNames = [ + 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); + 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); + } + + 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"); + } +} \ No newline at end of file diff --git a/arm_auxillary/pubspec.yaml b/arm_auxillary/pubspec.yaml index c585a6a9..d5477c75 100644 --- a/arm_auxillary/pubspec.yaml +++ b/arm_auxillary/pubspec.yaml @@ -10,9 +10,10 @@ environment: # Add regular dependencies here. dependencies: - burt_network: ^2.7.0 + burt_network: ^2.8.0 collection: ^1.19.0 subsystems: ^1.0.0 + video: ^1.1.0 dev_dependencies: very_good_analysis: ^6.0.0 diff --git a/burt_network/Protobuf b/burt_network/Protobuf index 7e77da8a..4a64299e 160000 --- a/burt_network/Protobuf +++ b/burt_network/Protobuf @@ -1 +1 @@ -Subproject commit 7e77da8a2c9d9ce2cd6e6751a6aa777af5e257ca +Subproject commit 4a64299eed7196bcd46dea9b24104a90e2abb258 diff --git a/burt_network/lib/src/generated/arm.pb.dart b/burt_network/lib/src/generated/arm.pb.dart index a2dd96b2..9fa972d8 100644 --- a/burt_network/lib/src/generated/arm.pb.dart +++ b/burt_network/lib/src/generated/arm.pb.dart @@ -29,12 +29,6 @@ class ArmData extends $pb.GeneratedMessage { $1.MotorData? shoulder, $1.MotorData? elbow, $2.Version? version, - $core.double? uSSDistance, - $1.MotorData? lift, - $1.MotorData? rotate, - $1.MotorData? pinch, - $core.int? servoAngle, - $3.BoolState? laserState, }) { final result = create(); if (currentPosition != null) result.currentPosition = currentPosition; @@ -43,12 +37,6 @@ class ArmData extends $pb.GeneratedMessage { if (shoulder != null) result.shoulder = shoulder; if (elbow != null) result.elbow = elbow; if (version != null) result.version = version; - if (uSSDistance != null) result.uSSDistance = uSSDistance; - if (lift != null) result.lift = lift; - if (rotate != null) result.rotate = rotate; - if (pinch != null) result.pinch = pinch; - if (servoAngle != null) result.servoAngle = servoAngle; - if (laserState != null) result.laserState = laserState; return result; } @@ -76,17 +64,6 @@ class ArmData extends $pb.GeneratedMessage { subBuilder: $1.MotorData.create) ..aOM<$2.Version>(6, _omitFieldNames ? '' : 'version', subBuilder: $2.Version.create) - ..aD(7, _omitFieldNames ? '' : 'USSDistance', - protoName: 'USS_distance', fieldType: $pb.PbFieldType.OF) - ..aOM<$1.MotorData>(8, _omitFieldNames ? '' : 'lift', - subBuilder: $1.MotorData.create) - ..aOM<$1.MotorData>(9, _omitFieldNames ? '' : 'rotate', - subBuilder: $1.MotorData.create) - ..aOM<$1.MotorData>(10, _omitFieldNames ? '' : 'pinch', - subBuilder: $1.MotorData.create) - ..aI(11, _omitFieldNames ? '' : 'servoAngle', protoName: 'servoAngle') - ..aE<$3.BoolState>(12, _omitFieldNames ? '' : 'laserState', - protoName: 'laserState', enumValues: $3.BoolState.values) ..hasRequiredFields = false; @$core.Deprecated('See https://github.com/google/protobuf.dart/issues/998.') @@ -107,7 +84,6 @@ class ArmData extends $pb.GeneratedMessage { _defaultInstance ??= $pb.GeneratedMessage.$_defaultFor(create); static ArmData? _defaultInstance; - /// Arm Commands @$pb.TagNumber(1) $0.Coordinates get currentPosition => $_getN(0); @$pb.TagNumber(1) @@ -173,68 +149,6 @@ class ArmData extends $pb.GeneratedMessage { void clearVersion() => $_clearField(6); @$pb.TagNumber(6) $2.Version ensureVersion() => $_ensure(5); - - /// USS data - @$pb.TagNumber(7) - $core.double get uSSDistance => $_getN(6); - @$pb.TagNumber(7) - set uSSDistance($core.double value) => $_setFloat(6, value); - @$pb.TagNumber(7) - $core.bool hasUSSDistance() => $_has(6); - @$pb.TagNumber(7) - void clearUSSDistance() => $_clearField(7); - - /// Gripper Commands - @$pb.TagNumber(8) - $1.MotorData get lift => $_getN(7); - @$pb.TagNumber(8) - set lift($1.MotorData value) => $_setField(8, value); - @$pb.TagNumber(8) - $core.bool hasLift() => $_has(7); - @$pb.TagNumber(8) - void clearLift() => $_clearField(8); - @$pb.TagNumber(8) - $1.MotorData ensureLift() => $_ensure(7); - - @$pb.TagNumber(9) - $1.MotorData get rotate => $_getN(8); - @$pb.TagNumber(9) - set rotate($1.MotorData value) => $_setField(9, value); - @$pb.TagNumber(9) - $core.bool hasRotate() => $_has(8); - @$pb.TagNumber(9) - void clearRotate() => $_clearField(9); - @$pb.TagNumber(9) - $1.MotorData ensureRotate() => $_ensure(8); - - @$pb.TagNumber(10) - $1.MotorData get pinch => $_getN(9); - @$pb.TagNumber(10) - set pinch($1.MotorData value) => $_setField(10, value); - @$pb.TagNumber(10) - $core.bool hasPinch() => $_has(9); - @$pb.TagNumber(10) - void clearPinch() => $_clearField(10); - @$pb.TagNumber(10) - $1.MotorData ensurePinch() => $_ensure(9); - - @$pb.TagNumber(11) - $core.int get servoAngle => $_getIZ(10); - @$pb.TagNumber(11) - set servoAngle($core.int value) => $_setSignedInt32(10, value); - @$pb.TagNumber(11) - $core.bool hasServoAngle() => $_has(10); - @$pb.TagNumber(11) - void clearServoAngle() => $_clearField(11); - - @$pb.TagNumber(12) - $3.BoolState get laserState => $_getN(11); - @$pb.TagNumber(12) - set laserState($3.BoolState value) => $_setField(12, value); - @$pb.TagNumber(12) - $core.bool hasLaserState() => $_has(11); - @$pb.TagNumber(12) - void clearLaserState() => $_clearField(12); } class ArmCommand extends $pb.GeneratedMessage { @@ -250,15 +164,6 @@ class ArmCommand extends $pb.GeneratedMessage { $core.double? ikZ, $core.bool? jab, $2.Version? version, - $3.BoolState? startUSS, - $1.MotorCommand? lift, - $1.MotorCommand? rotate, - $1.MotorCommand? pinch, - $core.bool? open, - $core.bool? close, - $core.bool? spin, - $core.int? servoAngle, - $3.BoolState? laserState, }) { final result = create(); if (stop != null) result.stop = stop; @@ -272,15 +177,6 @@ class ArmCommand extends $pb.GeneratedMessage { if (ikZ != null) result.ikZ = ikZ; if (jab != null) result.jab = jab; if (version != null) result.version = version; - if (startUSS != null) result.startUSS = startUSS; - if (lift != null) result.lift = lift; - if (rotate != null) result.rotate = rotate; - if (pinch != null) result.pinch = pinch; - if (open != null) result.open = open; - if (close != null) result.close = close; - if (spin != null) result.spin = spin; - if (servoAngle != null) result.servoAngle = servoAngle; - if (laserState != null) result.laserState = laserState; return result; } @@ -312,20 +208,6 @@ class ArmCommand extends $pb.GeneratedMessage { ..aOB(10, _omitFieldNames ? '' : 'jab') ..aOM<$2.Version>(11, _omitFieldNames ? '' : 'version', subBuilder: $2.Version.create) - ..aE<$3.BoolState>(12, _omitFieldNames ? '' : 'startUSS', - protoName: 'start_USS', enumValues: $3.BoolState.values) - ..aOM<$1.MotorCommand>(13, _omitFieldNames ? '' : 'lift', - subBuilder: $1.MotorCommand.create) - ..aOM<$1.MotorCommand>(14, _omitFieldNames ? '' : 'rotate', - subBuilder: $1.MotorCommand.create) - ..aOM<$1.MotorCommand>(15, _omitFieldNames ? '' : 'pinch', - subBuilder: $1.MotorCommand.create) - ..aOB(16, _omitFieldNames ? '' : 'open') - ..aOB(17, _omitFieldNames ? '' : 'close') - ..aOB(18, _omitFieldNames ? '' : 'spin') - ..aI(19, _omitFieldNames ? '' : 'servoAngle', protoName: 'servoAngle') - ..aE<$3.BoolState>(20, _omitFieldNames ? '' : 'laserState', - protoName: 'laserState', enumValues: $3.BoolState.values) ..hasRequiredFields = false; @$core.Deprecated('See https://github.com/google/protobuf.dart/issues/998.') @@ -346,7 +228,7 @@ class ArmCommand extends $pb.GeneratedMessage { $pb.GeneratedMessage.$_defaultFor(create); static ArmCommand? _defaultInstance; - /// General commands for arm + /// General commands @$pb.TagNumber(1) $core.bool get stop => $_getBF(0); @$pb.TagNumber(1) @@ -460,96 +342,321 @@ class ArmCommand extends $pb.GeneratedMessage { void clearVersion() => $_clearField(11); @$pb.TagNumber(11) $2.Version ensureVersion() => $_ensure(10); +} + +class GripperData extends $pb.GeneratedMessage { + factory GripperData({ + $1.MotorData? lift, + $1.MotorData? rotate, + $1.MotorData? pinch, + $2.Version? version, + $core.int? servoAngle, + $3.BoolState? laserState, + }) { + final result = create(); + if (lift != null) result.lift = lift; + if (rotate != null) result.rotate = rotate; + if (pinch != null) result.pinch = pinch; + if (version != null) result.version = version; + if (servoAngle != null) result.servoAngle = servoAngle; + if (laserState != null) result.laserState = laserState; + return result; + } + + GripperData._(); + + factory GripperData.fromBuffer($core.List<$core.int> data, + [$pb.ExtensionRegistry registry = $pb.ExtensionRegistry.EMPTY]) => + create()..mergeFromBuffer(data, registry); + factory GripperData.fromJson($core.String json, + [$pb.ExtensionRegistry registry = $pb.ExtensionRegistry.EMPTY]) => + create()..mergeFromJson(json, registry); + + static final $pb.BuilderInfo _i = $pb.BuilderInfo( + _omitMessageNames ? '' : 'GripperData', + createEmptyInstance: create) + ..aOM<$1.MotorData>(1, _omitFieldNames ? '' : 'lift', + subBuilder: $1.MotorData.create) + ..aOM<$1.MotorData>(2, _omitFieldNames ? '' : 'rotate', + subBuilder: $1.MotorData.create) + ..aOM<$1.MotorData>(3, _omitFieldNames ? '' : 'pinch', + subBuilder: $1.MotorData.create) + ..aOM<$2.Version>(4, _omitFieldNames ? '' : 'version', + subBuilder: $2.Version.create) + ..aI(5, _omitFieldNames ? '' : 'servoAngle', protoName: 'servoAngle') + ..aE<$3.BoolState>(6, _omitFieldNames ? '' : 'laserState', + protoName: 'laserState', enumValues: $3.BoolState.values) + ..hasRequiredFields = false; + + @$core.Deprecated('See https://github.com/google/protobuf.dart/issues/998.') + GripperData clone() => deepCopy(); + @$core.Deprecated('See https://github.com/google/protobuf.dart/issues/998.') + GripperData copyWith(void Function(GripperData) updates) => + super.copyWith((message) => updates(message as GripperData)) + as GripperData; + + @$core.override + $pb.BuilderInfo get info_ => _i; + + @$core.pragma('dart2js:noInline') + static GripperData create() => GripperData._(); + @$core.override + GripperData createEmptyInstance() => create(); + @$core.pragma('dart2js:noInline') + static GripperData getDefault() => _defaultInstance ??= + $pb.GeneratedMessage.$_defaultFor(create); + static GripperData? _defaultInstance; + + @$pb.TagNumber(1) + $1.MotorData get lift => $_getN(0); + @$pb.TagNumber(1) + set lift($1.MotorData value) => $_setField(1, value); + @$pb.TagNumber(1) + $core.bool hasLift() => $_has(0); + @$pb.TagNumber(1) + void clearLift() => $_clearField(1); + @$pb.TagNumber(1) + $1.MotorData ensureLift() => $_ensure(0); + + @$pb.TagNumber(2) + $1.MotorData get rotate => $_getN(1); + @$pb.TagNumber(2) + set rotate($1.MotorData value) => $_setField(2, value); + @$pb.TagNumber(2) + $core.bool hasRotate() => $_has(1); + @$pb.TagNumber(2) + void clearRotate() => $_clearField(2); + @$pb.TagNumber(2) + $1.MotorData ensureRotate() => $_ensure(1); + + @$pb.TagNumber(3) + $1.MotorData get pinch => $_getN(2); + @$pb.TagNumber(3) + set pinch($1.MotorData value) => $_setField(3, value); + @$pb.TagNumber(3) + $core.bool hasPinch() => $_has(2); + @$pb.TagNumber(3) + void clearPinch() => $_clearField(3); + @$pb.TagNumber(3) + $1.MotorData ensurePinch() => $_ensure(2); + + @$pb.TagNumber(4) + $2.Version get version => $_getN(3); + @$pb.TagNumber(4) + set version($2.Version value) => $_setField(4, value); + @$pb.TagNumber(4) + $core.bool hasVersion() => $_has(3); + @$pb.TagNumber(4) + void clearVersion() => $_clearField(4); + @$pb.TagNumber(4) + $2.Version ensureVersion() => $_ensure(3); - /// USS commands - @$pb.TagNumber(12) - $3.BoolState get startUSS => $_getN(11); - @$pb.TagNumber(12) - set startUSS($3.BoolState value) => $_setField(12, value); - @$pb.TagNumber(12) - $core.bool hasStartUSS() => $_has(11); - @$pb.TagNumber(12) - void clearStartUSS() => $_clearField(12); + @$pb.TagNumber(5) + $core.int get servoAngle => $_getIZ(4); + @$pb.TagNumber(5) + set servoAngle($core.int value) => $_setSignedInt32(4, value); + @$pb.TagNumber(5) + $core.bool hasServoAngle() => $_has(4); + @$pb.TagNumber(5) + void clearServoAngle() => $_clearField(5); + + @$pb.TagNumber(6) + $3.BoolState get laserState => $_getN(5); + @$pb.TagNumber(6) + set laserState($3.BoolState value) => $_setField(6, value); + @$pb.TagNumber(6) + $core.bool hasLaserState() => $_has(5); + @$pb.TagNumber(6) + void clearLaserState() => $_clearField(6); +} + +class GripperCommand extends $pb.GeneratedMessage { + factory GripperCommand({ + $core.bool? stop, + $core.bool? calibrate, + $1.MotorCommand? lift, + $1.MotorCommand? rotate, + $1.MotorCommand? pinch, + $core.bool? open, + $core.bool? close, + $core.bool? spin, + $2.Version? version, + $core.int? servoAngle, + $3.BoolState? laserState, + }) { + final result = create(); + if (stop != null) result.stop = stop; + if (calibrate != null) result.calibrate = calibrate; + if (lift != null) result.lift = lift; + if (rotate != null) result.rotate = rotate; + if (pinch != null) result.pinch = pinch; + if (open != null) result.open = open; + if (close != null) result.close = close; + if (spin != null) result.spin = spin; + if (version != null) result.version = version; + if (servoAngle != null) result.servoAngle = servoAngle; + if (laserState != null) result.laserState = laserState; + return result; + } + + GripperCommand._(); + + factory GripperCommand.fromBuffer($core.List<$core.int> data, + [$pb.ExtensionRegistry registry = $pb.ExtensionRegistry.EMPTY]) => + create()..mergeFromBuffer(data, registry); + factory GripperCommand.fromJson($core.String json, + [$pb.ExtensionRegistry registry = $pb.ExtensionRegistry.EMPTY]) => + create()..mergeFromJson(json, registry); + + static final $pb.BuilderInfo _i = $pb.BuilderInfo( + _omitMessageNames ? '' : 'GripperCommand', + createEmptyInstance: create) + ..aOB(1, _omitFieldNames ? '' : 'stop') + ..aOB(2, _omitFieldNames ? '' : 'calibrate') + ..aOM<$1.MotorCommand>(3, _omitFieldNames ? '' : 'lift', + subBuilder: $1.MotorCommand.create) + ..aOM<$1.MotorCommand>(4, _omitFieldNames ? '' : 'rotate', + subBuilder: $1.MotorCommand.create) + ..aOM<$1.MotorCommand>(5, _omitFieldNames ? '' : 'pinch', + subBuilder: $1.MotorCommand.create) + ..aOB(6, _omitFieldNames ? '' : 'open') + ..aOB(7, _omitFieldNames ? '' : 'close') + ..aOB(8, _omitFieldNames ? '' : 'spin') + ..aOM<$2.Version>(9, _omitFieldNames ? '' : 'version', + subBuilder: $2.Version.create) + ..aI(10, _omitFieldNames ? '' : 'servoAngle', protoName: 'servoAngle') + ..aE<$3.BoolState>(11, _omitFieldNames ? '' : 'laserState', + protoName: 'laserState', enumValues: $3.BoolState.values) + ..hasRequiredFields = false; + + @$core.Deprecated('See https://github.com/google/protobuf.dart/issues/998.') + GripperCommand clone() => deepCopy(); + @$core.Deprecated('See https://github.com/google/protobuf.dart/issues/998.') + GripperCommand copyWith(void Function(GripperCommand) updates) => + super.copyWith((message) => updates(message as GripperCommand)) + as GripperCommand; + + @$core.override + $pb.BuilderInfo get info_ => _i; + + @$core.pragma('dart2js:noInline') + static GripperCommand create() => GripperCommand._(); + @$core.override + GripperCommand createEmptyInstance() => create(); + @$core.pragma('dart2js:noInline') + static GripperCommand getDefault() => _defaultInstance ??= + $pb.GeneratedMessage.$_defaultFor(create); + static GripperCommand? _defaultInstance; + + /// General commands + @$pb.TagNumber(1) + $core.bool get stop => $_getBF(0); + @$pb.TagNumber(1) + set stop($core.bool value) => $_setBool(0, value); + @$pb.TagNumber(1) + $core.bool hasStop() => $_has(0); + @$pb.TagNumber(1) + void clearStop() => $_clearField(1); + + @$pb.TagNumber(2) + $core.bool get calibrate => $_getBF(1); + @$pb.TagNumber(2) + set calibrate($core.bool value) => $_setBool(1, value); + @$pb.TagNumber(2) + $core.bool hasCalibrate() => $_has(1); + @$pb.TagNumber(2) + void clearCalibrate() => $_clearField(2); /// Move individual motors - @$pb.TagNumber(13) - $1.MotorCommand get lift => $_getN(12); - @$pb.TagNumber(13) - set lift($1.MotorCommand value) => $_setField(13, value); - @$pb.TagNumber(13) - $core.bool hasLift() => $_has(12); - @$pb.TagNumber(13) - void clearLift() => $_clearField(13); - @$pb.TagNumber(13) - $1.MotorCommand ensureLift() => $_ensure(12); - - @$pb.TagNumber(14) - $1.MotorCommand get rotate => $_getN(13); - @$pb.TagNumber(14) - set rotate($1.MotorCommand value) => $_setField(14, value); - @$pb.TagNumber(14) - $core.bool hasRotate() => $_has(13); - @$pb.TagNumber(14) - void clearRotate() => $_clearField(14); - @$pb.TagNumber(14) - $1.MotorCommand ensureRotate() => $_ensure(13); - - @$pb.TagNumber(15) - $1.MotorCommand get pinch => $_getN(14); - @$pb.TagNumber(15) - set pinch($1.MotorCommand value) => $_setField(15, value); - @$pb.TagNumber(15) - $core.bool hasPinch() => $_has(14); - @$pb.TagNumber(15) - void clearPinch() => $_clearField(15); - @$pb.TagNumber(15) - $1.MotorCommand ensurePinch() => $_ensure(14); + @$pb.TagNumber(3) + $1.MotorCommand get lift => $_getN(2); + @$pb.TagNumber(3) + set lift($1.MotorCommand value) => $_setField(3, value); + @$pb.TagNumber(3) + $core.bool hasLift() => $_has(2); + @$pb.TagNumber(3) + void clearLift() => $_clearField(3); + @$pb.TagNumber(3) + $1.MotorCommand ensureLift() => $_ensure(2); + + @$pb.TagNumber(4) + $1.MotorCommand get rotate => $_getN(3); + @$pb.TagNumber(4) + set rotate($1.MotorCommand value) => $_setField(4, value); + @$pb.TagNumber(4) + $core.bool hasRotate() => $_has(3); + @$pb.TagNumber(4) + void clearRotate() => $_clearField(4); + @$pb.TagNumber(4) + $1.MotorCommand ensureRotate() => $_ensure(3); + + @$pb.TagNumber(5) + $1.MotorCommand get pinch => $_getN(4); + @$pb.TagNumber(5) + set pinch($1.MotorCommand value) => $_setField(5, value); + @$pb.TagNumber(5) + $core.bool hasPinch() => $_has(4); + @$pb.TagNumber(5) + void clearPinch() => $_clearField(5); + @$pb.TagNumber(5) + $1.MotorCommand ensurePinch() => $_ensure(4); /// Custom actions - @$pb.TagNumber(16) - $core.bool get open => $_getBF(15); - @$pb.TagNumber(16) - set open($core.bool value) => $_setBool(15, value); - @$pb.TagNumber(16) - $core.bool hasOpen() => $_has(15); - @$pb.TagNumber(16) - void clearOpen() => $_clearField(16); - - @$pb.TagNumber(17) - $core.bool get close => $_getBF(16); - @$pb.TagNumber(17) - set close($core.bool value) => $_setBool(16, value); - @$pb.TagNumber(17) - $core.bool hasClose() => $_has(16); - @$pb.TagNumber(17) - void clearClose() => $_clearField(17); - - @$pb.TagNumber(18) - $core.bool get spin => $_getBF(17); - @$pb.TagNumber(18) - set spin($core.bool value) => $_setBool(17, value); - @$pb.TagNumber(18) - $core.bool hasSpin() => $_has(17); - @$pb.TagNumber(18) - void clearSpin() => $_clearField(18); - - @$pb.TagNumber(19) - $core.int get servoAngle => $_getIZ(18); - @$pb.TagNumber(19) - set servoAngle($core.int value) => $_setSignedInt32(18, value); - @$pb.TagNumber(19) - $core.bool hasServoAngle() => $_has(18); - @$pb.TagNumber(19) - void clearServoAngle() => $_clearField(19); - - @$pb.TagNumber(20) - $3.BoolState get laserState => $_getN(19); - @$pb.TagNumber(20) - set laserState($3.BoolState value) => $_setField(20, value); - @$pb.TagNumber(20) - $core.bool hasLaserState() => $_has(19); - @$pb.TagNumber(20) - void clearLaserState() => $_clearField(20); + @$pb.TagNumber(6) + $core.bool get open => $_getBF(5); + @$pb.TagNumber(6) + set open($core.bool value) => $_setBool(5, value); + @$pb.TagNumber(6) + $core.bool hasOpen() => $_has(5); + @$pb.TagNumber(6) + void clearOpen() => $_clearField(6); + + @$pb.TagNumber(7) + $core.bool get close => $_getBF(6); + @$pb.TagNumber(7) + set close($core.bool value) => $_setBool(6, value); + @$pb.TagNumber(7) + $core.bool hasClose() => $_has(6); + @$pb.TagNumber(7) + void clearClose() => $_clearField(7); + + @$pb.TagNumber(8) + $core.bool get spin => $_getBF(7); + @$pb.TagNumber(8) + set spin($core.bool value) => $_setBool(7, value); + @$pb.TagNumber(8) + $core.bool hasSpin() => $_has(7); + @$pb.TagNumber(8) + void clearSpin() => $_clearField(8); + + @$pb.TagNumber(9) + $2.Version get version => $_getN(8); + @$pb.TagNumber(9) + set version($2.Version value) => $_setField(9, value); + @$pb.TagNumber(9) + $core.bool hasVersion() => $_has(8); + @$pb.TagNumber(9) + void clearVersion() => $_clearField(9); + @$pb.TagNumber(9) + $2.Version ensureVersion() => $_ensure(8); + + @$pb.TagNumber(10) + $core.int get servoAngle => $_getIZ(9); + @$pb.TagNumber(10) + set servoAngle($core.int value) => $_setSignedInt32(9, value); + @$pb.TagNumber(10) + $core.bool hasServoAngle() => $_has(9); + @$pb.TagNumber(10) + void clearServoAngle() => $_clearField(10); + + @$pb.TagNumber(11) + $3.BoolState get laserState => $_getN(10); + @$pb.TagNumber(11) + set laserState($3.BoolState value) => $_setField(11, value); + @$pb.TagNumber(11) + $core.bool hasLaserState() => $_has(10); + @$pb.TagNumber(11) + void clearLaserState() => $_clearField(11); } const $core.bool _omitFieldNames = diff --git a/burt_network/lib/src/generated/arm.pbjson.dart b/burt_network/lib/src/generated/arm.pbjson.dart index 279ff866..c824bc10 100644 --- a/burt_network/lib/src/generated/arm.pbjson.dart +++ b/burt_network/lib/src/generated/arm.pbjson.dart @@ -46,19 +46,6 @@ const ArmData$json = { }, {'1': 'elbow', '3': 5, '4': 1, '5': 11, '6': '.MotorData', '10': 'elbow'}, {'1': 'version', '3': 6, '4': 1, '5': 11, '6': '.Version', '10': 'version'}, - {'1': 'USS_distance', '3': 7, '4': 1, '5': 2, '10': 'USSDistance'}, - {'1': 'lift', '3': 8, '4': 1, '5': 11, '6': '.MotorData', '10': 'lift'}, - {'1': 'rotate', '3': 9, '4': 1, '5': 11, '6': '.MotorData', '10': 'rotate'}, - {'1': 'pinch', '3': 10, '4': 1, '5': 11, '6': '.MotorData', '10': 'pinch'}, - {'1': 'servoAngle', '3': 11, '4': 1, '5': 5, '10': 'servoAngle'}, - { - '1': 'laserState', - '3': 12, - '4': 1, - '5': 14, - '6': '.BoolState', - '10': 'laserState' - }, ], }; @@ -68,11 +55,7 @@ final $typed_data.Uint8List armDataDescriptor = $convert.base64Decode( '50UG9zaXRpb24SNAoOdGFyZ2V0UG9zaXRpb24YAiABKAsyDC5Db29yZGluYXRlc1IOdGFyZ2V0' 'UG9zaXRpb24SHgoEYmFzZRgDIAEoCzIKLk1vdG9yRGF0YVIEYmFzZRImCghzaG91bGRlchgEIA' 'EoCzIKLk1vdG9yRGF0YVIIc2hvdWxkZXISIAoFZWxib3cYBSABKAsyCi5Nb3RvckRhdGFSBWVs' - 'Ym93EiIKB3ZlcnNpb24YBiABKAsyCC5WZXJzaW9uUgd2ZXJzaW9uEiEKDFVTU19kaXN0YW5jZR' - 'gHIAEoAlILVVNTRGlzdGFuY2USHgoEbGlmdBgIIAEoCzIKLk1vdG9yRGF0YVIEbGlmdBIiCgZy' - 'b3RhdGUYCSABKAsyCi5Nb3RvckRhdGFSBnJvdGF0ZRIgCgVwaW5jaBgKIAEoCzIKLk1vdG9yRG' - 'F0YVIFcGluY2gSHgoKc2Vydm9BbmdsZRgLIAEoBVIKc2Vydm9BbmdsZRIqCgpsYXNlclN0YXRl' - 'GAwgASgOMgouQm9vbFN0YXRlUgpsYXNlclN0YXRl'); + 'Ym93EiIKB3ZlcnNpb24YBiABKAsyCC5WZXJzaW9uUgd2ZXJzaW9u'); @$core.Deprecated('Use armCommandDescriptor instead') const ArmCommand$json = { @@ -124,18 +107,57 @@ const ArmCommand$json = { '6': '.Version', '10': 'version' }, + ], +}; + +/// Descriptor for `ArmCommand`. Decode as a `google.protobuf.DescriptorProto`. +final $typed_data.Uint8List armCommandDescriptor = $convert.base64Decode( + 'CgpBcm1Db21tYW5kEhIKBHN0b3AYASABKAhSBHN0b3ASHAoJY2FsaWJyYXRlGAIgASgIUgljYW' + 'xpYnJhdGUSJQoGc3dpdmVsGAMgASgLMg0uTW90b3JDb21tYW5kUgZzd2l2ZWwSKQoIc2hvdWxk' + 'ZXIYBCABKAsyDS5Nb3RvckNvbW1hbmRSCHNob3VsZGVyEiMKBWVsYm93GAUgASgLMg0uTW90b3' + 'JDb21tYW5kUgVlbGJvdxIwCgxncmlwcGVyX2xpZnQYBiABKAsyDS5Nb3RvckNvbW1hbmRSC2dy' + 'aXBwZXJMaWZ0EhEKBGlrX3gYByABKAJSA2lrWBIRCgRpa195GAggASgCUgNpa1kSEQoEaWtfeh' + 'gJIAEoAlIDaWtaEhAKA2phYhgKIAEoCFIDamFiEiIKB3ZlcnNpb24YCyABKAsyCC5WZXJzaW9u' + 'Ugd2ZXJzaW9u'); + +@$core.Deprecated('Use gripperDataDescriptor instead') +const GripperData$json = { + '1': 'GripperData', + '2': [ + {'1': 'lift', '3': 1, '4': 1, '5': 11, '6': '.MotorData', '10': 'lift'}, + {'1': 'rotate', '3': 2, '4': 1, '5': 11, '6': '.MotorData', '10': 'rotate'}, + {'1': 'pinch', '3': 3, '4': 1, '5': 11, '6': '.MotorData', '10': 'pinch'}, + {'1': 'version', '3': 4, '4': 1, '5': 11, '6': '.Version', '10': 'version'}, + {'1': 'servoAngle', '3': 5, '4': 1, '5': 5, '10': 'servoAngle'}, { - '1': 'start_USS', - '3': 12, + '1': 'laserState', + '3': 6, '4': 1, '5': 14, '6': '.BoolState', - '10': 'startUSS' + '10': 'laserState' }, - {'1': 'lift', '3': 13, '4': 1, '5': 11, '6': '.MotorCommand', '10': 'lift'}, + ], +}; + +/// Descriptor for `GripperData`. Decode as a `google.protobuf.DescriptorProto`. +final $typed_data.Uint8List gripperDataDescriptor = $convert.base64Decode( + 'CgtHcmlwcGVyRGF0YRIeCgRsaWZ0GAEgASgLMgouTW90b3JEYXRhUgRsaWZ0EiIKBnJvdGF0ZR' + 'gCIAEoCzIKLk1vdG9yRGF0YVIGcm90YXRlEiAKBXBpbmNoGAMgASgLMgouTW90b3JEYXRhUgVw' + 'aW5jaBIiCgd2ZXJzaW9uGAQgASgLMgguVmVyc2lvblIHdmVyc2lvbhIeCgpzZXJ2b0FuZ2xlGA' + 'UgASgFUgpzZXJ2b0FuZ2xlEioKCmxhc2VyU3RhdGUYBiABKA4yCi5Cb29sU3RhdGVSCmxhc2Vy' + 'U3RhdGU='); + +@$core.Deprecated('Use gripperCommandDescriptor instead') +const GripperCommand$json = { + '1': 'GripperCommand', + '2': [ + {'1': 'stop', '3': 1, '4': 1, '5': 8, '10': 'stop'}, + {'1': 'calibrate', '3': 2, '4': 1, '5': 8, '10': 'calibrate'}, + {'1': 'lift', '3': 3, '4': 1, '5': 11, '6': '.MotorCommand', '10': 'lift'}, { '1': 'rotate', - '3': 14, + '3': 4, '4': 1, '5': 11, '6': '.MotorCommand', @@ -143,19 +165,20 @@ const ArmCommand$json = { }, { '1': 'pinch', - '3': 15, + '3': 5, '4': 1, '5': 11, '6': '.MotorCommand', '10': 'pinch' }, - {'1': 'open', '3': 16, '4': 1, '5': 8, '10': 'open'}, - {'1': 'close', '3': 17, '4': 1, '5': 8, '10': 'close'}, - {'1': 'spin', '3': 18, '4': 1, '5': 8, '10': 'spin'}, - {'1': 'servoAngle', '3': 19, '4': 1, '5': 5, '10': 'servoAngle'}, + {'1': 'open', '3': 6, '4': 1, '5': 8, '10': 'open'}, + {'1': 'close', '3': 7, '4': 1, '5': 8, '10': 'close'}, + {'1': 'spin', '3': 8, '4': 1, '5': 8, '10': 'spin'}, + {'1': 'version', '3': 9, '4': 1, '5': 11, '6': '.Version', '10': 'version'}, + {'1': 'servoAngle', '3': 10, '4': 1, '5': 5, '10': 'servoAngle'}, { '1': 'laserState', - '3': 20, + '3': 11, '4': 1, '5': 14, '6': '.BoolState', @@ -164,17 +187,12 @@ const ArmCommand$json = { ], }; -/// Descriptor for `ArmCommand`. Decode as a `google.protobuf.DescriptorProto`. -final $typed_data.Uint8List armCommandDescriptor = $convert.base64Decode( - 'CgpBcm1Db21tYW5kEhIKBHN0b3AYASABKAhSBHN0b3ASHAoJY2FsaWJyYXRlGAIgASgIUgljYW' - 'xpYnJhdGUSJQoGc3dpdmVsGAMgASgLMg0uTW90b3JDb21tYW5kUgZzd2l2ZWwSKQoIc2hvdWxk' - 'ZXIYBCABKAsyDS5Nb3RvckNvbW1hbmRSCHNob3VsZGVyEiMKBWVsYm93GAUgASgLMg0uTW90b3' - 'JDb21tYW5kUgVlbGJvdxIwCgxncmlwcGVyX2xpZnQYBiABKAsyDS5Nb3RvckNvbW1hbmRSC2dy' - 'aXBwZXJMaWZ0EhEKBGlrX3gYByABKAJSA2lrWBIRCgRpa195GAggASgCUgNpa1kSEQoEaWtfeh' - 'gJIAEoAlIDaWtaEhAKA2phYhgKIAEoCFIDamFiEiIKB3ZlcnNpb24YCyABKAsyCC5WZXJzaW9u' - 'Ugd2ZXJzaW9uEicKCXN0YXJ0X1VTUxgMIAEoDjIKLkJvb2xTdGF0ZVIIc3RhcnRVU1MSIQoEbG' - 'lmdBgNIAEoCzINLk1vdG9yQ29tbWFuZFIEbGlmdBIlCgZyb3RhdGUYDiABKAsyDS5Nb3RvckNv' - 'bW1hbmRSBnJvdGF0ZRIjCgVwaW5jaBgPIAEoCzINLk1vdG9yQ29tbWFuZFIFcGluY2gSEgoEb3' - 'BlbhgQIAEoCFIEb3BlbhIUCgVjbG9zZRgRIAEoCFIFY2xvc2USEgoEc3BpbhgSIAEoCFIEc3Bp' - 'bhIeCgpzZXJ2b0FuZ2xlGBMgASgFUgpzZXJ2b0FuZ2xlEioKCmxhc2VyU3RhdGUYFCABKA4yCi' - '5Cb29sU3RhdGVSCmxhc2VyU3RhdGU='); +/// Descriptor for `GripperCommand`. Decode as a `google.protobuf.DescriptorProto`. +final $typed_data.Uint8List gripperCommandDescriptor = $convert.base64Decode( + 'Cg5HcmlwcGVyQ29tbWFuZBISCgRzdG9wGAEgASgIUgRzdG9wEhwKCWNhbGlicmF0ZRgCIAEoCF' + 'IJY2FsaWJyYXRlEiEKBGxpZnQYAyABKAsyDS5Nb3RvckNvbW1hbmRSBGxpZnQSJQoGcm90YXRl' + 'GAQgASgLMg0uTW90b3JDb21tYW5kUgZyb3RhdGUSIwoFcGluY2gYBSABKAsyDS5Nb3RvckNvbW' + '1hbmRSBXBpbmNoEhIKBG9wZW4YBiABKAhSBG9wZW4SFAoFY2xvc2UYByABKAhSBWNsb3NlEhIK' + 'BHNwaW4YCCABKAhSBHNwaW4SIgoHdmVyc2lvbhgJIAEoCzIILlZlcnNpb25SB3ZlcnNpb24SHg' + 'oKc2Vydm9BbmdsZRgKIAEoBVIKc2Vydm9BbmdsZRIqCgpsYXNlclN0YXRlGAsgASgOMgouQm9v' + 'bFN0YXRlUgpsYXNlclN0YXRl'); diff --git a/burt_network/lib/src/generated/google/protobuf/timestamp.pb.dart b/burt_network/lib/src/generated/google/protobuf/timestamp.pb.dart new file mode 100644 index 00000000..4f92e8ac --- /dev/null +++ b/burt_network/lib/src/generated/google/protobuf/timestamp.pb.dart @@ -0,0 +1,198 @@ +// This is a generated file - do not edit. +// +// Generated from google/protobuf/timestamp.proto. + +// @dart = 3.3 + +// ignore_for_file: annotate_overrides, camel_case_types, comment_references +// ignore_for_file: constant_identifier_names +// ignore_for_file: curly_braces_in_flow_control_structures +// ignore_for_file: deprecated_member_use_from_same_package +// ignore_for_file: implementation_imports, library_prefixes +// ignore_for_file: non_constant_identifier_names, prefer_relative_imports + +import 'dart:core' as $core; + +import 'package:fixnum/fixnum.dart' as $fixnum; +import 'package:protobuf/protobuf.dart' as $pb; +import 'package:protobuf/src/protobuf/mixins/well_known.dart' as $mixin; + +export 'package:protobuf/protobuf.dart' show GeneratedMessageGenericExtensions; + +/// A Timestamp represents a point in time independent of any time zone or local +/// calendar, encoded as a count of seconds and fractions of seconds at +/// nanosecond resolution. The count is relative to an epoch at UTC midnight on +/// January 1, 1970, in the proleptic Gregorian calendar which extends the +/// Gregorian calendar backwards to year one. +/// +/// All minutes are 60 seconds long. Leap seconds are "smeared" so that no leap +/// second table is needed for interpretation, using a [24-hour linear +/// smear](https://developers.google.com/time/smear). +/// +/// The range is from 0001-01-01T00:00:00Z to 9999-12-31T23:59:59.999999999Z. By +/// restricting to that range, we ensure that we can convert to and from [RFC +/// 3339](https://www.ietf.org/rfc/rfc3339.txt) date strings. +/// +/// # Examples +/// +/// Example 1: Compute Timestamp from POSIX `time()`. +/// +/// Timestamp timestamp; +/// timestamp.set_seconds(time(NULL)); +/// timestamp.set_nanos(0); +/// +/// Example 2: Compute Timestamp from POSIX `gettimeofday()`. +/// +/// struct timeval tv; +/// gettimeofday(&tv, NULL); +/// +/// Timestamp timestamp; +/// timestamp.set_seconds(tv.tv_sec); +/// timestamp.set_nanos(tv.tv_usec * 1000); +/// +/// Example 3: Compute Timestamp from Win32 `GetSystemTimeAsFileTime()`. +/// +/// FILETIME ft; +/// GetSystemTimeAsFileTime(&ft); +/// UINT64 ticks = (((UINT64)ft.dwHighDateTime) << 32) | ft.dwLowDateTime; +/// +/// // A Windows tick is 100 nanoseconds. Windows epoch 1601-01-01T00:00:00Z +/// // is 11644473600 seconds before Unix epoch 1970-01-01T00:00:00Z. +/// Timestamp timestamp; +/// timestamp.set_seconds((INT64) ((ticks / 10000000) - 11644473600LL)); +/// timestamp.set_nanos((INT32) ((ticks % 10000000) * 100)); +/// +/// Example 4: Compute Timestamp from Java `System.currentTimeMillis()`. +/// +/// long millis = System.currentTimeMillis(); +/// +/// Timestamp timestamp = Timestamp.newBuilder().setSeconds(millis / 1000) +/// .setNanos((int) ((millis % 1000) * 1000000)).build(); +/// +/// Example 5: Compute Timestamp from Java `Instant.now()`. +/// +/// Instant now = Instant.now(); +/// +/// Timestamp timestamp = +/// Timestamp.newBuilder().setSeconds(now.getEpochSecond()) +/// .setNanos(now.getNano()).build(); +/// +/// Example 6: Compute Timestamp from current time in Python. +/// +/// timestamp = Timestamp() +/// timestamp.GetCurrentTime() +/// +/// # JSON Mapping +/// +/// In JSON format, the Timestamp type is encoded as a string in the +/// [RFC 3339](https://www.ietf.org/rfc/rfc3339.txt) format. That is, the +/// format is "{year}-{month}-{day}T{hour}:{min}:{sec}[.{frac_sec}]Z" +/// where {year} is always expressed using four digits while {month}, {day}, +/// {hour}, {min}, and {sec} are zero-padded to two digits each. The fractional +/// seconds, which can go up to 9 digits (i.e. up to 1 nanosecond resolution), +/// are optional. The "Z" suffix indicates the timezone ("UTC"); the timezone +/// is required. A proto3 JSON serializer should always use UTC (as indicated by +/// "Z") when printing the Timestamp type and a proto3 JSON parser should be +/// able to accept both UTC and other timezones (as indicated by an offset). +/// +/// For example, "2017-01-15T01:30:15.01Z" encodes 15.01 seconds past +/// 01:30 UTC on January 15, 2017. +/// +/// In JavaScript, one can convert a Date object to this format using the +/// standard +/// [toISOString()](https://developer.mozilla.org/en-US/docs/Web/JavaScript/Reference/Global_Objects/Date/toISOString) +/// method. In Python, a standard `datetime.datetime` object can be converted +/// to this format using +/// [`strftime`](https://docs.python.org/2/library/time.html#time.strftime) with +/// the time format spec '%Y-%m-%dT%H:%M:%S.%fZ'. Likewise, in Java, one can use +/// the Joda Time's [`ISODateTimeFormat.dateTime()`]( +/// http://joda-time.sourceforge.net/apidocs/org/joda/time/format/ISODateTimeFormat.html#dateTime() +/// ) to obtain a formatter capable of generating timestamps in this format. +class Timestamp extends $pb.GeneratedMessage with $mixin.TimestampMixin { + factory Timestamp({ + $fixnum.Int64? seconds, + $core.int? nanos, + }) { + final result = create(); + if (seconds != null) result.seconds = seconds; + if (nanos != null) result.nanos = nanos; + return result; + } + + Timestamp._(); + + factory Timestamp.fromBuffer($core.List<$core.int> data, + [$pb.ExtensionRegistry registry = $pb.ExtensionRegistry.EMPTY]) => + create()..mergeFromBuffer(data, registry); + factory Timestamp.fromJson($core.String json, + [$pb.ExtensionRegistry registry = $pb.ExtensionRegistry.EMPTY]) => + create()..mergeFromJson(json, registry); + + static final $pb.BuilderInfo _i = $pb.BuilderInfo( + _omitMessageNames ? '' : 'Timestamp', + package: + const $pb.PackageName(_omitMessageNames ? '' : 'google.protobuf'), + createEmptyInstance: create, + wellKnownType: $mixin.WellKnownType.timestamp) + ..aInt64(1, _omitFieldNames ? '' : 'seconds') + ..aI(2, _omitFieldNames ? '' : 'nanos') + ..hasRequiredFields = false; + + @$core.Deprecated('See https://github.com/google/protobuf.dart/issues/998.') + Timestamp clone() => deepCopy(); + @$core.Deprecated('See https://github.com/google/protobuf.dart/issues/998.') + Timestamp copyWith(void Function(Timestamp) updates) => + super.copyWith((message) => updates(message as Timestamp)) as Timestamp; + + @$core.override + $pb.BuilderInfo get info_ => _i; + + @$core.pragma('dart2js:noInline') + static Timestamp create() => Timestamp._(); + @$core.override + Timestamp createEmptyInstance() => create(); + @$core.pragma('dart2js:noInline') + static Timestamp getDefault() => + _defaultInstance ??= $pb.GeneratedMessage.$_defaultFor(create); + static Timestamp? _defaultInstance; + + /// Represents seconds of UTC time since Unix epoch 1970-01-01T00:00:00Z. Must + /// be between -315576000000 and 315576000000 inclusive (which corresponds to + /// 0001-01-01T00:00:00Z to 9999-12-31T23:59:59Z). + @$pb.TagNumber(1) + $fixnum.Int64 get seconds => $_getI64(0); + @$pb.TagNumber(1) + set seconds($fixnum.Int64 value) => $_setInt64(0, value); + @$pb.TagNumber(1) + $core.bool hasSeconds() => $_has(0); + @$pb.TagNumber(1) + void clearSeconds() => $_clearField(1); + + /// Non-negative fractions of a second at nanosecond resolution. This field is + /// the nanosecond portion of the duration, not an alternative to seconds. + /// Negative second values with fractions must still have non-negative nanos + /// values that count forward in time. Must be between 0 and 999,999,999 + /// inclusive. + @$pb.TagNumber(2) + $core.int get nanos => $_getIZ(1); + @$pb.TagNumber(2) + set nanos($core.int value) => $_setSignedInt32(1, value); + @$pb.TagNumber(2) + $core.bool hasNanos() => $_has(1); + @$pb.TagNumber(2) + void clearNanos() => $_clearField(2); + + /// Creates a new instance from [dateTime]. + /// + /// Time zone information will not be preserved. + static Timestamp fromDateTime($core.DateTime dateTime) { + final result = create(); + $mixin.TimestampMixin.setFromDateTime(result, dateTime); + return result; + } +} + +const $core.bool _omitFieldNames = + $core.bool.fromEnvironment('protobuf.omit_field_names'); +const $core.bool _omitMessageNames = + $core.bool.fromEnvironment('protobuf.omit_message_names'); diff --git a/burt_network/lib/src/generated/google/protobuf/timestamp.pbenum.dart b/burt_network/lib/src/generated/google/protobuf/timestamp.pbenum.dart new file mode 100644 index 00000000..cdbd00bc --- /dev/null +++ b/burt_network/lib/src/generated/google/protobuf/timestamp.pbenum.dart @@ -0,0 +1,11 @@ +// This is a generated file - do not edit. +// +// Generated from google/protobuf/timestamp.proto. + +// @dart = 3.3 + +// ignore_for_file: annotate_overrides, camel_case_types, comment_references +// ignore_for_file: constant_identifier_names +// ignore_for_file: curly_braces_in_flow_control_structures +// ignore_for_file: deprecated_member_use_from_same_package, library_prefixes +// ignore_for_file: non_constant_identifier_names, prefer_relative_imports diff --git a/burt_network/lib/src/generated/google/protobuf/timestamp.pbjson.dart b/burt_network/lib/src/generated/google/protobuf/timestamp.pbjson.dart new file mode 100644 index 00000000..739dd55a --- /dev/null +++ b/burt_network/lib/src/generated/google/protobuf/timestamp.pbjson.dart @@ -0,0 +1,30 @@ +// This is a generated file - do not edit. +// +// Generated from google/protobuf/timestamp.proto. + +// @dart = 3.3 + +// ignore_for_file: annotate_overrides, camel_case_types, comment_references +// ignore_for_file: constant_identifier_names +// ignore_for_file: curly_braces_in_flow_control_structures +// ignore_for_file: deprecated_member_use_from_same_package, library_prefixes +// ignore_for_file: non_constant_identifier_names, prefer_relative_imports +// ignore_for_file: unused_import + +import 'dart:convert' as $convert; +import 'dart:core' as $core; +import 'dart:typed_data' as $typed_data; + +@$core.Deprecated('Use timestampDescriptor instead') +const Timestamp$json = { + '1': 'Timestamp', + '2': [ + {'1': 'seconds', '3': 1, '4': 1, '5': 3, '10': 'seconds'}, + {'1': 'nanos', '3': 2, '4': 1, '5': 5, '10': 'nanos'}, + ], +}; + +/// Descriptor for `Timestamp`. Decode as a `google.protobuf.DescriptorProto`. +final $typed_data.Uint8List timestampDescriptor = $convert.base64Decode( + 'CglUaW1lc3RhbXASGAoHc2Vjb25kcxgBIAEoA1IHc2Vjb25kcxIUCgVuYW5vcxgCIAEoBVIFbm' + 'Fub3M='); diff --git a/burt_network/lib/src/generated/video.pb.dart b/burt_network/lib/src/generated/video.pb.dart index 2874078f..cbb325eb 100644 --- a/burt_network/lib/src/generated/video.pb.dart +++ b/burt_network/lib/src/generated/video.pb.dart @@ -41,6 +41,7 @@ class CameraDetails extends $pb.GeneratedMessage { $core.double? verticalFov, $core.int? streamWidth, $core.int? streamHeight, + $core.int? rotationQuarters, }) { final result = create(); if (name != null) result.name = name; @@ -59,6 +60,7 @@ class CameraDetails extends $pb.GeneratedMessage { if (verticalFov != null) result.verticalFov = verticalFov; if (streamWidth != null) result.streamWidth = streamWidth; if (streamHeight != null) result.streamHeight = streamHeight; + if (rotationQuarters != null) result.rotationQuarters = rotationQuarters; return result; } @@ -95,6 +97,8 @@ class CameraDetails extends $pb.GeneratedMessage { fieldType: $pb.PbFieldType.OF) ..aI(15, _omitFieldNames ? '' : 'streamWidth') ..aI(16, _omitFieldNames ? '' : 'streamHeight') + ..aI(17, _omitFieldNames ? '' : 'rotationQuarters', + protoName: 'rotationQuarters') ..hasRequiredFields = false; @$core.Deprecated('See https://github.com/google/protobuf.dart/issues/998.') @@ -266,6 +270,16 @@ class CameraDetails extends $pb.GeneratedMessage { $core.bool hasStreamHeight() => $_has(15); @$pb.TagNumber(16) void clearStreamHeight() => $_clearField(16); + + /// / Number of 90 degrees rotation increments + @$pb.TagNumber(17) + $core.int get rotationQuarters => $_getIZ(16); + @$pb.TagNumber(17) + set rotationQuarters($core.int value) => $_setSignedInt32(16, value); + @$pb.TagNumber(17) + $core.bool hasRotationQuarters() => $_has(16); + @$pb.TagNumber(17) + void clearRotationQuarters() => $_clearField(17); } /// / Reports data about a camera. diff --git a/burt_network/lib/src/generated/video.pbenum.dart b/burt_network/lib/src/generated/video.pbenum.dart index a6d716ac..810f2c08 100644 --- a/burt_network/lib/src/generated/video.pbenum.dart +++ b/burt_network/lib/src/generated/video.pbenum.dart @@ -70,6 +70,12 @@ class CameraName extends $pb.ProtobufEnum { CameraName._(7, _omitEnumNames ? '' : 'BOTTOM_LEFT'); static const CameraName BOTTOM_RIGHT = CameraName._(8, _omitEnumNames ? '' : 'BOTTOM_RIGHT'); + static const CameraName ARM_LEFT = + CameraName._(9, _omitEnumNames ? '' : 'ARM_LEFT'); + static const CameraName ARM_RIGHT = + CameraName._(10, _omitEnumNames ? '' : 'ARM_RIGHT'); + static const CameraName GAP_CAM = + CameraName._(11, _omitEnumNames ? '' : 'GAP_CAM'); static const $core.List values = [ CAMERA_NAME_UNDEFINED, @@ -81,10 +87,13 @@ class CameraName extends $pb.ProtobufEnum { SUBSYSTEM3, BOTTOM_LEFT, BOTTOM_RIGHT, + ARM_LEFT, + ARM_RIGHT, + GAP_CAM, ]; static final $core.List _byValue = - $pb.ProtobufEnum.$_initByValueList(values, 8); + $pb.ProtobufEnum.$_initByValueList(values, 11); static CameraName? valueOf($core.int value) => value < 0 || value >= _byValue.length ? null : _byValue[value]; diff --git a/burt_network/lib/src/generated/video.pbjson.dart b/burt_network/lib/src/generated/video.pbjson.dart index 2c532c00..dac71a4f 100644 --- a/burt_network/lib/src/generated/video.pbjson.dart +++ b/burt_network/lib/src/generated/video.pbjson.dart @@ -50,6 +50,9 @@ const CameraName$json = { {'1': 'SUBSYSTEM3', '2': 6}, {'1': 'BOTTOM_LEFT', '2': 7}, {'1': 'BOTTOM_RIGHT', '2': 8}, + {'1': 'ARM_LEFT', '2': 9}, + {'1': 'ARM_RIGHT', '2': 10}, + {'1': 'GAP_CAM', '2': 11}, ], }; @@ -58,7 +61,7 @@ final $typed_data.Uint8List cameraNameDescriptor = $convert.base64Decode( 'CgpDYW1lcmFOYW1lEhkKFUNBTUVSQV9OQU1FX1VOREVGSU5FRBAAEg8KC1JPVkVSX0ZST05UEA' 'ESDgoKUk9WRVJfUkVBUhACEhIKDkFVVE9OT01ZX0RFUFRIEAMSDgoKU1VCU1lTVEVNMRAEEg4K' 'ClNVQlNZU1RFTTIQBRIOCgpTVUJTWVNURU0zEAYSDwoLQk9UVE9NX0xFRlQQBxIQCgxCT1RUT0' - '1fUklHSFQQCA=='); + '1fUklHSFQQCBIMCghBUk1fTEVGVBAJEg0KCUFSTV9SSUdIVBAKEgsKB0dBUF9DQU0QCw=='); @$core.Deprecated('Use cameraDetailsDescriptor instead') const CameraDetails$json = { @@ -93,6 +96,13 @@ const CameraDetails$json = { {'1': 'vertical_fov', '3': 14, '4': 1, '5': 2, '10': 'verticalFov'}, {'1': 'stream_width', '3': 15, '4': 1, '5': 5, '10': 'streamWidth'}, {'1': 'stream_height', '3': 16, '4': 1, '5': 5, '10': 'streamHeight'}, + { + '1': 'rotationQuarters', + '3': 17, + '4': 1, + '5': 5, + '10': 'rotationQuarters' + }, ], }; @@ -107,7 +117,7 @@ final $typed_data.Uint8List cameraDetailsDescriptor = $convert.base64Decode( 'ZGlhZ29uYWxfZm92GAwgASgCUgtkaWFnb25hbEZvdhIlCg5ob3Jpem9udGFsX2ZvdhgNIAEoAl' 'INaG9yaXpvbnRhbEZvdhIhCgx2ZXJ0aWNhbF9mb3YYDiABKAJSC3ZlcnRpY2FsRm92EiEKDHN0' 'cmVhbV93aWR0aBgPIAEoBVILc3RyZWFtV2lkdGgSIwoNc3RyZWFtX2hlaWdodBgQIAEoBVIMc3' - 'RyZWFtSGVpZ2h0'); + 'RyZWFtSGVpZ2h0EioKEHJvdGF0aW9uUXVhcnRlcnMYESABKAVSEHJvdGF0aW9uUXVhcnRlcnM='); @$core.Deprecated('Use videoDataDescriptor instead') const VideoData$json = { From fef8cda366a3f4f9a121f9ce68626fa5ad33b388 Mon Sep 17 00:00:00 2001 From: preston Date: Sun, 1 Feb 2026 13:06:04 -0500 Subject: [PATCH 12/16] Fix small error --- arm_auxillary/lib/src/arm_camera_manager.dart | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/arm_auxillary/lib/src/arm_camera_manager.dart b/arm_auxillary/lib/src/arm_camera_manager.dart index 9b1daebe..cc1b0a37 100644 --- a/arm_auxillary/lib/src/arm_camera_manager.dart +++ b/arm_auxillary/lib/src/arm_camera_manager.dart @@ -130,6 +130,9 @@ class ArmCameraManager extends Service { 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: @@ -144,6 +147,12 @@ class ArmCameraManager extends Service { 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): From ebfc1900f1b6b1ee670f563c02136063b7e710b2 Mon Sep 17 00:00:00 2001 From: preston Date: Sun, 1 Feb 2026 13:12:23 -0500 Subject: [PATCH 13/16] Add Camera Stuff --- arm_auxillary/lib/arm_auxillary.dart | 7 +++ arm_auxillary/lib/src/arm_camera_manager.dart | 57 ++++++++++--------- arm_auxillary/pubspec.yaml | 2 + 3 files changed, 39 insertions(+), 27 deletions(-) diff --git a/arm_auxillary/lib/arm_auxillary.dart b/arm_auxillary/lib/arm_auxillary.dart index 8d75f86b..7952f100 100644 --- a/arm_auxillary/lib/arm_auxillary.dart +++ b/arm_auxillary/lib/arm_auxillary.dart @@ -3,6 +3,7 @@ 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(); @@ -24,6 +25,9 @@ class ArmAuxillary extends Service { 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, @@ -42,6 +46,8 @@ class ArmAuxillary extends Service { // 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 { @@ -64,6 +70,7 @@ class ArmAuxillary extends Service { Future dispose() async { logger.info("Arm Auxillary software shutting down..."); isReady = false; + await cameras.dispose(); await firmware.dispose(); await server.dispose(); logger.socket = null; diff --git a/arm_auxillary/lib/src/arm_camera_manager.dart b/arm_auxillary/lib/src/arm_camera_manager.dart index cc1b0a37..f870b837 100644 --- a/arm_auxillary/lib/src/arm_camera_manager.dart +++ b/arm_auxillary/lib/src/arm_camera_manager.dart @@ -27,12 +27,12 @@ class ArmCameraManager extends Service { @override Future 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; } @@ -40,9 +40,9 @@ class ArmCameraManager extends Service { @override Future 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.delayed(const Duration(milliseconds: 750)); @@ -57,14 +57,14 @@ class ArmCameraManager extends Service { await Future.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( name: VideoCommand().messageName, @@ -78,35 +78,38 @@ class ArmCameraManager extends Service { /// Spawns camera isolates for detected arm cameras Future _spawnArmCameras() async { logger.info("Detecting arm cameras..."); - + final armCameraNames = [ 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()); + 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, - ); + 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) { @@ -114,7 +117,7 @@ class ArmCameraManager extends Service { case FramePayload(:final details, :final screenshotPath): final image = data.image?.toU8List(); data.dispose(); - + if (_server != null && image != null) { _server!.sendMessage( VideoData( @@ -125,7 +128,7 @@ class ArmCameraManager extends Service { destination: videoSocket, ); } - + case LogPayload(): switch (data.level) { case LogLevel.all: @@ -154,7 +157,7 @@ class ArmCameraManager extends Service { case LogLevel.nothing: break; } - + case ObjectDetectionPayload(:final details, :final tags): if (_server != null) { final visionResult = VideoData( @@ -164,7 +167,7 @@ class ArmCameraManager extends Service { ); _server!.sendMessage(visionResult, destination: videoSocket); } - + default: logger.warning("Unknown payload type from camera isolate"); } @@ -173,7 +176,7 @@ class ArmCameraManager extends Service { /// 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); } @@ -183,18 +186,18 @@ class ArmCameraManager extends Service { 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"); } -} \ No newline at end of file +} diff --git a/arm_auxillary/pubspec.yaml b/arm_auxillary/pubspec.yaml index d5477c75..6ad5e95b 100644 --- a/arm_auxillary/pubspec.yaml +++ b/arm_auxillary/pubspec.yaml @@ -14,6 +14,8 @@ dependencies: collection: ^1.19.0 subsystems: ^1.0.0 video: ^1.1.0 + typed_isolate: ^6.0.0 + dartcv4: ^1.1.8 dev_dependencies: very_good_analysis: ^6.0.0 From 0abb96bc39a3ad9b5ce9e275da4dd956410813a1 Mon Sep 17 00:00:00 2001 From: preston Date: Sun, 8 Feb 2026 11:20:59 -0500 Subject: [PATCH 14/16] Regen proto for video --- burt_network/Protobuf | 2 +- burt_network/lib/src/generated/video.pb.dart | 14 -------------- burt_network/lib/src/generated/video.pbjson.dart | 9 +-------- 3 files changed, 2 insertions(+), 23 deletions(-) diff --git a/burt_network/Protobuf b/burt_network/Protobuf index 4a64299e..c28e07f2 160000 --- a/burt_network/Protobuf +++ b/burt_network/Protobuf @@ -1 +1 @@ -Subproject commit 4a64299eed7196bcd46dea9b24104a90e2abb258 +Subproject commit c28e07f2dbdc4fb2267dbd6b6e58143c6795909e diff --git a/burt_network/lib/src/generated/video.pb.dart b/burt_network/lib/src/generated/video.pb.dart index 76ad2087..a45c1689 100644 --- a/burt_network/lib/src/generated/video.pb.dart +++ b/burt_network/lib/src/generated/video.pb.dart @@ -41,7 +41,6 @@ class CameraDetails extends $pb.GeneratedMessage { $core.double? verticalFov, $core.int? streamWidth, $core.int? streamHeight, - $core.int? rotationQuarters, }) { final result = create(); if (name != null) result.name = name; @@ -60,7 +59,6 @@ class CameraDetails extends $pb.GeneratedMessage { if (verticalFov != null) result.verticalFov = verticalFov; if (streamWidth != null) result.streamWidth = streamWidth; if (streamHeight != null) result.streamHeight = streamHeight; - if (rotationQuarters != null) result.rotationQuarters = rotationQuarters; return result; } @@ -97,8 +95,6 @@ class CameraDetails extends $pb.GeneratedMessage { fieldType: $pb.PbFieldType.OF) ..aI(15, _omitFieldNames ? '' : 'streamWidth') ..aI(16, _omitFieldNames ? '' : 'streamHeight') - ..aI(17, _omitFieldNames ? '' : 'rotationQuarters', - protoName: 'rotationQuarters') ..hasRequiredFields = false; @$core.Deprecated('See https://github.com/google/protobuf.dart/issues/998.') @@ -270,16 +266,6 @@ class CameraDetails extends $pb.GeneratedMessage { $core.bool hasStreamHeight() => $_has(15); @$pb.TagNumber(16) void clearStreamHeight() => $_clearField(16); - - /// / Number of 90 degrees rotation increments - @$pb.TagNumber(17) - $core.int get rotationQuarters => $_getIZ(16); - @$pb.TagNumber(17) - set rotationQuarters($core.int value) => $_setSignedInt32(16, value); - @$pb.TagNumber(17) - $core.bool hasRotationQuarters() => $_has(16); - @$pb.TagNumber(17) - void clearRotationQuarters() => $_clearField(17); } /// / Reports data about a camera. diff --git a/burt_network/lib/src/generated/video.pbjson.dart b/burt_network/lib/src/generated/video.pbjson.dart index a6ba2fc6..a6a1da8f 100644 --- a/burt_network/lib/src/generated/video.pbjson.dart +++ b/burt_network/lib/src/generated/video.pbjson.dart @@ -96,13 +96,6 @@ const CameraDetails$json = { {'1': 'vertical_fov', '3': 14, '4': 1, '5': 2, '10': 'verticalFov'}, {'1': 'stream_width', '3': 15, '4': 1, '5': 5, '10': 'streamWidth'}, {'1': 'stream_height', '3': 16, '4': 1, '5': 5, '10': 'streamHeight'}, - { - '1': 'rotationQuarters', - '3': 17, - '4': 1, - '5': 5, - '10': 'rotationQuarters' - }, ], }; @@ -117,7 +110,7 @@ final $typed_data.Uint8List cameraDetailsDescriptor = $convert.base64Decode( 'ZGlhZ29uYWxfZm92GAwgASgCUgtkaWFnb25hbEZvdhIlCg5ob3Jpem9udGFsX2ZvdhgNIAEoAl' 'INaG9yaXpvbnRhbEZvdhIhCgx2ZXJ0aWNhbF9mb3YYDiABKAJSC3ZlcnRpY2FsRm92EiEKDHN0' 'cmVhbV93aWR0aBgPIAEoBVILc3RyZWFtV2lkdGgSIwoNc3RyZWFtX2hlaWdodBgQIAEoBVIMc3' - 'RyZWFtSGVpZ2h0EioKEHJvdGF0aW9uUXVhcnRlcnMYESABKAVSEHJvdGF0aW9uUXVhcnRlcnM='); + 'RyZWFtSGVpZ2h0'); @$core.Deprecated('Use videoDataDescriptor instead') const VideoData$json = { From d42740c6998c3b3d20a58f90eea8bcfd5145064d Mon Sep 17 00:00:00 2001 From: preston Date: Sun, 8 Feb 2026 11:31:17 -0500 Subject: [PATCH 15/16] Update proto stuff --- burt_network/Protobuf | 2 +- burt_network/lib/src/generated/arm.pb.dart | 612 ++++++++++++++---- .../lib/src/generated/arm.pbjson.dart | 147 ++++- 3 files changed, 623 insertions(+), 138 deletions(-) diff --git a/burt_network/Protobuf b/burt_network/Protobuf index c28e07f2..424e5379 160000 --- a/burt_network/Protobuf +++ b/burt_network/Protobuf @@ -1 +1 @@ -Subproject commit c28e07f2dbdc4fb2267dbd6b6e58143c6795909e +Subproject commit 424e5379c018e6fc71e034eb79cf48f2161cf828 diff --git a/burt_network/lib/src/generated/arm.pb.dart b/burt_network/lib/src/generated/arm.pb.dart index 858c4c6a..d9118b27 100644 --- a/burt_network/lib/src/generated/arm.pb.dart +++ b/burt_network/lib/src/generated/arm.pb.dart @@ -14,10 +14,10 @@ import 'dart:core' as $core; import 'package:protobuf/protobuf.dart' as $pb; -import 'geometry.pb.dart' as $0; -import 'motor.pb.dart' as $1; +import 'geometry.pb.dart' as $2; +import 'motor.pb.dart' as $0; import 'utils.pbenum.dart' as $3; -import 'version.pb.dart' as $2; +import 'version.pb.dart' as $1; export 'package:protobuf/protobuf.dart' show GeneratedMessageGenericExtensions; @@ -187,22 +187,163 @@ class JointAngleData extends $pb.GeneratedMessage { void clearWristPitchMulti() => $_clearField(10); } +class WristData extends $pb.GeneratedMessage { + factory WristData({ + $core.double? pitchAngle, + $core.double? rollAngle, + $0.MotorData? motorA, + $0.MotorData? motorB, + $3.BoolState? isMoving, + $3.BoolState? isCalibrated, + $1.Version? version, + }) { + final result = create(); + if (pitchAngle != null) result.pitchAngle = pitchAngle; + if (rollAngle != null) result.rollAngle = rollAngle; + if (motorA != null) result.motorA = motorA; + if (motorB != null) result.motorB = motorB; + if (isMoving != null) result.isMoving = isMoving; + if (isCalibrated != null) result.isCalibrated = isCalibrated; + if (version != null) result.version = version; + return result; + } + + WristData._(); + + factory WristData.fromBuffer($core.List<$core.int> data, + [$pb.ExtensionRegistry registry = $pb.ExtensionRegistry.EMPTY]) => + create()..mergeFromBuffer(data, registry); + factory WristData.fromJson($core.String json, + [$pb.ExtensionRegistry registry = $pb.ExtensionRegistry.EMPTY]) => + create()..mergeFromJson(json, registry); + + static final $pb.BuilderInfo _i = $pb.BuilderInfo( + _omitMessageNames ? '' : 'WristData', + createEmptyInstance: create) + ..aD(1, _omitFieldNames ? '' : 'pitchAngle', fieldType: $pb.PbFieldType.OF) + ..aD(2, _omitFieldNames ? '' : 'rollAngle', fieldType: $pb.PbFieldType.OF) + ..aOM<$0.MotorData>(3, _omitFieldNames ? '' : 'motorA', + protoName: 'motorA', subBuilder: $0.MotorData.create) + ..aOM<$0.MotorData>(4, _omitFieldNames ? '' : 'motorB', + protoName: 'motorB', subBuilder: $0.MotorData.create) + ..aE<$3.BoolState>(5, _omitFieldNames ? '' : 'isMoving', + enumValues: $3.BoolState.values) + ..aE<$3.BoolState>(6, _omitFieldNames ? '' : 'isCalibrated', + enumValues: $3.BoolState.values) + ..aOM<$1.Version>(7, _omitFieldNames ? '' : 'version', + subBuilder: $1.Version.create) + ..hasRequiredFields = false; + + @$core.Deprecated('See https://github.com/google/protobuf.dart/issues/998.') + WristData clone() => deepCopy(); + @$core.Deprecated('See https://github.com/google/protobuf.dart/issues/998.') + WristData copyWith(void Function(WristData) updates) => + super.copyWith((message) => updates(message as WristData)) as WristData; + + @$core.override + $pb.BuilderInfo get info_ => _i; + + @$core.pragma('dart2js:noInline') + static WristData create() => WristData._(); + @$core.override + WristData createEmptyInstance() => create(); + @$core.pragma('dart2js:noInline') + static WristData getDefault() => + _defaultInstance ??= $pb.GeneratedMessage.$_defaultFor(create); + static WristData? _defaultInstance; + + /// Virtual joint state (what the subsystem thinks pitch/roll are). + @$pb.TagNumber(1) + $core.double get pitchAngle => $_getN(0); + @$pb.TagNumber(1) + set pitchAngle($core.double value) => $_setFloat(0, value); + @$pb.TagNumber(1) + $core.bool hasPitchAngle() => $_has(0); + @$pb.TagNumber(1) + void clearPitchAngle() => $_clearField(1); + + @$pb.TagNumber(2) + $core.double get rollAngle => $_getN(1); + @$pb.TagNumber(2) + set rollAngle($core.double value) => $_setFloat(1, value); + @$pb.TagNumber(2) + $core.bool hasRollAngle() => $_has(1); + @$pb.TagNumber(2) + void clearRollAngle() => $_clearField(2); + + @$pb.TagNumber(3) + $0.MotorData get motorA => $_getN(2); + @$pb.TagNumber(3) + set motorA($0.MotorData value) => $_setField(3, value); + @$pb.TagNumber(3) + $core.bool hasMotorA() => $_has(2); + @$pb.TagNumber(3) + void clearMotorA() => $_clearField(3); + @$pb.TagNumber(3) + $0.MotorData ensureMotorA() => $_ensure(2); + + @$pb.TagNumber(4) + $0.MotorData get motorB => $_getN(3); + @$pb.TagNumber(4) + set motorB($0.MotorData value) => $_setField(4, value); + @$pb.TagNumber(4) + $core.bool hasMotorB() => $_has(3); + @$pb.TagNumber(4) + void clearMotorB() => $_clearField(4); + @$pb.TagNumber(4) + $0.MotorData ensureMotorB() => $_ensure(3); + + /// Overall health/status flags + @$pb.TagNumber(5) + $3.BoolState get isMoving => $_getN(4); + @$pb.TagNumber(5) + set isMoving($3.BoolState value) => $_setField(5, value); + @$pb.TagNumber(5) + $core.bool hasIsMoving() => $_has(4); + @$pb.TagNumber(5) + void clearIsMoving() => $_clearField(5); + + @$pb.TagNumber(6) + $3.BoolState get isCalibrated => $_getN(5); + @$pb.TagNumber(6) + set isCalibrated($3.BoolState value) => $_setField(6, value); + @$pb.TagNumber(6) + $core.bool hasIsCalibrated() => $_has(5); + @$pb.TagNumber(6) + void clearIsCalibrated() => $_clearField(6); + + @$pb.TagNumber(7) + $1.Version get version => $_getN(6); + @$pb.TagNumber(7) + set version($1.Version value) => $_setField(7, value); + @$pb.TagNumber(7) + $core.bool hasVersion() => $_has(6); + @$pb.TagNumber(7) + void clearVersion() => $_clearField(7); + @$pb.TagNumber(7) + $1.Version ensureVersion() => $_ensure(6); +} + class ArmData extends $pb.GeneratedMessage { factory ArmData({ - $0.Coordinates? currentPosition, - $0.Coordinates? targetPosition, - $1.MotorData? base, - $1.MotorData? shoulder, - $1.MotorData? elbow, - $2.Version? version, + $2.Coordinates? currentPosition, + $2.Coordinates? targetPosition, + $0.MotorData? base, + $0.MotorData? shoulder, + $0.MotorData? elbow, + $1.Version? version, $core.double? ussDistance, - $1.MotorData? lift, - $1.MotorData? rotate, - $1.MotorData? pinch, + $0.MotorData? lift, + $0.MotorData? rotate, + $0.MotorData? pinch, $core.int? servoAngle, $3.BoolState? laserState, - $1.MotorData? roll, + $0.MotorData? roll, JointAngleData? jointAngles, + WristData? wrist, + $3.BoolState? usingIk, + $2.Orientation? currentOrientation, + $2.Orientation? targetOrientation, }) { final result = create(); if (currentPosition != null) result.currentPosition = currentPosition; @@ -219,6 +360,11 @@ class ArmData extends $pb.GeneratedMessage { if (laserState != null) result.laserState = laserState; if (roll != null) result.roll = roll; if (jointAngles != null) result.jointAngles = jointAngles; + if (wrist != null) result.wrist = wrist; + if (usingIk != null) result.usingIk = usingIk; + if (currentOrientation != null) + result.currentOrientation = currentOrientation; + if (targetOrientation != null) result.targetOrientation = targetOrientation; return result; } @@ -234,32 +380,40 @@ class ArmData extends $pb.GeneratedMessage { static final $pb.BuilderInfo _i = $pb.BuilderInfo( _omitMessageNames ? '' : 'ArmData', createEmptyInstance: create) - ..aOM<$0.Coordinates>(1, _omitFieldNames ? '' : 'currentPosition', - subBuilder: $0.Coordinates.create) - ..aOM<$0.Coordinates>(2, _omitFieldNames ? '' : 'targetPosition', - subBuilder: $0.Coordinates.create) - ..aOM<$1.MotorData>(3, _omitFieldNames ? '' : 'base', - subBuilder: $1.MotorData.create) - ..aOM<$1.MotorData>(4, _omitFieldNames ? '' : 'shoulder', - subBuilder: $1.MotorData.create) - ..aOM<$1.MotorData>(5, _omitFieldNames ? '' : 'elbow', - subBuilder: $1.MotorData.create) - ..aOM<$2.Version>(6, _omitFieldNames ? '' : 'version', - subBuilder: $2.Version.create) + ..aOM<$2.Coordinates>(1, _omitFieldNames ? '' : 'currentPosition', + subBuilder: $2.Coordinates.create) + ..aOM<$2.Coordinates>(2, _omitFieldNames ? '' : 'targetPosition', + subBuilder: $2.Coordinates.create) + ..aOM<$0.MotorData>(3, _omitFieldNames ? '' : 'base', + subBuilder: $0.MotorData.create) + ..aOM<$0.MotorData>(4, _omitFieldNames ? '' : 'shoulder', + subBuilder: $0.MotorData.create) + ..aOM<$0.MotorData>(5, _omitFieldNames ? '' : 'elbow', + subBuilder: $0.MotorData.create) + ..aOM<$1.Version>(6, _omitFieldNames ? '' : 'version', + subBuilder: $1.Version.create) ..aD(7, _omitFieldNames ? '' : 'ussDistance', fieldType: $pb.PbFieldType.OF) - ..aOM<$1.MotorData>(8, _omitFieldNames ? '' : 'lift', - subBuilder: $1.MotorData.create) - ..aOM<$1.MotorData>(9, _omitFieldNames ? '' : 'rotate', - subBuilder: $1.MotorData.create) - ..aOM<$1.MotorData>(10, _omitFieldNames ? '' : 'pinch', - subBuilder: $1.MotorData.create) + ..aOM<$0.MotorData>(8, _omitFieldNames ? '' : 'lift', + subBuilder: $0.MotorData.create) + ..aOM<$0.MotorData>(9, _omitFieldNames ? '' : 'rotate', + subBuilder: $0.MotorData.create) + ..aOM<$0.MotorData>(10, _omitFieldNames ? '' : 'pinch', + subBuilder: $0.MotorData.create) ..aI(11, _omitFieldNames ? '' : 'servoAngle') ..aE<$3.BoolState>(12, _omitFieldNames ? '' : 'laserState', enumValues: $3.BoolState.values) - ..aOM<$1.MotorData>(13, _omitFieldNames ? '' : 'roll', - subBuilder: $1.MotorData.create) + ..aOM<$0.MotorData>(13, _omitFieldNames ? '' : 'roll', + subBuilder: $0.MotorData.create) ..aOM(14, _omitFieldNames ? '' : 'jointAngles', subBuilder: JointAngleData.create) + ..aOM(15, _omitFieldNames ? '' : 'wrist', + subBuilder: WristData.create) + ..aE<$3.BoolState>(16, _omitFieldNames ? '' : 'usingIk', + enumValues: $3.BoolState.values) + ..aOM<$2.Orientation>(17, _omitFieldNames ? '' : 'currentOrientation', + subBuilder: $2.Orientation.create) + ..aOM<$2.Orientation>(18, _omitFieldNames ? '' : 'targetOrientation', + subBuilder: $2.Orientation.create) ..hasRequiredFields = false; @$core.Deprecated('See https://github.com/google/protobuf.dart/issues/998.') @@ -282,70 +436,70 @@ class ArmData extends $pb.GeneratedMessage { /// Arm Commands @$pb.TagNumber(1) - $0.Coordinates get currentPosition => $_getN(0); + $2.Coordinates get currentPosition => $_getN(0); @$pb.TagNumber(1) - set currentPosition($0.Coordinates value) => $_setField(1, value); + set currentPosition($2.Coordinates value) => $_setField(1, value); @$pb.TagNumber(1) $core.bool hasCurrentPosition() => $_has(0); @$pb.TagNumber(1) void clearCurrentPosition() => $_clearField(1); @$pb.TagNumber(1) - $0.Coordinates ensureCurrentPosition() => $_ensure(0); + $2.Coordinates ensureCurrentPosition() => $_ensure(0); @$pb.TagNumber(2) - $0.Coordinates get targetPosition => $_getN(1); + $2.Coordinates get targetPosition => $_getN(1); @$pb.TagNumber(2) - set targetPosition($0.Coordinates value) => $_setField(2, value); + set targetPosition($2.Coordinates value) => $_setField(2, value); @$pb.TagNumber(2) $core.bool hasTargetPosition() => $_has(1); @$pb.TagNumber(2) void clearTargetPosition() => $_clearField(2); @$pb.TagNumber(2) - $0.Coordinates ensureTargetPosition() => $_ensure(1); + $2.Coordinates ensureTargetPosition() => $_ensure(1); @$pb.TagNumber(3) - $1.MotorData get base => $_getN(2); + $0.MotorData get base => $_getN(2); @$pb.TagNumber(3) - set base($1.MotorData value) => $_setField(3, value); + set base($0.MotorData value) => $_setField(3, value); @$pb.TagNumber(3) $core.bool hasBase() => $_has(2); @$pb.TagNumber(3) void clearBase() => $_clearField(3); @$pb.TagNumber(3) - $1.MotorData ensureBase() => $_ensure(2); + $0.MotorData ensureBase() => $_ensure(2); @$pb.TagNumber(4) - $1.MotorData get shoulder => $_getN(3); + $0.MotorData get shoulder => $_getN(3); @$pb.TagNumber(4) - set shoulder($1.MotorData value) => $_setField(4, value); + set shoulder($0.MotorData value) => $_setField(4, value); @$pb.TagNumber(4) $core.bool hasShoulder() => $_has(3); @$pb.TagNumber(4) void clearShoulder() => $_clearField(4); @$pb.TagNumber(4) - $1.MotorData ensureShoulder() => $_ensure(3); + $0.MotorData ensureShoulder() => $_ensure(3); @$pb.TagNumber(5) - $1.MotorData get elbow => $_getN(4); + $0.MotorData get elbow => $_getN(4); @$pb.TagNumber(5) - set elbow($1.MotorData value) => $_setField(5, value); + set elbow($0.MotorData value) => $_setField(5, value); @$pb.TagNumber(5) $core.bool hasElbow() => $_has(4); @$pb.TagNumber(5) void clearElbow() => $_clearField(5); @$pb.TagNumber(5) - $1.MotorData ensureElbow() => $_ensure(4); + $0.MotorData ensureElbow() => $_ensure(4); @$pb.TagNumber(6) - $2.Version get version => $_getN(5); + $1.Version get version => $_getN(5); @$pb.TagNumber(6) - set version($2.Version value) => $_setField(6, value); + set version($1.Version value) => $_setField(6, value); @$pb.TagNumber(6) $core.bool hasVersion() => $_has(5); @$pb.TagNumber(6) void clearVersion() => $_clearField(6); @$pb.TagNumber(6) - $2.Version ensureVersion() => $_ensure(5); + $1.Version ensureVersion() => $_ensure(5); /// USS data @$pb.TagNumber(7) @@ -359,37 +513,37 @@ class ArmData extends $pb.GeneratedMessage { /// Gripper Commands @$pb.TagNumber(8) - $1.MotorData get lift => $_getN(7); + $0.MotorData get lift => $_getN(7); @$pb.TagNumber(8) - set lift($1.MotorData value) => $_setField(8, value); + set lift($0.MotorData value) => $_setField(8, value); @$pb.TagNumber(8) $core.bool hasLift() => $_has(7); @$pb.TagNumber(8) void clearLift() => $_clearField(8); @$pb.TagNumber(8) - $1.MotorData ensureLift() => $_ensure(7); + $0.MotorData ensureLift() => $_ensure(7); @$pb.TagNumber(9) - $1.MotorData get rotate => $_getN(8); + $0.MotorData get rotate => $_getN(8); @$pb.TagNumber(9) - set rotate($1.MotorData value) => $_setField(9, value); + set rotate($0.MotorData value) => $_setField(9, value); @$pb.TagNumber(9) $core.bool hasRotate() => $_has(8); @$pb.TagNumber(9) void clearRotate() => $_clearField(9); @$pb.TagNumber(9) - $1.MotorData ensureRotate() => $_ensure(8); + $0.MotorData ensureRotate() => $_ensure(8); @$pb.TagNumber(10) - $1.MotorData get pinch => $_getN(9); + $0.MotorData get pinch => $_getN(9); @$pb.TagNumber(10) - set pinch($1.MotorData value) => $_setField(10, value); + set pinch($0.MotorData value) => $_setField(10, value); @$pb.TagNumber(10) $core.bool hasPinch() => $_has(9); @$pb.TagNumber(10) void clearPinch() => $_clearField(10); @$pb.TagNumber(10) - $1.MotorData ensurePinch() => $_ensure(9); + $0.MotorData ensurePinch() => $_ensure(9); @$pb.TagNumber(11) $core.int get servoAngle => $_getIZ(10); @@ -410,15 +564,15 @@ class ArmData extends $pb.GeneratedMessage { void clearLaserState() => $_clearField(12); @$pb.TagNumber(13) - $1.MotorData get roll => $_getN(12); + $0.MotorData get roll => $_getN(12); @$pb.TagNumber(13) - set roll($1.MotorData value) => $_setField(13, value); + set roll($0.MotorData value) => $_setField(13, value); @$pb.TagNumber(13) $core.bool hasRoll() => $_has(12); @$pb.TagNumber(13) void clearRoll() => $_clearField(13); @$pb.TagNumber(13) - $1.MotorData ensureRoll() => $_ensure(12); + $0.MotorData ensureRoll() => $_ensure(12); @$pb.TagNumber(14) JointAngleData get jointAngles => $_getN(13); @@ -430,31 +584,191 @@ class ArmData extends $pb.GeneratedMessage { void clearJointAngles() => $_clearField(14); @$pb.TagNumber(14) JointAngleData ensureJointAngles() => $_ensure(13); + + @$pb.TagNumber(15) + WristData get wrist => $_getN(14); + @$pb.TagNumber(15) + set wrist(WristData value) => $_setField(15, value); + @$pb.TagNumber(15) + $core.bool hasWrist() => $_has(14); + @$pb.TagNumber(15) + void clearWrist() => $_clearField(15); + @$pb.TagNumber(15) + WristData ensureWrist() => $_ensure(14); + + @$pb.TagNumber(16) + $3.BoolState get usingIk => $_getN(15); + @$pb.TagNumber(16) + set usingIk($3.BoolState value) => $_setField(16, value); + @$pb.TagNumber(16) + $core.bool hasUsingIk() => $_has(15); + @$pb.TagNumber(16) + void clearUsingIk() => $_clearField(16); + + @$pb.TagNumber(17) + $2.Orientation get currentOrientation => $_getN(16); + @$pb.TagNumber(17) + set currentOrientation($2.Orientation value) => $_setField(17, value); + @$pb.TagNumber(17) + $core.bool hasCurrentOrientation() => $_has(16); + @$pb.TagNumber(17) + void clearCurrentOrientation() => $_clearField(17); + @$pb.TagNumber(17) + $2.Orientation ensureCurrentOrientation() => $_ensure(16); + + @$pb.TagNumber(18) + $2.Orientation get targetOrientation => $_getN(17); + @$pb.TagNumber(18) + set targetOrientation($2.Orientation value) => $_setField(18, value); + @$pb.TagNumber(18) + $core.bool hasTargetOrientation() => $_has(17); + @$pb.TagNumber(18) + void clearTargetOrientation() => $_clearField(18); + @$pb.TagNumber(18) + $2.Orientation ensureTargetOrientation() => $_ensure(17); +} + +class WristCommand extends $pb.GeneratedMessage { + factory WristCommand({ + $core.bool? stop, + $core.bool? calibrate, + $0.MotorCommand? pitch, + $0.MotorCommand? roll, + $1.Version? version, + }) { + final result = create(); + if (stop != null) result.stop = stop; + if (calibrate != null) result.calibrate = calibrate; + if (pitch != null) result.pitch = pitch; + if (roll != null) result.roll = roll; + if (version != null) result.version = version; + return result; + } + + WristCommand._(); + + factory WristCommand.fromBuffer($core.List<$core.int> data, + [$pb.ExtensionRegistry registry = $pb.ExtensionRegistry.EMPTY]) => + create()..mergeFromBuffer(data, registry); + factory WristCommand.fromJson($core.String json, + [$pb.ExtensionRegistry registry = $pb.ExtensionRegistry.EMPTY]) => + create()..mergeFromJson(json, registry); + + static final $pb.BuilderInfo _i = $pb.BuilderInfo( + _omitMessageNames ? '' : 'WristCommand', + createEmptyInstance: create) + ..aOB(1, _omitFieldNames ? '' : 'stop') + ..aOB(2, _omitFieldNames ? '' : 'calibrate') + ..aOM<$0.MotorCommand>(3, _omitFieldNames ? '' : 'pitch', + subBuilder: $0.MotorCommand.create) + ..aOM<$0.MotorCommand>(4, _omitFieldNames ? '' : 'roll', + subBuilder: $0.MotorCommand.create) + ..aOM<$1.Version>(5, _omitFieldNames ? '' : 'version', + subBuilder: $1.Version.create) + ..hasRequiredFields = false; + + @$core.Deprecated('See https://github.com/google/protobuf.dart/issues/998.') + WristCommand clone() => deepCopy(); + @$core.Deprecated('See https://github.com/google/protobuf.dart/issues/998.') + WristCommand copyWith(void Function(WristCommand) updates) => + super.copyWith((message) => updates(message as WristCommand)) + as WristCommand; + + @$core.override + $pb.BuilderInfo get info_ => _i; + + @$core.pragma('dart2js:noInline') + static WristCommand create() => WristCommand._(); + @$core.override + WristCommand createEmptyInstance() => create(); + @$core.pragma('dart2js:noInline') + static WristCommand getDefault() => _defaultInstance ??= + $pb.GeneratedMessage.$_defaultFor(create); + static WristCommand? _defaultInstance; + + /// General commands for wrist + @$pb.TagNumber(1) + $core.bool get stop => $_getBF(0); + @$pb.TagNumber(1) + set stop($core.bool value) => $_setBool(0, value); + @$pb.TagNumber(1) + $core.bool hasStop() => $_has(0); + @$pb.TagNumber(1) + void clearStop() => $_clearField(1); + + @$pb.TagNumber(2) + $core.bool get calibrate => $_getBF(1); + @$pb.TagNumber(2) + set calibrate($core.bool value) => $_setBool(1, value); + @$pb.TagNumber(2) + $core.bool hasCalibrate() => $_has(1); + @$pb.TagNumber(2) + void clearCalibrate() => $_clearField(2); + + /// Virtual joints (preferred): command the coupled wrist DOFs. + /// Firmware may execute one axis at a time for mechanical safety. + @$pb.TagNumber(3) + $0.MotorCommand get pitch => $_getN(2); + @$pb.TagNumber(3) + set pitch($0.MotorCommand value) => $_setField(3, value); + @$pb.TagNumber(3) + $core.bool hasPitch() => $_has(2); + @$pb.TagNumber(3) + void clearPitch() => $_clearField(3); + @$pb.TagNumber(3) + $0.MotorCommand ensurePitch() => $_ensure(2); + + @$pb.TagNumber(4) + $0.MotorCommand get roll => $_getN(3); + @$pb.TagNumber(4) + set roll($0.MotorCommand value) => $_setField(4, value); + @$pb.TagNumber(4) + $core.bool hasRoll() => $_has(3); + @$pb.TagNumber(4) + void clearRoll() => $_clearField(4); + @$pb.TagNumber(4) + $0.MotorCommand ensureRoll() => $_ensure(3); + + @$pb.TagNumber(5) + $1.Version get version => $_getN(4); + @$pb.TagNumber(5) + set version($1.Version value) => $_setField(5, value); + @$pb.TagNumber(5) + $core.bool hasVersion() => $_has(4); + @$pb.TagNumber(5) + void clearVersion() => $_clearField(5); + @$pb.TagNumber(5) + $1.Version ensureVersion() => $_ensure(4); } class ArmCommand extends $pb.GeneratedMessage { factory ArmCommand({ $core.bool? stop, $core.bool? calibrate, - $1.MotorCommand? swivel, - $1.MotorCommand? shoulder, - $1.MotorCommand? elbow, - $1.MotorCommand? gripperLift, + $0.MotorCommand? swivel, + $0.MotorCommand? shoulder, + $0.MotorCommand? elbow, + $0.MotorCommand? gripperLift, $core.double? ikX, $core.double? ikY, $core.double? ikZ, $core.bool? jab, - $2.Version? version, + $1.Version? version, $3.BoolState? startUss, - $1.MotorCommand? lift, - $1.MotorCommand? rotate, - $1.MotorCommand? pinch, + $0.MotorCommand? lift, + $0.MotorCommand? rotate, + $0.MotorCommand? pinch, $core.bool? open, $core.bool? close, $core.bool? spin, $core.int? servoAngle, $3.BoolState? laserState, - $1.MotorCommand? roll, + $0.MotorCommand? roll, + WristCommand? wrist, + $3.BoolState? usingIk, + $core.double? ikPitch, + $core.double? ikYaw, + $2.Pose3d? pose, }) { final result = create(); if (stop != null) result.stop = stop; @@ -478,6 +792,11 @@ class ArmCommand extends $pb.GeneratedMessage { if (servoAngle != null) result.servoAngle = servoAngle; if (laserState != null) result.laserState = laserState; if (roll != null) result.roll = roll; + if (wrist != null) result.wrist = wrist; + if (usingIk != null) result.usingIk = usingIk; + if (ikPitch != null) result.ikPitch = ikPitch; + if (ikYaw != null) result.ikYaw = ikYaw; + if (pose != null) result.pose = pose; return result; } @@ -495,36 +814,44 @@ class ArmCommand extends $pb.GeneratedMessage { createEmptyInstance: create) ..aOB(1, _omitFieldNames ? '' : 'stop') ..aOB(2, _omitFieldNames ? '' : 'calibrate') - ..aOM<$1.MotorCommand>(3, _omitFieldNames ? '' : 'swivel', - subBuilder: $1.MotorCommand.create) - ..aOM<$1.MotorCommand>(4, _omitFieldNames ? '' : 'shoulder', - subBuilder: $1.MotorCommand.create) - ..aOM<$1.MotorCommand>(5, _omitFieldNames ? '' : 'elbow', - subBuilder: $1.MotorCommand.create) - ..aOM<$1.MotorCommand>(6, _omitFieldNames ? '' : 'gripperLift', - subBuilder: $1.MotorCommand.create) + ..aOM<$0.MotorCommand>(3, _omitFieldNames ? '' : 'swivel', + subBuilder: $0.MotorCommand.create) + ..aOM<$0.MotorCommand>(4, _omitFieldNames ? '' : 'shoulder', + subBuilder: $0.MotorCommand.create) + ..aOM<$0.MotorCommand>(5, _omitFieldNames ? '' : 'elbow', + subBuilder: $0.MotorCommand.create) + ..aOM<$0.MotorCommand>(6, _omitFieldNames ? '' : 'gripperLift', + subBuilder: $0.MotorCommand.create) ..aD(7, _omitFieldNames ? '' : 'ikX', fieldType: $pb.PbFieldType.OF) ..aD(8, _omitFieldNames ? '' : 'ikY', fieldType: $pb.PbFieldType.OF) ..aD(9, _omitFieldNames ? '' : 'ikZ', fieldType: $pb.PbFieldType.OF) ..aOB(10, _omitFieldNames ? '' : 'jab') - ..aOM<$2.Version>(11, _omitFieldNames ? '' : 'version', - subBuilder: $2.Version.create) + ..aOM<$1.Version>(11, _omitFieldNames ? '' : 'version', + subBuilder: $1.Version.create) ..aE<$3.BoolState>(12, _omitFieldNames ? '' : 'startUss', enumValues: $3.BoolState.values) - ..aOM<$1.MotorCommand>(13, _omitFieldNames ? '' : 'lift', - subBuilder: $1.MotorCommand.create) - ..aOM<$1.MotorCommand>(14, _omitFieldNames ? '' : 'rotate', - subBuilder: $1.MotorCommand.create) - ..aOM<$1.MotorCommand>(15, _omitFieldNames ? '' : 'pinch', - subBuilder: $1.MotorCommand.create) + ..aOM<$0.MotorCommand>(13, _omitFieldNames ? '' : 'lift', + subBuilder: $0.MotorCommand.create) + ..aOM<$0.MotorCommand>(14, _omitFieldNames ? '' : 'rotate', + subBuilder: $0.MotorCommand.create) + ..aOM<$0.MotorCommand>(15, _omitFieldNames ? '' : 'pinch', + subBuilder: $0.MotorCommand.create) ..aOB(16, _omitFieldNames ? '' : 'open') ..aOB(17, _omitFieldNames ? '' : 'close') ..aOB(18, _omitFieldNames ? '' : 'spin') ..aI(19, _omitFieldNames ? '' : 'servoAngle') ..aE<$3.BoolState>(20, _omitFieldNames ? '' : 'laserState', enumValues: $3.BoolState.values) - ..aOM<$1.MotorCommand>(21, _omitFieldNames ? '' : 'roll', - subBuilder: $1.MotorCommand.create) + ..aOM<$0.MotorCommand>(21, _omitFieldNames ? '' : 'roll', + subBuilder: $0.MotorCommand.create) + ..aOM(22, _omitFieldNames ? '' : 'wrist', + subBuilder: WristCommand.create) + ..aE<$3.BoolState>(23, _omitFieldNames ? '' : 'usingIk', + enumValues: $3.BoolState.values) + ..aD(24, _omitFieldNames ? '' : 'ikPitch', fieldType: $pb.PbFieldType.OF) + ..aD(25, _omitFieldNames ? '' : 'ikYaw', fieldType: $pb.PbFieldType.OF) + ..aOM<$2.Pose3d>(26, _omitFieldNames ? '' : 'pose', + subBuilder: $2.Pose3d.create) ..hasRequiredFields = false; @$core.Deprecated('See https://github.com/google/protobuf.dart/issues/998.') @@ -566,50 +893,48 @@ class ArmCommand extends $pb.GeneratedMessage { /// Move individual motors @$pb.TagNumber(3) - $1.MotorCommand get swivel => $_getN(2); + $0.MotorCommand get swivel => $_getN(2); @$pb.TagNumber(3) - set swivel($1.MotorCommand value) => $_setField(3, value); + set swivel($0.MotorCommand value) => $_setField(3, value); @$pb.TagNumber(3) $core.bool hasSwivel() => $_has(2); @$pb.TagNumber(3) void clearSwivel() => $_clearField(3); @$pb.TagNumber(3) - $1.MotorCommand ensureSwivel() => $_ensure(2); + $0.MotorCommand ensureSwivel() => $_ensure(2); @$pb.TagNumber(4) - $1.MotorCommand get shoulder => $_getN(3); + $0.MotorCommand get shoulder => $_getN(3); @$pb.TagNumber(4) - set shoulder($1.MotorCommand value) => $_setField(4, value); + set shoulder($0.MotorCommand value) => $_setField(4, value); @$pb.TagNumber(4) $core.bool hasShoulder() => $_has(3); @$pb.TagNumber(4) void clearShoulder() => $_clearField(4); @$pb.TagNumber(4) - $1.MotorCommand ensureShoulder() => $_ensure(3); + $0.MotorCommand ensureShoulder() => $_ensure(3); @$pb.TagNumber(5) - $1.MotorCommand get elbow => $_getN(4); + $0.MotorCommand get elbow => $_getN(4); @$pb.TagNumber(5) - set elbow($1.MotorCommand value) => $_setField(5, value); + set elbow($0.MotorCommand value) => $_setField(5, value); @$pb.TagNumber(5) $core.bool hasElbow() => $_has(4); @$pb.TagNumber(5) void clearElbow() => $_clearField(5); @$pb.TagNumber(5) - $1.MotorCommand ensureElbow() => $_ensure(4); + $0.MotorCommand ensureElbow() => $_ensure(4); - /// Needed for IK: If the wrist-lift moves, we need to re-calculate IK to keep the end-effector - /// stationary. See /Arm/src/ik/README.md in the Arm-Firmware repository. @$pb.TagNumber(6) - $1.MotorCommand get gripperLift => $_getN(5); + $0.MotorCommand get gripperLift => $_getN(5); @$pb.TagNumber(6) - set gripperLift($1.MotorCommand value) => $_setField(6, value); + set gripperLift($0.MotorCommand value) => $_setField(6, value); @$pb.TagNumber(6) $core.bool hasGripperLift() => $_has(5); @$pb.TagNumber(6) void clearGripperLift() => $_clearField(6); @$pb.TagNumber(6) - $1.MotorCommand ensureGripperLift() => $_ensure(5); + $0.MotorCommand ensureGripperLift() => $_ensure(5); /// Can be removed in future versions @$pb.TagNumber(7) @@ -650,15 +975,15 @@ class ArmCommand extends $pb.GeneratedMessage { void clearJab() => $_clearField(10); @$pb.TagNumber(11) - $2.Version get version => $_getN(10); + $1.Version get version => $_getN(10); @$pb.TagNumber(11) - set version($2.Version value) => $_setField(11, value); + set version($1.Version value) => $_setField(11, value); @$pb.TagNumber(11) $core.bool hasVersion() => $_has(10); @$pb.TagNumber(11) void clearVersion() => $_clearField(11); @$pb.TagNumber(11) - $2.Version ensureVersion() => $_ensure(10); + $1.Version ensureVersion() => $_ensure(10); /// USS commands @$pb.TagNumber(12) @@ -672,37 +997,37 @@ class ArmCommand extends $pb.GeneratedMessage { /// Move individual motors @$pb.TagNumber(13) - $1.MotorCommand get lift => $_getN(12); + $0.MotorCommand get lift => $_getN(12); @$pb.TagNumber(13) - set lift($1.MotorCommand value) => $_setField(13, value); + set lift($0.MotorCommand value) => $_setField(13, value); @$pb.TagNumber(13) $core.bool hasLift() => $_has(12); @$pb.TagNumber(13) void clearLift() => $_clearField(13); @$pb.TagNumber(13) - $1.MotorCommand ensureLift() => $_ensure(12); + $0.MotorCommand ensureLift() => $_ensure(12); @$pb.TagNumber(14) - $1.MotorCommand get rotate => $_getN(13); + $0.MotorCommand get rotate => $_getN(13); @$pb.TagNumber(14) - set rotate($1.MotorCommand value) => $_setField(14, value); + set rotate($0.MotorCommand value) => $_setField(14, value); @$pb.TagNumber(14) $core.bool hasRotate() => $_has(13); @$pb.TagNumber(14) void clearRotate() => $_clearField(14); @$pb.TagNumber(14) - $1.MotorCommand ensureRotate() => $_ensure(13); + $0.MotorCommand ensureRotate() => $_ensure(13); @$pb.TagNumber(15) - $1.MotorCommand get pinch => $_getN(14); + $0.MotorCommand get pinch => $_getN(14); @$pb.TagNumber(15) - set pinch($1.MotorCommand value) => $_setField(15, value); + set pinch($0.MotorCommand value) => $_setField(15, value); @$pb.TagNumber(15) $core.bool hasPinch() => $_has(14); @$pb.TagNumber(15) void clearPinch() => $_clearField(15); @$pb.TagNumber(15) - $1.MotorCommand ensurePinch() => $_ensure(14); + $0.MotorCommand ensurePinch() => $_ensure(14); /// Custom actions @$pb.TagNumber(16) @@ -751,15 +1076,64 @@ class ArmCommand extends $pb.GeneratedMessage { void clearLaserState() => $_clearField(20); @$pb.TagNumber(21) - $1.MotorCommand get roll => $_getN(20); + $0.MotorCommand get roll => $_getN(20); @$pb.TagNumber(21) - set roll($1.MotorCommand value) => $_setField(21, value); + set roll($0.MotorCommand value) => $_setField(21, value); @$pb.TagNumber(21) $core.bool hasRoll() => $_has(20); @$pb.TagNumber(21) void clearRoll() => $_clearField(21); @$pb.TagNumber(21) - $1.MotorCommand ensureRoll() => $_ensure(20); + $0.MotorCommand ensureRoll() => $_ensure(20); + + @$pb.TagNumber(22) + WristCommand get wrist => $_getN(21); + @$pb.TagNumber(22) + set wrist(WristCommand value) => $_setField(22, value); + @$pb.TagNumber(22) + $core.bool hasWrist() => $_has(21); + @$pb.TagNumber(22) + void clearWrist() => $_clearField(22); + @$pb.TagNumber(22) + WristCommand ensureWrist() => $_ensure(21); + + @$pb.TagNumber(23) + $3.BoolState get usingIk => $_getN(22); + @$pb.TagNumber(23) + set usingIk($3.BoolState value) => $_setField(23, value); + @$pb.TagNumber(23) + $core.bool hasUsingIk() => $_has(22); + @$pb.TagNumber(23) + void clearUsingIk() => $_clearField(23); + + @$pb.TagNumber(24) + $core.double get ikPitch => $_getN(23); + @$pb.TagNumber(24) + set ikPitch($core.double value) => $_setFloat(23, value); + @$pb.TagNumber(24) + $core.bool hasIkPitch() => $_has(23); + @$pb.TagNumber(24) + void clearIkPitch() => $_clearField(24); + + @$pb.TagNumber(25) + $core.double get ikYaw => $_getN(24); + @$pb.TagNumber(25) + set ikYaw($core.double value) => $_setFloat(24, value); + @$pb.TagNumber(25) + $core.bool hasIkYaw() => $_has(24); + @$pb.TagNumber(25) + void clearIkYaw() => $_clearField(25); + + @$pb.TagNumber(26) + $2.Pose3d get pose => $_getN(25); + @$pb.TagNumber(26) + set pose($2.Pose3d value) => $_setField(26, value); + @$pb.TagNumber(26) + $core.bool hasPose() => $_has(25); + @$pb.TagNumber(26) + void clearPose() => $_clearField(26); + @$pb.TagNumber(26) + $2.Pose3d ensurePose() => $_ensure(25); } const $core.bool _omitFieldNames = diff --git a/burt_network/lib/src/generated/arm.pbjson.dart b/burt_network/lib/src/generated/arm.pbjson.dart index 8b9cb1fc..8429942f 100644 --- a/burt_network/lib/src/generated/arm.pbjson.dart +++ b/burt_network/lib/src/generated/arm.pbjson.dart @@ -55,6 +55,42 @@ final $typed_data.Uint8List jointAngleDataDescriptor = $convert.base64Decode( 'KAJSCXJvbGxNdWx0aRIqChF3cmlzdF9waXRjaF9tdWx0aRgKIAEoAlIPd3Jpc3RQaXRjaE11bH' 'Rp'); +@$core.Deprecated('Use wristDataDescriptor instead') +const WristData$json = { + '1': 'WristData', + '2': [ + {'1': 'pitch_angle', '3': 1, '4': 1, '5': 2, '10': 'pitchAngle'}, + {'1': 'roll_angle', '3': 2, '4': 1, '5': 2, '10': 'rollAngle'}, + {'1': 'motorA', '3': 3, '4': 1, '5': 11, '6': '.MotorData', '10': 'motorA'}, + {'1': 'motorB', '3': 4, '4': 1, '5': 11, '6': '.MotorData', '10': 'motorB'}, + { + '1': 'is_moving', + '3': 5, + '4': 1, + '5': 14, + '6': '.BoolState', + '10': 'isMoving' + }, + { + '1': 'is_calibrated', + '3': 6, + '4': 1, + '5': 14, + '6': '.BoolState', + '10': 'isCalibrated' + }, + {'1': 'version', '3': 7, '4': 1, '5': 11, '6': '.Version', '10': 'version'}, + ], +}; + +/// Descriptor for `WristData`. Decode as a `google.protobuf.DescriptorProto`. +final $typed_data.Uint8List wristDataDescriptor = $convert.base64Decode( + 'CglXcmlzdERhdGESHwoLcGl0Y2hfYW5nbGUYASABKAJSCnBpdGNoQW5nbGUSHQoKcm9sbF9hbm' + 'dsZRgCIAEoAlIJcm9sbEFuZ2xlEiIKBm1vdG9yQRgDIAEoCzIKLk1vdG9yRGF0YVIGbW90b3JB' + 'EiIKBm1vdG9yQhgEIAEoCzIKLk1vdG9yRGF0YVIGbW90b3JCEicKCWlzX21vdmluZxgFIAEoDj' + 'IKLkJvb2xTdGF0ZVIIaXNNb3ZpbmcSLwoNaXNfY2FsaWJyYXRlZBgGIAEoDjIKLkJvb2xTdGF0' + 'ZVIMaXNDYWxpYnJhdGVkEiIKB3ZlcnNpb24YByABKAsyCC5WZXJzaW9uUgd2ZXJzaW9u'); + @$core.Deprecated('Use armDataDescriptor instead') const ArmData$json = { '1': 'ArmData', @@ -67,6 +103,14 @@ const ArmData$json = { '6': '.Coordinates', '10': 'currentPosition' }, + { + '1': 'current_orientation', + '3': 17, + '4': 1, + '5': 11, + '6': '.Orientation', + '10': 'currentOrientation' + }, { '1': 'target_position', '3': 2, @@ -75,6 +119,14 @@ const ArmData$json = { '6': '.Coordinates', '10': 'targetPosition' }, + { + '1': 'target_orientation', + '3': 18, + '4': 1, + '5': 11, + '6': '.Orientation', + '10': 'targetOrientation' + }, {'1': 'base', '3': 3, '4': 1, '5': 11, '6': '.MotorData', '10': 'base'}, { '1': 'shoulder', @@ -90,6 +142,7 @@ const ArmData$json = { {'1': 'uss_distance', '3': 7, '4': 1, '5': 2, '10': 'ussDistance'}, {'1': 'lift', '3': 8, '4': 1, '5': 11, '6': '.MotorData', '10': 'lift'}, {'1': 'rotate', '3': 9, '4': 1, '5': 11, '6': '.MotorData', '10': 'rotate'}, + {'1': 'wrist', '3': 15, '4': 1, '5': 11, '6': '.WristData', '10': 'wrist'}, {'1': 'pinch', '3': 10, '4': 1, '5': 11, '6': '.MotorData', '10': 'pinch'}, {'1': 'servo_angle', '3': 11, '4': 1, '5': 5, '10': 'servoAngle'}, { @@ -108,22 +161,59 @@ const ArmData$json = { '6': '.JointAngleData', '10': 'jointAngles' }, + { + '1': 'using_ik', + '3': 16, + '4': 1, + '5': 14, + '6': '.BoolState', + '10': 'usingIk' + }, ], }; /// Descriptor for `ArmData`. Decode as a `google.protobuf.DescriptorProto`. final $typed_data.Uint8List armDataDescriptor = $convert.base64Decode( 'CgdBcm1EYXRhEjcKEGN1cnJlbnRfcG9zaXRpb24YASABKAsyDC5Db29yZGluYXRlc1IPY3Vycm' - 'VudFBvc2l0aW9uEjUKD3RhcmdldF9wb3NpdGlvbhgCIAEoCzIMLkNvb3JkaW5hdGVzUg50YXJn' - 'ZXRQb3NpdGlvbhIeCgRiYXNlGAMgASgLMgouTW90b3JEYXRhUgRiYXNlEiYKCHNob3VsZGVyGA' - 'QgASgLMgouTW90b3JEYXRhUghzaG91bGRlchIgCgVlbGJvdxgFIAEoCzIKLk1vdG9yRGF0YVIF' - 'ZWxib3cSHgoEcm9sbBgNIAEoCzIKLk1vdG9yRGF0YVIEcm9sbBIiCgd2ZXJzaW9uGAYgASgLMg' - 'guVmVyc2lvblIHdmVyc2lvbhIhCgx1c3NfZGlzdGFuY2UYByABKAJSC3Vzc0Rpc3RhbmNlEh4K' - 'BGxpZnQYCCABKAsyCi5Nb3RvckRhdGFSBGxpZnQSIgoGcm90YXRlGAkgASgLMgouTW90b3JEYX' - 'RhUgZyb3RhdGUSIAoFcGluY2gYCiABKAsyCi5Nb3RvckRhdGFSBXBpbmNoEh8KC3NlcnZvX2Fu' - 'Z2xlGAsgASgFUgpzZXJ2b0FuZ2xlEisKC2xhc2VyX3N0YXRlGAwgASgOMgouQm9vbFN0YXRlUg' - 'psYXNlclN0YXRlEjIKDGpvaW50X2FuZ2xlcxgOIAEoCzIPLkpvaW50QW5nbGVEYXRhUgtqb2lu' - 'dEFuZ2xlcw=='); + 'VudFBvc2l0aW9uEj0KE2N1cnJlbnRfb3JpZW50YXRpb24YESABKAsyDC5PcmllbnRhdGlvblIS' + 'Y3VycmVudE9yaWVudGF0aW9uEjUKD3RhcmdldF9wb3NpdGlvbhgCIAEoCzIMLkNvb3JkaW5hdG' + 'VzUg50YXJnZXRQb3NpdGlvbhI7ChJ0YXJnZXRfb3JpZW50YXRpb24YEiABKAsyDC5PcmllbnRh' + 'dGlvblIRdGFyZ2V0T3JpZW50YXRpb24SHgoEYmFzZRgDIAEoCzIKLk1vdG9yRGF0YVIEYmFzZR' + 'ImCghzaG91bGRlchgEIAEoCzIKLk1vdG9yRGF0YVIIc2hvdWxkZXISIAoFZWxib3cYBSABKAsy' + 'Ci5Nb3RvckRhdGFSBWVsYm93Eh4KBHJvbGwYDSABKAsyCi5Nb3RvckRhdGFSBHJvbGwSIgoHdm' + 'Vyc2lvbhgGIAEoCzIILlZlcnNpb25SB3ZlcnNpb24SIQoMdXNzX2Rpc3RhbmNlGAcgASgCUgt1' + 'c3NEaXN0YW5jZRIeCgRsaWZ0GAggASgLMgouTW90b3JEYXRhUgRsaWZ0EiIKBnJvdGF0ZRgJIA' + 'EoCzIKLk1vdG9yRGF0YVIGcm90YXRlEiAKBXdyaXN0GA8gASgLMgouV3Jpc3REYXRhUgV3cmlz' + 'dBIgCgVwaW5jaBgKIAEoCzIKLk1vdG9yRGF0YVIFcGluY2gSHwoLc2Vydm9fYW5nbGUYCyABKA' + 'VSCnNlcnZvQW5nbGUSKwoLbGFzZXJfc3RhdGUYDCABKA4yCi5Cb29sU3RhdGVSCmxhc2VyU3Rh' + 'dGUSMgoMam9pbnRfYW5nbGVzGA4gASgLMg8uSm9pbnRBbmdsZURhdGFSC2pvaW50QW5nbGVzEi' + 'UKCHVzaW5nX2lrGBAgASgOMgouQm9vbFN0YXRlUgd1c2luZ0lr'); + +@$core.Deprecated('Use wristCommandDescriptor instead') +const WristCommand$json = { + '1': 'WristCommand', + '2': [ + {'1': 'stop', '3': 1, '4': 1, '5': 8, '10': 'stop'}, + {'1': 'calibrate', '3': 2, '4': 1, '5': 8, '10': 'calibrate'}, + { + '1': 'pitch', + '3': 3, + '4': 1, + '5': 11, + '6': '.MotorCommand', + '10': 'pitch' + }, + {'1': 'roll', '3': 4, '4': 1, '5': 11, '6': '.MotorCommand', '10': 'roll'}, + {'1': 'version', '3': 5, '4': 1, '5': 11, '6': '.Version', '10': 'version'}, + ], +}; + +/// Descriptor for `WristCommand`. Decode as a `google.protobuf.DescriptorProto`. +final $typed_data.Uint8List wristCommandDescriptor = $convert.base64Decode( + 'CgxXcmlzdENvbW1hbmQSEgoEc3RvcBgBIAEoCFIEc3RvcBIcCgljYWxpYnJhdGUYAiABKAhSCW' + 'NhbGlicmF0ZRIjCgVwaXRjaBgDIAEoCzINLk1vdG9yQ29tbWFuZFIFcGl0Y2gSIQoEcm9sbBgE' + 'IAEoCzINLk1vdG9yQ29tbWFuZFIEcm9sbBIiCgd2ZXJzaW9uGAUgASgLMgguVmVyc2lvblIHdm' + 'Vyc2lvbg=='); @$core.Deprecated('Use armCommandDescriptor instead') const ArmCommand$json = { @@ -167,6 +257,9 @@ const ArmCommand$json = { {'1': 'ik_x', '3': 7, '4': 1, '5': 2, '10': 'ikX'}, {'1': 'ik_y', '3': 8, '4': 1, '5': 2, '10': 'ikY'}, {'1': 'ik_z', '3': 9, '4': 1, '5': 2, '10': 'ikZ'}, + {'1': 'ik_pitch', '3': 24, '4': 1, '5': 2, '10': 'ikPitch'}, + {'1': 'ik_yaw', '3': 25, '4': 1, '5': 2, '10': 'ikYaw'}, + {'1': 'pose', '3': 26, '4': 1, '5': 11, '6': '.Pose3d', '10': 'pose'}, {'1': 'jab', '3': 10, '4': 1, '5': 8, '10': 'jab'}, { '1': 'version', @@ -193,6 +286,14 @@ const ArmCommand$json = { '6': '.MotorCommand', '10': 'rotate' }, + { + '1': 'wrist', + '3': 22, + '4': 1, + '5': 11, + '6': '.WristCommand', + '10': 'wrist' + }, { '1': 'pinch', '3': 15, @@ -213,6 +314,14 @@ const ArmCommand$json = { '6': '.BoolState', '10': 'laserState' }, + { + '1': 'using_ik', + '3': 23, + '4': 1, + '5': 14, + '6': '.BoolState', + '10': 'usingIk' + }, ], }; @@ -223,11 +332,13 @@ final $typed_data.Uint8List armCommandDescriptor = $convert.base64Decode( 'ZXIYBCABKAsyDS5Nb3RvckNvbW1hbmRSCHNob3VsZGVyEiMKBWVsYm93GAUgASgLMg0uTW90b3' 'JDb21tYW5kUgVlbGJvdxIhCgRyb2xsGBUgASgLMg0uTW90b3JDb21tYW5kUgRyb2xsEjAKDGdy' 'aXBwZXJfbGlmdBgGIAEoCzINLk1vdG9yQ29tbWFuZFILZ3JpcHBlckxpZnQSEQoEaWtfeBgHIA' - 'EoAlIDaWtYEhEKBGlrX3kYCCABKAJSA2lrWRIRCgRpa196GAkgASgCUgNpa1oSEAoDamFiGAog' - 'ASgIUgNqYWISIgoHdmVyc2lvbhgLIAEoCzIILlZlcnNpb25SB3ZlcnNpb24SJwoJc3RhcnRfdX' - 'NzGAwgASgOMgouQm9vbFN0YXRlUghzdGFydFVzcxIhCgRsaWZ0GA0gASgLMg0uTW90b3JDb21t' - 'YW5kUgRsaWZ0EiUKBnJvdGF0ZRgOIAEoCzINLk1vdG9yQ29tbWFuZFIGcm90YXRlEiMKBXBpbm' - 'NoGA8gASgLMg0uTW90b3JDb21tYW5kUgVwaW5jaBISCgRvcGVuGBAgASgIUgRvcGVuEhQKBWNs' - 'b3NlGBEgASgIUgVjbG9zZRISCgRzcGluGBIgASgIUgRzcGluEh8KC3NlcnZvX2FuZ2xlGBMgAS' - 'gFUgpzZXJ2b0FuZ2xlEisKC2xhc2VyX3N0YXRlGBQgASgOMgouQm9vbFN0YXRlUgpsYXNlclN0' - 'YXRl'); + 'EoAlIDaWtYEhEKBGlrX3kYCCABKAJSA2lrWRIRCgRpa196GAkgASgCUgNpa1oSGQoIaWtfcGl0' + 'Y2gYGCABKAJSB2lrUGl0Y2gSFQoGaWtfeWF3GBkgASgCUgVpa1lhdxIbCgRwb3NlGBogASgLMg' + 'cuUG9zZTNkUgRwb3NlEhAKA2phYhgKIAEoCFIDamFiEiIKB3ZlcnNpb24YCyABKAsyCC5WZXJz' + 'aW9uUgd2ZXJzaW9uEicKCXN0YXJ0X3VzcxgMIAEoDjIKLkJvb2xTdGF0ZVIIc3RhcnRVc3MSIQ' + 'oEbGlmdBgNIAEoCzINLk1vdG9yQ29tbWFuZFIEbGlmdBIlCgZyb3RhdGUYDiABKAsyDS5Nb3Rv' + 'ckNvbW1hbmRSBnJvdGF0ZRIjCgV3cmlzdBgWIAEoCzINLldyaXN0Q29tbWFuZFIFd3Jpc3QSIw' + 'oFcGluY2gYDyABKAsyDS5Nb3RvckNvbW1hbmRSBXBpbmNoEhIKBG9wZW4YECABKAhSBG9wZW4S' + 'FAoFY2xvc2UYESABKAhSBWNsb3NlEhIKBHNwaW4YEiABKAhSBHNwaW4SHwoLc2Vydm9fYW5nbG' + 'UYEyABKAVSCnNlcnZvQW5nbGUSKwoLbGFzZXJfc3RhdGUYFCABKA4yCi5Cb29sU3RhdGVSCmxh' + 'c2VyU3RhdGUSJQoIdXNpbmdfaWsYFyABKA4yCi5Cb29sU3RhdGVSB3VzaW5nSWs='); From aeaea942cebb85768bc92edfaffd755222f890d1 Mon Sep 17 00:00:00 2001 From: preston Date: Sun, 8 Feb 2026 11:33:28 -0500 Subject: [PATCH 16/16] Update Protobuf submodule to valid commit --- burt_network/Protobuf | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/burt_network/Protobuf b/burt_network/Protobuf index 424e5379..06dc109a 160000 --- a/burt_network/Protobuf +++ b/burt_network/Protobuf @@ -1 +1 @@ -Subproject commit 424e5379c018e6fc71e034eb79cf48f2161cf828 +Subproject commit 06dc109a951bb384cd6640bd64685647ed33efe5