[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.

wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp

@@ -4,10 +4,31 @@
 
 #include "Drivetrain.h"
 
-#include <frc/Timer.h>
-
 #include "ExampleGlobalMeasurementSensor.h"
 
+Drivetrain::Drivetrain() {
+  // We need to invert one side of the drivetrain so that positive voltages
+  // result in both sides moving forward. Depending on how your robot's
+  // gearbox is constructed, you might have to invert the left side instead.
+  m_rightGroup.SetInverted(true);
+
+  m_gyro.Reset();
+
+  // Set the distance per pulse for the drive encoders. We can simply use the
+  // distance traveled for one rotation of the wheel divided by the encoder
+  // resolution.
+  m_leftEncoder.SetDistancePerPulse(
+      (2 * std::numbers::pi * kWheelRadius / kEncoderResolution).value());
+  m_rightEncoder.SetDistancePerPulse(
+      (2 * std::numbers::pi * kWheelRadius / kEncoderResolution).value());
+
+  m_leftEncoder.Reset();
+  m_rightEncoder.Reset();
+
+  frc::SmartDashboard::PutData("FieldSim", &m_fieldSim);
+  frc::SmartDashboard::PutData("Approximation", &m_fieldApproximation);
+}
+
 void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) {
   const auto leftFeedforward = m_feedforward.Calculate(speeds.left);
   const auto rightFeedforward = m_feedforward.Calculate(speeds.right);
@@ -25,16 +46,86 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
   SetSpeeds(m_kinematics.ToWheelSpeeds({xSpeed, 0_mps, rot}));
 }
 
+void Drivetrain::PublishCameraToObject(
+    frc::Pose3d objectInField, frc::Transform3d robotToCamera,
+    nt::DoubleArrayEntry& cameraToObjectEntry,
+    frc::sim::DifferentialDrivetrainSim drivetrainSimulator) {
+  frc::Pose3d robotInField{drivetrainSimulator.GetPose()};
+  frc::Pose3d cameraInField = robotInField + robotToCamera;
+  frc::Transform3d cameraToObject{cameraInField, objectInField};
+
+  // Publishes double array with Translation3D elements {x, y, z} and Rotation3D
+  // elements {w, x, y, z} which describe the cameraToObject transformation.
+  std::array<double, 7> val{cameraToObject.X().value(),
+                            cameraToObject.Y().value(),
+                            cameraToObject.Z().value(),
+                            cameraToObject.Rotation().GetQuaternion().W(),
+                            cameraToObject.Rotation().GetQuaternion().X(),
+                            cameraToObject.Rotation().GetQuaternion().Y(),
+                            cameraToObject.Rotation().GetQuaternion().Z()};
+  cameraToObjectEntry.Set(val);
+}
+
+frc::Pose3d Drivetrain::ObjectToRobotPose(
+    frc::Pose3d objectInField, frc::Transform3d robotToCamera,
+    nt::DoubleArrayEntry& cameraToObjectEntry) {
+  std::vector<double> val{cameraToObjectEntry.Get()};
+
+  // Reconstruct cameraToObject Transform3D from networktables.
+  frc::Translation3d translation{units::meter_t{val[0]}, units::meter_t{val[1]},
+                                 units::meter_t{val[2]}};
+  frc::Rotation3d rotation{frc::Quaternion{val[3], val[4], val[5], val[6]}};
+  frc::Transform3d cameraToObject{translation, rotation};
+
+  return frc::ObjectToRobotPose(objectInField, cameraToObject, robotToCamera);
+}
+
 void Drivetrain::UpdateOdometry() {
   m_poseEstimator.Update(m_gyro.GetRotation2d(),
                          units::meter_t{m_leftEncoder.GetDistance()},
                          units::meter_t{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()),
-      frc::Timer::GetFPGATimestamp() - 0.3_s);
+  // Publish cameraToObject transformation to networktables --this would
+  // normally be handled by the computer vision solution.
+  PublishCameraToObject(m_objectInField, m_robotToCamera,
+                        m_cameraToObjectEntryRef, m_drivetrainSimulator);
+
+  // Compute the robot's field-relative position exclusively from vision
+  // measurements.
+  frc::Pose3d visionMeasurement3d = ObjectToRobotPose(
+      m_objectInField, m_robotToCamera, m_cameraToObjectEntryRef);
+
+  // Convert robot's pose from Pose3d to Pose2d needed to apply vision
+  // measurements.
+  frc::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,
+                                       frc::Timer::GetFPGATimestamp());
+}
+
+void Drivetrain::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(units::volt_t{m_leftGroup.Get()} *
+                                      frc::RobotController::GetInputVoltage(),
+                                  units::volt_t{m_rightGroup.Get()} *
+                                      frc::RobotController::GetInputVoltage());
+  m_drivetrainSimulator.Update(20_ms);
+
+  m_leftEncoderSim.SetDistance(m_drivetrainSimulator.GetLeftPosition().value());
+  m_leftEncoderSim.SetRate(m_drivetrainSimulator.GetLeftVelocity().value());
+  m_rightEncoderSim.SetDistance(
+      m_drivetrainSimulator.GetRightPosition().value());
+  m_rightEncoderSim.SetRate(m_drivetrainSimulator.GetRightVelocity().value());
+  m_gyroSim.SetAngle(-m_drivetrainSimulator.GetHeading().Degrees().value());
+}
+
+void Drivetrain::Periodic() {
+  UpdateOdometry();
+  m_fieldSim.SetRobotPose(m_drivetrainSimulator.GetPose());
+  m_fieldApproximation.SetRobotPose(m_poseEstimator.GetEstimatedPosition());
 }

wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Robot.cpp

@@ -15,6 +15,8 @@ class Robot : public frc::TimedRobot {
     m_drive.UpdateOdometry();
   }
 
+  void RobotPeriodic() override { m_drive.Periodic(); }
+
   void TeleopPeriodic() override {
     // Get the x speed. We are inverting this because Xbox controllers return
     // negative values when we push forward.
@@ -31,6 +33,8 @@ class Robot : public frc::TimedRobot {
     m_drive.Drive(xSpeed, rot);
   }
 
+  void SimulationPeriodic() override { m_drive.SimulationPeriodic(); }
+
  private:
   frc::XboxController m_controller{0};
 

wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h

@@ -7,13 +7,30 @@
 #include <numbers>
 
 #include <frc/AnalogGyro.h>
+#include <frc/ComputerVisionUtil.h>
 #include <frc/Encoder.h>
+#include <frc/RobotController.h>
+#include <frc/Timer.h>
+#include <frc/apriltag/AprilTagFieldLayout.h>
+#include <frc/apriltag/AprilTagFields.h>
 #include <frc/controller/PIDController.h>
 #include <frc/controller/SimpleMotorFeedforward.h>
 #include <frc/estimator/DifferentialDrivePoseEstimator.h>
+#include <frc/geometry/Pose2d.h>
+#include <frc/geometry/Pose3d.h>
+#include <frc/geometry/Quaternion.h>
+#include <frc/geometry/Transform3d.h>
 #include <frc/kinematics/DifferentialDriveKinematics.h>
 #include <frc/motorcontrol/MotorControllerGroup.h>
 #include <frc/motorcontrol/PWMSparkMax.h>
+#include <frc/simulation/AnalogGyroSim.h>
+#include <frc/simulation/DifferentialDrivetrainSim.h>
+#include <frc/simulation/EncoderSim.h>
+#include <frc/smartdashboard/Field2d.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc/system/plant/LinearSystemId.h>
+#include <networktables/DoubleArrayTopic.h>
+#include <networktables/NetworkTableInstance.h>
 #include <units/angle.h>
 #include <units/angular_velocity.h>
 #include <units/length.h>
@@ -24,40 +41,100 @@
  */
 class Drivetrain {
  public:
-  Drivetrain() {
-    // We need to invert one side of the drivetrain so that positive voltages
-    // result in both sides moving forward. Depending on how your robot's
-    // gearbox is constructed, you might have to invert the left side instead.
-    m_rightGroup.SetInverted(true);
-
-    m_gyro.Reset();
-    // Set the distance per pulse for the drive encoders. We can simply use the
-    // distance traveled for one rotation of the wheel divided by the encoder
-    // resolution.
-    m_leftEncoder.SetDistancePerPulse(
-        2 * std::numbers::pi * kWheelRadius.value() / kEncoderResolution);
-    m_rightEncoder.SetDistancePerPulse(
-        2 * std::numbers::pi * kWheelRadius.value() / kEncoderResolution);
-
-    m_leftEncoder.Reset();
-    m_rightEncoder.Reset();
-  }
+  Drivetrain();
 
   static constexpr units::meters_per_second_t kMaxSpeed =
       3.0_mps;  // 3 meters per second
   static constexpr units::radians_per_second_t kMaxAngularSpeed{
       std::numbers::pi};  // 1/2 rotation per second
 
+  /**
+   * Sets the desired wheel speeds.
+   *
+   * @param speeds The desired wheel speeds.
+   */
   void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds);
+
+  /** Drives the robot with the given linear velocity and angular velocity.
+   *
+   * @param xSpeed Linear velocity.
+   * @param rot Angular Velocity.
+   */
   void Drive(units::meters_per_second_t xSpeed,
              units::radians_per_second_t rot);
+
+  /**
+   * Updates the field-relative position.
+   */
   void UpdateOdometry();
 
+  /**
+   * This function is called periodically during simulation. */
+  void SimulationPeriodic();
+
+  /** This function is called periodically, regardless of mode. */
+  void Periodic();
+
+  /**
+   * Computes and publishes to a networktables 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.
+   * @param drivetrainSimulation A DifferentialDrivetrainSim modeling the
+   * robot's drivetrain.
+   */
+  void PublishCameraToObject(
+      frc::Pose3d objectInField, frc::Transform3d robotToCamera,
+      nt::DoubleArrayEntry& cameraToObjectEntry,
+      frc::sim::DifferentialDrivetrainSim drivetrainSimulator);
+
+  /**
+   * 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 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.
+   */
+  frc::Pose3d ObjectToRobotPose(frc::Pose3d objectInField,
+                                frc::Transform3d robotToCamera,
+                                nt::DoubleArrayEntry& cameraToObjectEntry);
+
  private:
   static constexpr units::meter_t kTrackWidth = 0.381_m * 2;
   static constexpr units::meter_t kWheelRadius = 0.0508_m;
   static constexpr int kEncoderResolution = 4096;
 
+  static constexpr std::array<double, 7> kDefaultVal{0.0, 0.0, 0.0, 0.0,
+                                                     0.0, 0.0, 0.0};
+
+  frc::Transform3d m_robotToCamera{
+      frc::Translation3d{1_m, 1_m, 1_m},
+      frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi / 2}}};
+
+  nt::NetworkTableInstance m_inst{nt::NetworkTableInstance::GetDefault()};
+  nt::DoubleArrayTopic m_cameraToObjectTopic{
+      m_inst.GetDoubleArrayTopic("m_cameraToObjectTopic")};
+  nt::DoubleArrayEntry m_cameraToObjectEntry =
+      m_cameraToObjectTopic.GetEntry(kDefaultVal);
+  nt::DoubleArrayEntry& m_cameraToObjectEntryRef = m_cameraToObjectEntry;
+
+  frc::AprilTagFieldLayout m_aprilTagFieldLayout{
+      frc::LoadAprilTagLayoutField(frc::AprilTagField::k2022RapidReact)};
+  frc::Pose3d m_objectInField{m_aprilTagFieldLayout.GetTagPose(0).value()};
+
   frc::PWMSparkMax m_leftLeader{1};
   frc::PWMSparkMax m_leftFollower{2};
   frc::PWMSparkMax m_rightLeader{3};
@@ -90,4 +167,16 @@ class Drivetrain {
   // Gains are for example purposes only - must be determined for your own
   // robot!
   frc::SimpleMotorFeedforward<units::meters> m_feedforward{1_V, 3_V / 1_mps};
+
+  // Simulation classes
+  frc::sim::AnalogGyroSim m_gyroSim{m_gyro};
+  frc::sim::EncoderSim m_leftEncoderSim{m_leftEncoder};
+  frc::sim::EncoderSim m_rightEncoderSim{m_rightEncoder};
+  frc::Field2d m_fieldSim;
+  frc::Field2d m_fieldApproximation;
+  frc::LinearSystem<2, 2, 2> m_drivetrainSystem =
+      frc::LinearSystemId::IdentifyDrivetrainSystem(
+          1.98_V / 1_mps, 0.2_V / 1_mps_sq, 1.5_V / 1_mps, 0.3_V / 1_mps_sq);
+  frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{
+      m_drivetrainSystem, kTrackWidth, frc::DCMotor::CIM(2), 8, 2_in};
 };