Add sequence ID, capture, publish and recieve timestamp to PhotonPipelineResult (#1305)

Closes #1304

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

@@ -19,25 +19,29 @@
 
 namespace photon {
 PhotonPipelineResult::PhotonPipelineResult(
-    units::millisecond_t latency, std::span<const PhotonTrackedTarget> targets)
-    : latency(latency),
-      targets(targets.data(), targets.data() + targets.size()) {}
-
-PhotonPipelineResult::PhotonPipelineResult(
-    units::millisecond_t latency, std::span<const PhotonTrackedTarget> targets,
+    int64_t sequenceID, units::microsecond_t captureTimestamp,
+    units::microsecond_t publishTimestamp,
+    std::span<const PhotonTrackedTarget> targets,
     MultiTargetPNPResult multitagResult)
-    : latency(latency),
+    : sequenceID(sequenceID),
+      captureTimestamp(captureTimestamp),
+      publishTimestamp(publishTimestamp),
       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;
+  return sequenceID == other.sequenceID &&
+         captureTimestamp == other.captureTimestamp &&
+         publishTimestamp == other.publishTimestamp &&
+         ntRecieveTimestamp == other.ntRecieveTimestamp &&
+         targets == other.targets && multitagResult == other.multitagResult;
 }
 
 Packet& operator<<(Packet& packet, const PhotonPipelineResult& result) {
   // Encode latency and number of targets.
-  packet << result.latency.value()
+  packet << result.sequenceID
+         << static_cast<int64_t>(result.captureTimestamp.value())
+         << static_cast<int64_t>(result.publishTimestamp.value())
          << static_cast<int8_t>(result.targets.size());
 
   // Encode the information of each target.
@@ -50,21 +54,35 @@ Packet& operator<<(Packet& packet, const PhotonPipelineResult& result) {
 
 Packet& operator>>(Packet& packet, PhotonPipelineResult& result) {
   // Decode latency, existence of targets, and number of targets.
-  double latencyMillis = 0;
+  int64_t sequenceID = 0;
+  int64_t capTS = 0;
+  int64_t pubTS = 0;
   int8_t targetCount = 0;
-  packet >> latencyMillis >> targetCount;
-  result.latency = units::millisecond_t(latencyMillis);
+  std::vector<PhotonTrackedTarget> targets;
+  MultiTargetPNPResult multitagResult;
 
-  result.targets.clear();
+  packet >> sequenceID >> capTS >> pubTS >> targetCount;
+
+  targets.clear();
+  targets.reserve(targetCount);
 
   // Decode the information of each target.
   for (int i = 0; i < targetCount; ++i) {
     PhotonTrackedTarget target;
     packet >> target;
-    result.targets.push_back(target);
+    targets.push_back(target);
   }
 
-  packet >> result.multitagResult;
+  packet >> multitagResult;
+
+  units::microsecond_t captureTS =
+      units::microsecond_t{static_cast<double>(capTS)};
+  units::microsecond_t publishTS =
+      units::microsecond_t{static_cast<double>(pubTS)};
+
+  result = PhotonPipelineResult{sequenceID, captureTS, publishTS, targets,
+                                multitagResult};
+
   return packet;
 }
 }  // namespace photon

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

@@ -41,7 +41,11 @@ wpi::Protobuf<photon::PhotonPipelineResult>::Unpack(
   }
 
   return photon::PhotonPipelineResult{
-      units::millisecond_t{m->latency_ms()}, targets,
+      m->sequence_id(),
+      units::microsecond_t{static_cast<double>(m->capture_timestamp_micros())},
+      units::microsecond_t{
+          static_cast<double>(m->nt_publish_timestamp_micros())},
+      targets,
       wpi::UnpackProtobuf<photon::MultiTargetPNPResult>(
           m->multi_target_result())};
 }
@@ -50,7 +54,9 @@ 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->set_sequence_id(value.sequenceID);
+  m->set_capture_timestamp_micros(value.captureTimestamp.value());
+  m->set_nt_publish_timestamp_micros(value.publishTimestamp.value());
 
   m->clear_targets();
   for (const auto& t : value.GetTargets()) {