Lidar Obstacle Detection

lib/src/detector/detector_interface.dart

@@ -5,6 +5,5 @@ abstract class DetectorInterface extends Service {
   DetectorInterface({required this.collection});
 
   bool findObstacles();
-  bool canSeeAruco();
   bool isOnSlope();
 }

lib/src/detector/network_detector.dart

@@ -6,9 +6,6 @@ class NetworkDetector extends DetectorInterface {
 
   NetworkDetector({required super.collection});
 
-  @override
-  bool canSeeAruco() => false;
-
   @override
   Future<void> dispose() async {}
 

lib/src/detector/rover_detector.dart

@@ -1,21 +1,118 @@
+import "dart:async";
+import "dart:math";
+
 import "package:autonomy/interfaces.dart";
 
 class RoverDetector extends DetectorInterface {
+  StreamSubscription<LidarPointCloud>? _subscription;
+
+  LidarPointCloud cloudCache = LidarPointCloud();
+
+  List<GpsCoordinates> queuedObstacles = [];
+  List<GpsCoordinates> previousObstacles = [];
+
   RoverDetector({required super.collection});
 
+  void _handleLidarCloud(LidarPointCloud cloud) {
+    if (cloud.cartesian.isNotEmpty) {
+      cloudCache.cartesian.clear();
+      cloudCache.cartesian.addAll(cloud.cartesian);
+    }
+    if (cloud.polar.isNotEmpty) {
+      cloudCache.polar.clear();
+      cloudCache.polar.addAll(cloud.polar);
+    }
+
+    _queueObstacles();
+  }
+
+  void _queueObstacles() {
+    final cartesian = cloudCache.cartesian;
+    final polar = cloudCache.polar;
+
+    if (cartesian.isEmpty || polar.isEmpty) {
+      return;
+    }
+
+    queuedObstacles.clear();
+
+    for (final point in cartesian) {
+      final angle = atan2(point.y, point.x) * 180 / pi;
+      final magnitude = sqrt(pow(point.x, 2) + pow(point.y, 2));
+
+      if (angle >= pi / 2) {
+        continue;
+      }
+
+      if (magnitude <= 0.1) {
+        continue;
+      }
+
+      final matchingPolar = polar.where(
+        (e) =>
+            (e.angle - angle.roundToDouble()).abs() <= 1 &&
+            (e.angle - angle.roundToDouble()).abs() != 0,
+      );
+
+      // no polar coordinates are near the cartesian coordinate
+      if (matchingPolar.isEmpty) {
+        continue;
+      }
+
+      // nearby polar coordinates do not match the cartesian distance, likely a false speck
+      if (!matchingPolar.any((e) => (e.distance - magnitude).abs() < 0.1)) {
+        continue;
+      }
+
+      final imuAngleRad = collection.imu.heading * pi / 180 + pi / 2;
+
+      final roverToPoint = (
+        long: point.x * cos(imuAngleRad) - point.y * sin(imuAngleRad),
+        lat: point.y * cos(imuAngleRad) + point.x * sin(imuAngleRad),
+      );
+
+      queuedObstacles.add(
+        (collection.gps.coordinates.inMeters + roverToPoint).toGps(),
+      );
+    }
+
+    cloudCache.cartesian.clear();
+    cloudCache.polar.clear();
+  }
+
   @override
   bool isOnSlope() => false;
 
   @override
-  bool findObstacles() => false;
+  bool findObstacles() {
+    if (queuedObstacles.isEmpty) return false;
 
-  @override
-//  bool canSeeAruco() => collection.video.data.arucoDetected == BoolState.YES;
-  bool canSeeAruco() => collection.video.flag;
+    collection.pathfinder.obstacles.removeAll(previousObstacles);
+    previousObstacles.clear();
+
+    for (final obstacle in queuedObstacles) {
+      collection.pathfinder.recordObstacle(obstacle);
+    }
+
+    previousObstacles.addAll(queuedObstacles);
+
+    queuedObstacles.clear();
+
+    return true;
+  }
 
   @override
-  Future<bool> init() async => true;
+  Future<bool> init() async {
+    _subscription = collection.server.messages.onMessage(
+      name: LidarPointCloud().messageName,
+      constructor: LidarPointCloud.fromBuffer,
+      callback: _handleLidarCloud,
+    );
+    return true;
+  }
 
   @override
-  Future<void> dispose() async { }
+  Future<void> dispose() async {
+    await _subscription?.cancel();
+  }
 }

lib/src/detector/sim_detector.dart

@@ -37,9 +37,6 @@ class DetectorSimulator extends DetectorInterface {
     return result;
   }
 
-  @override
-  bool canSeeAruco() => false;  // if can see [arucoPosition]
-
   @override
   bool isOnSlope() => false;  // if on [slopedLatitude]
 }

lib/src/drive/drive_config.dart

@@ -42,7 +42,7 @@ class DriveConfig {
 const roverConfig = DriveConfig(
   forwardThrottle: 0.2,
   turnThrottle: 0.075,
-  oneMeterDelay: Duration(milliseconds: 5500),
+  oneMeterDelay: Duration(milliseconds: 2250),
   turnDelay: Duration(milliseconds: 4500),
   subsystemsAddress: "192.168.1.20",
 );

lib/src/drive/sensor_drive.dart

@@ -95,29 +95,4 @@ class SensorDrive extends DriveInterface with RoverDriveCommands {
     collection.logger.trace("Current heading: $current");
     return collection.imu.isNear(orientation);
   }
-
-  @override
-  Future<bool> spinForAruco() async {
-    setThrottle(config.turnThrottle);
-    spinLeft();
-    final result = await waitFor(() => collection.detector.canSeeAruco())
-      .then((_) => true)
-      .timeout(config.turnDelay * 4, onTimeout: () => false);
-    await stop();
-    return result;
-  }
-
-  @override
-  Future<void> approachAruco() async {
-    const sizeThreshold = 0.2;
-    const epsilon = 0.00001;
-    setThrottle(config.forwardThrottle);
-    moveForward();
-    await waitFor(() {
-      final size = collection.video.arucoSize;
-      collection.logger.trace("The Aruco tag is at $size percent");
-      return (size.abs() < epsilon && !collection.detector.canSeeAruco()) || size >= sizeThreshold;
-    }).timeout(config.oneMeterDelay * 5);
-    await stop();
-  }
 }

lib/src/utils/a_star.dart

@@ -155,7 +155,8 @@ class AutonomyAStarState extends AStarState<AutonomyAStarState> {
   }
 
   bool isValidState(AutonomyAStarState state) =>
-    !collection.pathfinder.isObstacle(state.position)
+      !(state.instruction == DriveDirection.forward &&
+          collection.pathfinder.isObstacle(state.position))
     && !willDriveThroughObstacle(state);
 
   Iterable<AutonomyAStarState> _allNeighbors() => [