URC code: Aruco, obstacles, and stream networking (#3)

Co-authored-by: Binghamton Rover <[email protected]> Co-authored-by: Paul Ampadu <pgrwe> Co-authored-by: aidan ahram <[email protected]>
BinghamtonRover · Sep 10, 2024 · 2fe1ce1 · 2fe1ce1
1 parent e16f6c4
commit 2fe1ce1
Show file tree

Hide file tree

Showing 40 changed files with 958 additions and 266 deletions.
diff --git a/.github/workflows/analyze.yml b/.github/workflows/analyze.yml
@@ -16,12 +16,12 @@ jobs:
       # You can specify other versions if desired, see documentation here:
       # https://github.com/dart-lang/setup-dart/blob/main/README.md
       - uses: dart-lang/setup-dart@v1
-        with: 
-          sdk: 3.2.2
+        with:
+          sdk: 3.5.2
 
       - name: Install dependencies
         run: dart pub get
-        
+
       - name: Analyze project source
         run: dart analyze --fatal-infos
 

diff --git a/bin/arcuo.dart b/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/bin/aruco.dart b/bin/aruco.dart
@@ -0,0 +1,15 @@
+import "package:autonomy/autonomy.dart";
+
+void main() async {
+  final rover = RoverAutonomy();
+  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/bin/sensorless.dart b/bin/sensorless.dart
@@ -1,6 +1,7 @@
 import "package:burt_network/logging.dart";
 import "package:burt_network/generated.dart";
 
+import "package:autonomy/interfaces.dart";
 import "package:autonomy/simulator.dart";
 import "package:autonomy/rover.dart";
 

diff --git a/bin/task.dart b/bin/task.dart
@@ -12,13 +12,15 @@ final obstacles = <SimulatedObstacle>[
 // Enter in the Dashboard: Destination = (lat=7, long=0);
 
 void main() async {
-  Logger.level = LogLevel.all;
-  final simulator = AutonomySimulator();
-  final detector = DetectorSimulator(collection: simulator, obstacles: obstacles);
-  simulator.detector = detector;
+  Logger.level = LogLevel.debug;
+  final simulator = RoverAutonomy();
+  simulator.detector = DetectorSimulator(collection: simulator, obstacles: obstacles);
   simulator.pathfinder = RoverPathfinder(collection: simulator);
   simulator.orchestrator = RoverOrchestrator(collection: simulator);
-  simulator.drive = DriveSimulator(collection: simulator, shouldDelay: true);
+//  simulator.drive = SensorlessDrive(collection: simulator, useImu: true, useGps: true);
   await simulator.init();
-  await simulator.server.waitForConnection();
+	await simulator.waitForValue();
+	await simulator.drive.faceNorth();
+//	await simulator.drive.goForward();
+//  await simulator.server.waitForConnection();
 }
diff --git a/bin/test.dart b/bin/test.dart
@@ -0,0 +1,17 @@
+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, useImu: false);
+
+  await rover.init();
+
+  // rover.logger.info("Done");
+  // await rover.dispose();
+}
diff --git a/lib/interfaces.dart b/lib/interfaces.dart
@@ -9,7 +9,7 @@ export "src/interfaces/imu.dart";
 export "src/interfaces/imu_utils.dart";
 export "src/interfaces/pathfinding.dart";
 export "src/interfaces/server.dart";
-export "src/interfaces/realsense.dart";
+export "src/interfaces/video.dart";
 export "src/interfaces/receiver.dart";
 export "src/interfaces/reporter.dart";
 export "src/interfaces/service.dart";

diff --git a/lib/rover.dart b/lib/rover.dart
@@ -4,28 +4,34 @@ export "src/rover/orchestrator.dart";
 export "src/rover/sensorless.dart";
 
 import "package:autonomy/interfaces.dart";
-import "package:autonomy/simulator.dart";
-import "package:burt_network/logging.dart";
+import "package:burt_network/burt_network.dart";
 
 import "src/rover/pathfinding.dart";
-import "src/rover/server.dart";
 import "src/rover/drive.dart";
 import "src/rover/gps.dart";
 import "src/rover/imu.dart";
 import "src/rover/orchestrator.dart";
-import "src/rover/realsense.dart";
+import "src/rover/video.dart";
+import "src/rover/detector.dart";
 
 /// A collection of all the different services used by the autonomy program.
 class RoverAutonomy extends AutonomyInterface {
 	/// A server to communicate with the dashboard and receive data from the subsystems.
-	@override late final AutonomyServer server = AutonomyServer(collection: this);
+	// @override late final AutonomyServer server = AutonomyServer(collection: this);
+  @override late final server = RoverSocket(port: 8003, device: Device.AUTONOMY, collection: this);
 	/// A helper class to handle driving the rover.
 	@override late DriveInterface drive = RoverDrive(collection: this);
   @override late GpsInterface gps = RoverGps(collection: this);
   @override late ImuInterface imu = RoverImu(collection: this);
   @override late final logger = BurtLogger(socket: server);
-  @override late final pathfinder = RoverPathfinder(collection: this);
-  @override late final detector = DetectorSimulator(collection: this, obstacles: []);
-  @override late final realsense = RoverRealsense(collection: this);
-  @override late final orchestrator = RoverOrchestrator(collection: this); 
+  @override late PathfindingInterface pathfinder = RoverPathfinder(collection: this);
+  @override late DetectorInterface detector = RoverDetector(collection: this);
+  @override late VideoInterface video = RoverVideo(collection: this);
+  @override late OrchestratorInterface orchestrator = RoverOrchestrator(collection: this);
+
+  @override
+  Future<void> onDisconnect() async {
+    await super.onDisconnect();
+    await orchestrator.abort();
+  }
 }
diff --git a/lib/simulator.dart b/lib/simulator.dart
@@ -3,36 +3,34 @@ export "src/simulator/drive.dart";
 export "src/simulator/gps.dart";
 export "src/simulator/imu.dart";
 export "src/simulator/realsense.dart";
-export "src/simulator/server.dart";
 
 import "package:autonomy/interfaces.dart";
 import "package:autonomy/src/simulator/orchestrator.dart";
-import "package:burt_network/logging.dart";
+import "package:burt_network/burt_network.dart";
 
 import "src/simulator/detector.dart";
 import "src/simulator/drive.dart";
 import "src/simulator/gps.dart";
 import "src/simulator/imu.dart";
 import "src/simulator/pathfinding.dart";
 import "src/simulator/realsense.dart";
-import "src/simulator/server.dart";
 
 class AutonomySimulator extends AutonomyInterface {
   @override late final logger = BurtLogger(socket: server);
-  @override late ServerInterface server = SimulatorServer(collection: this);
+  @override late final server = RoverSocket(port: 8003, device: Device.AUTONOMY, collection: this);
   @override late GpsInterface gps = GpsSimulator(collection: this);
   @override late ImuInterface imu = ImuSimulator(collection: this);
   @override late DriveInterface drive = DriveSimulator(collection: this);
   @override late PathfindingInterface pathfinder = PathfindingSimulator(collection: this);
   @override late DetectorInterface detector = DetectorSimulator(collection: this, obstacles: []);
-  @override late RealSenseInterface realsense = RealSenseSimulator(collection: this);
+  @override late VideoInterface video = VideoSimulator(collection: this);
   @override late OrchestratorInterface orchestrator = OrchestratorSimulator(collection: this);
 
   bool isInitialized = false;
 
   @override
   Future<bool> init() { isInitialized = true; return super.init(); }
-  
+
   @override
   Future<void> dispose() { isInitialized = false; return super.dispose(); }
 }
diff --git a/lib/src/interfaces/autonomy.dart b/lib/src/interfaces/autonomy.dart
@@ -1,4 +1,4 @@
-import "package:burt_network/logging.dart";
+import "package:burt_network/burt_network.dart";
 
 import "package:autonomy/interfaces.dart";
 
@@ -7,12 +7,12 @@ abstract class AutonomyInterface extends Service with Receiver {
   GpsInterface get gps;
   ImuInterface get imu;
   DriveInterface get drive;
-  ServerInterface get server;
+  RoverSocket get server;
   PathfindingInterface get pathfinder;
   DetectorInterface get detector;
-  RealSenseInterface get realsense;
+  VideoInterface get video;
   OrchestratorInterface get orchestrator;
-  
+
   @override
   Future<bool> init() async {
     var result = true;
@@ -22,42 +22,45 @@ abstract class AutonomyInterface extends Service with Receiver {
     result &= await server.init();
     result &= await pathfinder.init();
     result &= await detector.init();
-    result &= await realsense.init();
+    result &= await video.init();
     result &= await orchestrator.init();
     if (result) {
       logger.info("Autonomy initialized");
     } else {
       logger.warning("Autonomy could not initialize");
     }
+//    await waitForValue();
     return result;
+
   }
-  
+
   @override
   Future<void> dispose() async {
-    await server.dispose();
     await gps.dispose();
     await imu.dispose();
     await drive.dispose();
     await pathfinder.dispose();
     await detector.dispose();
-    await realsense.dispose();
+    await video.dispose();
     await orchestrator.dispose();
+    await server.dispose();
     logger.info("Autonomy disposed");
   }
-  
+
   Future<void> restart() async {
     await dispose();
     await init();
   }
-  
+
   @override
   Future<void> waitForValue() async {
     logger.info("Waiting for readings...");
     await gps.waitForValue();
     await imu.waitForValue();
-    await realsense.waitForValue();
+    await video.waitForValue();
+    logger.info("Received GPS and IMU values");
   }
 
   @override
-  bool get hasValue => gps.hasValue && imu.hasValue && realsense.hasValue;
+  bool get hasValue => gps.hasValue && imu.hasValue && video.hasValue;
 }
diff --git a/lib/src/interfaces/detector.dart b/lib/src/interfaces/detector.dart
@@ -1,6 +1,9 @@
-import "service.dart";
+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/lib/src/interfaces/drive.dart b/lib/src/interfaces/drive.dart
@@ -42,6 +42,7 @@ enum DriveOrientation {
 
 abstract class DriveInterface extends Service {
   AutonomyInterface collection;
+  DriveOrientation orientation = DriveOrientation.north;
   DriveInterface({required this.collection});
 
   Future<void> goDirection(DriveDirection direction) async => switch (direction) {
@@ -58,9 +59,21 @@ abstract class DriveInterface extends Service {
   Future<void> turnRight();
   Future<void> stop();
 
+  Future<void> faceDirection(DriveOrientation orientation) async {
+    this.orientation = orientation;
+  }
+
   Future<void> followPath(List<AutonomyAStarState> 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<bool> spinForAruco() async => false;
+  Future<void> approachAruco() async { }
 }
diff --git a/lib/src/interfaces/gps_utils.dart b/lib/src/interfaces/gps_utils.dart
@@ -49,7 +49,8 @@ extension GpsUtils on GpsCoordinates {
     longitude: longitude + other.longitude,
   );
 
-  String prettyPrint() => "(lat=${(latitude * GpsUtils.metersPerLatitude).toStringAsFixed(2)}, long=${(longitude * GpsUtils.metersPerLongitude).toStringAsFixed(2)})";
+//  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,

diff --git a/lib/src/interfaces/imu.dart b/lib/src/interfaces/imu.dart
@@ -7,7 +7,10 @@ abstract class ImuInterface extends Service with Receiver {
 
   double get heading => raw.z;
   Orientation get raw;
-  DriveOrientation? get orientation => DriveOrientation.fromRaw(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);
 }
diff --git a/lib/src/interfaces/imu_utils.dart b/lib/src/interfaces/imu_utils.dart
@@ -1,7 +1,7 @@
 import "package:burt_network/generated.dart";
 
 extension OrientationUtils on Orientation {
-  static const double epsilon = 15;
+  static const double epsilon = 10;
 
   static final north = Orientation(z: 0);
   static final west = Orientation(z: 90);

diff --git a/lib/src/interfaces/orchestrator.dart b/lib/src/interfaces/orchestrator.dart
@@ -1,7 +1,7 @@
 import "dart:io";
 
-import "package:burt_network/generated.dart";
 import "package:autonomy/interfaces.dart";
+import "package:burt_network/burt_network.dart";
 import "package:meta/meta.dart";
 
 abstract class OrchestratorInterface extends Service {
@@ -11,6 +11,13 @@ abstract class OrchestratorInterface extends Service {
   AutonomyCommand? currentCommand;
   AutonomyState currentState = AutonomyState.AUTONOMY_STATE_UNDEFINED;
   Future<void> 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;
@@ -21,11 +28,21 @@ abstract class OrchestratorInterface extends Service {
     switch (command.task) {
       case AutonomyTask.GPS_ONLY: await handleGpsTask(command);
       case AutonomyTask.VISUAL_MARKER: await handleArucoTask(command);
-      // TODO: Add more tasks 
+      // TODO: Add more tasks
       default: collection.logger.error("Unrecognized task: ${command.task}");  // ignore: no_default_cases
     }
   }
 
+  @override
+  Future<bool> init() async {
+    collection.server.messages.onMessage(
+      name: AutonomyCommand().messageName,
+      constructor: AutonomyCommand.fromBuffer,
+      callback: onCommand,
+    );
+    return true;
+  }
+
   @mustCallSuper
   Future<void> abort() async {
     currentCommand = null;
@@ -35,7 +52,7 @@ abstract class OrchestratorInterface extends Service {
     await collection.dispose();
     exit(1);
   }
-  
+
   Future<void> handleGpsTask(AutonomyCommand command);
   Future<void> handleArucoTask(AutonomyCommand command);
   Future<void> handleHammerTask(AutonomyCommand command);