fix cpp

PhotonVision · Mar 28, 2024 · 6104044 · 6104044
1 parent c6f170f
commit 6104044
Show file tree

Hide file tree

Showing 11 changed files with 59 additions and 45 deletions.
diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
@@ -541,10 +541,13 @@ public PhotonPipelineResult process(
 
         // put this simulated data to NT
         var now = RobotController.getFPGATime();
-        int seq = 0;
         var ret =
                 new PhotonPipelineResult(
-                        seq, now - (long) (latencyMillis * 1000), now, detectableTgts, multitagResult);
+                        heartbeatCounter,
+                        now - (long) (latencyMillis * 1000),
+                        now,
+                        detectableTgts,
+                        multitagResult);
         ret.setRecieveTimestampMicros(now);
         return ret;
     }

diff --git a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp
@@ -29,6 +29,7 @@
 #include <string_view>
 
 #include <frc/Errors.h>
+#include <frc/RobotController.h>
 #include <frc/Timer.h>
 #include <opencv2/core.hpp>
 #include <opencv2/core/mat.hpp>
@@ -115,7 +116,8 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() {
   PhotonPipelineResult result;
 
   // Fill the packet with latest data and populate result.
-  units::microsecond_t now = frc::RobotController::GetFPGATime();
+  units::microsecond_t now =
+      units::microsecond_t(frc::RobotController::GetFPGATime());
   const auto value = rawBytesEntry.Get();
   if (!value.size()) return result;
 

diff --git a/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h b/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h
@@ -40,6 +40,7 @@
 #include <utility>
 #include <vector>
 
+#include <frc/Timer.h>
 #include <frc/apriltag/AprilTagFieldLayout.h>
 #include <frc/apriltag/AprilTagFields.h>
 #include <units/math.h>
@@ -369,7 +370,10 @@ class PhotonCameraSim {
       multiTagResults = MultiTargetPNPResult{pnpResult, usedIds};
     }
 
-    return PhotonPipelineResult{latency, detectableTgts, multiTagResults};
+    units::second_t now = frc::Timer::GetFPGATimestamp();
+
+    return PhotonPipelineResult{heartbeatCounter, now - latency, now,
+                                detectableTgts, multiTagResults};
   }
   void SubmitProcessedFrame(const PhotonPipelineResult& result) {
     SubmitProcessedFrame(result, wpi::Now());
@@ -426,7 +430,7 @@ class PhotonCameraSim {
   PhotonCamera* cam;
 
   NTTopicSet ts{};
-  uint64_t heartbeatCounter{0};
+  int64_t heartbeatCounter{0};
 
   uint64_t nextNTEntryTime{wpi::Now()};
 

diff --git a/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h b/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h
@@ -96,12 +96,12 @@ class SimCameraProperties {
                                       units::radian_t{std::numbers::pi / 2.0}})
                                      .Radians()}},
         frc::Translation3d{
-            1_m,
-            frc::Rotation3d{0_rad,
-                            (GetPixelPitch(0) + frc::Rotation2d{units::radian_t{
-                                                    std::numbers::pi / 2.0}})
-                                .Radians(),
-                            0_rad}},
+            1_m, frc::Rotation3d{0_rad,
+                                 (GetPixelPitch(0) +
+                                  frc::Rotation2d{
+                                      units::radian_t{std::numbers::pi / 2.0}})
+                                     .Radians(),
+                                 0_rad}},
         frc::Translation3d{
             1_m, frc::Rotation3d{0_rad,
                                  (GetPixelPitch(height) +

diff --git a/photon-lib/src/main/native/include/photon/simulation/SimPhotonCamera.h b/photon-lib/src/main/native/include/photon/simulation/SimPhotonCamera.h
@@ -90,7 +90,7 @@ class SimPhotonCamera : public PhotonCamera {
     latencyMillisEntry.SetDouble(latency.to<double>());
     std::sort(targetList.begin(), targetList.end(),
               [&](auto lhs, auto rhs) { return sortMode(lhs, rhs); });
-    PhotonPipelineResult newResult{latency, targetList};
+    PhotonPipelineResult newResult{0, 0_s, latency, targetList};
     Packet packet{};
     packet << newResult;
 

diff --git a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java
@@ -596,7 +596,7 @@ void averageBestPoses() {
                                                 new TargetCorner(3, 4),
                                                 new TargetCorner(5, 6),
                                                 new TargetCorner(7, 8))))); // 3 3 3 ambig .4
-        cameraOne.result.setRecieveTimestampMicros(20);
+        cameraOne.result.setRecieveTimestampMicros(20 * 1000000);
 
         PhotonPoseEstimator estimator =
                 new PhotonPoseEstimator(

diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp
@@ -83,8 +83,8 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
           0.4, corners, detectedCorners}};
 
   cameraOne.test = true;
