Add Photonlib (#231)

Merges Photonlib into Photonvision, along with the Photonlib code examples. Also creates a new PhotonTargeting library teams can depend on.

photon-lib/src/main/native/include/photonlib/Packet.h

@@ -0,0 +1,121 @@
+/*
+ * 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/>.
+ */
+
+#pragma once
+
+#include <algorithm>
+#include <string>
+#include <vector>
+
+#include <wpi/Endian.h>
+
+namespace photonlib {
+
+/**
+ * A packet that holds byte-packed data to be sent over NetworkTables.
+ */
+class Packet {
+ public:
+  /**
+   * Constructs an empty packet.
+   */
+  Packet() = default;
+
+  /**
+   * Constructs a packet with the given data.
+   * @param data The packet data.
+   */
+  explicit Packet(std::vector<char> data) : packetData(data) {}
+
+  /**
+   * Clears the packet and resets the read and write positions.
+   */
+  void Clear() {
+    packetData.clear();
+    readPos = 0;
+    writePos = 0;
+  }
+
+  /**
+   * Returns the packet data.
+   * @return The packet data.
+   */
+  const std::vector<char>& GetData() { return packetData; }
+
+  /**
+   * Returns the number of bytes in the data.
+   * @return The number of bytes in the data.
+   */
+  size_t GetDataSize() const { return packetData.size(); }
+
+  /**
+   * Adds a value to the data buffer. This should only be used with PODs.
+   * @tparam T The data type.
+   * @param src The data source.
+   * @return A reference to the current object.
+   */
+  template <typename T>
+  Packet& operator<<(T src) {
+    packetData.resize(packetData.size() + sizeof(T));
+    std::memcpy(packetData.data() + writePos, &src, sizeof(T));
+
+    if constexpr (wpi::support::endian::system_endianness() ==
+                  wpi::support::endianness::little) {
+      // Reverse to big endian for network conventions.
+      std::reverse(packetData.data() + writePos,
+                   packetData.data() + writePos + sizeof(T));
+    }
+
+    writePos += sizeof(T);
+    return *this;
+  }
+
+  /**
+   * Extracts a value to the provided destination.
+   * @tparam T The type of value to extract.
+   * @param value The value to extract.
+   * @return A reference to the current object.
+   */
+  template <typename T>
+  Packet& operator>>(T& value) {
+    std::memcpy(&value, packetData.data() + readPos, sizeof(T));
+
+    if constexpr (wpi::support::endian::system_endianness() ==
+                  wpi::support::endianness::little) {
+      // Reverse to little endian for host.
+      char& raw = reinterpret_cast<char&>(value);
+      std::reverse(&raw, &raw + sizeof(T));
+    }
+
+    readPos += sizeof(T);
+    return *this;
+  }
+
+  bool operator==(const Packet& right) const {
+    return packetData == right.packetData;
+  }
+  bool operator!=(const Packet& right) const { return !operator==(right); }
+
+ private:
+  // Data stored in the packet
+  std::vector<char> packetData;
+
+  size_t readPos = 0;
+  size_t writePos = 0;
+};
+
+}  // namespace photonlib

photon-lib/src/main/native/include/photonlib/PhotonCamera.h

@@ -0,0 +1,141 @@
+/*
+ * 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/>.
+ */
+
+#pragma once
+
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableEntry.h>
+#include <networktables/NetworkTableInstance.h>
+
+#include <memory>
+#include <string>
+
+#include "photonlib/PhotonPipelineResult.h"
+
+namespace photonlib {
+
+enum LEDMode : int { kDefault = -1, kOff = 0, kOn = 1, kBlink = 2 };
+
+/**
+ * Represents a camera that is connected to PhotonVision.ß
+ */
+class PhotonCamera {
+ public:
+  /**
+   * Constructs a PhotonCamera from a root table.
+   * @param rootTable The root table that the camera is broadcasting information
+   * over.
+   */
+  explicit PhotonCamera(std::shared_ptr<nt::NetworkTable> rootTable);
+
+  /**
+   * Constructs a PhotonCamera from the name of the camera.
+   * @param cameraName The nickname of the camera (found in the PhotonVision
+   * UI).
+   */
+  explicit PhotonCamera(const std::string& cameraName);
+
+  /**
+   * Returns the latest pipeline result.
+   * @return The latest pipeline result.
+   */
+  PhotonPipelineResult GetLatestResult() const;
+
+  /**
+   * Toggles driver mode.
+   * @param driverMode Whether to set driver mode.
+   */
+  void SetDriverMode(bool driverMode);
+
+  /**
+   * Returns whether the camera is in driver mode.
+   * @return Whether the camera is in driver mode.
+   */
+  bool GetDriverMode() const;
+
+  /**
+   * Request the camera to save a new image file from the input
+   * camera stream with overlays.
+   * Images take up space in the filesystem of the PhotonCamera.
+   * Calling it frequently will fill up disk space and eventually
+   * cause the system to stop working.
+   * Clear out images in /opt/photonvision/photonvision_config/imgSaves
+   * frequently to prevent issues.
+   */
+  void TakeInputSnapshot(void);
+
+  /**
+   * Request the camera to save a new image file from the output
+   * stream with overlays.
+   * Images take up space in the filesystem of the PhotonCamera.
+   * Calling it frequently will fill up disk space and eventually
+   * cause the system to stop working.
+   * Clear out images in /opt/photonvision/photonvision_config/imgSaves
+   * frequently to prevent issues.
+   */
+  void TakeOutputSnapshot(void);
+
+  /**
+   * Allows the user to select the active pipeline index.
+   * @param index The active pipeline index.
+   */
+  void SetPipelineIndex(int index);
+
+  /**
+   * Returns the active pipeline index.
+   * @return The active pipeline index.
+   */
+  int GetPipelineIndex() const;
+
+  /**
+   * Returns the current LED mode.
+   * @return The current LED mode.
+   */
+  LEDMode GetLEDMode() const;
+
+  /**
+   * Sets the LED mode.
+   * @param led The mode to set to.
+   */
+  void SetLEDMode(LEDMode led);
+
+  /**
+   * Returns whether the latest target result has targets.
+   * @return Whether the latest target result has targets.
+   */
+  bool HasTargets() const { return GetLatestResult().HasTargets(); }
+
+ private:
+  std::shared_ptr<nt::NetworkTable> mainTable =
+      nt::NetworkTableInstance::GetDefault().GetTable("photonvision");
+
+ protected:
+  nt::NetworkTableEntry rawBytesEntry;
+  nt::NetworkTableEntry driverModeEntry;
+  nt::NetworkTableEntry inputSaveImgEntry;
+  nt::NetworkTableEntry outputSaveImgEntry;
+  nt::NetworkTableEntry pipelineIndexEntry;
+  nt::NetworkTableEntry ledModeEntry;
+
+  mutable Packet packet;
+
+  bool driverMode;
+  double pipelineIndex;
+  mutable LEDMode mode;
+};
+
+}  // namespace photonlib

photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h

@@ -0,0 +1,100 @@
+/*
+ * 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/>.
+ */
+
+#pragma once
+
+#include <string>
+
+#include <frc/DriverStation.h>
+#include <units/time.h>
+#include <wpi/ArrayRef.h>
+#include <wpi/SmallVector.h>
+
+#include "photonlib/Packet.h"
+#include "photonlib/PhotonTrackedTarget.h"
+
+namespace photonlib {
+/**
+ * Represents a pipeline result from a PhotonCamera.
+ */
+class PhotonPipelineResult {
+ public:
+  /**
+   * Constructs an empty pipeline result.
+   */
+  PhotonPipelineResult() = default;
+
+  /**
+   * Constructs a pipeline result.
+   * @param latency The latency in the pipeline.
+   * @param targets The list of targets identified by the pipeline.
+   */
+  PhotonPipelineResult(units::second_t latency,
+                       wpi::ArrayRef<PhotonTrackedTarget> targets);
+
+  /**
+   * Returns the best target in this pipeline result. If there are no targets,
+   * this method will return an empty target with all values set to zero. The
+   * best target is determined by the target sort mode in the PhotonVision UI.
+   *
+   * @return The best target of the pipeline result.
+   */
+  PhotonTrackedTarget GetBestTarget() const {
+    if (!HasTargets() && !HAS_WARNED) {
+      ::frc::DriverStation::ReportError(
+          "This PhotonPipelineResult object has no targets associated with it! "
+          "Please check hasTargets() before calling this method. For more "
+          "information, please review the PhotonLib documentation at "
+          "http://docs.photonvision.org");
+      HAS_WARNED = true;
+    }
+    return hasTargets ? targets[0] : PhotonTrackedTarget();
+  }
+
+  /**
+   * Returns the latency in the pipeline.
+   * @return The latency in the pipeline.
+   */
+  units::second_t GetLatency() const { return latency; }
+
+  /**
+   * Returns whether the pipeline has targets.
+   * @return Whether the pipeline has targets.
+   */
+  bool HasTargets() const { return hasTargets; }
+
+  /**
+   * Returns a reference to the vector of targets.
+   * @return A reference to the vector of targets.
+   */
+  const wpi::ArrayRef<PhotonTrackedTarget> GetTargets() const {
+    return targets;
+  }
+
+  bool operator==(const PhotonPipelineResult& other) const;
+  bool operator!=(const PhotonPipelineResult& other) const;
+
+  friend Packet& operator<<(Packet& packet, const PhotonPipelineResult& result);
+  friend Packet& operator>>(Packet& packet, PhotonPipelineResult& result);
+
+ private:
+  units::second_t latency;
+  bool hasTargets;
+  wpi::SmallVector<PhotonTrackedTarget, 10> targets;
+  inline static bool HAS_WARNED = false;
+};
+}  // namespace photonlib

photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h

@@ -0,0 +1,93 @@
+/*
+ * 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/>.
+ */
+
+#pragma once
+
+#include <cstddef>
+#include <string>
+#include <vector>
+
+#include <frc/geometry/Transform2d.h>
+
+#include "photonlib/Packet.h"
+
+namespace photonlib {
+/**
+ * Represents a tracked target within a pipeline.
+ */
+class PhotonTrackedTarget {
+ public:
+  /**
+   * Constructs an empty target.
+   */
+  PhotonTrackedTarget() = default;
+
+  /**
+   * Constructs a target.
+   * @param yaw The yaw of the target.
+   * @param pitch The pitch of the target.
+   * @param area The area of the target.
+   * @param skew The skew of the target.
+   * @param pose The camera-relative pose of the target.
+   */
+  PhotonTrackedTarget(double yaw, double pitch, double area, double skew,
+                      const frc::Transform2d& pose);
+
+  /**
+   * Returns the target yaw (positive-left).
+   * @return The target yaw.
+   */
+  double GetYaw() const { return yaw; }
+
+  /**
+   * Returns the target pitch (positive-up)
+   * @return The target pitch.
+   */
+  double GetPitch() const { return pitch; }
+
+  /**
+   * Returns the target area (0-100).
+   * @return The target area.
+   */
+  double GetArea() const { return area; }
+
+  /**
+   * Returns the target skew (counter-clockwise positive).
+   * @return The target skew.
+   */
+  double GetSkew() const { return skew; }
+
+  /**
+   * Returns the pose of the target relative to the robot.
+   * @return The pose of the target relative to the robot.
+   */
+  frc::Transform2d GetCameraRelativePose() const { return cameraToTarget; }
+
+  bool operator==(const PhotonTrackedTarget& other) const;
+  bool operator!=(const PhotonTrackedTarget& other) const;
+
+  friend Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target);
+  friend Packet& operator>>(Packet& packet, PhotonTrackedTarget& target);
+
+ private:
+  double yaw = 0;
+  double pitch = 0;
+  double area = 0;
+  double skew = 0;
+  frc::Transform2d cameraToTarget;
+};
+}  // namespace photonlib

