[examples] Add Computer Vision Pose Estimation and Latency Compensation Example (#4901)

This PR updates the existing differentialdriveposeestimator example to include computer vision pose estimation and latency compensation.

The example generates a simulated cameraToTarget transformation, which is then fed into ComputerVisionUtil.objectToRobotPose() to compute the robot's field-relative position exclusively from vision measurements. The vision measurements are applied through DifferentialDrivePoseEstimator.addVisionMeasurement().

The updated example constructs an AprilTagFieldLayout from JSON. This requires a deploy directory, something which isn't currently supported in wpilibjExamples and wpilibcExamples.

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java

@@ -4,21 +4,42 @@
 
 package edu.wpi.first.wpilibj.examples.differentialdriveposeestimator;
 
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.ComputerVisionUtil;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
 import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.networktables.DoubleArrayEntry;
+import edu.wpi.first.networktables.DoubleArrayTopic;
 import edu.wpi.first.wpilibj.AnalogGyro;
 import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.motorcontrol.MotorController;
 import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj.simulation.AnalogGyroSim;
+import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import java.io.IOException;
 
 /** Represents a differential drive style drivetrain. */
 public class Drivetrain {
@@ -50,6 +71,18 @@ public class Drivetrain {
   private final DifferentialDriveKinematics m_kinematics =
       new DifferentialDriveKinematics(kTrackWidth);
 
+  private final Pose3d m_objectInField;
+
+  private final Transform3d m_robotToCamera =
+      new Transform3d(new Translation3d(1, 1, 1), new Rotation3d(0, 0, Math.PI / 2));
+
+  private final DoubleArrayEntry m_cameraToObjectEntry;
+
+  private final double[] m_defaultVal = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+
+  private final Field2d m_fieldSim = new Field2d();
+  private final Field2d m_fieldApproximation = new Field2d();
+
   /* Here we use DifferentialDrivePoseEstimator so that we can fuse odometry readings. The
   numbers used  below are robot specific, and should be tuned. */
   private final DifferentialDrivePoseEstimator m_poseEstimator =
@@ -65,11 +98,21 @@ public class Drivetrain {
   // Gains are for example purposes only - must be determined for your own robot!
   private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
 
+  // Simulation classes
+  private final AnalogGyroSim m_gyroSim = new AnalogGyroSim(m_gyro);
+  private final EncoderSim m_leftEncoderSim = new EncoderSim(m_leftEncoder);
+  private final EncoderSim m_rightEncoderSim = new EncoderSim(m_rightEncoder);
+  private final LinearSystem<N2, N2, N2> m_drivetrainSystem =
+      LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3);
+  private final DifferentialDrivetrainSim m_drivetrainSimulator =
+      new DifferentialDrivetrainSim(
+          m_drivetrainSystem, DCMotor.getCIM(2), 8, kTrackWidth, kWheelRadius, null);
+
   /**
    * Constructs a differential drive object. Sets the encoder distance per pulse and resets the
    * gyro.
    */
-  public Drivetrain() {
+  public Drivetrain(DoubleArrayTopic cameraToObjectTopic) {
     m_gyro.reset();
 
     // We need to invert one side of the drivetrain so that positive voltages
@@ -85,6 +128,21 @@ public class Drivetrain {
 
     m_leftEncoder.reset();
     m_rightEncoder.reset();
+
+    m_cameraToObjectEntry = cameraToObjectTopic.getEntry(m_defaultVal);
+
+    try {
+      m_objectInField =
+          AprilTagFieldLayout.loadFromResource(AprilTagFields.k2022RapidReact.m_resourceFile)
+              .getTagPose(0)
+              .get();
+    } catch (IOException e) {
+      e.printStackTrace();
+      throw new RuntimeException();
+    }
+
+    SmartDashboard.putData("Field", m_fieldSim);
+    SmartDashboard.putData("FieldEstimation", m_fieldApproximation);
   }
 
   /**
@@ -115,16 +173,109 @@ public class Drivetrain {
     setSpeeds(wheelSpeeds);
   }
 
+  /**
+   * Computes and publishes to a network tables topic the transformation from the camera's pose to
+   * the object's pose. This function exists solely for the purposes of simulation, and this would
+   * normally be handled by computer vision.
+   *
+   * <p>The object could be a target or a fiducial marker.
+   *
+   * @param objectInField The object's field-relative position.
+   * @param robotToCamera The transformation from the robot's pose to the camera's pose.
+   * @param cameraToObjectEntry The networktables entry publishing and querying example computer
+   *     vision measurements.
+   */
+  public void publishCameraToObject(
+      Pose3d objectInField,
+      Transform3d robotToCamera,
+      DoubleArrayEntry cameraToObjectEntry,
+      DifferentialDrivetrainSim drivetrainSimulator) {
+    Pose3d robotInField = new Pose3d(drivetrainSimulator.getPose());
+    Pose3d cameraInField = robotInField.plus(robotToCamera);
+    Transform3d cameraToObject = new Transform3d(cameraInField, objectInField);
+
+    // Publishes double array with Translation3D elements {x, y, z} and Rotation3D elements {w, x,
+    // y, z} which describe
+    // the cameraToObject transformation.
+    double[] val = {
+      cameraToObject.getX(),
+      cameraToObject.getY(),
+      cameraToObject.getZ(),
+      cameraToObject.getRotation().getQuaternion().getW(),
+      cameraToObject.getRotation().getQuaternion().getX(),
+      cameraToObject.getRotation().getQuaternion().getY(),
+      cameraToObject.getRotation().getQuaternion().getZ()
+    };
+    cameraToObjectEntry.set(val);
+  }
+
+  /**
+   * Queries the camera-to-object transformation from networktables to compute the robot's
+   * field-relative pose from vision measurements.
+   *
+   * <p>The object could be a target or a fiducial marker.
+   *
+   * @param objectInField The object's field-relative pose.
+   * @param robotToCamera The transformation from the robot's pose to the camera's pose.
+   * @param cameraToObjectEntry The networktables entry publishing and querying example computer
+   *     vision measurements.
+   */
+  public Pose3d objectToRobotPose(
+      Pose3d objectInField, Transform3d robotToCamera, DoubleArrayEntry cameraToObjectEntry) {
+    double[] val = cameraToObjectEntry.get();
+
+    // Reconstruct cameraToObject Transform3D from networktables.
+    Translation3d translation = new Translation3d(val[0], val[1], val[2]);
+    Rotation3d rotation = new Rotation3d(new Quaternion(val[3], val[4], val[5], val[6]));
+    Transform3d cameraToObject = new Transform3d(translation, rotation);
+
+    return ComputerVisionUtil.objectToRobotPose(objectInField, cameraToObject, robotToCamera);
+  }
+
   /** Updates the field-relative position. */
   public void updateOdometry() {
     m_poseEstimator.update(
         m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
 
-    // Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
-    // a real robot, this must be calculated based either on latency or timestamps.
-    m_poseEstimator.addVisionMeasurement(
-        ExampleGlobalMeasurementSensor.getEstimatedGlobalPose(
-            m_poseEstimator.getEstimatedPosition()),
-        Timer.getFPGATimestamp() - 0.3);
+    // Publish cameraToObject transformation to networktables --this would normally be handled by
+    // the
+    // computer vision solution.
+    publishCameraToObject(
+        m_objectInField, m_robotToCamera, m_cameraToObjectEntry, m_drivetrainSimulator);
+
+    // Compute the robot's field-relative position exclusively from vision measurements.
+    Pose3d visionMeasurement3d =
+        objectToRobotPose(m_objectInField, m_robotToCamera, m_cameraToObjectEntry);
+
+    // Convert robot pose from Pose3d to Pose2d needed to apply vision measurements.
+    Pose2d visionMeasurement2d = visionMeasurement3d.toPose2d();
+
+    // Apply vision measurements. For simulation purposes only, we don't input a latency delay -- on
+    // a real robot, this must be calculated based either on known latency or timestamps.
+    m_poseEstimator.addVisionMeasurement(visionMeasurement2d, Timer.getFPGATimestamp());
+  }
+
+  /** This function is called periodically during simulation. */
+  public void simulationPeriodic() {
+    // To update our simulation, we set motor voltage inputs, update the
+    // simulation, and write the simulated positions and velocities to our
+    // simulated encoder and gyro.
+    m_drivetrainSimulator.setInputs(
+        m_leftGroup.get() * RobotController.getInputVoltage(),
+        m_rightGroup.get() * RobotController.getInputVoltage());
+    m_drivetrainSimulator.update(0.02);
+
+    m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters());
+    m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocityMetersPerSecond());
+    m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPositionMeters());
+    m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocityMetersPerSecond());
+    m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees());
+  }
+
+  /** This function is called periodically, no matter the mode. */
+  public void periodic() {
+    updateOdometry();
+    m_fieldSim.setRobotPose(m_drivetrainSimulator.getPose());
+    m_fieldApproximation.setRobotPose(m_poseEstimator.getEstimatedPosition());
   }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Robot.java

@@ -5,12 +5,18 @@
 package edu.wpi.first.wpilibj.examples.differentialdriveposeestimator;
 
 import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.networktables.DoubleArrayTopic;
+import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.XboxController;
 
 public class Robot extends TimedRobot {
+  private final NetworkTableInstance m_inst = NetworkTableInstance.getDefault();
+  private final DoubleArrayTopic m_doubleArrayTopic =
+      m_inst.getDoubleArrayTopic("m_doubleArrayTopic");
+
   private final XboxController m_controller = new XboxController(0);
-  private final Drivetrain m_drive = new Drivetrain();
+  private final Drivetrain m_drive = new Drivetrain(m_doubleArrayTopic);
 
   // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
   private final SlewRateLimiter m_speedLimiter = new SlewRateLimiter(3);
@@ -22,6 +28,16 @@ public class Robot extends TimedRobot {
     m_drive.updateOdometry();
   }
 
+  @Override
+  public void simulationPeriodic() {
+    m_drive.simulationPeriodic();
+  }
+
+  @Override
+  public void robotPeriodic() {
+    m_drive.periodic();
+  }
+
   @Override
   public void teleopPeriodic() {
     // Get the x speed. We are inverting this because Xbox controllers return