-  cameraOne.testResult = {2_ms, targets};
-  cameraOne.testResult.SetTimestamp(units::second_t(11));
+  cameraOne.testResult = {0, 0_s, 2_ms, targets};
+  cameraOne.testResult.SetRecieveTimestamp(units::second_t(11));
 
   photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY,
                                         std::move(cameraOne), {});
@@ -138,8 +138,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
           0.4, corners, detectedCorners}};
 
   cameraOne.test = true;
-  cameraOne.testResult = {2_ms, targets};
-  cameraOne.testResult.SetTimestamp(17_s);
+  cameraOne.testResult = {0, 0_s, 2_ms, targets};
+  cameraOne.testResult.SetRecieveTimestamp(17_s);
 
   photon::PhotonPoseEstimator estimator(
       aprilTags, photon::CLOSEST_TO_CAMERA_HEIGHT, std::move(cameraOne),
@@ -181,8 +181,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
           0.4, corners, detectedCorners}};
 
   cameraOne.test = true;
-  cameraOne.testResult = {2_ms, targets};
-  cameraOne.testResult.SetTimestamp(units::second_t(17));
+  cameraOne.testResult = {0, 0_s, 2_ms, targets};
+  cameraOne.testResult.SetRecieveTimestamp(units::second_t(17));
 
   photon::PhotonPoseEstimator estimator(
       aprilTags, photon::CLOSEST_TO_REFERENCE_POSE, std::move(cameraOne), {});
@@ -225,8 +225,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
           0.4, corners, detectedCorners}};
 
   cameraOne.test = true;
-  cameraOne.testResult = {2_ms, targets};
-  cameraOne.testResult.SetTimestamp(units::second_t(17));
+  cameraOne.testResult = {0, 0_s, 2_ms, targets};
+  cameraOne.testResult.SetRecieveTimestamp(units::second_t(17));
 
   photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE,
                                         std::move(cameraOne), {});
@@ -259,8 +259,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
           0.4, corners, detectedCorners}};
 
-  estimator.GetCamera()->testResult = {2_ms, targetsThree};
-  estimator.GetCamera()->testResult.SetTimestamp(units::second_t(21));
+  estimator.GetCamera()->testResult = {0, 0_s, 2_ms, targetsThree};
+  estimator.GetCamera()->testResult.SetRecieveTimestamp(units::second_t(21));
 
   estimatedPose = estimator.Update();
   ASSERT_TRUE(estimatedPose);
@@ -300,8 +300,8 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
           0.4, corners, detectedCorners}};
 
   cameraOne.test = true;
-  cameraOne.testResult = {2_ms, targets};
-  cameraOne.testResult.SetTimestamp(units::second_t(15));
+  cameraOne.testResult = {0, 0_ms, 2_ms, targets};
+  cameraOne.testResult.SetRecieveTimestamp(units::second_t(15));
 
   photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS,
                                         std::move(cameraOne), {});
@@ -347,17 +347,18 @@ TEST(PhotonPoseEstimatorTest, PoseCache) {
                                         std::move(cameraOne), {});
 
   // empty input, expect empty out
-  estimator.GetCamera()->testResult = {2_ms, {}};
-  estimator.GetCamera()->testResult.SetTimestamp(units::second_t(1));
+  estimator.GetCamera()->testResult = {0, 0_s, 2_ms, {}};
+  estimator.GetCamera()->testResult.SetRecieveTimestamp(units::second_t(1));
   auto estimatedPose = estimator.Update();
   EXPECT_FALSE(estimatedPose);
 
   // Set result, and update -- expect present and timestamp to be 15
-  estimator.GetCamera()->testResult = {3_ms, targets};
-  estimator.GetCamera()->testResult.SetTimestamp(units::second_t(15));
+  estimator.GetCamera()->testResult = {0, 0_s, 3_ms, targets};
+  estimator.GetCamera()->testResult.SetRecieveTimestamp(units::second_t(15));
   estimatedPose = estimator.Update();
   EXPECT_TRUE(estimatedPose);