photon-lib/src/main/native/include/photonlib/PhotonUtils.h

@@ -0,0 +1,173 @@
+/*
+ * 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/>.
+ */
+
+#pragma once
+
+#include <frc/geometry/Pose2d.h>
+#include <frc/geometry/Rotation2d.h>
+#include <frc/geometry/Transform2d.h>
+#include <frc/geometry/Translation2d.h>
+#include <units/angle.h>
+#include <units/length.h>
+#include <units/math.h>
+
+namespace photonlib {
+class PhotonUtils {
+ public:
+  /**
+   * Algorithm from
+   * https://docs.limelightvision.io/en/latest/cs_estimating_distance.html
+   * Estimates range to a target using the target's elevation. This method can
+   * produce more stable results than SolvePNP when well tuned, if the full 6d
+   * robot pose is not required.
+   *
+   * @param cameraHeight The height of the camera off the floor.
+   * @param targetHeight The height of the target off the floor.
+   * @param cameraPitch The pitch of the camera from the horizontal plane.
+   * Positive valueBytes up.
+   * @param targetPitch The pitch of the target in the camera's lens. Positive
+   * values up.
+   * @return The estimated distance to the target.
+   */
+  static units::meter_t CalculateDistanceToTarget(units::meter_t cameraHeight,
+                                                  units::meter_t targetHeight,
+                                                  units::radian_t cameraPitch,
+                                                  units::radian_t targetPitch) {
+    return (targetHeight - cameraHeight) /
+           units::math::tan(cameraPitch + targetPitch);
+  }
+
+  /**
+   * Estimate the Translation2d of the target relative to the camera.
+   *
+   * @param targetDistance The distance to the target.
+   * @param yaw            The observed yaw of the target.
+   *
+   * @return The target's camera-relative translation.
+   */
+  static frc::Translation2d EstimateCameraToTargetTranslation(
+      units::meter_t targetDistance, const frc::Rotation2d& yaw) {
+    return {targetDistance * yaw.Cos(), targetDistance * yaw.Sin()};
+  }
+
+  /**
+   * Estimate the position of the robot in the field.
+   *
+   * @param cameraHeightMeters The physical height of the camera off the floor
+   * in meters.
+   * @param targetHeightMeters The physical height of the target off the floor
+   * in meters. This should be the height of whatever is being targeted (i.e. if
+   * the targeting region is set to top, this should be the height of the top of
+   * the target).
+   * @param cameraPitchRadians The pitch of the camera from the horizontal plane
+   *                           in radians. Positive values up.
+   * @param targetPitchRadians The pitch of the target in the camera's lens in
+   *                           radians. Positive values up.
+   * @param targetYaw          The observed yaw of the target. Note that this
+   *                           *must* be CCW-positive, and Photon returns
+   *                           CW-positive.
+   * @param gyroAngle          The current robot gyro angle, likely from
+   * odometry.
+   * @param fieldToTarget      A frc::Pose2d representing the target position in
+   * the field coordinate system.
+   * @param cameraToRobot      The position of the robot relative to the camera.
+   *                           If the camera was mounted 3 inches behind the
+   *                           "origin" (usually physical center) of the robot,
+   *                           this would be frc::Transform2d(3 inches, 0
+   * inches, 0 degrees).
+   * @return The position of the robot in the field.
+   */
+  static frc::Pose2d EstimateFieldToRobot(
+      units::meter_t cameraHeight, units::meter_t targetHeight,
+      units::radian_t cameraPitch, units::radian_t targetPitch,
+      const frc::Rotation2d& targetYaw, const frc::Rotation2d& gyroAngle,
+      const frc::Pose2d& fieldToTarget, const frc::Transform2d& cameraToRobot) {
+    return EstimateFieldToRobot(
+        EstimateCameraToTarget(
+            EstimateCameraToTargetTranslation(
+                CalculateDistanceToTarget(cameraHeight, targetHeight,
+                                          cameraPitch, targetPitch),
+                targetYaw),
+            fieldToTarget, gyroAngle),
+        fieldToTarget, cameraToRobot);
+  }
+
+  /**
+   * Estimates a {@link frc::Transform2d} that maps the camera position to the
+   * target position, using the robot's gyro. Note that the gyro angle provided
+   * *must* line up with the field coordinate system -- that is, it should read
+   * zero degrees when pointed towards the opposing alliance station, and
+   * increase as the robot rotates CCW.
+   *
+   * @param cameraToTargetTranslation A Translation2d that encodes the x/y
+   *                                  position of the target relative to the
+   *                                  camera.
+   * @param fieldToTarget             A frc::Pose2d representing the target
+   * position in the field coordinate system.
+   * @param gyroAngle                 The current robot gyro angle, likely from
+   *                                  odometry.
+   * @return A frc::Transform2d that takes us from the camera to the target.
+   */
+  static frc::Transform2d EstimateCameraToTarget(
+      const frc::Translation2d& cameraToTargetTranslation,
+      const frc::Pose2d& fieldToTarget, const frc::Rotation2d& gyroAngle) {
+    // This pose maps our camera at the origin out to our target, in the robot
+    // reference frame
+    // The translation part of this frc::Transform2d is from the above step, and
+    // the rotation uses our robot's gyro.
+    return frc::Transform2d(cameraToTargetTranslation,
+                            -gyroAngle - fieldToTarget.Rotation());
+  }
+
+  /**
+   * Estimates the pose of the robot in the field coordinate system, given the
+   * position of the target relative to the camera, the target relative to the
+   * field, and the robot relative to the camera.
+   *
+   * @param cameraToTarget The position of the target relative to the camera.
+   * @param fieldToTarget  The position of the target in the field.
+   * @param cameraToRobot  The position of the robot relative to the camera. If
+   *                       the camera was mounted 3 inches behind the "origin"
+   *                       (usually physical center) of the robot, this would be
+   *                       frc::Transform2d(3 inches, 0 inches, 0 degrees).
+   * @return The position of the robot in the field.
+   */
+  static frc::Pose2d EstimateFieldToRobot(
+      const frc::Transform2d& cameraToTarget, const frc::Pose2d& fieldToTarget,
+      const frc::Transform2d& cameraToRobot) {
+    return EstimateFieldToCamera(cameraToTarget, fieldToTarget)
+        .TransformBy(cameraToRobot);
+  }
+
+  /**
+   * Estimates the pose of the camera in the field coordinate system, given the
+   * position of the target relative to the camera, and the target relative to
+   * the field. This *only* tracks the position of the camera, not the position
+   * of the robot itself.
+   *
+   * @param cameraToTarget The position of the target relative to the camera.
+   * @param fieldToTarget  The position of the target in the field.
+   * @return The position of the camera in the field.
+   */
+  static frc::Pose2d EstimateFieldToCamera(
+      const frc::Transform2d& cameraToTarget,
+      const frc::Pose2d& fieldToTarget) {
+    auto targetToCamera = cameraToTarget.Inverse();
+    return fieldToTarget.TransformBy(targetToCamera);
+  }
+};
+}  // namespace photonlib

photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h

@@ -0,0 +1,65 @@
+/*
+ * 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/>.
+ */
+
+#pragma once
+
+#include <memory>
+#include <string>
+
+#include <units/time.h>
+#include <wpi/ArrayRef.h>
+#include <wpi/SmallVector.h>
+
+#include "photonlib/Packet.h"
+#include "photonlib/PhotonCamera.h"
+
+namespace photonlib {
+
+/**
+ * Represents a camera that is connected to PhotonVision.ß
+ */
+class SimPhotonCamera : public PhotonCamera {
+ public:
+  /**
+   * Constructs a Simulated PhotonCamera from a root table.
+   *
+   * @param rootTable The root table that the camera is broadcasting information
+   *                  over.
+   */
+  explicit SimPhotonCamera(std::shared_ptr<nt::NetworkTable> rootTable);
+
+  /**
+   * Constructs a Simulated PhotonCamera from the name of the camera.
+   *
+   * @param cameraName The nickname of the camera (found in the PhotonVision
+   *                   UI).
+   */
+  explicit SimPhotonCamera(const std::string& cameraName);
+
+  /**
+   * Simulate one processed frame of vision data, putting one result to NT.
+   * @param latency Latency of frame processing
+   * @param tgtList Set of targets detected
+   */
+  void SubmitProcessedFrame(units::second_t latency,
+                            wpi::ArrayRef<PhotonTrackedTarget> tgtList);
+
+ private:
+  mutable Packet simPacket;
+};
+
+}  // namespace photonlib

