Add check for packet of incorrect length (#629)

Co-authored-by: Matt <matthew.morley.ca@gmail.com>

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

@@ -99,13 +99,15 @@ class Packet {
    */
   template <typename T>
   Packet& operator>>(T& value) {
-    std::memcpy(&value, packetData.data() + readPos, sizeof(T));
+    if (!packetData.empty()) {
+      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));
+      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);

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

@@ -0,0 +1,85 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <cmath>
+
+#include "photonlib/PhotonTrackedTarget.h"
+
+namespace photonlib {
+
+namespace PhotonTargetSortMode {
+
+struct Smallest {
+  inline bool operator()(const PhotonTrackedTarget& target1,
+                         const PhotonTrackedTarget& target2) {
+    return target1.GetArea() < target2.GetArea();
+  }
+};
+
+struct Largest {
+  inline bool operator()(const PhotonTrackedTarget& target1,
+                         const PhotonTrackedTarget& target2) {
+    return target1.GetArea() > target2.GetArea();
+  }
+};
+
+struct Highest {
+  inline bool operator()(const PhotonTrackedTarget& target1,
+                         const PhotonTrackedTarget& target2) {
+    return target1.GetPitch() < target2.GetPitch();
+  }
+};
+
+struct Lowest {
+  inline bool operator()(const PhotonTrackedTarget& target1,
+                         const PhotonTrackedTarget& target2) {
+    return target1.GetPitch() > target2.GetPitch();
+  }
+};
+
+struct RightMost {
+  inline bool operator()(const PhotonTrackedTarget& target1,
+                         const PhotonTrackedTarget& target2) {
+    return target1.GetYaw() < target2.GetYaw();
+  }
+};
+
+struct LeftMost {
+  inline bool operator()(const PhotonTrackedTarget& target1,
+                         const PhotonTrackedTarget& target2) {
+    return target1.GetYaw() > target2.GetYaw();
+  }
+};
+
+struct CenterMost {
+  inline bool operator()(const PhotonTrackedTarget& target1,
+                         const PhotonTrackedTarget& target2) {
+    return std::pow(target1.GetPitch(), 2) + std::pow(target1.GetYaw(), 2) <
+           std::pow(target2.GetPitch(), 2) + std::pow(target2.GetYaw(), 2);
+  }
+};
+}  // namespace PhotonTargetSortMode
+}  // namespace photonlib

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

@@ -0,0 +1,126 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <algorithm>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <networktables/NetworkTableInstance.h>
+
+#include "photonlib/PhotonCamera.h"
+#include "photonlib/PhotonTargetSortMode.h"
+
+namespace photonlib {
+class SimPhotonCamera : public PhotonCamera {
+ public:
+  SimPhotonCamera(std::shared_ptr<nt::NetworkTableInstance> instance,
+                  const std::string& cameraName)
+      : PhotonCamera(instance, cameraName) {
+    latencyMillisEntry = rootTable->GetEntry("latencyMillis");
+    hasTargetEntry = rootTable->GetEntry("hasTargetEntry");
+    targetPitchEntry = rootTable->GetEntry("targetPitchEntry");
+    targetYawEntry = rootTable->GetEntry("targetYawEntry");
+    targetAreaEntry = rootTable->GetEntry("targetAreaEntry");
+    targetSkewEntry = rootTable->GetEntry("targetSkewEntry");
+    targetPoseEntry = rootTable->GetEntry("targetPoseEntry");
+    versionEntry = instance->GetTable("photonvision")->GetEntry("version");
+    // versionEntry.SetString(PhotonVersion.versionString);
+  }
+
+  explicit SimPhotonCamera(const std::string& cameraName)
+      : SimPhotonCamera(std::make_shared<nt::NetworkTableInstance>(
+                            nt::NetworkTableInstance::GetDefault()),
+                        cameraName) {}
+
+  /**
+   * Simulate one processed frame of vision data, putting one result to NT.
+   *
+   * @param latency Latency of the provided frame
+   * @param targetList List of targets detected
+   */
+  void SubmitProcessedFrame(units::millisecond_t latency,
+                            std::vector<PhotonTrackedTarget> targetList) {
+    SubmitProcessedFrame(latency, PhotonTargetSortMode::LeftMost(), targetList);
+  }
+
+  /**
+   * Simulate one processed frame of vision data, putting one result to NT.
+   *
+   * @param latency Latency of the provided frame
+   * @param sortMode Order in which to sort targets
+   * @param targetList List of targets detected
+   */
+  void SubmitProcessedFrame(
+      units::millisecond_t latency,
+      std::function<bool(const PhotonTrackedTarget& target1,
+                         const PhotonTrackedTarget& target2)>
+          sortMode,
+      std::vector<PhotonTrackedTarget> targetList) {
+    latencyMillisEntry.SetDouble(latency.to<double>());
+    std::sort(targetList.begin(), targetList.end(),
+              [&](auto lhs, auto rhs) { return sortMode(lhs, rhs); });
+    PhotonPipelineResult newResult{latency, targetList};
+    Packet packet{};
+    packet << newResult;
+    rawBytesEntry.SetRaw(
+        std::string_view{packet.GetData().data(), packet.GetDataSize()});
+
+    std::string rawBytesGet = rawBytesEntry.GetRaw("ohono");
+
+    bool hasTargets = newResult.HasTargets();
+    hasTargetEntry.SetBoolean(hasTargets);
+    if (!hasTargets) {
+      targetPitchEntry.SetDouble(0.0);
+      targetYawEntry.SetDouble(0.0);
+      targetAreaEntry.SetDouble(0.0);
+      targetPoseEntry.SetDoubleArray({0.0, 0.0, 0.0});
+      targetSkewEntry.SetDouble(0.0);
+    } else {
+      PhotonTrackedTarget bestTarget = newResult.GetBestTarget();
+      targetPitchEntry.SetDouble(bestTarget.GetPitch());
+      targetYawEntry.SetDouble(bestTarget.GetYaw());
+      targetAreaEntry.SetDouble(bestTarget.GetArea());
+      targetSkewEntry.SetDouble(bestTarget.GetSkew());
+
+      frc::Transform3d transform = bestTarget.GetBestCameraToTarget();
+      targetPoseEntry.SetDoubleArray(
+          {transform.X().to<double>(), transform.Y().to<double>(),
+           transform.Rotation().ToRotation2d().Degrees().to<double>()});
+    }
+  }
+
+ private:
+  nt::NetworkTableEntry latencyMillisEntry;
+  nt::NetworkTableEntry hasTargetEntry;
+  nt::NetworkTableEntry targetPitchEntry;
+  nt::NetworkTableEntry targetYawEntry;
+  nt::NetworkTableEntry targetAreaEntry;
+  nt::NetworkTableEntry targetSkewEntry;
+  nt::NetworkTableEntry targetPoseEntry;
+  nt::NetworkTableEntry versionEntry;
+};
+}  // namespace photonlib

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

@@ -0,0 +1,208 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <frc/smartdashboard/Field2d.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <units/angle.h>
+#include <units/area.h>
+
+#include "SimPhotonCamera.h"
+#include "SimVisionTarget.h"
+
+namespace photonlib {
+class SimVisionSystem {
+ public:
+  SimPhotonCamera cam;
+  units::radian_t camHorizFOV{0};
+  units::radian_t camVertFOV{0};
+  units::meter_t maxLEDRange{0};
+  int cameraResWidth{0};
+  int cameraResHeight{0};
+  double minTargetArea{0.0};
+  frc::Transform3d cameraToRobot;
+
+  frc::Field2d dbgField;
+  frc::FieldObject2d* dbgRobot;
+  frc::FieldObject2d* dbgCamera;
+
+  std::vector<SimVisionTarget> targetList;
+
+  /**
+   * Create a simulated vision system involving a camera and coprocessor mounted
+   * on a mobile robot running PhotonVision, detecting one or more targets
+   * scattered around the field. This assumes a fairly simple and
+   * distortion-less pinhole camera model.
+   *
+   * @param camName Name of the PhotonVision camera to create. Align it with the
+   * settings you use in the PhotonVision GUI.
+   * @param camDiagFOV Diagonal Field of View of the camera used. Align it with
+   * the manufacturer specifications, and/or whatever is configured in the
+   * PhotonVision Setting page.
+   * @param cameraToRobot Transform to move from the camera's mount position to
+   * the robot's position
+   * @param maxLEDRange Maximum distance at which your camera can illuminate the
+   * target and make it visible. Set to 9000 or more if your vision system does
+   * not rely on LED's.
+   * @param cameraResWidth Width of your camera's image sensor in pixels
+   * @param cameraResHeight Height of your camera's image sensor in pixels
+   * @param minTargetArea Minimum area that that the target should be before
+   * it's recognized as a target by the camera. Match this with your contour
+   * filtering settings in the PhotonVision GUI.
+   */
+  SimVisionSystem(std::string camName, units::degree_t camDiagFOV,
+                  frc::Transform3d cameraToRobot, units::meter_t maxLEDRange,
+                  int cameraResWidth, int cameraResHeight, double minTargetArea)
+      : cam(camName),
+        camHorizFOV((camDiagFOV * cameraResWidth) /
+                    std::hypot(cameraResWidth, cameraResHeight)),
+        camVertFOV((camDiagFOV * cameraResHeight) /
+                   std::hypot(cameraResWidth, cameraResHeight)),
+        maxLEDRange(maxLEDRange),
+        cameraResWidth(cameraResWidth),
+        cameraResHeight(cameraResHeight),
+        minTargetArea(minTargetArea),
+        cameraToRobot(cameraToRobot),
+        dbgField(),
+        dbgRobot(dbgField.GetRobotObject()),
+        dbgCamera(dbgField.GetObject(camName + " Camera")) {
+    frc::SmartDashboard::PutData(camName + " Sim Field", &dbgField);
+  }
+
+  /**
+   * Add a target on the field which your vision system is designed to detect.
+   * The PhotonCamera from this system will report the location of the robot
+   * relative to the subset of these targets which are visible from the given
+   * robot position.
+   *
+   * @param target Target to add to the simulated field
+   */
+  void AddSimVisionTarget(SimVisionTarget target) {
+    targetList.push_back(target);
+    dbgField.GetObject("Target " + std::to_string(target.targetId))
+        ->SetPose(target.targetPose.ToPose2d());
+  }
+
+  /**
+   * Adjust the camera position relative to the robot. Use this if your camera
+   * is on a gimbal or turret or some other mobile platform.
+   *
+   * @param newCameraToRobot New Transform from the robot to the camera
+   */
+  void MoveCamera(frc::Transform3d newCameraToRobot) {
+    cameraToRobot = newCameraToRobot;
+  }
+
+  /**
+   * Periodic update. Call this once per frame of image data you wish to process
+   * and send to NetworkTables
+   *
+   * @param robotPose current pose of the robot on the field. Will be used to
+   * calculate which targets are actually in view, where they are at relative to
+   * the robot, and relevant PhotonVision parameters.
+   */
+  void ProcessFrame(frc::Pose2d robotPose) {
+    ProcessFrame(frc::Pose3d{
+        robotPose.X(), robotPose.Y(), 0.0_m,
+        frc::Rotation3d{0_rad, 0_rad, robotPose.Rotation().Radians()}});
+  }
+
+  /**
+   * Periodic update. Call this once per frame of image data you wish to process
+   * and send to NetworkTables
+   *
+   * @param robotPose current pose of the robot in space. Will be used to
+   * calculate which targets are actually in view, where they are at relative to
+   * the robot, and relevant PhotonVision parameters.
+   */
+  void ProcessFrame(frc::Pose3d robotPose) {
+    frc::Pose3d cameraPose = robotPose.TransformBy(cameraToRobot.Inverse());
+    dbgRobot->SetPose(robotPose.ToPose2d());
+    dbgCamera->SetPose(cameraPose.ToPose2d());
+
+    std::vector<PhotonTrackedTarget> visibleTargetList{};
+
+    for (const auto& target : targetList) {
+      frc::Transform3d camToTargetTransform{cameraPose, target.targetPose};
+      frc::Translation3d camToTargetTranslation{
+          camToTargetTransform.Translation()};
+
+      frc::Translation3d altTranslation{camToTargetTranslation.X(),
+                                        -1.0 * camToTargetTranslation.Y(),
+                                        camToTargetTranslation.Z()};
+      frc::Rotation3d altRotation{camToTargetTransform.Rotation() * -1.0};
+      frc::Transform3d camToTargetAltTransform{altTranslation, altRotation};
+      units::meter_t dist{camToTargetTranslation.Norm()};
+      double areaPixels{target.targetArea / GetM2PerPx(dist)};
+      units::radian_t yaw{units::math::atan2(camToTargetTranslation.Y(),
+                                             camToTargetTranslation.X())};
+      units::meter_t cameraHeightOffGround{cameraPose.Z()};
+      units::meter_t targetHeightAboveGround(target.targetPose.Z());
+      units::radian_t camPitch{cameraPose.Rotation().Y()};
+      frc::Transform2d transformAlongGround{cameraPose.ToPose2d(),
+                                            target.targetPose.ToPose2d()};
+      units::meter_t distanceAlongGround{
+          transformAlongGround.Translation().Norm()};
+      units::radian_t pitch =
+          units::math::atan2(targetHeightAboveGround - cameraHeightOffGround,
+                             distanceAlongGround) -
+          camPitch;
+
+      if (CamCamSeeTarget(dist, yaw, pitch, areaPixels)) {
+        visibleTargetList.push_back(
+            PhotonTrackedTarget{yaw.convert<units::degree>().to<double>(),
+                                pitch.convert<units::degree>().to<double>(),
+                                areaPixels,
+                                0.0,
+                                target.targetId,
+                                camToTargetTransform,
+                                {{0, 0}, {0, 0}, {0, 0}, {0, 0}}});
+      }
+
+      cam.SubmitProcessedFrame(0_s, visibleTargetList);
+    }
+  }
+
+  units::square_meter_t GetM2PerPx(units::meter_t dist) {
+    units::meter_t widthMPerPx =
+        2 * dist * units::math::tan(camHorizFOV / 2) / cameraResWidth;
+    units::meter_t heightMPerPx =
+        2 * dist * units::math::tan(camVertFOV / 2) / cameraResHeight;
+    return widthMPerPx * heightMPerPx;
+  }
+
+  bool CamCamSeeTarget(units::meter_t dist, units::radian_t yaw,
+                       units::radian_t pitch, double area) {
+    bool inRange = dist < maxLEDRange;
+    bool inHorizAngle = units::math::abs(yaw) < camHorizFOV / 2;
+    bool inVertAngle = units::math::abs(pitch) < camVertFOV / 2;
+    bool targetBigEnough = area > minTargetArea;
+    return (inRange && inHorizAngle && inVertAngle && targetBigEnough);
+  }
+};
+}  // namespace photonlib

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

@@ -0,0 +1,59 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <frc/geometry/Pose3d.h>
+#include <units/area.h>
+
+namespace photonlib {
+class SimVisionTarget {
+ public:
+  SimVisionTarget() = default;
+
+  /**
+   * Describes a vision target located somewhere on the field that your
+   * SimVisionSystem can detect.
+   *
+   * @param targetPose Pose3d of the target in field-relative coordinates
+   * @param targetWidth Width of the outer bounding box of the target.
+   * @param targetHeight Pair Height of the outer bounding box of the
+   * target.
+   * @param targetId Id of the target
+   */
+  SimVisionTarget(frc::Pose3d targetPose, units::meter_t targetWidth,
+                  units::meter_t targetHeight, int targetId)
+      : targetPose(targetPose),
+        targetWidth(targetWidth),
+        targetHeight(targetHeight),
+        targetArea(targetHeight * targetWidth),
+        targetId(targetId) {}
+
+  frc::Pose3d targetPose;
+  units::meter_t targetWidth;
+  units::meter_t targetHeight;
+  units::square_meter_t targetArea;
+  int targetId;
+};
+}  // namespace photonlib

photon-lib/src/test/native/cpp/SimVisionSystemTest.cpp

@@ -0,0 +1,362 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include "gtest/gtest.h"
+#include "photonlib/PhotonUtils.h"
+#include "photonlib/SimVisionSystem.h"
+
+class SimVisionSystemTest : public ::testing::Test {
+  void SetUp() override {
+    nt::NetworkTableInstance::GetDefault().StartServer();
+    photonlib::PhotonCamera::SetVersionCheckEnabled(false);
+  }
+
+  void TearDown() override {}
+};
+
+class SimVisionSystemTestWithParamsTest
+    : public SimVisionSystemTest,
+      public testing::WithParamInterface<units::degree_t> {};
+class SimVisionSystemTestDistanceParamsTest
+    : public SimVisionSystemTest,
+      public testing::WithParamInterface<
+          std::tuple<units::foot_t, units::degree_t, units::foot_t>> {};
+
+TEST_F(SimVisionSystemTest, TestEmpty) {
+  photonlib::SimVisionSystem sys{
+      "Test", 80.0_deg, frc::Transform3d{}, 99999_m, 320, 240, 0};
+  SUCCEED();
+}
+
+TEST_F(SimVisionSystemTest, TestVisibilityCupidShuffle) {
+  const frc::Pose3d targetPose{
+      {15.98_m, 0_m, 2_m},
+      frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}};
+  photonlib::SimVisionSystem sys{
+      "Test", 80.0_deg, frc::Transform3d{}, 99999_m, 640, 480, 0};
+  sys.AddSimVisionTarget(photonlib::SimVisionTarget{targetPose, 1_m, 3_m, 3});
+
+  // To the right, to the right
+  frc::Pose2d robotPose{{5_m, 0_m}, frc::Rotation2d{-70_deg}};
+  sys.ProcessFrame(robotPose);
+  ASSERT_FALSE(sys.cam.GetLatestResult().HasTargets());
+
+  // To the right, to the right
+  robotPose = frc::Pose2d{{5_m, 0_m}, frc::Rotation2d{-95_deg}};
+  sys.ProcessFrame(robotPose);
+  ASSERT_FALSE(sys.cam.GetLatestResult().HasTargets());
+
+  // To the left, to the left
+  robotPose = frc::Pose2d{{5_m, 0_m}, frc::Rotation2d{90_deg}};
+  sys.ProcessFrame(robotPose);
+  ASSERT_FALSE(sys.cam.GetLatestResult().HasTargets());
+
+  // To the left, to the left
+  robotPose = frc::Pose2d{{5_m, 0_m}, frc::Rotation2d{65_deg}};
+  sys.ProcessFrame(robotPose);
+  ASSERT_FALSE(sys.cam.GetLatestResult().HasTargets());
+
+  // now kick, now kick
+  robotPose = frc::Pose2d{{2_m, 0_m}, frc::Rotation2d{5_deg}};
+  sys.ProcessFrame(robotPose);
+  ASSERT_TRUE(sys.cam.GetLatestResult().HasTargets());
+
+  // now kick, now kick
+  robotPose = frc::Pose2d{{2_m, 0_m}, frc::Rotation2d{-5_deg}};
+  sys.ProcessFrame(robotPose);
+  ASSERT_TRUE(sys.cam.GetLatestResult().HasTargets());
+
+  // now walk it by yourself
+  robotPose = frc::Pose2d{{2_m, 0_m}, frc::Rotation2d{-179_deg}};
+  sys.ProcessFrame(robotPose);
+  ASSERT_FALSE(sys.cam.GetLatestResult().HasTargets());
+
+  // now walk it by yourself
+  sys.MoveCamera(frc::Transform3d{
+      frc::Translation3d{},
+      frc::Rotation3d{0_rad, 0_rad, units::constants::detail::PI_VAL * 1_rad}});
+  sys.ProcessFrame(robotPose);
+  ASSERT_TRUE(sys.cam.GetLatestResult().HasTargets());
+}
+
+TEST_F(SimVisionSystemTest, TestNotVisibleVertOne) {
+  const frc::Pose3d targetPose{
+      {15.98_m, 0_m, 1_m},
+      frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}};
+  photonlib::SimVisionSystem sys{
+      "Test", 80.0_deg, frc::Transform3d{}, 99999_m, 640, 480, 0};
+  sys.AddSimVisionTarget(photonlib::SimVisionTarget{targetPose, 1_m, 3_m, 3});
+
+  frc::Pose2d robotPose{{5_m, 0_m}, frc::Rotation2d{5_deg}};
+  sys.ProcessFrame(robotPose);
+  ASSERT_TRUE(sys.cam.GetLatestResult().HasTargets());
+
+  sys.MoveCamera(frc::Transform3d{
+      frc::Translation3d{0_m, 0_m, 5000_m},
+      frc::Rotation3d{0_rad, 0_rad, units::constants::detail::PI_VAL * 1_rad}});
+  sys.ProcessFrame(robotPose);
+  ASSERT_FALSE(sys.cam.GetLatestResult().HasTargets());
+}
+
+TEST_F(SimVisionSystemTest, TestNotVisibleVertTwo) {
+  const frc::Pose3d targetPose{
+      {15.98_m, 0_m, 2_m},
+      frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}};
+  frc::Transform3d robotToCamera{
+      frc::Translation3d{0_m, 0_m, 1_m},
+      frc::Rotation3d{0_deg, (units::constants::detail::PI_VAL / 4) * 1_rad,
+                      0_deg}};
+  photonlib::SimVisionSystem sys{
+      "Test", 80.0_deg, robotToCamera.Inverse(), 99999_m, 1234, 1234, 0};
+  sys.AddSimVisionTarget(
+      photonlib::SimVisionTarget{targetPose, 3_m, 0.5_m, 1736});
+
+  frc::Pose2d robotPose{{14.98_m, 0_m}, frc::Rotation2d{5_deg}};
+  sys.ProcessFrame(robotPose);
+  ASSERT_TRUE(sys.cam.GetLatestResult().HasTargets());
+
+  robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}};
+  sys.ProcessFrame(robotPose);
+  ASSERT_FALSE(sys.cam.GetLatestResult().HasTargets());
+}
+
+TEST_F(SimVisionSystemTest, TestNotVisibleTargetSize) {
+  const frc::Pose3d targetPose{
+      {15.98_m, 0_m, 1_m},
+      frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}};
+  photonlib::SimVisionSystem sys{
+      "Test", 80.0_deg, frc::Transform3d{}, 99999_m, 640, 480, 20.0};
+  sys.AddSimVisionTarget(
+      photonlib::SimVisionTarget{targetPose, 0.1_m, 0.025_m, 24});
+
+  frc::Pose2d robotPose{{12_m, 0_m}, frc::Rotation2d{5_deg}};
+  sys.ProcessFrame(robotPose);
+  ASSERT_TRUE(sys.cam.GetLatestResult().HasTargets());
+
+  robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}};
+  sys.ProcessFrame(robotPose);
+  ASSERT_FALSE(sys.cam.GetLatestResult().HasTargets());
+}
+
+TEST_F(SimVisionSystemTest, TestNotVisibleTooFarForLEDs) {
+  const frc::Pose3d targetPose{
+      {15.98_m, 0_m, 1_m},
+      frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}};
+  photonlib::SimVisionSystem sys{
+      "Test", 80.0_deg, frc::Transform3d{}, 10_m, 640, 480, 1.0};
+  sys.AddSimVisionTarget(
+      photonlib::SimVisionTarget{targetPose, 1_m, 0.25_m, 78});
+
+  frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{5_deg}};
+  sys.ProcessFrame(robotPose);
+  ASSERT_TRUE(sys.cam.GetLatestResult().HasTargets());
+
+  robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}};
+  sys.ProcessFrame(robotPose);
+  ASSERT_FALSE(sys.cam.GetLatestResult().HasTargets());
+}
+
+TEST_P(SimVisionSystemTestWithParamsTest, YawAngles) {
+  const frc::Pose3d targetPose{
+      {15.98_m, 0_m, 0_m},
+      frc::Rotation3d{0_deg, 0_deg,
+                      (3 * units::constants::detail::PI_VAL / 4) * 1_rad}};
+  frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{GetParam() * -1.0}};
+  photonlib::SimVisionSystem sys{
+      "Test", 120.0_deg, frc::Transform3d{}, 99999_m, 640, 480, 0};
+  sys.AddSimVisionTarget(
+      photonlib::SimVisionTarget{targetPose, 0.5_m, 0.5_m, 23});
+
+  sys.ProcessFrame(robotPose);
+
+  auto results = sys.cam.GetLatestResult();
+
+  ASSERT_TRUE(results.HasTargets());
+
+  photonlib::PhotonTrackedTarget target = results.GetBestTarget();
+
+  ASSERT_NEAR(GetParam().to<double>(), target.GetYaw(), 0.0001);
+}
+
+TEST_P(SimVisionSystemTestWithParamsTest, PitchAngles) {
+  const frc::Pose3d targetPose{
+      {15.98_m, 0_m, 0_m},
+      frc::Rotation3d{0_deg, 0_deg,
+                      (3 * units::constants::detail::PI_VAL / 4) * 1_rad}};
+  frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{0_deg}};
+  photonlib::SimVisionSystem sys{
+      "Test", 120.0_deg, frc::Transform3d{}, 99999_m, 640, 480, 0};
+  sys.AddSimVisionTarget(
+      photonlib::SimVisionTarget{targetPose, 0.5_m, 0.5_m, 23});
+
+  sys.MoveCamera(frc::Transform3d{frc::Translation3d{},
+                                  frc::Rotation3d{0_deg, GetParam(), 0_deg}});
+  sys.ProcessFrame(robotPose);
+
+  auto results = sys.cam.GetLatestResult();
+
+  ASSERT_TRUE(results.HasTargets());
+
+  photonlib::PhotonTrackedTarget target = results.GetBestTarget();
+
+  ASSERT_NEAR(GetParam().to<double>(), target.GetPitch(), 0.0001);
+}
+
+INSTANTIATE_TEST_SUITE_P(AnglesTests, SimVisionSystemTestWithParamsTest,
+                         testing::Values(-10_deg, -5_deg, -0_deg, -1_deg,
+                                         -2_deg, 5_deg, 7_deg, 10.23_deg,
+                                         20.21_deg, -19.999_deg));
+
+TEST_P(SimVisionSystemTestDistanceParamsTest, DistanceCalc) {
+  units::foot_t distParam;
+  units::degree_t pitchParam;
+  units::foot_t heightParam;
+  std::tie(distParam, pitchParam, heightParam) = GetParam();
+
+  const frc::Pose3d targetPose{
+      {15.98_m, 0_m, 1_m},
+      frc::Rotation3d{0_deg, 0_deg,
+                      (units::constants::detail::PI_VAL * 0.98) * 1_rad}};
+  frc::Pose3d robotPose{{15.98_m - distParam, 0_m, 0_m}, frc::Rotation3d{}};
+  frc::Transform3d robotToCamera{frc::Translation3d{0_m, 0_m, heightParam},
+                                 frc::Rotation3d{0_deg, pitchParam, 0_deg}};
+  photonlib::SimVisionSystem sys{
+      "absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysoho"
+      "wsyourdaygoingihopegoodhaveagreatrestofyourlife",
+      160.0_deg,
+      robotToCamera.Inverse(),
+      99999_m,
+      640,
+      480,
+      0};
+  sys.AddSimVisionTarget(
+      photonlib::SimVisionTarget{targetPose, 0.5_m, 0.5_m, 0});
+  sys.ProcessFrame(robotPose);
+
+  auto results = sys.cam.GetLatestResult();
+
+  ASSERT_TRUE(results.HasTargets());
+
+  photonlib::PhotonTrackedTarget target = results.GetBestTarget();
+
+  ASSERT_NEAR(target.GetYaw(), 0.0, 0.0001);
+
+  units::meter_t dist = photonlib::PhotonUtils::CalculateDistanceToTarget(
+      robotToCamera.Z(), targetPose.Z(), pitchParam,
+      units::degree_t{target.GetPitch()});
+  ASSERT_NEAR(dist.to<double>(),
+              distParam.convert<units::meters>().to<double>(), 0.001);
+}
+
+INSTANTIATE_TEST_SUITE_P(
+    DistanceParamsTests, SimVisionSystemTestDistanceParamsTest,
+    testing::Values(std::make_tuple(5_ft, 15.98_deg, 0_ft),
+                    std::make_tuple(6_ft, 15.98_deg, 1_ft),
+                    std::make_tuple(10_ft, 15.98_deg, 0_ft),
+                    std::make_tuple(15_ft, 15.98_deg, 2_ft),
+                    std::make_tuple(19.95_ft, 15.98_deg, 0_ft),
+                    std::make_tuple(20_ft, 15.98_deg, 0_ft),
+                    std::make_tuple(5_ft, 42_deg, 1_ft),
+                    std::make_tuple(6_ft, 42_deg, 0_ft),
+                    std::make_tuple(10_ft, 42_deg, 2_ft),
+                    std::make_tuple(15_ft, 42_deg, 0.5_ft),
+                    std::make_tuple(19.42_ft, 15.98_deg, 0_ft),
+                    std::make_tuple(20_ft, 42_deg, 0_ft),
+                    std::make_tuple(5_ft, 55_deg, 2_ft),
+                    std::make_tuple(6_ft, 55_deg, 0_ft),
+                    std::make_tuple(10_ft, 54_deg, 2.2_ft),
+                    std::make_tuple(15_ft, 53_deg, 0_ft),
+                    std::make_tuple(19.52_ft, 15.98_deg, 1.1_ft)));
+
+TEST_F(SimVisionSystemTest, TestMultipleTargets) {
+  const frc::Pose3d targetPoseL{
+      {15.98_m, 2_m, 0_m},
+      frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}};
+  const frc::Pose3d targetPoseC{
+      {15.98_m, 0_m, 0_m},
+      frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}};
+  const frc::Pose3d targetPoseR{
+      {15.98_m, -2_m, 0_m},
+      frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}};
+  photonlib::SimVisionSystem sys{
+      "Test", 160.0_deg, frc::Transform3d{}, 99999_m, 640, 480, 20.0};
+  sys.AddSimVisionTarget(photonlib::SimVisionTarget{
+      targetPoseL.TransformBy(frc::Transform3d{
+          frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}),
+      0.25_m, 0.25_m, 1});
+  sys.AddSimVisionTarget(photonlib::SimVisionTarget{
+      targetPoseC.TransformBy(frc::Transform3d{
+          frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}),
+      0.25_m, 0.25_m, 2});
+  sys.AddSimVisionTarget(photonlib::SimVisionTarget{
+      targetPoseR.TransformBy(frc::Transform3d{
+          frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}),
+      0.25_m, 0.25_m, 3});
+
+  sys.AddSimVisionTarget(photonlib::SimVisionTarget{
+      targetPoseL.TransformBy(frc::Transform3d{
+          frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}),
+      0.25_m, 0.25_m, 4});
+  sys.AddSimVisionTarget(photonlib::SimVisionTarget{
+      targetPoseC.TransformBy(frc::Transform3d{
+          frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}),
+      0.25_m, 0.25_m, 5});
+  sys.AddSimVisionTarget(photonlib::SimVisionTarget{
+      targetPoseR.TransformBy(frc::Transform3d{
+          frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}),
+      0.25_m, 0.25_m, 6});
+
+  sys.AddSimVisionTarget(photonlib::SimVisionTarget{
+      targetPoseL.TransformBy(frc::Transform3d{
+          frc::Translation3d{0_m, 0_m, 0.5_m}, frc::Rotation3d{}}),
+      0.25_m, 0.25_m, 7});
+  sys.AddSimVisionTarget(photonlib::SimVisionTarget{
+      targetPoseC.TransformBy(frc::Transform3d{
+          frc::Translation3d{0_m, 0_m, 0.5_m}, frc::Rotation3d{}}),
+      0.25_m, 0.25_m, 8});
+  sys.AddSimVisionTarget(photonlib::SimVisionTarget{
+      targetPoseL.TransformBy(frc::Transform3d{
+          frc::Translation3d{0_m, 0_m, 0.75_m}, frc::Rotation3d{}}),
+      0.25_m, 0.25_m, 9});
+  sys.AddSimVisionTarget(photonlib::SimVisionTarget{
+      targetPoseR.TransformBy(frc::Transform3d{
+          frc::Translation3d{0_m, 0_m, 0.75_m}, frc::Rotation3d{}}),
+      0.25_m, 0.25_m, 10});
+  sys.AddSimVisionTarget(photonlib::SimVisionTarget{
+      targetPoseL.TransformBy(frc::Transform3d{
+          frc::Translation3d{0_m, 0_m, 0.25_m}, frc::Rotation3d{}}),
+      0.25_m, 0.25_m, 11});
+
+  frc::Pose2d robotPose{{6_m, 0_m}, frc::Rotation2d{.25_deg}};
+  sys.ProcessFrame(robotPose);
+
+  auto results = sys.cam.GetLatestResult();
+
+  ASSERT_TRUE(results.HasTargets());
+
+  wpi::span<const photonlib::PhotonTrackedTarget> targetList =
+      results.GetTargets();
+
+  ASSERT_EQ(targetList.size(), size_t(11));
+}

photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java

@@ -129,6 +129,9 @@ public class Packet {
      * @return A decoded byte from the packet.
      */
     public byte decodeByte() {
+        if (packetData.length < readPos) {
+            return '\0';
+        }
         return packetData[readPos++];
     }
 
@@ -138,6 +141,9 @@ public class Packet {
      * @return A decoded int from the packet.
      */
     public int decodeInt() {
+        if (packetData.length < readPos + 3) {
+            return 0;
+        }
         return (0xff & packetData[readPos++]) << 24
                 | (0xff & packetData[readPos++]) << 16
                 | (0xff & packetData[readPos++]) << 8
@@ -150,6 +156,9 @@ public class Packet {
      * @return A decoded double from the packet.
      */
     public double decodeDouble() {
+        if (packetData.length < (readPos + 7)) {
+            return 0;
+        }
         long data =
                 (long) (0xff & packetData[readPos++]) << 56
                         | (long) (0xff & packetData[readPos++]) << 48
@@ -168,6 +177,9 @@ public class Packet {
      * @return A decoded boolean from the packet.
      */
     public boolean decodeBoolean() {
+        if (packetData.length < readPos) {
+            return false;
+        }
         return packetData[readPos++] == 1;
     }
 }