[photon-targeting] Move C++ targeting classes to photon-targetting (#1009)

* add classes to targeting and update gradle

* rename me

* Finish cleanup

* Formatting fixes

* just use common.gradle

* Update build.gradle

* Update config.gradle

* remove typo

* simplify

* Add Packet Headers

* move simulation classes into simulation folder

* draw in dependency

* fix

* Everything working minus tests cause im lazy

* formatting fixes

REMEMBER TO REMOVE UNUSED IMPORTS, IM JUST TOO LAZY TO CHECK RN

* move packet test to targeting

* Formatting fixes

* remove TargetCorner from c++

im not 100% sure but just doing std::pair<double, double> is sufficient and shouldnt conflict with protobuf

* think i added packet

* fix namespace issue

* organize imports in photon-targeting

* Formatting fixes

* remove ctors

* fix typo

* Add PNP and Multitag packet tests

* revert TargetCorner class

* Add Test placeholders

* remove annoying print

* Reorganize build and publish process

channeling inner Thad

* add targeting as flag

* Update config.gradle

* fix issue with platform binaries not building

* Update photonlib.json.in

casing still needs to be checked

* add minimum level back

* add back UTF-8 encoding of javadoc

* simplify publish model for photon-lib

* fix windows symbol generation

* formatting fixes

* move task from main gradle to config

* Update config.gradle

photon-targeting/src/main/native/cpp/photon/targeting/MultiTargetPNPResult.cpp

@@ -0,0 +1,56 @@
+/*
+ * 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/>.
+ */
+
+#include "photon/targeting/MultiTargetPNPResult.h"
+
+namespace photon {
+
+bool MultiTargetPNPResult::operator==(const MultiTargetPNPResult& other) const {
+  return other.result == result && other.fiducialIdsUsed == fiducialIdsUsed;
+}
+
+Packet& operator<<(Packet& packet, const MultiTargetPNPResult& result) {
+  packet << result.result;
+
+  size_t i;
+  for (i = 0; i < result.fiducialIdsUsed.capacity(); i++) {
+    if (i < result.fiducialIdsUsed.size()) {
+      packet << static_cast<int16_t>(result.fiducialIdsUsed[i]);
+    } else {
+      packet << static_cast<int16_t>(-1);
+    }
+  }
+
+  return packet;
+}
+
+Packet& operator>>(Packet& packet, MultiTargetPNPResult& result) {
+  packet >> result.result;
+
+  result.fiducialIdsUsed.clear();
+  for (size_t i = 0; i < result.fiducialIdsUsed.capacity(); i++) {
+    int16_t id = 0;
+    packet >> id;
+
+    if (id > -1) {
+      result.fiducialIdsUsed.push_back(id);
+    }
+  }
+
+  return packet;
+}
+}  // namespace photon

photon-targeting/src/main/native/cpp/photon/targeting/PNPResult.cpp

@@ -0,0 +1,77 @@
+/*
+ * 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/>.
+ */
+
+#include "photon/targeting/PNPResult.h"
+
+namespace photon {
+bool PNPResult::operator==(const PNPResult& other) const {
+  return other.isPresent == isPresent && other.best == best &&
+         other.bestReprojErr == bestReprojErr && other.alt == alt &&
+         other.altReprojErr == altReprojErr && other.ambiguity == ambiguity;
+}
+
+// Encode a transform3d
+Packet& operator<<(Packet& packet, const frc::Transform3d& transform) {
+  packet << transform.Translation().X().value()
+         << transform.Translation().Y().value()
+         << transform.Translation().Z().value()
+         << transform.Rotation().GetQuaternion().W()
+         << transform.Rotation().GetQuaternion().X()
+         << transform.Rotation().GetQuaternion().Y()
+         << transform.Rotation().GetQuaternion().Z();
+
+  return packet;
+}
+
+// Decode a transform3d
+Packet& operator>>(Packet& packet, frc::Transform3d& transform) {
+  frc::Transform3d ret;
+
+  // We use these for best and alt transforms below
+  double x = 0;
+  double y = 0;
+  double z = 0;
+  double w = 0;
+
+  // decode and unitify translation
+  packet >> x >> y >> z;
+  const auto translation = frc::Translation3d(
+      units::meter_t(x), units::meter_t(y), units::meter_t(z));
+
+  // decode and add units to rotation
+  packet >> w >> x >> y >> z;
+  const auto rotation = frc::Rotation3d(frc::Quaternion(w, x, y, z));
+
+  transform = frc::Transform3d(translation, rotation);
+
+  return packet;
+}
+
+Packet& operator<<(Packet& packet, PNPResult const& result) {
+  packet << result.isPresent << result.best << result.alt
+         << result.bestReprojErr << result.altReprojErr << result.ambiguity;
+
+  return packet;
+}
+
+Packet& operator>>(Packet& packet, PNPResult& result) {
+  packet >> result.isPresent >> result.best >> result.alt >>
+      result.bestReprojErr >> result.altReprojErr >> result.ambiguity;
+
+  return packet;
+}
+}  // namespace photon

photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp

@@ -0,0 +1,67 @@
+/*
+ * 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/>.
+ */
+
+#include "photon/targeting/PhotonPipelineResult.h"
+
+namespace photon {
+PhotonPipelineResult::PhotonPipelineResult(
+    units::second_t latency, std::span<const PhotonTrackedTarget> targets)
+    : latency(latency),
+      targets(targets.data(), targets.data() + targets.size()) {}
+
+PhotonPipelineResult::PhotonPipelineResult(
+    units::second_t latency, std::span<const PhotonTrackedTarget> targets,
+    MultiTargetPNPResult multitagResult)
+    : latency(latency),
+      targets(targets.data(), targets.data() + targets.size()),
+      multitagResult(multitagResult) {}
+
+bool PhotonPipelineResult::operator==(const PhotonPipelineResult& other) const {
+  return latency == other.latency && targets == other.targets &&
+         multitagResult == other.multitagResult;
+}
+
+Packet& operator<<(Packet& packet, const PhotonPipelineResult& result) {
+  // Encode latency and number of targets.
+  packet << result.latency.value() * 1000 << result.multitagResult
+         << static_cast<int8_t>(result.targets.size());
+
+  // Encode the information of each target.
+  for (auto& target : result.targets) packet << target;
+
+  // Return the packet
+  return packet;
+}
+
+Packet& operator>>(Packet& packet, PhotonPipelineResult& result) {
+  // Decode latency, existence of targets, and number of targets.
+  double latencyMillis = 0;
+  int8_t targetCount = 0;
+  packet >> latencyMillis >> result.multitagResult >> targetCount;
+  result.latency = units::second_t(latencyMillis / 1000.0);
+
+  result.targets.clear();
+
+  // Decode the information of each target.
+  for (int i = 0; i < targetCount; ++i) {
+    PhotonTrackedTarget target;
+    packet >> target;
+    result.targets.push_back(target);
+  }
+  return packet;
+}
+}  // namespace photon

photon-targeting/src/main/native/cpp/photon/targeting/PhotonTrackedTarget.cpp

@@ -0,0 +1,135 @@
+/*
+ * 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/>.
+ */
+
+#include "photon/targeting/PhotonTrackedTarget.h"
+
+#include <iostream>
+#include <utility>
+
+#include <frc/geometry/Translation2d.h>
+#include <wpi/SmallVector.h>
+
+static constexpr const uint8_t MAX_CORNERS = 8;
+
+namespace photon {
+
+PhotonTrackedTarget::PhotonTrackedTarget(
+    double yaw, double pitch, double area, double skew, int id,
+    const frc::Transform3d& pose, const frc::Transform3d& alternatePose,
+    double ambiguity,
+    const wpi::SmallVector<std::pair<double, double>, 4> minAreaRectCorners,
+    const std::vector<std::pair<double, double>> detectedCorners)
+    : yaw(yaw),
+      pitch(pitch),
+      area(area),
+      skew(skew),
+      fiducialId(id),
+      bestCameraToTarget(pose),
+      altCameraToTarget(alternatePose),
+      poseAmbiguity(ambiguity),
+      minAreaRectCorners(minAreaRectCorners),
+      detectedCorners(detectedCorners) {}
+
+bool PhotonTrackedTarget::operator==(const PhotonTrackedTarget& other) const {
+  return other.yaw == yaw && other.pitch == pitch && other.area == area &&
+         other.skew == skew && other.bestCameraToTarget == bestCameraToTarget &&
+         other.minAreaRectCorners == minAreaRectCorners;
+}
+
+Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) {
+  packet << target.yaw << target.pitch << target.area << target.skew
+         << target.fiducialId
+         << target.bestCameraToTarget.Translation().X().value()
+         << target.bestCameraToTarget.Translation().Y().value()
+         << target.bestCameraToTarget.Translation().Z().value()
+         << target.bestCameraToTarget.Rotation().GetQuaternion().W()
+         << target.bestCameraToTarget.Rotation().GetQuaternion().X()
+         << target.bestCameraToTarget.Rotation().GetQuaternion().Y()
+         << target.bestCameraToTarget.Rotation().GetQuaternion().Z()
+         << target.altCameraToTarget.Translation().X().value()
+         << target.altCameraToTarget.Translation().Y().value()
+         << target.altCameraToTarget.Translation().Z().value()
+         << target.altCameraToTarget.Rotation().GetQuaternion().W()
+         << target.altCameraToTarget.Rotation().GetQuaternion().X()
+         << target.altCameraToTarget.Rotation().GetQuaternion().Y()
+         << target.altCameraToTarget.Rotation().GetQuaternion().Z()
+         << target.poseAmbiguity;
+
+  for (int i = 0; i < 4; i++) {
+    packet << target.minAreaRectCorners[i].first
+           << target.minAreaRectCorners[i].second;
+  }
+
+  uint8_t num_corners =
+      std::min<uint8_t>(target.detectedCorners.size(), MAX_CORNERS);
+  packet << num_corners;
+  for (size_t i = 0; i < target.detectedCorners.size(); i++) {
+    packet << target.detectedCorners[i].first
+           << target.detectedCorners[i].second;
+  }
+
+  return packet;
+}
+
+Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) {
+  packet >> target.yaw >> target.pitch >> target.area >> target.skew >>
+      target.fiducialId;
+
+  // We use these for best and alt transforms below
+  double x = 0;
+  double y = 0;
+  double z = 0;
+  double w = 0;
+
+  // First transform is the "best" pose
+  packet >> x >> y >> z;
+  const auto bestTranslation = frc::Translation3d(
+      units::meter_t(x), units::meter_t(y), units::meter_t(z));
+  packet >> w >> x >> y >> z;
+  const auto bestRotation = frc::Rotation3d(frc::Quaternion(w, x, y, z));
+  target.bestCameraToTarget = frc::Transform3d(bestTranslation, bestRotation);
+
+  // Second transform is the "alternate" pose
+  packet >> x >> y >> z;
+  const auto altTranslation = frc::Translation3d(
+      units::meter_t(x), units::meter_t(y), units::meter_t(z));
+  packet >> w >> x >> y >> z;
+  const auto altRotation = frc::Rotation3d(frc::Quaternion(w, x, y, z));
+  target.altCameraToTarget = frc::Transform3d(altTranslation, altRotation);
+
+  packet >> target.poseAmbiguity;
+
+  target.minAreaRectCorners.clear();
+  double first = 0;
+  double second = 0;
+  for (int i = 0; i < 4; i++) {
+    packet >> first >> second;
+    target.minAreaRectCorners.emplace_back(first, second);
+  }
+
+  uint8_t numCorners = 0;
+  packet >> numCorners;
+  target.detectedCorners.clear();
+  target.detectedCorners.reserve(numCorners);
+  for (size_t i = 0; i < numCorners; i++) {
+    packet >> first >> second;
+    target.detectedCorners.emplace_back(first, second);
+  }
+
+  return packet;
+}
+}  // namespace photon