From 77f2ce6f3ab68c996cfddbd2ef28f234ffb69de4 Mon Sep 17 00:00:00 2001 From: Gold87 <91761103+Gold872@users.noreply.github.com> Date: Sun, 9 Feb 2025 10:49:28 -0500 Subject: [PATCH 1/6] Add lidar obstacle detection --- lib/src/detector/rover_detector.dart | 91 ++++++++++++++++++++++++++-- lib/src/drive/drive_config.dart | 2 +- lib/src/utils/a_star.dart | 3 +- 3 files changed, 88 insertions(+), 8 deletions(-) diff --git a/lib/src/detector/rover_detector.dart b/lib/src/detector/rover_detector.dart index 3ed0161..de83e67 100644 --- a/lib/src/detector/rover_detector.dart +++ b/lib/src/detector/rover_detector.dart @@ -1,21 +1,100 @@ +import "dart:async"; +import "dart:math"; + import "package:autonomy/interfaces.dart"; class RoverDetector extends DetectorInterface { + StreamSubscription? _subscription; + + LidarPointCloud cloudCache = LidarPointCloud(); + + List queuedObstacles = []; + 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; + } + + 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)); + + 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.05)) { + continue; + } + + final imuAngleRad = collection.imu.heading * pi / 180; + + 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; + for (final obstacle in queuedObstacles) { + collection.pathfinder.recordObstacle(obstacle); + } + + queuedObstacles.clear(); + + return true; + } @override - Future init() async => true; + Future init() async { + _subscription = collection.server.messages.onMessage( + name: LidarPointCloud().messageName, + constructor: LidarPointCloud.fromBuffer, + callback: _handleLidarCloud, + ); + return true; + } @override - Future dispose() async { } + Future dispose() async { + await _subscription?.cancel(); + } } diff --git a/lib/src/drive/drive_config.dart b/lib/src/drive/drive_config.dart index eed787e..c0cd43e 100644 --- a/lib/src/drive/drive_config.dart +++ b/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", ); diff --git a/lib/src/utils/a_star.dart b/lib/src/utils/a_star.dart index 41b132f..dd0de0c 100644 --- a/lib/src/utils/a_star.dart +++ b/lib/src/utils/a_star.dart @@ -155,7 +155,8 @@ class AutonomyAStarState extends AStarState { } bool isValidState(AutonomyAStarState state) => - !collection.pathfinder.isObstacle(state.position) + !(state.instruction == DriveDirection.forward && + collection.pathfinder.isObstacle(state.position)) && !willDriveThroughObstacle(state); Iterable _allNeighbors() => [ From e7b0f4e0f2769c6294fe6c453c38b2f9607acca9 Mon Sep 17 00:00:00 2001 From: Gold87 <91761103+Gold872@users.noreply.github.com> Date: Sun, 9 Feb 2025 10:52:19 -0500 Subject: [PATCH 2/6] Fixed build errors --- lib/src/detector/detector_interface.dart | 1 - 1 file changed, 1 deletion(-) diff --git a/lib/src/detector/detector_interface.dart b/lib/src/detector/detector_interface.dart index c42a694..8d5909e 100644 --- a/lib/src/detector/detector_interface.dart +++ b/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(); } From 5c1b5894c452e9f23ff8b299118a86c0a51c256e Mon Sep 17 00:00:00 2001 From: Gold87 <91761103+Gold872@users.noreply.github.com> Date: Sun, 9 Feb 2025 10:52:55 -0500 Subject: [PATCH 3/6] Remove all implementations of canSeeAruco --- lib/src/detector/network_detector.dart | 3 --- lib/src/detector/sim_detector.dart | 3 --- lib/src/drive/sensor_drive.dart | 18 +++++++++--------- 3 files changed, 9 insertions(+), 15 deletions(-) diff --git a/lib/src/detector/network_detector.dart b/lib/src/detector/network_detector.dart index c91934d..37e7796 100644 --- a/lib/src/detector/network_detector.dart +++ b/lib/src/detector/network_detector.dart @@ -6,9 +6,6 @@ class NetworkDetector extends DetectorInterface { NetworkDetector({required super.collection}); - @override - bool canSeeAruco() => false; - @override Future dispose() async {} diff --git a/lib/src/detector/sim_detector.dart b/lib/src/detector/sim_detector.dart index dfac538..4b8e57a 100644 --- a/lib/src/detector/sim_detector.dart +++ b/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] } diff --git a/lib/src/drive/sensor_drive.dart b/lib/src/drive/sensor_drive.dart index 7a9654f..7d62634 100644 --- a/lib/src/drive/sensor_drive.dart +++ b/lib/src/drive/sensor_drive.dart @@ -109,15 +109,15 @@ class SensorDrive extends DriveInterface with RoverDriveCommands { @override Future 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); + // 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(); } } From ed065cbe3c35ba71d5fa4260413e686167d882c7 Mon Sep 17 00:00:00 2001 From: Gold87 <91761103+Gold872@users.noreply.github.com> Date: Sun, 9 Feb 2025 10:54:09 -0500 Subject: [PATCH 4/6] Remove aruco methods (i think) --- lib/src/drive/sensor_drive.dart | 25 ------------------------- 1 file changed, 25 deletions(-) diff --git a/lib/src/drive/sensor_drive.dart b/lib/src/drive/sensor_drive.dart index 7d62634..c2fcead 100644 --- a/lib/src/drive/sensor_drive.dart +++ b/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 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 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(); - } } From afc808232d07f557408155cd9ecf862da500aced Mon Sep 17 00:00:00 2001 From: Gold87 <91761103+Gold872@users.noreply.github.com> Date: Tue, 11 Feb 2025 21:33:49 -0500 Subject: [PATCH 5/6] Tested with actual lidar --- lib/src/detector/rover_detector.dart | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/lib/src/detector/rover_detector.dart b/lib/src/detector/rover_detector.dart index de83e67..b795740 100644 --- a/lib/src/detector/rover_detector.dart +++ b/lib/src/detector/rover_detector.dart @@ -9,6 +9,7 @@ class RoverDetector extends DetectorInterface { LidarPointCloud cloudCache = LidarPointCloud(); List queuedObstacles = []; + List previousObstacles = []; RoverDetector({required super.collection}); @@ -37,6 +38,10 @@ class RoverDetector extends DetectorInterface { final angle = atan2(point.y, point.x) * 180 / pi; final magnitude = sqrt(pow(point.x, 2) + pow(point.y, 2)); + if (magnitude <= 0.1) { + continue; + } + final matchingPolar = polar.where((e) => (e.angle - angle.roundToDouble()).abs() <= 1 && (e.angle - angle.roundToDouble()).abs() != 0, @@ -46,16 +51,17 @@ class RoverDetector extends DetectorInterface { 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.05)) { + if (!matchingPolar.any((e) => (e.distance - magnitude).abs() < 0.1)) { continue; } - final imuAngleRad = collection.imu.heading * pi / 180; + 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) + long: point.x * cos(imuAngleRad) - point.y * sin(imuAngleRad), + lat: point.y * cos(imuAngleRad) + point.x * sin(imuAngleRad) ); queuedObstacles.add( @@ -74,10 +80,15 @@ class RoverDetector extends DetectorInterface { bool findObstacles() { if (queuedObstacles.isEmpty) return false; + collection.pathfinder.obstacles.removeAll(previousObstacles); + previousObstacles.clear(); + for (final obstacle in queuedObstacles) { collection.pathfinder.recordObstacle(obstacle); } + previousObstacles.addAll(queuedObstacles); + queuedObstacles.clear(); return true; From 8a711fd23a00ec603f7d83724d8ecfa4765cf753 Mon Sep 17 00:00:00 2001 From: Gold87 <91761103+Gold872@users.noreply.github.com> Date: Wed, 12 Feb 2025 22:10:35 -0500 Subject: [PATCH 6/6] Ignore obstacles 90 degrees off --- lib/src/detector/rover_detector.dart | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/lib/src/detector/rover_detector.dart b/lib/src/detector/rover_detector.dart index b795740..8026148 100644 --- a/lib/src/detector/rover_detector.dart +++ b/lib/src/detector/rover_detector.dart @@ -34,15 +34,22 @@ class RoverDetector extends DetectorInterface { 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) => + final matchingPolar = polar.where( + (e) => (e.angle - angle.roundToDouble()).abs() <= 1 && (e.angle - angle.roundToDouble()).abs() != 0, ); @@ -61,7 +68,7 @@ class RoverDetector extends DetectorInterface { final roverToPoint = ( long: point.x * cos(imuAngleRad) - point.y * sin(imuAngleRad), - lat: point.y * cos(imuAngleRad) + point.x * sin(imuAngleRad) + lat: point.y * cos(imuAngleRad) + point.x * sin(imuAngleRad), ); queuedObstacles.add(