Add protobuf publish setting slider (#1075)

Allows logging software and live data view to see results. Also removes the requirement for AScope to keep up with the packet serde schema and instead just use the Protobuf descriptor.

photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java

@@ -26,6 +26,7 @@ import edu.wpi.first.networktables.IntegerPublisher;
 import edu.wpi.first.networktables.IntegerSubscriber;
 import edu.wpi.first.networktables.IntegerTopic;
 import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.ProtobufPublisher;
 import edu.wpi.first.networktables.PubSubOption;
 import org.photonvision.targeting.PhotonPipelineResult;
 
@@ -41,6 +42,7 @@ public class NTTopicSet {
     public NetworkTable subTable;
 
     public PacketPublisher<PhotonPipelineResult> resultPublisher;
+    public ProtobufPublisher<PhotonPipelineResult> protoResultPublisher;
 
     public IntegerPublisher pipelineIndexPublisher;
     public IntegerSubscriber pipelineIndexRequestSub;
@@ -76,6 +78,10 @@ public class NTTopicSet {
                         .publish("rawBytes", PubSubOption.periodic(0.01), PubSubOption.sendAll(true));
 
         resultPublisher = new PacketPublisher<>(rawBytesEntry, PhotonPipelineResult.serde);
+        protoResultPublisher =
+                subTable
+                        .getProtobufTopic("result_proto", PhotonPipelineResult.proto)
+                        .publish(PubSubOption.periodic(0.01), PubSubOption.sendAll(true));
 
         pipelineIndexPublisher = subTable.getIntegerTopic("pipelineIndexState").publish();
         pipelineIndexRequestSub = subTable.getIntegerTopic("pipelineIndexRequest").subscribe(0);

photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java

@@ -21,6 +21,7 @@ import java.util.ArrayList;
 import java.util.List;
 import org.photonvision.common.dataflow.structures.Packet;
 import org.photonvision.common.dataflow.structures.PacketSerde;
+import org.photonvision.targeting.proto.MultiTargetPNPResultProto;
 
 public class MultiTargetPNPResult {
     // Seeing 32 apriltags at once seems like a sane limit
@@ -103,4 +104,5 @@ public class MultiTargetPNPResult {
     }
 
     public static final APacketSerde serde = new APacketSerde();
+    public static final MultiTargetPNPResultProto proto = new MultiTargetPNPResultProto();
 }

photon-targeting/src/main/java/org/photonvision/targeting/PNPResult.java

@@ -20,6 +20,7 @@ package org.photonvision.targeting;
 import edu.wpi.first.math.geometry.Transform3d;
 import org.photonvision.common.dataflow.structures.Packet;
 import org.photonvision.common.dataflow.structures.PacketSerde;
+import org.photonvision.targeting.proto.PNPResultProto;
 import org.photonvision.utils.PacketUtils;
 
 /**
@@ -180,4 +181,5 @@ public class PNPResult {
     }
 
     public static final APacketSerde serde = new APacketSerde();
+    public static final PNPResultProto proto = new PNPResultProto();
 }

photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java

@@ -21,6 +21,7 @@ import java.util.ArrayList;
 import java.util.List;
 import org.photonvision.common.dataflow.structures.Packet;
 import org.photonvision.common.dataflow.structures.PacketSerde;
+import org.photonvision.targeting.proto.PhotonPipelineResultProto;
 
 /** Represents a pipeline result from a PhotonCamera. */
 public class PhotonPipelineResult {
@@ -225,4 +226,5 @@ public class PhotonPipelineResult {
     }
 
     public static final APacketSerde serde = new APacketSerde();
+    public static final PhotonPipelineResultProto proto = new PhotonPipelineResultProto();
 }

photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java

@@ -22,6 +22,7 @@ import java.util.ArrayList;
 import java.util.List;
 import org.photonvision.common.dataflow.structures.Packet;
 import org.photonvision.common.dataflow.structures.PacketSerde;
+import org.photonvision.targeting.proto.PhotonTrackedTargetProto;
 import org.photonvision.utils.PacketUtils;
 
 public class PhotonTrackedTarget {
@@ -278,4 +279,5 @@ public class PhotonTrackedTarget {
     }
 
     public static final APacketSerde serde = new APacketSerde();
+    public static final PhotonTrackedTargetProto proto = new PhotonTrackedTargetProto();
 }

photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java

@@ -20,6 +20,7 @@ package org.photonvision.targeting;
 import java.util.Objects;
 import org.photonvision.common.dataflow.structures.Packet;
 import org.photonvision.common.dataflow.structures.PacketSerde;
+import org.photonvision.targeting.proto.TargetCornerProto;
 
 /**
  * Represents a point in an image at the corner of the minimum-area bounding rectangle, in pixels.
@@ -71,4 +72,5 @@ public class TargetCorner {
     }
 
     public static final APacketSerde serde = new APacketSerde();
+    public static final TargetCornerProto proto = new TargetCornerProto();
 }

photon-targeting/src/main/java/org/photonvision/targeting/proto/MultiTargetPNPResultProto.java

@@ -0,0 +1,69 @@
+/*
+ * 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/>.
+ */
+
+package org.photonvision.targeting.proto;
+
+import edu.wpi.first.util.protobuf.Protobuf;
+import java.util.ArrayList;
+import org.photonvision.proto.Photon.ProtobufMultiTargetPNPResult;
+import org.photonvision.targeting.MultiTargetPNPResult;
+import org.photonvision.targeting.PNPResult;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+import us.hebi.quickbuf.RepeatedInt;
+
+public class MultiTargetPNPResultProto
+        implements Protobuf<MultiTargetPNPResult, ProtobufMultiTargetPNPResult> {
+    @Override
+    public Class<MultiTargetPNPResult> getTypeClass() {
+        return MultiTargetPNPResult.class;
+    }
+
+    @Override
+    public Descriptor getDescriptor() {
+        return ProtobufMultiTargetPNPResult.getDescriptor();
+    }
+
+    @Override
+    public Protobuf<?, ?>[] getNested() {
+        return new Protobuf<?, ?>[] {PNPResult.proto};
+    }
+
+    @Override
+    public ProtobufMultiTargetPNPResult createMessage() {
+        return ProtobufMultiTargetPNPResult.newInstance();
+    }
+
+    @Override
+    public MultiTargetPNPResult unpack(ProtobufMultiTargetPNPResult msg) {
+        ArrayList<Integer> fidIdsUsed = new ArrayList<>(msg.getFiducialIdsUsed().length());
+        for (var packedFidId : msg.getFiducialIdsUsed()) {
+            fidIdsUsed.add(packedFidId);
+        }
+
+        return new MultiTargetPNPResult(PNPResult.proto.unpack(msg.getEstimatedPose()), fidIdsUsed);
+    }
+
+    @Override
+    public void pack(ProtobufMultiTargetPNPResult msg, MultiTargetPNPResult value) {
+        PNPResult.proto.pack(msg.getMutableEstimatedPose(), value.estimatedPose);
+
+        RepeatedInt idsUsed = msg.getMutableFiducialIdsUsed().reserve(value.fiducialIDsUsed.size());
+        for (int i = 0; i < value.fiducialIDsUsed.size(); i++) {
+            idsUsed.add(value.fiducialIDsUsed.get(i));
+        }
+    }
+}

photon-targeting/src/main/java/org/photonvision/targeting/proto/PNPResultProto.java

@@ -0,0 +1,70 @@
+/*
+ * 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/>.
+ */
+
+package org.photonvision.targeting.proto;
+
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import org.photonvision.proto.Photon.ProtobufPNPResult;
+import org.photonvision.targeting.PNPResult;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class PNPResultProto implements Protobuf<PNPResult, ProtobufPNPResult> {
+    @Override
+    public Class<PNPResult> getTypeClass() {
+        return PNPResult.class;
+    }
+
+    @Override
+    public Descriptor getDescriptor() {
+        return ProtobufPNPResult.getDescriptor();
+    }
+
+    @Override
+    public Protobuf<?, ?>[] getNested() {
+        return new Protobuf<?, ?>[] {Transform3d.proto};
+    }
+
+    @Override
+    public ProtobufPNPResult createMessage() {
+        return ProtobufPNPResult.newInstance();
+    }
+
+    @Override
+    public PNPResult unpack(ProtobufPNPResult msg) {
+        if (!msg.getIsPresent()) {
+            return new PNPResult();
+        }
+
+        return new PNPResult(
+                Transform3d.proto.unpack(msg.getBest()),
+                Transform3d.proto.unpack(msg.getAlt()),
+                msg.getAmbiguity(),
+                msg.getBestReprojErr(),
+                msg.getAltReprojErr());
+    }
+
+    @Override
+    public void pack(ProtobufPNPResult msg, PNPResult value) {
+        Transform3d.proto.pack(msg.getMutableBest(), value.best);
+        Transform3d.proto.pack(msg.getMutableAlt(), value.alt);
+        msg.setAmbiguity(value.ambiguity)
+                .setBestReprojErr(value.bestReprojErr)
+                .setAltReprojErr(value.altReprojErr)
+                .setIsPresent(value.isPresent);
+    }
+}

photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java

@@ -0,0 +1,64 @@
+/*
+ * 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/>.
+ */
+
+package org.photonvision.targeting.proto;
+
+import edu.wpi.first.util.protobuf.Protobuf;
+import org.photonvision.proto.Photon.ProtobufPhotonPipelineResult;
+import org.photonvision.targeting.MultiTargetPNPResult;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class PhotonPipelineResultProto
+        implements Protobuf<PhotonPipelineResult, ProtobufPhotonPipelineResult> {
+    @Override
+    public Class<PhotonPipelineResult> getTypeClass() {
+        return PhotonPipelineResult.class;
+    }
+
+    @Override
+    public Descriptor getDescriptor() {
+        return ProtobufPhotonPipelineResult.getDescriptor();
+    }
+
+    @Override
+    public Protobuf<?, ?>[] getNested() {
+        return new Protobuf<?, ?>[] {PhotonTrackedTarget.proto, MultiTargetPNPResult.proto};
+    }
+
+    @Override
+    public ProtobufPhotonPipelineResult createMessage() {
+        return ProtobufPhotonPipelineResult.newInstance();
+    }
+
+    @Override
+    public PhotonPipelineResult unpack(ProtobufPhotonPipelineResult msg) {
+        return new PhotonPipelineResult(
+                msg.getLatencyMs(),
+                PhotonTrackedTarget.proto.unpack(msg.getTargets()),
+                MultiTargetPNPResult.proto.unpack(msg.getMultiTargetResult()));
+    }
+
+    @Override
+    public void pack(ProtobufPhotonPipelineResult msg, PhotonPipelineResult value) {
+        PhotonTrackedTarget.proto.pack(msg.getMutableTargets(), value.getTargets());
+        MultiTargetPNPResult.proto.pack(msg.getMutableMultiTargetResult(), value.getMultiTagResult());
+
+        msg.setLatencyMs(value.getLatencyMillis());
+    }
+}

photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonTrackedTargetProto.java

@@ -0,0 +1,99 @@
+/*
+ * 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/>.
+ */
+
+package org.photonvision.targeting.proto;
+
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import java.util.ArrayList;
+import java.util.List;
+import org.photonvision.proto.Photon.ProtobufPhotonTrackedTarget;
+import org.photonvision.targeting.PhotonTrackedTarget;
+import org.photonvision.targeting.TargetCorner;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+import us.hebi.quickbuf.RepeatedMessage;
+
+public class PhotonTrackedTargetProto
+        implements Protobuf<PhotonTrackedTarget, ProtobufPhotonTrackedTarget> {
+    @Override
+    public Class<PhotonTrackedTarget> getTypeClass() {
+        return PhotonTrackedTarget.class;
+    }
+
+    @Override
+    public Descriptor getDescriptor() {
+        return ProtobufPhotonTrackedTarget.getDescriptor();
+    }
+
+    @Override
+    public Protobuf<?, ?>[] getNested() {
+        return new Protobuf<?, ?>[] {Transform3d.proto, TargetCorner.proto};
+    }
+
+    @Override
+    public ProtobufPhotonTrackedTarget createMessage() {
+        return ProtobufPhotonTrackedTarget.newInstance();
+    }
+
+    @Override
+    public PhotonTrackedTarget unpack(ProtobufPhotonTrackedTarget msg) {
+        return new PhotonTrackedTarget(
+                msg.getYaw(),
+                msg.getPitch(),
+                msg.getArea(),
+                msg.getSkew(),
+                msg.getFiducialId(),
+                Transform3d.proto.unpack(msg.getBestCameraToTarget()),
+                Transform3d.proto.unpack(msg.getAltCameraToTarget()),
+                msg.getPoseAmbiguity(),
+                TargetCorner.proto.unpack(msg.getMinAreaRectCorners()),
+                TargetCorner.proto.unpack(msg.getDetectedCorners()));
+    }
+
+    public List<PhotonTrackedTarget> unpack(RepeatedMessage<ProtobufPhotonTrackedTarget> msg) {
+        ArrayList<PhotonTrackedTarget> targets = new ArrayList<>(msg.length());
+        for (ProtobufPhotonTrackedTarget target : msg) {
+            targets.add(unpack(target));
+        }
+        return targets;
+    }
+
+    @Override
+    public void pack(ProtobufPhotonTrackedTarget msg, PhotonTrackedTarget value) {
+        msg.setYaw(value.getYaw())
+                .setPitch(value.getPitch())
+                .setSkew(value.getSkew())
+                .setArea(value.getArea())
+                .setFiducialId(value.getFiducialId())
+                .setPoseAmbiguity(value.getPoseAmbiguity());
+
+        Transform3d.proto.pack(msg.getMutableBestCameraToTarget(), value.getBestCameraToTarget());
+        Transform3d.proto.pack(msg.getMutableAltCameraToTarget(), value.getAlternateCameraToTarget());
+
+        TargetCorner.proto.pack(msg.getMutableMinAreaRectCorners(), value.getMinAreaRectCorners());
+        TargetCorner.proto.pack(msg.getMutableDetectedCorners(), value.getDetectedCorners());
+    }
+
+    public void pack(
+            RepeatedMessage<ProtobufPhotonTrackedTarget> msg, List<PhotonTrackedTarget> value) {
+        var targets = msg.reserve(value.size());
+        for (PhotonTrackedTarget trackedTarget : value) {
+            var target = targets.next();
+            pack(target, trackedTarget);
+        }
+    }
+}

photon-targeting/src/main/java/org/photonvision/targeting/proto/TargetCornerProto.java

@@ -0,0 +1,69 @@
+/*
+ * 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/>.
+ */
+
+package org.photonvision.targeting.proto;
+
+import edu.wpi.first.util.protobuf.Protobuf;
+import java.util.ArrayList;
+import java.util.List;
+import org.photonvision.proto.Photon.ProtobufTargetCorner;
+import org.photonvision.targeting.TargetCorner;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+import us.hebi.quickbuf.RepeatedMessage;
+
+public class TargetCornerProto implements Protobuf<TargetCorner, ProtobufTargetCorner> {
+    @Override
+    public Class<TargetCorner> getTypeClass() {
+        return TargetCorner.class;
+    }
+
+    @Override
+    public Descriptor getDescriptor() {
+        return ProtobufTargetCorner.getDescriptor();
+    }
+
+    @Override
+    public ProtobufTargetCorner createMessage() {
+        return ProtobufTargetCorner.newInstance();
+    }
+
+    @Override
+    public TargetCorner unpack(ProtobufTargetCorner msg) {
+        return new TargetCorner(msg.getX(), msg.getY());
+    }
+
+    public List<TargetCorner> unpack(RepeatedMessage<ProtobufTargetCorner> msg) {
+        ArrayList<TargetCorner> corners = new ArrayList<>(msg.length());
+        for (ProtobufTargetCorner corner : msg) {
+            corners.add(unpack(corner));
+        }
+        return corners;
+    }
+
+    @Override
+    public void pack(ProtobufTargetCorner msg, TargetCorner value) {
+        msg.setX(value.x).setY(value.y);
+    }
+
+    public void pack(RepeatedMessage<ProtobufTargetCorner> msg, List<TargetCorner> value) {
+        var corners = msg.reserve(value.size());
+        for (TargetCorner targetCorner : value) {
+            var corner = corners.next();
+            pack(corner, targetCorner);
+        }
+    }
+}

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

@@ -19,12 +19,12 @@
 
 namespace photon {
 PhotonPipelineResult::PhotonPipelineResult(
-    units::second_t latency, std::span<const PhotonTrackedTarget> targets)
+    units::millisecond_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,
+    units::millisecond_t latency, std::span<const PhotonTrackedTarget> targets,
     MultiTargetPNPResult multitagResult)
     : latency(latency),
       targets(targets.data(), targets.data() + targets.size()),
@@ -37,7 +37,7 @@ bool PhotonPipelineResult::operator==(const PhotonPipelineResult& other) const {
 
 Packet& operator<<(Packet& packet, const PhotonPipelineResult& result) {
   // Encode latency and number of targets.
-  packet << result.latency.value() * 1000 << result.multitagResult
+  packet << result.latency.value() << result.multitagResult
          << static_cast<int8_t>(result.targets.size());
 
   // Encode the information of each target.
@@ -52,7 +52,7 @@ Packet& operator>>(Packet& packet, PhotonPipelineResult& result) {
   double latencyMillis = 0;
   int8_t targetCount = 0;
   packet >> latencyMillis >> result.multitagResult >> targetCount;
-  result.latency = units::second_t(latencyMillis / 1000.0);
+  result.latency = units::millisecond_t(latencyMillis);
 
   result.targets.clear();
 

photon-targeting/src/main/native/cpp/photon/targeting/proto/MultiTargetPNPResultProto.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/proto/MultiTargetPNPResultProto.h"
+
+#include "photon.pb.h"
+#include "photon/targeting/proto/PNPResultProto.h"
+
+google::protobuf::Message* wpi::Protobuf<photon::MultiTargetPNPResult>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      photonvision::proto::ProtobufMultiTargetPNPResult>(arena);
+}
+
+photon::MultiTargetPNPResult
+wpi::Protobuf<photon::MultiTargetPNPResult>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m =
+      static_cast<const photonvision::proto::ProtobufMultiTargetPNPResult*>(
+          &msg);
+
+  wpi::SmallVector<int16_t, 32> fiducialIdsUsed;
+  for (int i = 0; i < m->fiducial_ids_used_size(); i++) {
+    fiducialIdsUsed.push_back(m->fiducial_ids_used(i));
+  }
+
+  return photon::MultiTargetPNPResult{
+      wpi::UnpackProtobuf<photon::PNPResult>(m->estimated_pose()),
+      fiducialIdsUsed};
+}
+
+void wpi::Protobuf<photon::MultiTargetPNPResult>::Pack(
+    google::protobuf::Message* msg, const photon::MultiTargetPNPResult& value) {
+  auto m = static_cast<photonvision::proto::ProtobufMultiTargetPNPResult*>(msg);
+
+  wpi::PackProtobuf(m->mutable_estimated_pose(), value.result);
+
+  m->clear_fiducial_ids_used();
+  for (const auto& t : value.fiducialIdsUsed) {
+    m->add_fiducial_ids_used(t);
+  }
+}

photon-targeting/src/main/native/cpp/photon/targeting/proto/PNPResultProto.cpp

@@ -0,0 +1,54 @@
+/*
+ * 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/proto/PNPResultProto.h"
+
+#include "photon.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<photon::PNPResult>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      photonvision::proto::ProtobufPNPResult>(arena);
+}
+
+photon::PNPResult wpi::Protobuf<photon::PNPResult>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const photonvision::proto::ProtobufPNPResult*>(&msg);
+
+  if (!m->is_present()) {
+    return photon::PNPResult();
+  }
+
+  return photon::PNPResult{true,
+                           wpi::UnpackProtobuf<frc::Transform3d>(m->best()),
+                           m->best_reproj_err(),
+                           wpi::UnpackProtobuf<frc::Transform3d>(m->alt()),
+                           m->alt_reproj_err(),
+                           m->ambiguity()};
+}
+
+void wpi::Protobuf<photon::PNPResult>::Pack(google::protobuf::Message* msg,
+                                            const photon::PNPResult& value) {
+  auto m = static_cast<photonvision::proto::ProtobufPNPResult*>(msg);
+
+  m->set_is_present(value.isPresent);
+  wpi::PackProtobuf(m->mutable_best(), value.best);
+  m->set_best_reproj_err(value.bestReprojErr);
+  wpi::PackProtobuf(m->mutable_alt(), value.alt);
+  m->set_alt_reproj_err(value.altReprojErr);
+  m->set_ambiguity(value.ambiguity);
+}

photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonPipelineResultProto.cpp

@@ -0,0 +1,61 @@
+/*
+ * 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/proto/PhotonPipelineResultProto.h"
+
+#include "photon.pb.h"
+#include "photon/targeting/proto/MultiTargetPNPResultProto.h"
+#include "photon/targeting/proto/PhotonTrackedTargetProto.h"
+
+google::protobuf::Message* wpi::Protobuf<photon::PhotonPipelineResult>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      photonvision::proto::ProtobufPhotonPipelineResult>(arena);
+}
+
+photon::PhotonPipelineResult
+wpi::Protobuf<photon::PhotonPipelineResult>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m =
+      static_cast<const photonvision::proto::ProtobufPhotonPipelineResult*>(
+          &msg);
+
+  std::vector<photon::PhotonTrackedTarget> targets;
+  targets.reserve(m->targets_size());
+  for (const auto& t : m->targets()) {
+    targets.emplace_back(wpi::UnpackProtobuf<photon::PhotonTrackedTarget>(t));
+  }
+
+  return photon::PhotonPipelineResult{
+      units::millisecond_t{m->latency_ms()}, targets,
+      wpi::UnpackProtobuf<photon::MultiTargetPNPResult>(
+          m->multi_target_result())};
+}
+
+void wpi::Protobuf<photon::PhotonPipelineResult>::Pack(
+    google::protobuf::Message* msg, const photon::PhotonPipelineResult& value) {
+  auto m = static_cast<photonvision::proto::ProtobufPhotonPipelineResult*>(msg);
+
+  m->set_latency_ms(value.latency.value());
+
+  m->clear_targets();
+  for (const auto& t : value.GetTargets()) {
+    wpi::PackProtobuf(m->add_targets(), t);
+  }
+
+  wpi::PackProtobuf(m->mutable_multi_target_result(), value.multitagResult);
+}

photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonTrackedTargetProto.cpp

@@ -0,0 +1,84 @@
+/*
+ * 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/proto/PhotonTrackedTargetProto.h"
+
+#include "photon.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<photon::PhotonTrackedTarget>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      photonvision::proto::ProtobufPhotonTrackedTarget>(arena);
+}
+
+photon::PhotonTrackedTarget wpi::Protobuf<photon::PhotonTrackedTarget>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const photonvision::proto::ProtobufPhotonTrackedTarget*>(
+      &msg);
+
+  wpi::SmallVector<std::pair<double, double>, 4> minAreaRectCorners;
+  for (const auto& t : m->min_area_rect_corners()) {
+    minAreaRectCorners.emplace_back(t.x(), t.y());
+  }
+
+  std::vector<std::pair<double, double>> detectedCorners;
+  detectedCorners.reserve(m->detected_corners_size());
+  for (const auto& t : m->detected_corners()) {
+    detectedCorners.emplace_back(t.x(), t.y());
+  }
+
+  return photon::PhotonTrackedTarget{
+      m->yaw(),
+      m->pitch(),
+      m->area(),
+      m->skew(),
+      m->fiducial_id(),
+      wpi::UnpackProtobuf<frc::Transform3d>(m->best_camera_to_target()),
+      wpi::UnpackProtobuf<frc::Transform3d>(m->alt_camera_to_target()),
+      m->pose_ambiguity(),
+      minAreaRectCorners,
+      detectedCorners};
+}
+
+void wpi::Protobuf<photon::PhotonTrackedTarget>::Pack(
+    google::protobuf::Message* msg, const photon::PhotonTrackedTarget& value) {
+  auto m = static_cast<photonvision::proto::ProtobufPhotonTrackedTarget*>(msg);
+
+  m->set_yaw(value.yaw);
+  m->set_pitch(value.pitch);
+  m->set_area(value.area);
+  m->set_skew(value.skew);
+  m->set_fiducial_id(value.fiducialId);
+  wpi::PackProtobuf(m->mutable_best_camera_to_target(),
+                    value.bestCameraToTarget);
+  wpi::PackProtobuf(m->mutable_alt_camera_to_target(), value.altCameraToTarget);
+  m->set_pose_ambiguity(value.poseAmbiguity);
+
+  m->clear_min_area_rect_corners();
+  for (const auto& t : value.GetMinAreaRectCorners()) {
+    auto* corner = m->add_min_area_rect_corners();
+    corner->set_x(t.first);
+    corner->set_y(t.second);
+  }
+
+  m->clear_detected_corners();
+  for (const auto& t : value.GetDetectedCorners()) {
+    auto* corner = m->add_detected_corners();
+    corner->set_x(t.first);
+    corner->set_y(t.second);
+  }
+}

photon-targeting/src/main/native/include/geometry3d.pb.h

@@ -0,0 +1,22 @@
+/*
+ * 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
+// So wpilib publishes protbufs here at wpimath/protobuf. but generated code
+// assumes that the protobuf includes are on your include path. So we need this
+// stupid shim
+#include "wpimath/protobuf/geometry3d.pb.h"

photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h

@@ -44,7 +44,7 @@ class PhotonPipelineResult {
    * @param latency The latency in the pipeline.
    * @param targets The list of targets identified by the pipeline.
    */
-  PhotonPipelineResult(units::second_t latency,
+  PhotonPipelineResult(units::millisecond_t latency,
                        std::span<const PhotonTrackedTarget> targets);
 
   /**
@@ -53,7 +53,7 @@ class PhotonPipelineResult {
    * @param targets The list of targets identified by the pipeline.
    * @param multitagResult The multitarget result
    */
-  PhotonPipelineResult(units::second_t latency,
+  PhotonPipelineResult(units::millisecond_t latency,
                        std::span<const PhotonTrackedTarget> targets,
                        MultiTargetPNPResult multitagResult);
 
@@ -81,7 +81,7 @@ class PhotonPipelineResult {
    * Returns the latency in the pipeline.
    * @return The latency in the pipeline.
    */
-  units::second_t GetLatency() const { return latency; }
+  units::millisecond_t GetLatency() const { return latency; }
 
   /**
    * Returns the estimated time the frame was taken,
@@ -125,7 +125,7 @@ class PhotonPipelineResult {
   friend Packet& operator<<(Packet& packet, const PhotonPipelineResult& result);
   friend Packet& operator>>(Packet& packet, PhotonPipelineResult& result);
 
-  units::second_t latency = 0_s;
+  units::millisecond_t latency = 0_s;
   units::second_t timestamp = -1_s;
   wpi::SmallVector<PhotonTrackedTarget, 10> targets;
   MultiTargetPNPResult multitagResult;

photon-targeting/src/main/native/include/photon/targeting/proto/MultiTargetPNPResultProto.h

@@ -0,0 +1,31 @@
+/*
+ * 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 <wpi/protobuf/Protobuf.h>
+
+#include "photon/targeting/MultiTargetPNPResult.h"
+
+template <>
+struct wpi::Protobuf<photon::MultiTargetPNPResult> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static photon::MultiTargetPNPResult Unpack(
+      const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const photon::MultiTargetPNPResult& value);
+};

photon-targeting/src/main/native/include/photon/targeting/proto/PNPResultProto.h

@@ -0,0 +1,30 @@
+/*
+ * 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 <wpi/protobuf/Protobuf.h>
+
+#include "photon/targeting/PNPResult.h"
+
+template <>
+struct wpi::Protobuf<photon::PNPResult> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static photon::PNPResult Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const photon::PNPResult& value);
+};

photon-targeting/src/main/native/include/photon/targeting/proto/PhotonPipelineResultProto.h

@@ -0,0 +1,31 @@
+/*
+ * 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 <wpi/protobuf/Protobuf.h>
+
+#include "photon/targeting/PhotonPipelineResult.h"
+
+template <>
+struct wpi::Protobuf<photon::PhotonPipelineResult> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static photon::PhotonPipelineResult Unpack(
+      const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const photon::PhotonPipelineResult& value);
+};

photon-targeting/src/main/native/include/photon/targeting/proto/PhotonTrackedTargetProto.h

@@ -0,0 +1,31 @@
+/*
+ * 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 <wpi/protobuf/Protobuf.h>
+
+#include "photon/targeting/PhotonTrackedTarget.h"
+
+template <>
+struct wpi::Protobuf<photon::PhotonTrackedTarget> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static photon::PhotonTrackedTarget Unpack(
+      const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const photon::PhotonTrackedTarget& value);
+};

photon-targeting/src/main/proto/photon.proto

@@ -0,0 +1,63 @@
+/*
+ * 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/>.
+ */
+
+syntax = "proto3";
+
+package photonvision.proto;
+
+import "geometry3d.proto";
+
+option java_package = "org.photonvision.proto";
+
+message ProtobufTargetCorner {
+  double x = 1;
+  double y = 2;
+}
+
+message ProtobufPNPResult {
+  bool is_present = 1;
+  wpi.proto.ProtobufTransform3d best = 2;
+  double best_reproj_err = 3;
+  optional wpi.proto.ProtobufTransform3d alt = 4;
+  optional double alt_reproj_err = 5;
+  double ambiguity = 6;
+}
+
+message ProtobufMultiTargetPNPResult {
+  ProtobufPNPResult estimated_pose = 1;
+  repeated int32 fiducial_ids_used = 2;
+}
+
+message ProtobufPhotonTrackedTarget {
+  double yaw = 1;
+  double pitch = 2;
+  double area = 3;
+  double skew = 4;
+  int32 fiducial_id = 5;
+  wpi.proto.ProtobufTransform3d best_camera_to_target = 6;
+  wpi.proto.ProtobufTransform3d alt_camera_to_target = 7;
+  double pose_ambiguity = 8;
+  repeated ProtobufTargetCorner min_area_rect_corners = 9;
+  repeated ProtobufTargetCorner detected_corners = 10;
+}
+
+message ProtobufPhotonPipelineResult {
+  double latency_ms = 1;
+
+  repeated ProtobufPhotonTrackedTarget targets = 2;
+  ProtobufMultiTargetPNPResult multi_target_result = 3;
+}