Send corners of min area rectangles (#382)

Adds a new corners entry to targets. Breaks byte-packed backwards compatibility. 

Co-authored-by: Tyler Veness <calcmogul@gmail.com>

photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp

@@ -18,19 +18,28 @@
 #include "photonlib/PhotonTrackedTarget.h"
 
 #include <iostream>
+#include <utility>
 
 #include <frc/geometry/Translation2d.h>
+#include <wpi/SmallVector.h>
 
 namespace photonlib {
 
-PhotonTrackedTarget::PhotonTrackedTarget(double yaw, double pitch, double area,
-                                         double skew,
-                                         const frc::Transform2d& pose)
-    : yaw(yaw), pitch(pitch), area(area), skew(skew), cameraToTarget(pose) {}
+PhotonTrackedTarget::PhotonTrackedTarget(
+    double yaw, double pitch, double area, double skew,
+    const frc::Transform2d& pose,
+    const wpi::SmallVector<std::pair<double, double>, 4> corners)
+    : yaw(yaw),
+      pitch(pitch),
+      area(area),
+      skew(skew),
+      cameraToTarget(pose),
+      corners(corners) {}
 
 bool PhotonTrackedTarget::operator==(const PhotonTrackedTarget& other) const {
   return other.yaw == yaw && other.pitch == pitch && other.area == area &&
-         other.skew == skew && other.cameraToTarget == cameraToTarget;
+         other.skew == skew && other.cameraToTarget == cameraToTarget &&
+         other.corners == corners;
 }
 
 bool PhotonTrackedTarget::operator!=(const PhotonTrackedTarget& other) const {
@@ -38,10 +47,16 @@ bool PhotonTrackedTarget::operator!=(const PhotonTrackedTarget& other) const {
 }
 
 Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) {
-  return packet << target.yaw << target.pitch << target.area << target.skew
-                << target.cameraToTarget.Translation().X().value()
-                << target.cameraToTarget.Translation().Y().value()
-                << target.cameraToTarget.Rotation().Degrees().value();
+  packet << target.yaw << target.pitch << target.area << target.skew
+         << target.cameraToTarget.Translation().X().value()
+         << target.cameraToTarget.Translation().Y().value()
+         << target.cameraToTarget.Rotation().Degrees().value();
+
+  for (int i = 0; i < 4; i++) {
+    packet << target.corners[i].first << target.corners[i].second;
+  }
+
+  return packet;
 }
 
 Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) {
@@ -54,6 +69,15 @@ Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) {
   target.cameraToTarget =
       frc::Transform2d(frc::Translation2d(units::meter_t(x), units::meter_t(y)),
                        units::degree_t(rot));
+
+  target.corners.clear();
+  for (int i = 0; i < 4; i++) {
+    double first = 0;
+    double second = 0;
+    packet >> first >> second;
+    target.corners.emplace_back(first, second);
+  }
+
   return packet;
 }
 

photon-lib/src/main/native/cpp/photonlib/SimVisionSystem.cpp

@@ -86,7 +86,8 @@ void SimVisionSystem::ProcessFrame(frc::Pose2d robotPose) {
 
     if (CamCanSeeTarget(distHypot, yawAngle, pitchAngle, area)) {
       PhotonTrackedTarget newTgt = PhotonTrackedTarget(
-          yawAngle.value(), pitchAngle.value(), area, 0.0, camToTargetTrans);
+          yawAngle.value(), pitchAngle.value(), area, 0.0, camToTargetTrans,
+          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}});
       visibleTgtList.push_back(newTgt);
     }
   }

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

@@ -19,9 +19,11 @@
 
 #include <cstddef>
 #include <string>
+#include <utility>
 #include <vector>
 
 #include <frc/geometry/Transform2d.h>
+#include <wpi/SmallVector.h>
 
 #include "photonlib/Packet.h"
 
@@ -43,9 +45,12 @@ class PhotonTrackedTarget {
    * @param area The area of the target.
    * @param skew The skew of the target.
    * @param pose The camera-relative pose of the target.
+   * @Param corners The corners of the bounding rectangle.
    */
-  PhotonTrackedTarget(double yaw, double pitch, double area, double skew,
-                      const frc::Transform2d& pose);
+  PhotonTrackedTarget(
+      double yaw, double pitch, double area, double skew,
+      const frc::Transform2d& pose,
+      const wpi::SmallVector<std::pair<double, double>, 4> corners);
 
   /**
    * Returns the target yaw (positive-left).
@@ -71,6 +76,13 @@ class PhotonTrackedTarget {
    */
   double GetSkew() const { return skew; }
 
+  /**
+   * Returns the corners of the minimum area rectangle bounding this target.
+   */
+  wpi::SmallVector<std::pair<double, double>, 4> GetCorners() const {
+    return corners;
+  }
+
   /**
    * Returns the pose of the target relative to the robot.
    * @return The pose of the target relative to the robot.
@@ -89,5 +101,6 @@ class PhotonTrackedTarget {
   double area = 0;
   double skew = 0;
   frc::Transform2d cameraToTarget;
+  wpi::SmallVector<std::pair<double, double>, 4> corners;
 };
 }  // namespace photonlib