From 636e1252dc6d499673f025f32a66d2d61b14376d Mon Sep 17 00:00:00 2001 From: Eric Ratliff Date: Tue, 3 Feb 2026 00:46:00 -0600 Subject: [PATCH] feat: Add localization template with grid-based sensor fusion MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Implements comprehensive robot localization system as third template option. Teams can now start with professional positioning and navigation code. Template Features: - 12x12 field grid system (12-inch cells) - Multi-sensor fusion (encoders, IMU, vision) - Kalman-filter-style sensor combination - Fault-tolerant positioning (graceful degradation) - 21 files, ~1,500 lines, 3 passing tests Core Components: - GridCell/Pose2D/FieldGrid - Coordinate system - RobotLocalizer - Sensor fusion engine - OdometryTracker - Dead reckoning from encoders - ImuLocalizer - Heading from gyroscope - VisionLocalizer - Position from AprilTags Sensor Fusion Strategy: Priority 1: Vision (AprilTags) → ±2" accuracy, 100% confidence Priority 2: IMU + Odometry → ±4" accuracy, 70% confidence Priority 3: Odometry only → ±12" accuracy, 40% confidence System gracefully degrades when sensors fail, maintaining operation even with partial sensor availability. Hardware Abstraction: - Interfaces (Encoder, GyroSensor, VisionCamera) - Mock implementations for unit testing - Teams implement FTC wrappers for their hardware Documentation: - LOCALIZATION_GUIDE.md - System architecture and usage - GRID_SYSTEM.md - Field coordinate reference - README.md - Quick start and examples Usage: weevil new my-robot --template localization cd my-robot ./gradlew test # 3 tests pass in < 1 second This teaches professional robotics concepts (sensor fusion, fault tolerance, coordinate systems) not found in other FTC tools. Positions Nexus Workshops as teaching advanced autonomous programming. Updated src/templates/mod.rs to register localization template with proper metadata and feature descriptions. All tests passing (10/10 template tests). --- src/templates/mod.rs | 51 +++++++++++++++++-- templates/localization/README.md.template | 2 +- templates/localization/docs/GRID_SYSTEM.md | 2 +- templates/localization/settings.gradle | 1 - .../localization/settings.gradle.kts.template | 17 +++++++ .../main/java/robot/hardware/GyroSensor.java | 1 + .../java/robot/localization/ImuLocalizer.java | 7 ++- .../java/robot/hardware/MockGyroSensor.java | 5 +- .../robot/localization/SensorFusionTest.java | 4 ++ 9 files changed, 80 insertions(+), 10 deletions(-) delete mode 100644 templates/localization/settings.gradle create mode 100644 templates/localization/settings.gradle.kts.template diff --git a/src/templates/mod.rs b/src/templates/mod.rs index 30e2326..83165e8 100644 --- a/src/templates/mod.rs +++ b/src/templates/mod.rs @@ -8,6 +8,7 @@ use colored::*; // Embed template directories at compile time static BASIC_TEMPLATE: Dir = include_dir!("$CARGO_MANIFEST_DIR/templates/basic"); static TESTING_TEMPLATE: Dir = include_dir!("$CARGO_MANIFEST_DIR/templates/testing"); +static LOCALIZATION_TEMPLATE: Dir = include_dir!("$CARGO_MANIFEST_DIR/templates/localization"); pub struct TemplateManager { #[allow(dead_code)] @@ -39,13 +40,14 @@ impl TemplateManager { } pub fn template_exists(&self, name: &str) -> bool { - matches!(name, "basic" | "testing") + matches!(name, "basic" | "testing" | "localization") } pub fn list_templates(&self) -> Vec { vec![ - " basic - Minimal FTC project (default)".to_string(), - " testing - Testing showcase with examples".to_string(), + " basic - Minimal FTC project (default)".to_string(), + " testing - Testing showcase with examples".to_string(), + " localization - Grid-based positioning with sensor fusion".to_string(), ] } @@ -68,6 +70,14 @@ impl TemplateManager { test_count: 45, is_default: false, }, + TemplateInfo { + name: "localization".to_string(), + description: "Grid-based robot localization with sensor fusion".to_string(), + file_count: 21, + line_count: 1500, + test_count: 3, + is_default: false, + }, ]) } @@ -90,6 +100,14 @@ impl TemplateManager { test_count: 45, is_default: false, }, + "localization" => TemplateInfo { + name: "localization".to_string(), + description: "Grid-based robot localization system with multi-sensor fusion and fault tolerance.".to_string(), + file_count: 21, + line_count: 1500, + test_count: 3, + is_default: false, + }, _ => bail!("Unknown template: {}", name), }; @@ -109,6 +127,16 @@ impl TemplateManager { println!(); } + if info.name == "localization" { + println!("{}", "Features:".bright_white().bold()); + println!(" • 12x12 field grid system (12-inch cells)"); + println!(" • Multi-sensor fusion (encoders + IMU + vision)"); + println!(" • Fault-tolerant positioning (graceful degradation)"); + println!(" • Kalman-filter-style sensor fusion"); + println!(" • Professional robotics patterns"); + println!(); + } + println!("{}", "Files included:".bright_white().bold()); println!(" {} files", info.file_count); println!(" ~{} lines of code", info.line_count); @@ -132,6 +160,7 @@ impl TemplateManager { let template_dir = match template_name { "basic" => &BASIC_TEMPLATE, "testing" => &TESTING_TEMPLATE, + "localization" => &LOCALIZATION_TEMPLATE, _ => bail!("Unknown template: {}", template_name), }; @@ -233,6 +262,7 @@ mod tests { let mgr = TemplateManager::new().unwrap(); assert!(mgr.template_exists("basic")); assert!(mgr.template_exists("testing")); + assert!(mgr.template_exists("localization")); assert!(!mgr.template_exists("nonexistent")); } @@ -240,6 +270,19 @@ mod tests { fn test_list_templates() { let mgr = TemplateManager::new().unwrap(); let templates = mgr.list_templates(); - assert_eq!(templates.len(), 2); + assert_eq!(templates.len(), 3); + assert!(templates[0].contains("basic")); + assert!(templates[1].contains("testing")); + assert!(templates[2].contains("localization")); + } + + #[test] + fn test_template_info_all() { + let mgr = TemplateManager::new().unwrap(); + let infos = mgr.get_template_info_all().unwrap(); + assert_eq!(infos.len(), 3); + assert_eq!(infos[0].name, "basic"); + assert_eq!(infos[1].name, "testing"); + assert_eq!(infos[2].name, "localization"); } } \ No newline at end of file diff --git a/templates/localization/README.md.template b/templates/localization/README.md.template index 2d3096f..cd6a29d 100644 --- a/templates/localization/README.md.template +++ b/templates/localization/README.md.template @@ -11,7 +11,7 @@ Grid-based robot localization with sensor fusion and fault tolerance. - **Grid System** - 12x12 field grid (12" cells) - **Sensor Fusion** - Combine encoders, IMU, vision - **Fault Tolerance** - Graceful sensor failure handling -- **20+ Tests** - All passing +- **3 Tests** - All passing ## Quick Start diff --git a/templates/localization/docs/GRID_SYSTEM.md b/templates/localization/docs/GRID_SYSTEM.md index 4ff5265..dd127f3 100644 --- a/templates/localization/docs/GRID_SYSTEM.md +++ b/templates/localization/docs/GRID_SYSTEM.md @@ -11,7 +11,7 @@ 9 . . . . . . . . . . . . 8 . . . . . . . . . . . . 7 . . . . . . . . . . . . - 6 . . . . . . X . . . . . + 6 . . . . . X . . . . . . 5 . . . . . . . . . . . . 4 . . . . . . . . . . . . 3 . . . . . . . . . . . . diff --git a/templates/localization/settings.gradle b/templates/localization/settings.gradle deleted file mode 100644 index 2866605..0000000 --- a/templates/localization/settings.gradle +++ /dev/null @@ -1 +0,0 @@ -rootProject.name = '{{PROJECT_NAME}}' diff --git a/templates/localization/settings.gradle.kts.template b/templates/localization/settings.gradle.kts.template new file mode 100644 index 0000000..a9c546b --- /dev/null +++ b/templates/localization/settings.gradle.kts.template @@ -0,0 +1,17 @@ +pluginManagement { + repositories { + google() + mavenCentral() + gradlePluginPortal() + } +} + +dependencyResolutionManagement { + repositoriesMode.set(RepositoriesMode.PREFER_SETTINGS) + repositories { + mavenCentral() + google() + } +} + +rootProject.name = "{{PROJECT_NAME}}" diff --git a/templates/localization/src/main/java/robot/hardware/GyroSensor.java b/templates/localization/src/main/java/robot/hardware/GyroSensor.java index 2d5380d..d6384e1 100644 --- a/templates/localization/src/main/java/robot/hardware/GyroSensor.java +++ b/templates/localization/src/main/java/robot/hardware/GyroSensor.java @@ -4,4 +4,5 @@ public interface GyroSensor { double getHeading(); boolean isConnected(); void calibrate(); + boolean isCalibrated(); } diff --git a/templates/localization/src/main/java/robot/localization/ImuLocalizer.java b/templates/localization/src/main/java/robot/localization/ImuLocalizer.java index 0f775f8..8ca6eb5 100644 --- a/templates/localization/src/main/java/robot/localization/ImuLocalizer.java +++ b/templates/localization/src/main/java/robot/localization/ImuLocalizer.java @@ -13,14 +13,17 @@ public class ImuLocalizer { public void calibrate(double initialHeading) { if (gyro.isConnected()) { + gyro.calibrate(); this.headingOffset = initialHeading - gyro.getHeading(); } } public Double getHeading() { - if (!gyro.isConnected()) return null; + if (!gyro.isConnected() || !gyro.isCalibrated()) return null; return Pose2D.normalizeAngle(gyro.getHeading() + headingOffset); } - public boolean isWorking() { return gyro.isConnected(); } + public boolean isWorking() { + return gyro.isConnected() && gyro.isCalibrated(); + } } diff --git a/templates/localization/src/test/java/robot/hardware/MockGyroSensor.java b/templates/localization/src/test/java/robot/hardware/MockGyroSensor.java index 55ab3e1..dfb1625 100644 --- a/templates/localization/src/test/java/robot/hardware/MockGyroSensor.java +++ b/templates/localization/src/test/java/robot/hardware/MockGyroSensor.java @@ -3,11 +3,14 @@ package robot.hardware; public class MockGyroSensor implements GyroSensor { private double heading = 0; private boolean connected = true; + private boolean calibrated = true; public double getHeading() { return heading; } public boolean isConnected() { return connected; } - public void calibrate() { } + public void calibrate() { calibrated = true; } + public boolean isCalibrated() { return calibrated; } public void setHeading(double h) { heading = h; } public void setConnected(boolean c) { connected = c; } + public void setCalibrated(boolean c) { calibrated = c; } } diff --git a/templates/localization/src/test/java/robot/localization/SensorFusionTest.java b/templates/localization/src/test/java/robot/localization/SensorFusionTest.java index 46ed3ea..5c50038 100644 --- a/templates/localization/src/test/java/robot/localization/SensorFusionTest.java +++ b/templates/localization/src/test/java/robot/localization/SensorFusionTest.java @@ -34,11 +34,15 @@ class SensorFusionTest { MockGyroSensor gyro = new MockGyroSensor(); MockVisionCamera camera = new MockVisionCamera(); + // Set a pose so vision is actually "working" (not just connected) + camera.setPose(new Pose2D(12, 12, 0)); + OdometryTracker odometry = new OdometryTracker(left, right); ImuLocalizer imu = new ImuLocalizer(gyro); VisionLocalizer vision = new VisionLocalizer(camera); RobotLocalizer localizer = new RobotLocalizer(odometry, imu, vision); + localizer.update(); // Need to update to actually use vision assertEquals(1.0, localizer.getConfidence(), 0.01);