-  EXPECT_NEAR(15, estimatedPose.value().timestamp.to<double>(), 1e-6);
+  EXPECT_NEAR((15_s - 3_ms).to<double>(),
+              estimatedPose.value().timestamp.to<double>(), 1e-6);
 
   // And again -- now pose cache should be empty
   estimatedPose = estimator.Update();

diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp
@@ -39,8 +39,9 @@ bool PhotonPipelineResult::operator==(const PhotonPipelineResult& other) const {
 
 Packet& operator<<(Packet& packet, const PhotonPipelineResult& result) {
   // Encode latency and number of targets.
-  packet << result.sequenceID << result.captureTimestamp.value()
-         << result.publishTimestamp.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.
@@ -74,12 +75,13 @@ Packet& operator>>(Packet& packet, PhotonPipelineResult& result) {
 
   packet >> multitagResult;
 
-  units::microsecond_t captureTS = units::microsecond_t{static_cast<double>(capTS)};
-  units::microsecond_t publishTS = units::microsecond_t{static_cast<double>(pubTS)};
+  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};
+  result = PhotonPipelineResult{sequenceID, captureTS, publishTS, targets,
+                                multitagResult};
 
   return packet;
 }

diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonPipelineResultProto.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonPipelineResultProto.cpp
@@ -43,7 +43,8 @@ wpi::Protobuf<photon::PhotonPipelineResult>::Unpack(
   return photon::PhotonPipelineResult{
       m->sequence_id(),
       units::microsecond_t{static_cast<double>(m->capture_timestamp_micros())},
-      units::microsecond_t{static_cast<double>(m->nt_publish_timestamp_micros())},
+      units::microsecond_t{
+          static_cast<double>(m->nt_publish_timestamp_micros())},
       targets,
       wpi::UnpackProtobuf<photon::MultiTargetPNPResult>(
           m->multi_target_result())};

diff --git a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h
@@ -50,10 +50,11 @@ class PhotonPipelineResult {
    * @param targets The list of targets identified by the pipeline.
    * @param multitagResult The multitarget result. Default to empty
    */
-  PhotonPipelineResult(int64_t sequenceID, units::microsecond_t captureTimestamp, 
-                        units::microsecond_t publishTimestamp,
-                         std::span<const PhotonTrackedTarget> targets,
-                         MultiTargetPNPResult multitagResult = {});
+  PhotonPipelineResult(int64_t sequenceID,
+                       units::microsecond_t captureTimestamp,
+                       units::microsecond_t publishTimestamp,
+                       std::span<const PhotonTrackedTarget> targets,
+                       MultiTargetPNPResult multitagResult = {});
 
   /**
    * Returns the best target in this pipeline result. If there are no targets,
@@ -104,7 +105,7 @@ class PhotonPipelineResult {
    * The number of non-empty frames processed by this camera since boot. Useful
    * to checking if a camera is alive.
    */
-  const int64_t SequenceID() const { return sequenceID; }
+  int64_t SequenceID() const { return sequenceID; }
 
   /** Sets the FPGA timestamp this result was recieved by robot code */
   void SetRecieveTimestamp(const units::second_t timestamp) {
@@ -139,7 +140,7 @@ class PhotonPipelineResult {
   units::microsecond_t publishTimestamp;
   // Since we don't trust NT time sync, keep track of when we got this packet
   // into robot code
-  units::microsecond_t ntRecieveTimestamp;
+  units::microsecond_t ntRecieveTimestamp = -1_s;
 
   wpi::SmallVector<PhotonTrackedTarget, 10> targets;
   MultiTargetPNPResult multitagResult;

diff --git a/photon-targeting/src/test/native/cpp/PacketTest.cpp b/photon-targeting/src/test/native/cpp/PacketTest.cpp
@@ -109,7 +109,7 @@ TEST(PacketTest, PhotonPipelineResult) {
           {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
            std::pair{7, 8}}}};
 
-  photon::PhotonPipelineResult result2{0, 0_s, 2_s, targets};
+  photon::PhotonPipelineResult result2{0, 0_s, 1_s, targets};
   photon::Packet p2;
   p2 << result2;