photon-lib/src/main/native/include/photonlib/SimVisionSystem.h

@@ -0,0 +1,74 @@
+/*
+ * 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/>.
+ */
+
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <frc/geometry/Translation2d.h>
+#include <units/angle.h>
+#include <units/area.h>
+#include <units/length.h>
+#include <units/time.h>
+#include <wpi/ArrayRef.h>
+#include <wpi/SmallVector.h>
+
+#include "photonlib/SimPhotonCamera.h"
+#include "photonlib/SimVisionTarget.h"
+
+namespace photonlib {
+
+/**
+ * Represents a camera that is connected to PhotonVision.
+ */
+class SimVisionSystem {
+ public:
+  explicit SimVisionSystem(const std::string& name, units::degree_t camDiagFOV,
+                           units::degree_t camPitch,
+                           frc::Transform2d cameraToRobot,
+                           units::meter_t cameraHeightOffGround,
+                           units::meter_t maxLEDRange, int cameraResWidth,
+                           int cameraResHeight, double minTargetArea);
+
+  void AddSimVisionTarget(SimVisionTarget tgt);
+  void MoveCamera(frc::Transform2d newcameraToRobot,
+                  units::meter_t newCamHeight, units::degree_t newCamPitch);
+  void ProcessFrame(frc::Pose2d robotPose);
+
+ private:
+  units::degree_t camDiagFOV;
+  units::degree_t camPitch;
+  frc::Transform2d cameraToRobot;
+  units::meter_t cameraHeightOffGround;
+  units::meter_t maxLEDRange;
+  int cameraResWidth;
+  int cameraResHeight;
+  double minTargetArea;
+  units::degree_t camHorizFOV;
+  units::degree_t camVertFOV;
+  std::vector<SimVisionTarget> tgtList = {};
+
+  double GetM2PerPx(units::meter_t dist);
+  bool CamCanSeeTarget(units::meter_t distHypot, units::degree_t yaw,
+                       units::degree_t pitch, double area);
+
+ public:
+  SimPhotonCamera cam = photonlib::SimPhotonCamera("Default");
+};
+
+}  // namespace photonlib

photon-lib/src/main/native/include/photonlib/SimVisionTarget.h

@@ -0,0 +1,45 @@
+/*
+ * 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/>.
+ */
+
+#pragma once
+
+#include <frc/geometry/Pose2d.h>
+#include <units/area.h>
+#include <units/length.h>
+
+namespace photonlib {
+
+/**
+ * Represents a target on the field which the vision processing system could
+ * detect.
+ */
+class SimVisionTarget {
+ public:
+  explicit SimVisionTarget(frc::Pose2d& targetPos,
+                           units::meter_t targetHeightAboveGround,
+                           units::meter_t targetWidth,
+                           units::meter_t targetHeight);
+
+  frc::Pose2d targetPos;
+  units::meter_t targetHeightAboveGround;
+  units::meter_t targetWidth;
+  units::meter_t targetHeight;
+  double targetInfill_pct;
+  units::square_meter_t tgtArea;
+};
+
+}  // namespace photonlib