Simulation Support for YALL

example/build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2025.3.1"
+    id "edu.wpi.first.GradleRIO" version "2025.3.2"
 }
 
 java {

example/src/main/java/frc/robot/subsystems/DrivebaseSubsystem.java

@@ -1,6 +1,5 @@
 package frc.robot.subsystems;
 
-
 import static edu.wpi.first.units.Units.DegreesPerSecond;
 import static edu.wpi.first.units.Units.Inches;
 import static edu.wpi.first.units.Units.Meters;
@@ -20,43 +19,45 @@
 import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
 import edu.wpi.first.math.kinematics.DifferentialDriveWheelPositions;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import java.util.Optional;
 import java.util.function.DoubleSupplier;
 import limelight.Limelight;
+import limelight.LimelightModel;
 import limelight.networktables.AngularVelocity3d;
 import limelight.networktables.LimelightPoseEstimator;
 import limelight.networktables.LimelightResults;
 import limelight.networktables.LimelightSettings.LEDMode;
 import limelight.networktables.Orientation3d;
 import limelight.networktables.PoseEstimate;
 import limelight.networktables.target.pipeline.NeuralClassifier;
+import limelight.sim.LimelightSim;
+import limelight.sim.LimelightPipelineSim;
 
-public class DrivebaseSubsystem extends SubsystemBase
-{
+public class DrivebaseSubsystem extends SubsystemBase {
 
   SparkMax left, right;
   RelativeEncoder leftEncoder, rightEncoder;
   AHRS navx;
 
-  double                         driveGearRatio      = 1.0;
-  double                         wheelDiameterMeters = 4.0;
-  double                         trackWidth          = Units.inchesToMeters(20);
-  DifferentialDrive              differentialDrive;
+  double driveGearRatio = 1.0;
+  double wheelDiameterMeters = 4.0;
+  double trackWidth = Units.inchesToMeters(20);
+  DifferentialDrive differentialDrive;
   DifferentialDrivePoseEstimator differentialDrivePoseEstimator;
-  DifferentialDriveKinematics    differentialDriveKinematics;
-  Pose3d                         cameraOffset        = new Pose3d(Inches.of(5).in(Meters),
-                                                                  Inches.of(5).in(Meters),
-                                                                  Inches.of(5).in(Meters),
-                                                                  Rotation3d.kZero);
-  Limelight                      limelight;
-  LimelightPoseEstimator         poseEstimator;
-
-
-  public DrivebaseSubsystem()
-  {
+  DifferentialDriveKinematics differentialDriveKinematics;
+  Pose3d cameraOffset = new Pose3d(Inches.of(5).in(Meters),
+      Inches.of(5).in(Meters),
+      Inches.of(5).in(Meters),
+      Rotation3d.kZero);
+  Limelight limelight;
+  LimelightSim limelightSim;
+  LimelightPoseEstimator poseEstimator;
+
+  public DrivebaseSubsystem() {
     left = new SparkMax(1, MotorType.kBrushless);
     right = new SparkMax(2, MotorType.kBrushless);
     navx = new AHRS(NavXComType.kMXP_SPI);
@@ -76,16 +77,22 @@ public DrivebaseSubsystem()
     // Create the pose estimator
     differentialDriveKinematics = new DifferentialDriveKinematics(trackWidth);
     differentialDrivePoseEstimator = new DifferentialDrivePoseEstimator(differentialDriveKinematics,
-                                                                        navx.getRotation2d(),
-                                                                        0,
-                                                                        0,
-                                                                        Pose2d.kZero); // Starting at (0,0)
+        navx.getRotation2d(),
+        0,
+        0,
+        Pose2d.kZero); // Starting at (0,0)
 
     limelight = new Limelight("limelight");
     limelight.getSettings()
-             .withLimelightLEDMode(LEDMode.PipelineControl)
-             .withCameraOffset(cameraOffset)
-             .save();
+        .withLimelightLEDMode(LEDMode.PipelineControl)
+        .withCameraOffset(cameraOffset)
+        .save();
+
+    limelightSim = new LimelightSim(limelight, LimelightModel.Limelight3G)
+        .withCameraToRobotOffset(cameraOffset)
+        .withDefaultPipeline(1).withPipelines(
+            new LimelightPipelineSim("pipe_color"));
+
     poseEstimator = limelight.getPoseEstimator(true);
 
   }
@@ -95,10 +102,9 @@ public DrivebaseSubsystem()
    *
    * @return {@link DifferentialDriveWheelPositions}
    */
-  private DifferentialDriveWheelPositions getWheelPositions()
-  {
+  private DifferentialDriveWheelPositions getWheelPositions() {
     return new DifferentialDriveWheelPositions(leftEncoder.getPosition() * wheelDiameterMeters,
-                                               rightEncoder.getPosition() * wheelDiameterMeters);
+        rightEncoder.getPosition() * wheelDiameterMeters);
   }
 
   /**
@@ -108,52 +114,50 @@ private DifferentialDriveWheelPositions getWheelPositions()
    * @param right Right speed (-1, 1)
    * @return {@link Command} to drive the robot.
    */
-  public Command drive(DoubleSupplier left, DoubleSupplier right)
-  {
+  public Command drive(DoubleSupplier left, DoubleSupplier right) {
     return run(() -> {
       differentialDrive.tankDrive(left.getAsDouble() * 0.8, right.getAsDouble() * 0.8);
     });
   }
 
   @Override
-  public void periodic()
-  {
+  public void periodic() {
     differentialDrivePoseEstimator.update(navx.getRotation2d(), getWheelPositions());
 
     // Required for megatag2
     limelight.getSettings()
-             .withRobotOrientation(new Orientation3d(navx.getRotation3d(),
-                                                     new AngularVelocity3d(DegreesPerSecond.of(0),
-                                                                           DegreesPerSecond.of(0),
-                                                                           DegreesPerSecond.of(0))))
-             .save();
+        .withRobotOrientation(new Orientation3d(navx.getRotation3d(),
+            new AngularVelocity3d(DegreesPerSecond.of(0),
+                DegreesPerSecond.of(0),
+                DegreesPerSecond.of(0))))
+        .save();
+
+    if (RobotBase.isSimulation()) {
+      limelightSim.update(differentialDrivePoseEstimator.getEstimatedPosition());
+    }
 
     // Get the vision estimate.
     Optional<PoseEstimate> visionEstimate = poseEstimator.getPoseEstimate(); // BotPose.BLUE_MEGATAG2.get(limelight);
     visionEstimate.ifPresent((PoseEstimate poseEstimate) -> {
       // If the average tag distance is less than 4 meters,
       // there are more than 0 tags in view,
-      // and the average ambiguity between tags is less than 30% then we update the pose estimation.
-      if (poseEstimate.avgTagDist < 4 && poseEstimate.tagCount > 0 && poseEstimate.getMinTagAmbiguity() < 0.3)
-      {
+      // and the average ambiguity between tags is less than 30% then we update the
+      // pose estimation.
+      if (poseEstimate.avgTagDist < 4 && poseEstimate.tagCount > 0 && poseEstimate.getMinTagAmbiguity() < 0.3) {
         differentialDrivePoseEstimator.addVisionMeasurement(poseEstimate.pose.toPose2d(),
-                                                            poseEstimate.timestampSeconds);
+            poseEstimate.timestampSeconds);
       }
     });
 
     limelight.getLatestResults().ifPresent((LimelightResults result) -> {
-      for (NeuralClassifier object : result.targets_Classifier)
-      {
+      for (NeuralClassifier object : result.targets_Classifier) {
         // Classifier says its a note.
-        if (object.className.equals("algae"))
-        {
-          if (object.ty > 2 && object.ty < 1)
-          {
+        if (object.className.equals("algae")) {
+          if (object.ty > 2 && object.ty < 1) {
             // do stuff
           }
         }
       }
     });
   }
 }
-

yall/java/limelight/CameraTargetRelation.java

@@ -0,0 +1,59 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package limelight;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform3d;
+
+/**
+ * Holds various helper geometries describing the relation between camera and
+ * target.
+ */
+public class CameraTargetRelation {
+        public final Pose3d camPose;
+        public final Transform3d camToTarg;
+        public final double camToTargDist;
+        public final double camToTargDistXY;
+        public final Rotation2d camToTargYaw;
+        public final Rotation2d camToTargPitch;
+
+        /** Angle from the camera's relative x-axis */
+        public final Rotation2d camToTargAngle;
+
+        public final Transform3d targToCam;
+        public final Rotation2d targToCamYaw;
+        public final Rotation2d targToCamPitch;
+
+        /** Angle from the target's relative x-axis */
+        public final Rotation2d targToCamAngle;
+
+        public CameraTargetRelation(Pose3d cameraPose, Pose3d targetPose) {
+                this.camPose = cameraPose;
+                camToTarg = new Transform3d(cameraPose, targetPose);
+                camToTargDist = camToTarg.getTranslation().getNorm();
+                camToTargDistXY = Math.hypot(camToTarg.getTranslation().getX(), camToTarg.getTranslation().getY());
+                camToTargYaw = new Rotation2d(camToTarg.getX(), camToTarg.getY());
+                camToTargPitch = new Rotation2d(camToTargDistXY, -camToTarg.getZ());
+                camToTargAngle = new Rotation2d(Math.hypot(camToTargYaw.getRadians(), camToTargPitch.getRadians()));
+                targToCam = new Transform3d(targetPose, cameraPose);
+                targToCamYaw = new Rotation2d(targToCam.getX(), targToCam.getY());
+                targToCamPitch = new Rotation2d(camToTargDistXY, -targToCam.getZ());
+                targToCamAngle = new Rotation2d(Math.hypot(targToCamYaw.getRadians(), targToCamPitch.getRadians()));
+        }
+}