[commands] Add a warning to schedule docs (NFC) (#7073)

Signed-off-by: Jade Turner <spacey-sooty@proton.me>

apriltag/convert_apriltag_layouts.py

@@ -7,10 +7,11 @@ AprilTagFields expects.
 
 The input CSV has the following format:
 
-* Columns: ID, X, Y, Z, Rotation
+* Columns: ID, X, Y, Z, Z Rotation, Y Rotation
 * ID is a positive integer
 * X, Y, and Z are decimal inches
-* Rotation is yaw in degrees
+* Z Rotation is yaw in degrees
+* Y Rotation is pitch in degrees
 
 The values come from a table in the layout marking diagram (e.g.,
 https://firstfrc.blob.core.windows.net/frc2024/FieldAssets/2024LayoutMarkingDiagram.pdf).
@@ -48,13 +49,14 @@ def main():
                 x = float(row[1])
                 y = float(row[2])
                 z = float(row[3])
-                rotation = float(row[4])
+                zRotation = float(row[4])
+                yRotation = float(row[5])
 
                 # Turn yaw into quaternion
                 q = geometry.Rotation3d(
-                    units.radians(0.0),
-                    units.radians(0.0),
-                    units.degreesToRadians(rotation),
+                    units.radians(0),
+                    units.degreesToRadians(yRotation),
+                    units.degreesToRadians(zRotation),
                 ).getQuaternion()
 
                 json_data["tags"].append(

apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java

@@ -13,13 +13,15 @@ public enum AprilTagFields {
   /** 2023 Charged Up. */
   k2023ChargedUp("2023-chargedup.json"),
   /** 2024 Crescendo. */
-  k2024Crescendo("2024-crescendo.json");
+  k2024Crescendo("2024-crescendo.json"),
+  /** 2025 Reefscape. */
+  k2025Reefscape("2025-reefscape.json");
 
   /** Base resource directory. */
   public static final String kBaseResourceDir = "/edu/wpi/first/apriltag/";
 
   /** Alias to the current game. */
-  public static final AprilTagFields kDefaultField = k2024Crescendo;
+  public static final AprilTagFields kDefaultField = k2025Reefscape;
 
   /** Resource filename. */
   public final String m_resourceFile;

apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp

@@ -133,6 +133,7 @@ namespace frc {
 std::string_view GetResource_2022_rapidreact_json();
 std::string_view GetResource_2023_chargedup_json();
 std::string_view GetResource_2024_crescendo_json();
+std::string_view GetResource_2025_reefscape_json();
 
 }  // namespace frc
 
@@ -148,6 +149,9 @@ AprilTagFieldLayout AprilTagFieldLayout::LoadField(AprilTagField field) {
     case AprilTagField::k2024Crescendo:
       fieldString = GetResource_2024_crescendo_json();
       break;
+    case AprilTagField::k2025Reefscape:
+      fieldString = GetResource_2025_reefscape_json();
+      break;
     case AprilTagField::kNumFields:
       throw std::invalid_argument("Invalid Field");
   }

apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h

@@ -20,8 +20,10 @@ enum class AprilTagField {
   k2023ChargedUp,
   /// 2024 Crescendo.
   k2024Crescendo,
+  /// 2025 Reefscape.
+  k2025Reefscape,
   /// Alias to the current game.
-  kDefaultField = k2024Crescendo,
+  kDefaultField = k2025Reefscape,
 
   // This is a placeholder for denoting the last supported field. This should
   // always be the last entry in the enum and should not be used by users

apriltag/src/main/native/resources/edu/wpi/first/apriltag/2024-crescendo.csv

@@ -1,17 +0,0 @@
-ID,X,Y,Z,Rotation
-1,593.68,9.68,53.38,120
-2,637.21,34.79,53.38,120
-3,652.73,196.17,57.13,180
-4,652.73,218.42,57.13,180
-5,578.77,323.00,53.38,270
-6,72.5,323.00,53.38,270
-7,-1.50,218.42,57.13,0
-8,-1.50,196.17,57.13,0
-9,14.02,34.79,53.38,60
-10,57.54,9.68,53.38,60
-11,468.69,146.19,52.00,300
-12,468.69,177.10,52.00,60
-13,441.74,161.62,52.00,180
-14,209.48,161.62,52.00,0
-15,182.73,177.10,52.00,120
-16,182.73,146.19,52.00,240

apriltag/src/main/native/resources/edu/wpi/first/apriltag/2025-reefscape.csv

@@ -0,0 +1,23 @@
+ID,X,Y,Z,Z-Rotation,X-Rotation
+1,657.37,25.8,58.5,126,0
+2,657.37,291.2,58.5,234,0
+3,455.15,317.15,51.25,270,0
+4,365.2,241.64,73.54,0,30
+5,365.2,75.39,73.54,0,30
+6,530.49,130.17,12.13,300,0
+7,546.87,158.5,12.13,0,0
+8,530.49,186.83,12.13,60,0
+9,497.77,186.83,12.13,120,0
+10,481.39,158.5,12.13,180,0
+11,497.77,130.17,12.13,240,0
+12,33.51,25.8,58.5,54,0
+13,33.51,291.2,58.5,306,0
+14,325.68,241.64,73.54,180,30
+15,325.68,75.39,73.54,180,30
+16,235.73,-0.15,51.25,90,0
+17,160.39,130.17,12.13,240,0
+18,144,158.5,12.13,180,0
+19,160.39,186.83,12.13,120,0
+20,193.1,186.83,12.13,60,0
+21,209.49,158.5,12.13,0,0
+22,193.1,130.17,12.13,300,0

apriltag/src/main/native/resources/edu/wpi/first/apriltag/2025-reefscape.json

@@ -0,0 +1,404 @@
+{
+  "tags": [
+    {
+      "ID": 1,
+      "pose": {
+        "translation": {
+          "x": 16.697198,
+          "y": 0.65532,
+          "z": 1.4859
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.4539904997395468,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.8910065241883678
+          }
+        }
+      }
+    },
+    {
+      "ID": 2,
+      "pose": {
+        "translation": {
+          "x": 16.697198,
+          "y": 7.3964799999999995,
+          "z": 1.4859
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.45399049973954675,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.8910065241883679
+          }
+        }
+      }
+    },
+    {
+      "ID": 3,
+      "pose": {
+        "translation": {
+          "x": 11.560809999999998,
+          "y": 8.05561,
+          "z": 1.30175
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 4,
+      "pose": {
+        "translation": {
+          "x": 9.276079999999999,
+          "y": 6.137656,
+          "z": 1.8679160000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.9659258262890683,
+            "X": 0.0,
+            "Y": 0.25881904510252074,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 5,
+      "pose": {
+        "translation": {
+          "x": 9.276079999999999,
+          "y": 1.914906,
+          "z": 1.8679160000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.9659258262890683,
+            "X": 0.0,
+            "Y": 0.25881904510252074,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 6,
+      "pose": {
+        "translation": {
+          "x": 13.474446,
+          "y": 3.3063179999999996,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.8660254037844387,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.49999999999999994
+          }
+        }
+      }
+    },
+    {
+      "ID": 7,
+      "pose": {
+        "translation": {
+          "x": 13.890498,
+          "y": 4.0259,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 8,
+      "pose": {
+        "translation": {
+          "x": 13.474446,
+          "y": 4.745482,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.8660254037844387,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.49999999999999994
+          }
+        }
+      }
+    },
+    {
+      "ID": 9,
+      "pose": {
+        "translation": {
+          "x": 12.643358,
+          "y": 4.745482,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.5000000000000001,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.8660254037844386
+          }
+        }
+      }
+    },
+    {
+      "ID": 10,
+      "pose": {
+        "translation": {
+          "x": 12.227305999999999,
+          "y": 4.0259,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 11,
+      "pose": {
+        "translation": {
+          "x": 12.643358,
+          "y": 3.3063179999999996,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.4999999999999998,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.8660254037844387
+          }
+        }
+      }
+    },
+    {
+      "ID": 12,
+      "pose": {
+        "translation": {
+          "x": 0.851154,
+          "y": 0.65532,
+          "z": 1.4859
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.8910065241883679,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.45399049973954675
+          }
+        }
+      }
+    },
+    {
+      "ID": 13,
+      "pose": {
+        "translation": {
+          "x": 0.851154,
+          "y": 7.3964799999999995,
+          "z": 1.4859
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.8910065241883678,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.45399049973954686
+          }
+        }
+      }
+    },
+    {
+      "ID": 14,
+      "pose": {
+        "translation": {
+          "x": 8.272272,
+          "y": 6.137656,
+          "z": 1.8679160000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 5.914589856893349e-17,
+            "X": -0.25881904510252074,
+            "Y": 1.5848095757158825e-17,
+            "Z": 0.9659258262890683
+          }
+        }
+      }
+    },
+    {
+      "ID": 15,
+      "pose": {
+        "translation": {
+          "x": 8.272272,
+          "y": 1.914906,
+          "z": 1.8679160000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 5.914589856893349e-17,
+            "X": -0.25881904510252074,
+            "Y": 1.5848095757158825e-17,
+            "Z": 0.9659258262890683
+          }
+        }
+      }
+    },
+    {
+      "ID": 16,
+      "pose": {
+        "translation": {
+          "x": 5.9875419999999995,
+          "y": -0.0038099999999999996,
+          "z": 1.30175
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.7071067811865476,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 17,
+      "pose": {
+        "translation": {
+          "x": 4.073905999999999,
+          "y": 3.3063179999999996,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.4999999999999998,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.8660254037844387
+          }
+        }
+      }
+    },
+    {
+      "ID": 18,
+      "pose": {
+        "translation": {
+          "x": 3.6576,
+          "y": 4.0259,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 19,
+      "pose": {
+        "translation": {
+          "x": 4.073905999999999,
+          "y": 4.745482,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.5000000000000001,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.8660254037844386
+          }
+        }
+      }
+    },
+    {
+      "ID": 20,
+      "pose": {
+        "translation": {
+          "x": 4.904739999999999,
+          "y": 4.745482,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.8660254037844387,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.49999999999999994
+          }
+        }
+      }
+    },
+    {
+      "ID": 21,
+      "pose": {
+        "translation": {
+          "x": 5.321046,
+          "y": 4.0259,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 22,
+      "pose": {
+        "translation": {
+          "x": 4.904739999999999,
+          "y": 3.3063179999999996,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.8660254037844387,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.49999999999999994
+          }
+        }
+      }
+    }
+  ],
+  "field": {
+    "length": 17.548,
+    "width": 8.052
+  }
+}

cscore/src/main/java/edu/wpi/first/cscore/CvSink.java

@@ -6,6 +6,7 @@ package edu.wpi.first.cscore;
 
 import edu.wpi.first.util.PixelFormat;
 import edu.wpi.first.util.RawFrame;
+import edu.wpi.first.util.TimestampSource;
 import java.nio.ByteBuffer;
 import org.opencv.core.CvType;
 import org.opencv.core.Mat;
@@ -220,4 +221,22 @@ public class CvSink extends ImageSink {
     }
     return rv;
   }
+
+  /**
+   * Get the last time a frame was grabbed. This uses the same time base as wpi::Now().
+   *
+   * @return Time in 1 us increments.
+   */
+  public long getLastFrameTime() {
+    return m_frame.getTimestamp();
+  }
+
+  /**
+   * Get the time source for the timestamp the last frame was grabbed at.
+   *
+   * @return Time source
+   */
+  public TimestampSource getLastFrameTimeSource() {
+    return m_frame.getTimestampSource();
+  }
 }

cscore/src/main/native/cpp/Frame.cpp

@@ -16,18 +16,22 @@
 
 using namespace cs;
 
-Frame::Frame(SourceImpl& source, std::string_view error, Time time)
+Frame::Frame(SourceImpl& source, std::string_view error, Time time,
+             WPI_TimestampSource timeSrc)
     : m_impl{source.AllocFrameImpl().release()} {
   m_impl->refcount = 1;
   m_impl->error = error;
   m_impl->time = time;
+  m_impl->timeSource = timeSrc;
 }
 
-Frame::Frame(SourceImpl& source, std::unique_ptr<Image> image, Time time)
+Frame::Frame(SourceImpl& source, std::unique_ptr<Image> image, Time time,
+             WPI_TimestampSource timeSrc)
     : m_impl{source.AllocFrameImpl().release()} {
   m_impl->refcount = 1;
   m_impl->error.resize(0);
   m_impl->time = time;
+  m_impl->timeSource = timeSrc;
   m_impl->images.push_back(image.release());
 }
 

cscore/src/main/native/cpp/Frame.h

@@ -39,6 +39,7 @@ class Frame {
     wpi::recursive_mutex mutex;
     std::atomic_int refcount{0};
     Time time{0};
+    WPI_TimestampSource timeSource{WPI_TIMESRC_UNKNOWN};
     SourceImpl& source;
     std::string error;
     wpi::SmallVector<Image*, 4> images;
@@ -48,9 +49,11 @@ class Frame {
  public:
   Frame() noexcept = default;
 
-  Frame(SourceImpl& source, std::string_view error, Time time);
+  Frame(SourceImpl& source, std::string_view error, Time time,
+        WPI_TimestampSource timeSrc);
 
-  Frame(SourceImpl& source, std::unique_ptr<Image> image, Time time);
+  Frame(SourceImpl& source, std::unique_ptr<Image> image, Time time,
+        WPI_TimestampSource timeSrc);
 
   Frame(const Frame& frame) noexcept : m_impl{frame.m_impl} {
     if (m_impl) {
@@ -75,6 +78,9 @@ class Frame {
   }
 
   Time GetTime() const { return m_impl ? m_impl->time : 0; }
+  WPI_TimestampSource GetTimeSource() const {
+    return m_impl ? m_impl->timeSource : WPI_TIMESRC_UNKNOWN;
+  }
 
   std::string_view GetError() const {
     if (!m_impl) {

cscore/src/main/native/cpp/RawSinkImpl.cpp

@@ -120,6 +120,8 @@ uint64_t RawSinkImpl::GrabFrameImpl(WPI_RawFrame& rawFrame,
   rawFrame.pixelFormat = newImage->pixelFormat;
   rawFrame.size = newImage->size();
   std::copy(newImage->data(), newImage->data() + rawFrame.size, rawFrame.data);
+  rawFrame.timestamp = incomingFrame.GetTime();
+  rawFrame.timestampSrc = incomingFrame.GetTimeSource();
 
   return incomingFrame.GetTime();
 }

cscore/src/main/native/cpp/SourceImpl.cpp

@@ -29,7 +29,7 @@ SourceImpl::SourceImpl(std::string_view name, wpi::Logger& logger,
       m_notifier(notifier),
       m_telemetry(telemetry),
       m_name{name} {
-  m_frame = Frame{*this, std::string_view{}, 0};
+  m_frame = Frame{*this, std::string_view{}, 0, WPI_TIMESRC_UNKNOWN};
 }
 
 SourceImpl::~SourceImpl() {
@@ -95,7 +95,8 @@ Frame SourceImpl::GetNextFrame(double timeout, Frame::Time lastFrameTime) {
   if (!m_frameCv.wait_for(
           lock, std::chrono::milliseconds(static_cast<int>(timeout * 1000)),
           [=, this] { return m_frame.GetTime() != lastFrameTime; })) {
-    m_frame = Frame{*this, "timed out getting frame", wpi::Now()};
+    m_frame = Frame{*this, "timed out getting frame", wpi::Now(),
+                    WPI_TIMESRC_UNKNOWN};
   }
   return m_frame;
 }
@@ -103,7 +104,7 @@ Frame SourceImpl::GetNextFrame(double timeout, Frame::Time lastFrameTime) {
 void SourceImpl::Wakeup() {
   {
     std::scoped_lock lock{m_frameMutex};
-    m_frame = Frame{*this, std::string_view{}, 0};
+    m_frame = Frame{*this, std::string_view{}, 0, WPI_TIMESRC_UNKNOWN};
   }
   m_frameCv.notify_all();
 }
@@ -463,7 +464,8 @@ std::unique_ptr<Image> SourceImpl::AllocImage(
 }
 
 void SourceImpl::PutFrame(VideoMode::PixelFormat pixelFormat, int width,
-                          int height, std::string_view data, Frame::Time time) {
+                          int height, std::string_view data, Frame::Time time,
+                          WPI_TimestampSource timeSrc) {
   if (pixelFormat == VideoMode::PixelFormat::kBGRA) {
     // Write BGRA as BGR to save a copy
     auto image =
@@ -480,10 +482,11 @@ void SourceImpl::PutFrame(VideoMode::PixelFormat pixelFormat, int width,
           fmt::ptr(data.data()), data.size());
   std::memcpy(image->data(), data.data(), data.size());
 
-  PutFrame(std::move(image), time);
+  PutFrame(std::move(image), time, timeSrc);
 }
 
-void SourceImpl::PutFrame(std::unique_ptr<Image> image, Frame::Time time) {
+void SourceImpl::PutFrame(std::unique_ptr<Image> image, Frame::Time time,
+                          WPI_TimestampSource timeSrc) {
   // Update telemetry
   m_telemetry.RecordSourceFrames(*this, 1);
   m_telemetry.RecordSourceBytes(*this, static_cast<int>(image->size()));
@@ -491,7 +494,7 @@ void SourceImpl::PutFrame(std::unique_ptr<Image> image, Frame::Time time) {
   // Update frame
   {
     std::scoped_lock lock{m_frameMutex};
-    m_frame = Frame{*this, std::move(image), time};
+    m_frame = Frame{*this, std::move(image), time, timeSrc};
   }
 
   // Signal listeners
@@ -502,7 +505,7 @@ void SourceImpl::PutError(std::string_view msg, Frame::Time time) {
   // Update frame
   {
     std::scoped_lock lock{m_frameMutex};
-    m_frame = Frame{*this, msg, time};
+    m_frame = Frame{*this, msg, time, WPI_TIMESRC_UNKNOWN};
   }
 
   // Signal listeners

cscore/src/main/native/cpp/SourceImpl.h

@@ -13,6 +13,7 @@
 #include <vector>
 
 #include <wpi/Logger.h>
+#include <wpi/RawFrame.h>
 #include <wpi/condition_variable.h>
 #include <wpi/json_fwd.h>
 #include <wpi/mutex.h>
@@ -141,8 +142,10 @@ class SourceImpl : public PropertyContainer {
                            std::string_view valueStr) override;
 
   void PutFrame(VideoMode::PixelFormat pixelFormat, int width, int height,
-                std::string_view data, Frame::Time time);
-  void PutFrame(std::unique_ptr<Image> image, Frame::Time time);
+                std::string_view data, Frame::Time time,
+                WPI_TimestampSource timeSrc = WPI_TIMESRC_FRAME_DEQUEUE);
+  void PutFrame(std::unique_ptr<Image> image, Frame::Time time,
+                WPI_TimestampSource timeSrc = WPI_TIMESRC_FRAME_DEQUEUE);
   void PutError(std::string_view msg, Frame::Time time);
 
   // Notification functions for corresponding atomics

cscore/src/main/native/include/cscore_cv.h

@@ -8,6 +8,7 @@
 #include <functional>
 
 #include <opencv2/core/mat.hpp>
+#include <wpi/RawFrame.h>
 
 #include "cscore_oo.h"
 #include "cscore_raw.h"
@@ -172,6 +173,23 @@ class CvSink : public ImageSink {
   uint64_t GrabFrameDirectLastTime(cv::Mat& image, uint64_t lastFrameTime,
                                    double timeout = 0.225);
 
+  /**
+   * Get the last time a frame was grabbed. This uses the same time base as
+   * wpi::Now().
+   *
+   * @return Time in 1 us increments.
+   */
+  [[nodiscard]]
+  uint64_t LastFrameTime();
+
+  /**
+   * Get the time source for the timestamp the last frame was grabbed at.
+   *
+   * @return Time source
+   */
+  [[nodiscard]]
+  WPI_TimestampSource LastFrameTimeSource();
+
  private:
   constexpr int GetCvFormat(WPI_PixelFormat pixelFormat);
 
@@ -405,6 +423,14 @@ inline uint64_t CvSink::GrabFrameDirectLastTime(cv::Mat& image,
   return timestamp;
 }
 
+inline uint64_t CvSink::LastFrameTime() {
+  return rawFrame.timestamp;
+}
+
+inline WPI_TimestampSource CvSink::LastFrameTimeSource() {
+  return static_cast<WPI_TimestampSource>(rawFrame.timestampSrc);
+}
+
 }  // namespace cs
 
 #endif  // CSCORE_CSCORE_CV_H_

cscore/src/main/native/linux/UsbCameraImpl.cpp

@@ -555,8 +555,51 @@ void UsbCameraImpl::CameraThreadMain() {
           good = false;
         }
         if (good) {
+          Frame::Time frameTime{wpi::Now()};
+          WPI_TimestampSource timeSource{WPI_TIMESRC_FRAME_DEQUEUE};
+
+          // check the timestamp time
+          auto tsFlags = buf.flags & V4L2_BUF_FLAG_TIMESTAMP_MASK;
+          SDEBUG4("Flags {}", tsFlags);
+          if (tsFlags == V4L2_BUF_FLAG_TIMESTAMP_UNKNOWN) {
+            SDEBUG4("Got unknown time for frame - default to wpi::Now");
+          } else if (tsFlags == V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC) {
+            SDEBUG4("Got valid monotonic time for frame");
+            // we can't go directly to frametime, since the rest of cscore
+            // expects us to use wpi::Now, which is in an arbitrary timebase
+            // (see timestamp.cpp). Best I can do is (approximately) translate
+            // between timebases
+
+            // grab current time in the same timebase as buf.timestamp
+            struct timespec ts;
+            if (clock_gettime(CLOCK_MONOTONIC, &ts) == 0) {
+              int64_t nowTime = {ts.tv_sec * 1'000'000 + ts.tv_nsec / 1000};
+              int64_t bufTime = {buf.timestamp.tv_sec * 1'000'000 +
+                                 buf.timestamp.tv_usec};
+              // And offset frameTime by the latency
+              int64_t offset{nowTime - bufTime};
+              frameTime -= offset;
+
+              // Figure out the timestamp's source
+              int tsrcFlags = buf.flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+              if (tsrcFlags == V4L2_BUF_FLAG_TSTAMP_SRC_EOF) {
+                timeSource = WPI_TIMESRC_V4L_EOF;
+              } else if (tsrcFlags == V4L2_BUF_FLAG_TSTAMP_SRC_SOE) {
+                timeSource = WPI_TIMESRC_V4L_SOE;
+              } else {
+                timeSource = WPI_TIMESRC_UNKNOWN;
+              }
+              SDEBUG4("Frame was {} uS old, flags {}, source {}", offset,
+                      tsrcFlags, static_cast<int>(timeSource));
+            } else {
+              // Can't do anything if we can't access the clock, leave default
+            }
+          } else if (tsFlags == V4L2_BUF_FLAG_TIMESTAMP_COPY) {
+            SDEBUG4("Got valid copy time for frame - default to wpi::Now");
+          }
+
           PutFrame(static_cast<VideoMode::PixelFormat>(m_mode.pixelFormat),
-                   width, height, image, wpi::Now());  // TODO: time
+                   width, height, image, frameTime, timeSource);
         }
       }
 

epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/EpilogueBackend.java

@@ -198,10 +198,9 @@ public interface EpilogueBackend {
    *
    * @param identifier the identifier of the data field
    * @param value the new value of the data field
-   * @param <U> the dimension of the unit
    */
-  default <U extends Unit> void log(String identifier, Measure<U> value) {
-    log(identifier, value, value.baseUnit());
+  default void log(String identifier, Measure<?> value) {
+    log(identifier, value.baseUnitMagnitude());
   }
 
   /**
@@ -213,7 +212,7 @@ public interface EpilogueBackend {
    * @param <U> the dimension of the unit
    */
   default <U extends Unit> void log(String identifier, Measure<U> value, U unit) {
-    log(identifier + " (" + unit.symbol() + ")", value.in(unit));
+    log(identifier, value.in(unit));
   }
 
   /**

fieldImages/src/main/java/edu/wpi/first/fields/Fields.java

@@ -16,12 +16,13 @@ public enum Fields {
   k2021Slalom("2021-slalompath.json"),
   k2022RapidReact("2022-rapidreact.json"),
   k2023ChargedUp("2023-chargedup.json"),
-  k2024Crescendo("2024-crescendo.json");
+  k2024Crescendo("2024-crescendo.json"),
+  k2025Reefscape("2025-reefscape.json");
 
   public static final String kBaseResourceDir = "/edu/wpi/first/fields/";
 
   /** Alias to the current game. */
-  public static final Fields kDefaultField = k2024Crescendo;
+  public static final Fields kDefaultField = k2025Reefscape;
 
   public final String m_resourceFile;
 

fieldImages/src/main/native/cpp/fields.cpp

@@ -16,10 +16,13 @@
 #include "fields/2022-rapidreact.h"
 #include "fields/2023-chargedup.h"
 #include "fields/2024-crescendo.h"
+#include "fields/2025-reefscape.h"
 
 using namespace fields;
 
 static const Field kFields[] = {
+    {"2025 Reefscape", GetResource_2025_reefscape_json,
+     GetResource_2025_field_png},
     {"2024 Crescendo", GetResource_2024_crescendo_json,
      GetResource_2024_field_png},
     {"2023 Charged Up", GetResource_2023_chargedup_json,

fieldImages/src/main/native/include/fields/2025-reefscape.h

@@ -0,0 +1,12 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string_view>
+
+namespace fields {
+std::string_view GetResource_2025_reefscape_json();
+std::string_view GetResource_2025_field_png();
+}  // namespace fields

fieldImages/src/main/native/resources/edu/wpi/first/fields/2025-reefscape.json

@@ -0,0 +1,19 @@
+{
+  "game": "Reefscape",
+  "field-image": "2025-field.png",
+  "field-corners": {
+    "top-left": [
+      534,
+      291
+    ],
+    "bottom-right": [
+      3466,
+      1638
+    ]
+  },
+  "field-size": [
+    57.573,
+    26.417
+  ],
+  "field-unit": "foot"
+}

glass/src/lib/native/cpp/other/Field2D.cpp

@@ -373,13 +373,12 @@ void FieldInfo::DisplaySettings() {
     }
     ImGui::EndCombo();
   }
-  if (m_builtin.empty() && ImGui::Button("Load image...")) {
+  if (m_builtin.empty() && ImGui::Button("Load JSON/image...")) {
     m_fileOpener = std::make_unique<pfd::open_file>(
-        "Choose field image", "",
-        std::vector<std::string>{"Image File",
+        "Choose field JSON/image", "",
+        std::vector<std::string>{"PathWeaver JSON File", "*.json", "Image File",
                                  "*.jpg *.jpeg *.png *.bmp *.psd *.tga *.gif "
-                                 "*.hdr *.pic *.ppm *.pgm",
-                                 "PathWeaver JSON File", "*.json"});
+                                 "*.hdr *.pic *.ppm *.pgm"});
   }
   if (ImGui::Button("Reset image")) {
     Reset();
@@ -586,17 +585,29 @@ FieldFrameData FieldInfo::GetFrameData(ImVec2 min, ImVec2 max) const {
     max.x -= (m_imageWidth - m_right) * scale;
     max.y -= (m_imageHeight - m_bottom) * scale;
   } else if ((max.x - min.x) > 40 && (max.y - min.y > 40)) {
+    // scale padding to be proportional to aspect ratio
+    float width = max.x - min.x;
+    float height = max.y - min.y;
+    float padX, padY;
+    if (width > height) {
+      padX = 20 * width / height;
+      padY = 20;
+    } else {
+      padX = 20;
+      padY = 20 * height / width;
+    }
+
     // ensure there's some padding
-    min.x += 20;
-    max.x -= 20;
-    min.y += 20;
-    max.y -= 20;
+    min.x += padX;
+    max.x -= padX;
+    min.y += padY;
+    max.y -= padY;
 
     // also pad the image so it's the same size as the box
-    ffd.imageMin.x += 20;
-    ffd.imageMax.x -= 20;
-    ffd.imageMin.y += 20;
-    ffd.imageMax.y -= 20;
+    ffd.imageMin.x += padX;
+    ffd.imageMax.x -= padX;
+    ffd.imageMin.y += padY;
+    ffd.imageMax.y -= padY;
   }
 
   ffd.min = min;

wpical/src/main/native/cpp/WPIcal.cpp

@@ -9,12 +9,14 @@
 
 #include <fstream>
 #include <iostream>
+#include <map>
 #include <memory>
 #include <numbers>
 #include <string>
 #include <vector>
 
 #include <GLFW/glfw3.h>
+#include <fmt/format.h>
 #include <imgui.h>
 #include <portable-file-dialogs.h>
 #include <tagpose.h>
@@ -58,6 +60,112 @@ void drawCheck() {
   ImGui::NewLine();
 }
 
+void processFileSelector(std::unique_ptr<pfd::open_file>& selector,
+                         std::string& selected_file) {
+  if (selector && selector->ready(0)) {
+    auto selectedFiles = selector->result();
+    if (!selectedFiles.empty()) {
+      selected_file = selectedFiles[0];
+    }
+    selector.reset();
+  }
+}
+
+void processFilesSelector(std::unique_ptr<pfd::open_file>& selector,
+                          std::vector<std::string>& selected_files) {
+  if (selector && selector->ready(0)) {
+    auto selectedFiles = selector->result();
+    if (!selectedFiles.empty()) {
+      selected_files = selectedFiles;
+    }
+    selector.reset();
+  }
+}
+
+void processDirectorySelector(std::unique_ptr<pfd::select_folder>& selector,
+                              std::string& selected_directory) {
+  if (selector && selector->ready(0)) {
+    auto selectedFiles = selector->result();
+    if (!selectedFiles.empty()) {
+      selected_directory = selectedFiles;
+    }
+    selector.reset();
+  }
+}
+
+void openFileButton(const char* text, std::string& selected_file,
+                    std::unique_ptr<pfd::open_file>& selector,
+                    const std::string& file_type,
+                    const std::string& file_extensions) {
+  if (ImGui::Button(text)) {
+    selector = std::make_unique<pfd::open_file>(
+        "Select File", "", std::vector<std::string>{file_type, file_extensions},
+        pfd::opt::none);
+  }
+}
+
+void openFilesButton(const char* text, std::vector<std::string>& selected_files,
+                     std::unique_ptr<pfd::open_file>& selector,
+                     const std::string& file_type,
+                     const std::string& file_extensions) {
+  if (ImGui::Button(text)) {
+    selector = std::make_unique<pfd::open_file>(
+        "Select File", "", std::vector<std::string>{file_type, file_extensions},
+        pfd::opt::multiselect);
+  }
+}
+
+void openDirectoryButton(const char* text,
+                         std::unique_ptr<pfd::select_folder>& selector,
+                         std::string& selected_directory) {
+  if (ImGui::Button(text)) {
+    selector = std::make_unique<pfd::select_folder>("Select Directory", "");
+  }
+}
+
+std::string getFileName(std::string path) {
+  size_t lastSlash = path.find_last_of("/\\");
+  size_t lastDot = path.find_last_of(".");
+  return path.substr(lastSlash + 1, lastDot - lastSlash - 1);
+}
+
+static bool EmitEntryTarget(int tag_id, std::string& file) {
+  if (!file.empty()) {
+    auto text = fmt::format("{}: {}", tag_id, file);
+    ImGui::TextUnformatted(text.c_str());
+  } else {
+    ImGui::Text("Tag ID %i: <none (DROP HERE)>", tag_id);
+  }
+  bool rv = false;
+  if (ImGui::BeginDragDropTarget()) {
+    if (const ImGuiPayload* payload =
+            ImGui::AcceptDragDropPayload("FieldCalibration")) {
+      file = *(std::string*)payload->Data;
+      rv = true;
+    }
+    ImGui::EndDragDropTarget();
+  }
+  return rv;
+}
+
+void saveCalibration(wpi::json& field, std::string& output_directory,
+                     std::string output_name, bool& isCalibrating) {
+  if (!field.empty() && !output_directory.empty()) {
+    std::cout << "Saving calibration to " << output_directory << std::endl;
+    std::ofstream out(output_directory + "/" + output_name + ".json");
+    out << field.dump(4);
+    out.close();
+
+    std::ofstream fmap(output_directory + "/" + output_name + ".fmap");
+    fmap << fmap::convertfmap(field).dump(4);
+    fmap.close();
+
+    field.clear();
+    output_directory.clear();
+    isCalibrating = false;
+  }
+}
+
 static void DisplayGui() {
   ImGui::GetStyle().WindowRounding = 0;
 
@@ -82,19 +190,28 @@ static void DisplayGui() {
   ImGui::EndMenuBar();
 
   static std::unique_ptr<pfd::open_file> camera_intrinsics_selector;
-  static std::string selected_camera_intrinsics;
-
   static std::unique_ptr<pfd::open_file> field_map_selector;
-  static std::string selected_field_map;
+  static std::unique_ptr<pfd::open_file> output_calibration_json_selector;
+  static std::unique_ptr<pfd::open_file> combination_calibrations_selector;
 
   static std::unique_ptr<pfd::select_folder>
       field_calibration_directory_selector;
-  static std::string selected_field_calibration_directory;
-
   static std::unique_ptr<pfd::select_folder> download_directory_selector;
-  static std::string selected_download_directory;
 
-  static std::string calibration_json_path;
+  static wpi::json field_calibration_json;
+  static wpi::json field_combination_json;
+
+  static std::string selected_camera_intrinsics;
+  static std::string selected_field_map;
+  static std::string selected_field_calibration_directory;
+  static std::string selected_download_directory;
+  static std::string output_calibration_json_path;
+  static std::vector<std::string> selected_combination_calibrations;
+
+  static std::map<int, std::string> combiner_map;
+  static int current_combiner_tag_id = 0;
+
+  static bool isCalibrating = false;
 
   cameracalibration::CameraModel cameraModel = {
       .intrinsic_matrix = Eigen::Matrix<double, 3, 3>::Identity(),
@@ -118,13 +235,12 @@ static void DisplayGui() {
 
   static Fieldmap currentCalibrationMap;
   static Fieldmap currentReferenceMap;
+  static Fieldmap currentCombinerMap;
 
   // camera matrix selector button
-  if (ImGui::Button("Upload Camera Intrinsics")) {
-    camera_intrinsics_selector = std::make_unique<pfd::open_file>(
-        "Select Camera Intrinsics JSON", "",
-        std::vector<std::string>{"JSON", "*.json"}, pfd::opt::none);
-  }
+  openFileButton("Select Camera Intrinsics JSON", selected_camera_intrinsics,
+                 camera_intrinsics_selector, "JSON Files", "*.json");
+  processFileSelector(camera_intrinsics_selector, selected_camera_intrinsics);
 
   ImGui::SameLine();
   ImGui::Text("Or");
@@ -136,50 +252,25 @@ static void DisplayGui() {
     ImGui::OpenPopup("Camera Calibration");
   }
 
-  if (camera_intrinsics_selector) {
-    auto selectedFiles = camera_intrinsics_selector->result();
-    if (!selectedFiles.empty()) {
-      selected_camera_intrinsics = selectedFiles[0];
-    }
-    camera_intrinsics_selector.reset();
-  }
-
   if (!selected_camera_intrinsics.empty()) {
     drawCheck();
   }
 
   // field json selector button
-  if (ImGui::Button("Select Field Map JSON")) {
-    field_map_selector = std::make_unique<pfd::open_file>(
-        "Select Json File", "",
-        std::vector<std::string>{"JSON Files", "*.json"}, pfd::opt::none);
-  }
-
-  if (field_map_selector) {
-    auto selectedFiles = field_map_selector->result();
-    if (!selectedFiles.empty()) {
-      selected_field_map = selectedFiles[0];
-    }
-    field_map_selector.reset();
-  }
+  openFileButton("Select Field Map JSON", selected_field_map,
+                 field_map_selector, "JSON Files", "*.json");
+  processFileSelector(field_map_selector, selected_field_map);
 
   if (!selected_field_map.empty()) {
     drawCheck();
   }
 
   // field calibration directory selector button
-  if (ImGui::Button("Select Field Calibration Folder")) {
-    field_calibration_directory_selector = std::make_unique<pfd::select_folder>(
-        "Select Field Calibration Folder", "");
-  }
-
-  if (field_calibration_directory_selector) {
-    auto selectedFiles = field_calibration_directory_selector->result();
-    if (!selectedFiles.empty()) {
-      selected_field_calibration_directory = selectedFiles;
-    }
-    field_calibration_directory_selector.reset();
-  }
+  openDirectoryButton("Select Field Calibration Directory",
+                      field_calibration_directory_selector,
+                      selected_field_calibration_directory);
+  processDirectorySelector(field_calibration_directory_selector,
+                           selected_field_calibration_directory);
 
   if (!selected_field_calibration_directory.empty()) {
     drawCheck();
@@ -191,46 +282,35 @@ static void DisplayGui() {
 
   // calibrate button
   if (ImGui::Button("Calibrate!!!")) {
-    if (!selected_field_calibration_directory.empty() &&
-        !selected_camera_intrinsics.empty() && !selected_field_map.empty()) {
+    int calibrationOutput = fieldcalibration::calibrate(
+        selected_field_calibration_directory.c_str(), field_calibration_json,
+        selected_camera_intrinsics, selected_field_map.c_str(), pinnedTag,
+        showDebug);
+
+    if (calibrationOutput == 1) {
+      ImGui::OpenPopup("Field Calibration Error");
+    }
+
+    if (selected_download_directory.empty() &&
+        !field_calibration_json.empty() && !download_directory_selector) {
       download_directory_selector =
           std::make_unique<pfd::select_folder>("Select Download Folder", "");
-      if (download_directory_selector) {
-        auto selectedFiles = download_directory_selector->result();
-        if (!selectedFiles.empty()) {
-          selected_download_directory = selectedFiles;
-        }
-        download_directory_selector.reset();
-      }
-
-      calibration_json_path = selected_download_directory + "/output.json";
-
-      int calibrationOutput = fieldcalibration::calibrate(
-          selected_field_calibration_directory.c_str(), calibration_json_path,
-          selected_camera_intrinsics, selected_field_map.c_str(), pinnedTag,
-          showDebug);
-
-      if (calibrationOutput == 1) {
-        ImGui::OpenPopup("Field Calibration Error");
-      } else if (calibrationOutput == 0) {
-        std::ifstream caljsonpath(calibration_json_path);
-        try {
-          wpi::json fmap = fmap::convertfmap(wpi::json::parse(caljsonpath));
-          std::ofstream out(selected_download_directory + "/output.fmap");
-          out << fmap.dump(4);
-          out.close();
-          ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always);
-          ImGui::OpenPopup("Visualize Calibration");
-        } catch (...) {
-          ImGui::OpenPopup("Fmap Conversion Error");
-        }
-      }
     }
   }
+
+  processDirectorySelector(download_directory_selector,
+                           selected_download_directory);
+  saveCalibration(field_calibration_json, selected_download_directory,
+                  "field_calibration", isCalibrating);
+
   if (ImGui::Button("Visualize")) {
     ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always);
     ImGui::OpenPopup("Visualize Calibration");
   }
+  if (ImGui::Button("Combine Calibrations")) {
+    ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always);
+    ImGui::OpenPopup("Combine Calibrations");
+  }
   if (selected_field_calibration_directory.empty() ||
       selected_camera_intrinsics.empty() || selected_field_map.empty()) {
     ImGui::TextWrapped(
@@ -320,21 +400,11 @@ static void DisplayGui() {
     }
 
     if (mrcal) {
-      if (ImGui::Button("Select Camera Calibration Video")) {
-        camera_intrinsics_selector = std::make_unique<pfd::open_file>(
-            "Select Camera Calibration Video", "",
-            std::vector<std::string>{"Video Files",
-                                     "*.mp4 *.mov *.m4v *.mkv *.avi"},
-            pfd::opt::none);
-      }
-
-      if (camera_intrinsics_selector) {
-        auto selectedFiles = camera_intrinsics_selector->result();
-        if (!selectedFiles.empty()) {
-          selected_camera_intrinsics = selectedFiles[0];
-        }
-        camera_intrinsics_selector.reset();
-      }
+      openFileButton("Select Camera Calibration Video",
+                     selected_camera_intrinsics, camera_intrinsics_selector,
+                     "Video Files", "*.mp4 *.mov *.m4v *.mkv *.avi");
+      processFileSelector(camera_intrinsics_selector,
+                          selected_camera_intrinsics);
 
       ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
       ImGui::InputDouble("Square Width (in)", &squareWidth);
@@ -379,21 +449,11 @@ static void DisplayGui() {
         }
       }
     } else {
-      if (ImGui::Button("Select Camera Calibration Video")) {
-        camera_intrinsics_selector = std::make_unique<pfd::open_file>(
-            "Select Camera Calibration Video", "",
-            std::vector<std::string>{"Video Files",
-                                     "*.mp4 *.mov *.m4v *.mkv *.avi"},
-            pfd::opt::none);
-      }
-
-      if (camera_intrinsics_selector) {
-        auto selectedFiles = camera_intrinsics_selector->result();
-        if (!selectedFiles.empty()) {
-          selected_camera_intrinsics = selectedFiles[0];
-        }
-        camera_intrinsics_selector.reset();
-      }
+      openFileButton("Select Camera Calibration Video",
+                     selected_camera_intrinsics, camera_intrinsics_selector,
+                     "Video Files", "*.mp4 *.mov *.m4v *.mkv *.avi");
+      processFileSelector(camera_intrinsics_selector,
+                          selected_camera_intrinsics);
 
       ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
       ImGui::InputDouble("Square Width (in)", &squareWidth);
@@ -446,26 +506,19 @@ static void DisplayGui() {
   // visualize calibration popup
   if (ImGui::BeginPopupModal("Visualize Calibration", NULL,
                              ImGuiWindowFlags_AlwaysAutoResize)) {
-    if (ImGui::Button("Load Calibrated Field")) {
-      calibration_json_path =
-          std::make_unique<pfd::open_file>(
-              "Select Json File", "",
-              std::vector<std::string>{"JSON Files", "*.json"}, pfd::opt::none)
-              ->result()[0];
-    }
+    openFileButton("Select Calibration JSON", output_calibration_json_path,
+                   output_calibration_json_selector, "JSON", "*.json");
+    processFileSelector(output_calibration_json_selector,
+                        output_calibration_json_path);
 
-    if (!calibration_json_path.empty()) {
+    if (!output_calibration_json_path.empty()) {
       ImGui::SameLine();
       drawCheck();
     }
 
-    if (ImGui::Button("Load Reference Field")) {
-      selected_field_map =
-          std::make_unique<pfd::open_file>(
-              "Select Json File", "",
-              std::vector<std::string>{"JSON Files", "*.json"}, pfd::opt::none)
-              ->result()[0];
-    }
+    openFileButton("Select Ideal Field Map", selected_field_map,
+                   field_map_selector, "JSON", "*.json");
+    processFileSelector(field_map_selector, selected_field_map);
 
     if (!selected_field_map.empty()) {
       ImGui::SameLine();
@@ -477,58 +530,76 @@ static void DisplayGui() {
     ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
     ImGui::InputInt("Reference Tag", &referenceTag);
 
-    if (!calibration_json_path.empty() && !selected_field_map.empty()) {
-      std::ifstream calJson(calibration_json_path);
+    if (!output_calibration_json_path.empty() && !selected_field_map.empty()) {
+      std::ifstream calJson(output_calibration_json_path);
       std::ifstream refJson(selected_field_map);
 
       currentCalibrationMap = Fieldmap(wpi::json::parse(calJson));
       currentReferenceMap = Fieldmap(wpi::json::parse(refJson));
 
-      double xDiff = currentReferenceMap.getTag(focusedTag).xPos -
-                     currentCalibrationMap.getTag(focusedTag).xPos;
-      double yDiff = currentReferenceMap.getTag(focusedTag).yPos -
-                     currentCalibrationMap.getTag(focusedTag).yPos;
-      double zDiff = currentReferenceMap.getTag(focusedTag).zPos -
-                     currentCalibrationMap.getTag(focusedTag).zPos;
-      double yawDiff = currentReferenceMap.getTag(focusedTag).yawRot -
-                       currentCalibrationMap.getTag(focusedTag).yawRot;
-      double pitchDiff = currentReferenceMap.getTag(focusedTag).pitchRot -
-                         currentCalibrationMap.getTag(focusedTag).pitchRot;
-      double rollDiff = currentReferenceMap.getTag(focusedTag).rollRot -
-                        currentCalibrationMap.getTag(focusedTag).rollRot;
+      if (currentCalibrationMap.getNumTags() !=
+          currentReferenceMap.getNumTags()) {
+        ImGui::TextWrapped(
+            "The number of tags in the calibration output and the ideal field "
+            "map "
+            "do not match. Please ensure that the calibration output and ideal "
+            "field "
+            "map have the same number of tags.");
+      } else if (currentReferenceMap.hasTag(focusedTag) &&
+                 currentReferenceMap.hasTag(referenceTag)) {
+        double xDiff = currentReferenceMap.getTag(focusedTag).xPos -
+                       currentCalibrationMap.getTag(focusedTag).xPos;
+        double yDiff = currentReferenceMap.getTag(focusedTag).yPos -
+                       currentCalibrationMap.getTag(focusedTag).yPos;
+        double zDiff = currentReferenceMap.getTag(focusedTag).zPos -
+                       currentCalibrationMap.getTag(focusedTag).zPos;
+        double yawDiff = currentReferenceMap.getTag(focusedTag).yawRot -
+                         currentCalibrationMap.getTag(focusedTag).yawRot;
+        double pitchDiff = currentReferenceMap.getTag(focusedTag).pitchRot -
+                           currentCalibrationMap.getTag(focusedTag).pitchRot;
+        double rollDiff = currentReferenceMap.getTag(focusedTag).rollRot -
+                          currentCalibrationMap.getTag(focusedTag).rollRot;
 
-      double xRef = currentCalibrationMap.getTag(referenceTag).xPos -
-                    currentCalibrationMap.getTag(focusedTag).xPos;
-      double yRef = currentCalibrationMap.getTag(referenceTag).yPos -
-                    currentCalibrationMap.getTag(focusedTag).yPos;
-      double zRef = currentCalibrationMap.getTag(referenceTag).zPos -
-                    currentCalibrationMap.getTag(focusedTag).zPos;
+        double xRef = currentCalibrationMap.getTag(referenceTag).xPos -
+                      currentCalibrationMap.getTag(focusedTag).xPos;
+        double yRef = currentCalibrationMap.getTag(referenceTag).yPos -
+                      currentCalibrationMap.getTag(focusedTag).yPos;
+        double zRef = currentCalibrationMap.getTag(referenceTag).zPos -
+                      currentCalibrationMap.getTag(focusedTag).zPos;
 
-      ImGui::TextWrapped("X Difference: %s (m)", std::to_string(xDiff).c_str());
-      ImGui::TextWrapped("Y Difference: %s (m)", std::to_string(yDiff).c_str());
-      ImGui::TextWrapped("Z Difference: %s (m)", std::to_string(zDiff).c_str());
+        ImGui::TextWrapped("X Difference: %s (m)",
+                           std::to_string(xDiff).c_str());
+        ImGui::TextWrapped("Y Difference: %s (m)",
+                           std::to_string(yDiff).c_str());
+        ImGui::TextWrapped("Z Difference: %s (m)",
+                           std::to_string(zDiff).c_str());
 
-      ImGui::TextWrapped(
-          "Yaw Difference %s°",
-          std::to_string(
-              Fieldmap::minimizeAngle(yawDiff * (180.0 / std::numbers::pi)))
-              .c_str());
-      ImGui::TextWrapped(
-          "Pitch Difference %s°",
-          std::to_string(
-              Fieldmap::minimizeAngle(pitchDiff * (180.0 / std::numbers::pi)))
-              .c_str());
-      ImGui::TextWrapped(
-          "Roll Difference %s°",
-          std::to_string(
-              Fieldmap::minimizeAngle(rollDiff * (180.0 / std::numbers::pi)))
-              .c_str());
+        ImGui::TextWrapped(
+            "Yaw Difference %s°",
+            std::to_string(
+                Fieldmap::minimizeAngle(yawDiff * (180.0 / std::numbers::pi)))
+                .c_str());
+        ImGui::TextWrapped(
+            "Pitch Difference %s°",
+            std::to_string(
+                Fieldmap::minimizeAngle(pitchDiff * (180.0 / std::numbers::pi)))
+                .c_str());
+        ImGui::TextWrapped(
+            "Roll Difference %s°",
+            std::to_string(
+                Fieldmap::minimizeAngle(rollDiff * (180.0 / std::numbers::pi)))
+                .c_str());
 
-      ImGui::NewLine();
+        ImGui::NewLine();
 
-      ImGui::TextWrapped("X Reference: %s (m)", std::to_string(xRef).c_str());
-      ImGui::TextWrapped("Y Reference: %s (m)", std::to_string(yRef).c_str());
-      ImGui::TextWrapped("Z Reference: %s (m)", std::to_string(zRef).c_str());
+        ImGui::TextWrapped("X Reference: %s (m)", std::to_string(xRef).c_str());
+        ImGui::TextWrapped("Y Reference: %s (m)", std::to_string(yRef).c_str());
+        ImGui::TextWrapped("Z Reference: %s (m)", std::to_string(zRef).c_str());
+      } else {
+        ImGui::TextWrapped(
+            "Please select tags that are in the ideal field map and "
+            "calibration map");
+      }
     }
 
     if (ImGui::Button("Close")) {
@@ -537,6 +608,78 @@ static void DisplayGui() {
     ImGui::EndPopup();
   }
 
+  if (ImGui::BeginPopupModal("Combine Calibrations", NULL,
+                             ImGuiWindowFlags_AlwaysAutoResize)) {
+    openFileButton("Select Ideal Map", selected_field_map, field_map_selector,
+                   "JSON", "*.json");
+    processFileSelector(field_map_selector, selected_field_map);
+    if (!selected_field_map.empty()) {
+      drawCheck();
+      std::ifstream json(selected_field_map);
+      currentReferenceMap = Fieldmap(wpi::json::parse(json));
+      currentCombinerMap = currentReferenceMap;
+    }
+    openFilesButton("Select Field Calibrations",
+                    selected_combination_calibrations,
+                    combination_calibrations_selector, "JSON", "*.json");
+    processFilesSelector(combination_calibrations_selector,
+                         selected_combination_calibrations);
+
+    if (!selected_field_map.empty() &&
+        !selected_combination_calibrations.empty()) {
+      for (std::string& file : selected_combination_calibrations) {
+        ImGui::Selectable(getFileName(file).c_str(), false,
+                          ImGuiSelectableFlags_DontClosePopups);
+        if (ImGui::BeginDragDropSource()) {
+          ImGui::SetDragDropPayload("FieldCalibration", &file, sizeof(file));
+          ImGui::TextUnformatted(file.c_str());
+          ImGui::EndDragDropSource();
+        }
+      }
+
+      for (auto& [key, val] : combiner_map) {
+        EmitEntryTarget(key, val);
+      }
+
+      ImGui::InputInt("Tag ID", &current_combiner_tag_id);
+      ImGui::SameLine();
+      if (ImGui::Button("Add", ImVec2(0, 0)) &&
+          currentCombinerMap.hasTag(current_combiner_tag_id)) {
+        combiner_map.emplace(current_combiner_tag_id, "");
+      }
+      ImGui::SameLine();
+      if (ImGui::Button("Remove", ImVec2(0, 0))) {
+        combiner_map.erase(current_combiner_tag_id);
+      }
+    }
+    ImGui::Separator();
+    if (ImGui::Button("Close", ImVec2(0, 0))) {
+      ImGui::CloseCurrentPopup();
+    }
+    ImGui::SameLine();
+    if (ImGui::Button("Download", ImVec2(0, 0))) {
+      for (auto& [key, val] : combiner_map) {
+        std::ifstream json(val);
+        Fieldmap map(wpi::json::parse(json));
+        currentCombinerMap.replaceTag(key, map.getTag(key));
+      }
+      field_combination_json = currentCombinerMap.toJson();
+    }
+
+    if (selected_download_directory.empty() &&
+        !field_combination_json.empty() && !download_directory_selector) {
+      download_directory_selector =
+          std::make_unique<pfd::select_folder>("Select Download Folder", "");
+    }
+
+    processDirectorySelector(download_directory_selector,
+                             selected_download_directory);
+    saveCalibration(field_combination_json, selected_download_directory,
+                    "combined_calibration", isCalibrating);
+
+    ImGui::EndPopup();
+  }
+
   ImGui::End();
 }
 

wpical/src/main/native/cpp/fieldcalibration.cpp

@@ -23,7 +23,6 @@
 #include <opencv2/highgui.hpp>
 #include <opencv2/imgproc.hpp>
 #include <opencv2/videoio.hpp>
-#include <wpi/json.h>
 
 #include "apriltag.h"
 #include "tag36h11.h"
@@ -433,7 +432,7 @@ inline bool process_video_file(
 }
 
 int fieldcalibration::calibrate(std::string input_dir_path,
-                                std::string output_file_path,
+                                wpi::json& output_json,
                                 std::string camera_model_path,
                                 std::string ideal_map_path, int pinned_tag_id,
                                 bool show_debug_window) {
@@ -605,8 +604,7 @@ int fieldcalibration::calibrate(std::string input_dir_path,
       {"length", static_cast<double>(json.at("field").at("length"))},
       {"width", static_cast<double>(json.at("field").at("width"))}};
 
-  std::ofstream output_file(output_file_path);
-  output_file << observed_map_json.dump(4) << std::endl;
+  output_json = observed_map_json;
 
   return 0;
 }

wpical/src/main/native/cpp/tagpose.cpp

@@ -5,8 +5,10 @@
 #include <tagpose.h>
 
 namespace tag {
-Pose::Pose(double xpos, double ypos, double zpos, double w, double x, double y,
-           double z, double field_length_meters, double field_width_meters) {
+Pose::Pose(int tag_id, double xpos, double ypos, double zpos, double w,
+           double x, double y, double z, double field_length_meters,
+           double field_width_meters) {
+  tagId = tag_id;
   xPos = xpos;
   yPos = ypos;
   zPos = zpos;
@@ -26,4 +28,16 @@ Pose::Pose(double xpos, double ypos, double zpos, double w, double x, double y,
   pitchRot = eulerAngles[1];
   yawRot = eulerAngles[2];
 }
+
+wpi::json Pose::toJson() {
+  return {{"ID", tagId},
+          {"pose",
+           {{"translation", {{"x", xPos}, {"y", yPos}, {"z", zPos}}},
+            {"rotation",
+             {{"quaternion",
+               {{"W", quaternion.w()},
+                {"X", quaternion.x()},
+                {"Y", quaternion.y()},
+                {"Z", quaternion.z()}}}}}}}};
+}
 }  // namespace tag

wpical/src/main/native/include/fieldcalibration.h

@@ -6,10 +6,12 @@
 
 #include <string>
 
+#include <wpi/json.h>
+
 #include "cameracalibration.h"
 
 namespace fieldcalibration {
-int calibrate(std::string input_dir_path, std::string output_file_path,
+int calibrate(std::string input_dir_path, wpi::json& output_json,
               std::string camera_model_path, std::string ideal_map_path,
               int pinned_tag_id, bool show_debug_window);
 }  // namespace fieldcalibration

wpical/src/main/native/include/fieldmap.h

@@ -5,7 +5,7 @@
 #pragma once
 
 #include <cmath>
-#include <vector>
+#include <map>
 
 #include <tagpose.h>
 #include <wpi/json.h>
@@ -19,6 +19,7 @@ class Fieldmap {
     double field_width_meters =
         static_cast<double>(json.at("field").at("width"));
     for (const auto& tag : json.at("tags").items()) {
+      double tag_id = static_cast<int>(tag.value().at("ID"));
       double tagXPos =
           static_cast<double>(tag.value().at("pose").at("translation").at("x"));
       double tagYPos =
@@ -34,15 +35,30 @@ class Fieldmap {
       double tagZQuat = static_cast<double>(
           tag.value().at("pose").at("rotation").at("quaternion").at("Z"));
 
-      tagVec.emplace_back(tagXPos, tagYPos, tagZPos, tagWQuat, tagXQuat,
-                          tagYQuat, tagZQuat, field_length_meters,
-                          field_width_meters);
+      tagMap.emplace(
+          tag_id, tag::Pose(tag_id, tagXPos, tagYPos, tagZPos, tagWQuat,
+                            tagXQuat, tagYQuat, tagZQuat, field_length_meters,
+                            field_width_meters));
     }
+    fieldLength = field_length_meters;
+    fieldWidth = field_width_meters;
   }
 
-  const tag::Pose& getTag(size_t tag) const { return tagVec[tag - 1]; }
+  const tag::Pose& getTag(size_t tag) const { return tagMap.at(tag); }
 
-  int getNumTags() const { return tagVec.size(); }
+  int getNumTags() const { return tagMap.size(); }
+
+  bool hasTag(int tag) { return tagMap.find(tag) != tagMap.end(); }
+
+  wpi::json toJson() {
+    wpi::json json;
+    for (auto& [key, val] : tagMap) {
+      json["tags"].push_back(val.toJson());
+    }
+    json["field"]["length"] = fieldLength;
+    json["field"]["width"] = fieldWidth;
+    return json;
+  }
 
   static double minimizeAngle(double angle) {
     angle = std::fmod(angle, 360);
@@ -54,6 +70,13 @@ class Fieldmap {
     return angle;
   }
 
+  void replaceTag(int tag_id, tag::Pose pose) {
+    tagMap.erase(tag_id);
+    tagMap.emplace(tag_id, pose);
+  }
+
  private:
-  std::vector<tag::Pose> tagVec;
+  double fieldLength;
+  double fieldWidth;
+  std::map<int, tag::Pose> tagMap;
 };

wpical/src/main/native/include/tagpose.h

@@ -6,15 +6,19 @@
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
+#include <wpi/json.h>
 
 namespace tag {
 class Pose {
  public:
-  Pose(double xpos, double ypos, double zpos, double w, double x, double y,
-       double z, double field_length_meters, double field_width_meters);
+  Pose(int tag_id, double xpos, double ypos, double zpos, double w, double x,
+       double y, double z, double field_length_meters,
+       double field_width_meters);
+  int tagId;
   double xPos, yPos, zPos, yawRot, rollRot, pitchRot;
   Eigen::Quaterniond quaternion;
   Eigen::Matrix3d rotationMatrix;
   Eigen::Matrix4d transformMatrixFmap;
+  wpi::json toJson();
 };
 }  // namespace tag

wpical/src/test/native/cpp/test_calibrate.cpp

@@ -19,6 +19,9 @@ cameracalibration::CameraModel cameraModel = {
     .intrinsic_matrix = Eigen::Matrix<double, 3, 3>::Identity(),
     .distortion_coefficients = Eigen::Matrix<double, 8, 1>::Zero(),
     .avg_reprojection_error = 0.0};
+
+wpi::json output_json;
+
 #ifdef __linux__
 const std::string fileSuffix = ".avi";
 const std::string videoLocation = "/altfieldvideo";
@@ -58,7 +61,7 @@ TEST(Camera_CalibrationTest, MRcal_Atypical) {
 
 TEST(Field_CalibrationTest, Typical) {
   int ret = fieldcalibration::calibrate(
-      projectRootPath + videoLocation, calSavePath,
+      projectRootPath + videoLocation, output_json,
       calSavePath + "/cameracalibration.json",
       projectRootPath + "/2024-crescendo.json", 3, false);
   EXPECT_EQ(ret, 0);
@@ -66,7 +69,7 @@ TEST(Field_CalibrationTest, Typical) {
 
 TEST(Field_CalibrationTest, Atypical_Bad_Camera_Model_Directory) {
   int ret = fieldcalibration::calibrate(
-      projectRootPath + videoLocation, calSavePath,
+      projectRootPath + videoLocation, output_json,
       projectRootPath + videoLocation + "/long" + fileSuffix,
       projectRootPath + "/2024-crescendo.json", 3, false);
   EXPECT_EQ(ret, 1);
@@ -74,7 +77,7 @@ TEST(Field_CalibrationTest, Atypical_Bad_Camera_Model_Directory) {
 
 TEST(Field_CalibrationTest, Atypical_Bad_Ideal_JSON) {
   int ret = fieldcalibration::calibrate(
-      projectRootPath + videoLocation, calSavePath,
+      projectRootPath + videoLocation, output_json,
       calSavePath + "/cameracalibration.json",
       calSavePath + "/cameracalibration.json", 3, false);
   EXPECT_EQ(ret, 1);
@@ -82,7 +85,7 @@ TEST(Field_CalibrationTest, Atypical_Bad_Ideal_JSON) {
 
 TEST(Field_CalibrationTest, Atypical_Bad_Input_Directory) {
   int ret = fieldcalibration::calibrate(
-      projectRootPath + "", calSavePath,
+      projectRootPath + "", output_json,
       calSavePath + "/cameracalibration.json",
       projectRootPath + "/2024-crescendo.json", 3, false);
   EXPECT_EQ(ret, 1);
@@ -90,7 +93,7 @@ TEST(Field_CalibrationTest, Atypical_Bad_Input_Directory) {
 
 TEST(Field_CalibrationTest, Atypical_Bad_Pinned_Tag) {
   int ret = fieldcalibration::calibrate(
-      projectRootPath + videoLocation, calSavePath,
+      projectRootPath + videoLocation, output_json,
       calSavePath + "/cameracalibration.json",
       projectRootPath + "/2024-crescendo.json", 42, false);
   EXPECT_EQ(ret, 1);
@@ -98,7 +101,7 @@ TEST(Field_CalibrationTest, Atypical_Bad_Pinned_Tag) {
 
 TEST(Field_CalibrationTest, Atypical_Bad_Pinned_Tag_Negative) {
   int ret = fieldcalibration::calibrate(
-      projectRootPath + videoLocation, calSavePath,
+      projectRootPath + videoLocation, output_json,
       calSavePath + "/cameracalibration.json",
       projectRootPath + "/2024-crescendo.json", -1, false);
   EXPECT_EQ(ret, 1);

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java

@@ -180,6 +180,9 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
    * using those requirements have been scheduled as interruptible. If this is the case, they will
    * be interrupted and the command will be scheduled.
    *
+   * <p>WARNING: using this function directly can often lead to unexpected behavior and should be
+   * avoided. Instead Triggers should be used to schedule Commands.
+   *
    * @param command the command to schedule. If null, no-op.
    */
   private void schedule(Command command) {
@@ -230,6 +233,9 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
   /**
    * Schedules multiple commands for execution. Does nothing for commands already scheduled.
    *
+   * <p>WARNING: using this function directly can often lead to unexpected behavior and should be
+   * avoided. Instead Triggers should be used to schedule Commands.
+   *
    * @param commands the commands to schedule. No-op on null.
    */
   public void schedule(Command... commands) {

wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h

@@ -88,6 +88,10 @@ class CommandScheduler final : public wpi::Sendable,
    * interruptible. If this is the case, they will be interrupted and the
    * command will be scheduled.
    *
+   * @warning Using this function directly can often lead to unexpected behavior
+   * and should be avoided. Instead Triggers should be used to schedule
+   * Commands.
+   *
    * @param command the command to schedule
    */
   void Schedule(const CommandPtr& command);
@@ -112,6 +116,10 @@ class CommandScheduler final : public wpi::Sendable,
    *
    * The pointer must remain valid through the entire lifecycle of the command.
    *
+   * @warning Using this function directly can often lead to unexpected behavior
+   * and should be avoided. Instead Triggers should be used to schedule
+   * Commands.
+   *
    * @param command the command to schedule
    */
   void Schedule(Command* command);
@@ -120,6 +128,10 @@ class CommandScheduler final : public wpi::Sendable,
    * Schedules multiple commands for execution. Does nothing for commands
    * already scheduled.
    *
+   * @warning Using this function directly can often lead to unexpected behavior
+   * and should be avoided. Instead Triggers should be used to schedule
+   * Commands.
+   *
    * @param commands the commands to schedule
    */
   void Schedule(std::span<Command* const> commands);
@@ -128,6 +140,10 @@ class CommandScheduler final : public wpi::Sendable,
    * Schedules multiple commands for execution. Does nothing for commands
    * already scheduled.
    *
+   * @warning Using this function directly can often lead to unexpected behavior
+   * and should be avoided. Instead Triggers should be used to schedule
+   * Commands.
+   *
    * @param commands the commands to schedule
    */
   void Schedule(std::initializer_list<Command*> commands);

wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java

@@ -117,11 +117,11 @@ public class Rotation2d
   public Rotation2d(double x, double y) {
     double magnitude = Math.hypot(x, y);
     if (magnitude > 1e-6) {
-      m_sin = y / magnitude;
       m_cos = x / magnitude;
+      m_sin = y / magnitude;
     } else {
-      m_sin = 0.0;
       m_cos = 1.0;
+      m_sin = 0.0;
       MathSharedStore.reportError(
           "x and y components of Rotation2d are zero\n", Thread.currentThread().getStackTrace());
     }

wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java

@@ -216,6 +216,17 @@ public class Translation3d
     return new Translation3d(qprime.getX(), qprime.getY(), qprime.getZ());
   }
 
+  /**
+   * Rotates this translation around another translation in 3D space.
+   *
+   * @param other The other translation to rotate around.
+   * @param rot The rotation to rotate the translation by.
+   * @return The new rotated translation.
+   */
+  public Translation3d rotateAround(Translation3d other, Rotation3d rot) {
+    return this.minus(other).rotateBy(rot).plus(other);
+  }
+
   /**
    * Returns a Translation2d representing this Translation3d projected into the X-Y plane.
    *

wpimath/src/main/native/include/frc/geometry/Rotation2d.h

@@ -55,11 +55,11 @@ class WPILIB_DLLEXPORT Rotation2d {
   constexpr Rotation2d(double x, double y) {
     double magnitude = gcem::hypot(x, y);
     if (magnitude > 1e-6) {
-      m_sin = y / magnitude;
       m_cos = x / magnitude;
+      m_sin = y / magnitude;
     } else {
-      m_sin = 0.0;
       m_cos = 1.0;
+      m_sin = 0.0;
       if (!std::is_constant_evaluated()) {
         wpi::math::MathSharedStore::ReportError(
             "x and y components of Rotation2d are zero\n{}",

wpimath/src/main/native/include/frc/geometry/Translation3d.h

@@ -148,6 +148,18 @@ class WPILIB_DLLEXPORT Translation3d {
                          units::meter_t{qprime.Z()}};
   }
 
+  /**
+   * Rotates this translation around another translation in 3D space.
+   *
+   * @param other The other translation to rotate around.
+   * @param rot The rotation to rotate the translation by.
+   * @return The new rotated translation.
+   */
+  constexpr Translation3d RotateAround(const Translation3d& other,
+                                       const Rotation3d& rot) const {
+    return (*this - other).RotateBy(rot) + other;
+  }
+
   /**
    * Returns a Translation2d representing this Translation3d projected into the
    * X-Y plane.

wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java

@@ -78,6 +78,40 @@ class Translation3dTest {
         () -> assertEquals(3.0, rotated3.getZ(), kEpsilon));
   }
 
+  @Test
+  void testRotateAround() {
+    var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
+    var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
+    var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var translation = new Translation3d(1.0, 2.0, 3.0);
+    var around = new Translation3d(3.0, 2.0, 1.0);
+
+    var rotated1 =
+        translation.rotateAround(around, new Rotation3d(xAxis, Units.degreesToRadians(90.0)));
+
+    assertAll(
+        () -> assertEquals(1.0, rotated1.getX(), kEpsilon),
+        () -> assertEquals(0.0, rotated1.getY(), kEpsilon),
+        () -> assertEquals(1.0, rotated1.getZ(), kEpsilon));
+
+    var rotated2 =
+        translation.rotateAround(around, new Rotation3d(yAxis, Units.degreesToRadians(90.0)));
+
+    assertAll(
+        () -> assertEquals(5.0, rotated2.getX(), kEpsilon),
+        () -> assertEquals(2.0, rotated2.getY(), kEpsilon),
+        () -> assertEquals(3.0, rotated2.getZ(), kEpsilon));
+
+    var rotated3 =
+        translation.rotateAround(around, new Rotation3d(zAxis, Units.degreesToRadians(90.0)));
+
+    assertAll(
+        () -> assertEquals(3.0, rotated3.getX(), kEpsilon),
+        () -> assertEquals(0.0, rotated3.getY(), kEpsilon),
+        () -> assertEquals(3.0, rotated3.getZ(), kEpsilon));
+  }
+
   @Test
   void testToTranslation2d() {
     var translation = new Translation3d(1.0, 2.0, 3.0);

wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp

@@ -35,7 +35,16 @@ TEST(Translation2dTest, RotateBy) {
   const auto rotated = another.RotateBy(90_deg);
 
   EXPECT_NEAR(0.0, rotated.X().value(), 1e-9);
-  EXPECT_DOUBLE_EQ(3.0, rotated.Y().value());
+  EXPECT_NEAR(3.0, rotated.Y().value(), 1e-9);
+}
+
+TEST(Translation2dTest, RotateAround) {
+  const Translation2d translation{2_m, 1_m};
+  const Translation2d other{3_m, 2_m};
+  const auto rotated = translation.RotateAround(other, 180_deg);
+
+  EXPECT_NEAR(4.0, rotated.X().value(), 1e-9);
+  EXPECT_NEAR(3.0, rotated.Y().value(), 1e-9);
 }
 
 TEST(Translation2dTest, Multiplication) {

wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp

@@ -57,6 +57,33 @@ TEST(Translation3dTest, RotateBy) {
   EXPECT_NEAR(rotated3.Z().value(), 3.0, kEpsilon);
 }
 
+TEST(Translation3dTest, RotateAround) {
+  Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
+  Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
+  Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+  const Translation3d translation{1_m, 2_m, 3_m};
+  const Translation3d around{3_m, 2_m, 1_m};
+
+  const auto rotated1 =
+      translation.RotateAround(around, Rotation3d{xAxis, 90_deg});
+  EXPECT_NEAR(rotated1.X().value(), 1.0, kEpsilon);
+  EXPECT_NEAR(rotated1.Y().value(), 0.0, kEpsilon);
+  EXPECT_NEAR(rotated1.Z().value(), 1.0, kEpsilon);
+
+  const auto rotated2 =
+      translation.RotateAround(around, Rotation3d{yAxis, 90_deg});
+  EXPECT_NEAR(rotated2.X().value(), 5.0, kEpsilon);
+  EXPECT_NEAR(rotated2.Y().value(), 2.0, kEpsilon);
+  EXPECT_NEAR(rotated2.Z().value(), 3.0, kEpsilon);
+
+  const auto rotated3 =
+      translation.RotateAround(around, Rotation3d{zAxis, 90_deg});
+  EXPECT_NEAR(rotated3.X().value(), 3.0, kEpsilon);
+  EXPECT_NEAR(rotated3.Y().value(), 0.0, kEpsilon);
+  EXPECT_NEAR(rotated3.Z().value(), 3.0, kEpsilon);
+}
+
 TEST(Translation3dTest, ToTranslation2d) {
   Translation3d translation{1_m, 2_m, 3_m};
   Translation2d expected{1_m, 2_m};

wpiutil/src/main/java/edu/wpi/first/util/RawFrame.java

@@ -18,6 +18,8 @@ public class RawFrame implements AutoCloseable {
   private int m_height;
   private int m_stride;
   private PixelFormat m_pixelFormat = PixelFormat.kUnknown;
+  private long m_time;
+  private TimestampSource m_timeSource = TimestampSource.kUnknown;
 
   /** Construct a new empty RawFrame. */
   public RawFrame() {
@@ -43,12 +45,15 @@ public class RawFrame implements AutoCloseable {
    * @param stride The number of bytes in each row of image data
    * @param pixelFormat The PixelFormat of the frame
    */
-  void setDataJNI(ByteBuffer data, int width, int height, int stride, int pixelFormat) {
+  void setDataJNI(
+      ByteBuffer data, int width, int height, int stride, int pixelFormat, long time, int timeSrc) {
     m_data = data;
     m_width = width;
     m_height = height;
     m_stride = stride;
     m_pixelFormat = PixelFormat.getFromInt(pixelFormat);
+    m_time = time;
+    m_timeSource = TimestampSource.getFromInt(timeSrc);
   }
 
   /**
@@ -59,11 +64,13 @@ public class RawFrame implements AutoCloseable {
    * @param stride The number of bytes in each row of image data
    * @param pixelFormat The PixelFormat of the frame
    */
-  void setInfoJNI(int width, int height, int stride, int pixelFormat) {
+  void setInfoJNI(int width, int height, int stride, int pixelFormat, long time, int timeSrc) {
     m_width = width;
     m_height = height;
     m_stride = stride;
     m_pixelFormat = PixelFormat.getFromInt(pixelFormat);
+    m_time = time;
+    m_timeSource = TimestampSource.getFromInt(timeSrc);
   }
 
   /**
@@ -110,6 +117,19 @@ public class RawFrame implements AutoCloseable {
         pixelFormat.getValue());
   }
 
+  /**
+   * Update this frame's timestamp info.
+   *
+   * @param frameTime the time this frame was grabbed at. This uses the same time base as
+   *     wpi::Now(), in us.
+   * @param frameTimeSource the time source for the timestamp this frame was grabbed at.
+   */
+  public void setTimeInfo(long frameTime, TimestampSource frameTimeSource) {
+    m_time = frameTime;
+    m_timeSource = frameTimeSource;
+    WPIUtilJNI.setRawFrameTime(m_nativeObj, frameTime, frameTimeSource.getValue());
+  }
+
   /**
    * Get the pointer to native representation of this frame.
    *
@@ -185,4 +205,22 @@ public class RawFrame implements AutoCloseable {
   public PixelFormat getPixelFormat() {
     return m_pixelFormat;
   }
+
+  /**
+   * Get the time this frame was grabbed at. This uses the same time base as wpi::Now(), in us.
+   *
+   * @return Time in 1 us increments.
+   */
+  public long getTimestamp() {
+    return m_time;
+  }
+
+  /**
+   * Get the time source for the timestamp this frame was grabbed at.
+   *
+   * @return Time source
+   */
+  public TimestampSource getTimestampSource() {
+    return m_timeSource;
+  }
 }

wpiutil/src/main/java/edu/wpi/first/util/TimestampSource.java

@@ -0,0 +1,51 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.util;
+
+/**
+ * Options for where the timestamp an {@link RawFrame} was captured at can be measured relative to.
+ */
+public enum TimestampSource {
+  /** unknown. */
+  kUnknown(0),
+  /**
+   * wpi::Now when the new frame was dequeued by CSCore. Does not account for camera exposure time
+   * or V4L latency.
+   */
+  kFrameDequeue(1),
+  /** End of Frame. Same as V4L2_BUF_FLAG_TSTAMP_SRC_EOF, translated into wpi::Now's timebase. */
+  kV4LEOF(2),
+  /**
+   * Start of Exposure. Same as V4L2_BUF_FLAG_TSTAMP_SRC_SOE, translated into wpi::Now's timebase.
+   */
+  kV4LSOE(3);
+
+  private final int value;
+
+  TimestampSource(int value) {
+    this.value = value;
+  }
+
+  /**
+   * Gets the integer value of the pixel format.
+   *
+   * @return Integer value
+   */
+  public int getValue() {
+    return value;
+  }
+
+  private static final TimestampSource[] s_values = values();
+
+  /**
+   * Gets a TimestampSource enum value from its integer value.
+   *
+   * @param timestampSource integer value
+   * @return Enum value
+   */
+  public static TimestampSource getFromInt(int timestampSource) {
+    return s_values[timestampSource];
+  }
+}

wpiutil/src/main/java/edu/wpi/first/util/WPIUtilJNI.java

@@ -175,6 +175,8 @@ public class WPIUtilJNI {
   static native void setRawFrameInfo(
       long frame, int size, int width, int height, int stride, int pixelFormat);
 
+  static native void setRawFrameTime(long frame, long timestamp, int timeSource);
+
   /**
    * Waits for a handle to be signaled.
    *

wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp

@@ -426,6 +426,24 @@ Java_edu_wpi_first_util_WPIUtilJNI_setRawFrameData
   f->pixelFormat = pixelFormat;
 }
 
+/*
+ * Class:     edu_wpi_first_util_WPIUtilJNI
+ * Method:    setRawFrameTime
+ * Signature: (JJI)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_util_WPIUtilJNI_setRawFrameTime
+  (JNIEnv* env, jclass, jlong frame, jlong time, jint timeSource)
+{
+  auto* f = reinterpret_cast<wpi::RawFrame*>(frame);
+  if (!f) {
+    wpi::ThrowNullPointerException(env, "frame is null");
+    return;
+  }
+  f->timestamp = time;
+  f->timestampSrc = timeSource;
+}
+
 /*
  * Class:     edu_wpi_first_util_WPIUtilJNI
  * Method:    setRawFrameInfo

wpiutil/src/main/native/include/wpi/RawFrame.h

@@ -34,13 +34,15 @@ typedef struct WPI_RawFrame {  // NOLINT
   uint8_t* data;
   // function to free image data (may be NULL)
   void (*freeFunc)(void* cbdata, void* data, size_t capacity);
-  void* freeCbData;  // data passed to freeFunc
-  size_t capacity;   // data buffer capacity, in bytes
-  size_t size;       // actual size of data, in bytes
-  int pixelFormat;   // WPI_PixelFormat
-  int width;         // width of image, in pixels
-  int height;        // height of image, in pixels
-  int stride;        // size of each row of data, in bytes (may be 0)
+  void* freeCbData;    // data passed to freeFunc
+  size_t capacity;     // data buffer capacity, in bytes
+  size_t size;         // actual size of data, in bytes
+  int pixelFormat;     // WPI_PixelFormat
+  int width;           // width of image, in pixels
+  int height;          // height of image, in pixels
+  int stride;          // size of each row of data, in bytes (may be 0)
+  uint64_t timestamp;  // image capture timestamp
+  int timestampSrc;    // WPI_TimestampSource
 } WPI_RawFrame;
 
 /**
@@ -58,6 +60,21 @@ enum WPI_PixelFormat {
   WPI_PIXFMT_BGRA,         // BGRA 8-8-8-8-, 32 bpp
 };
 
+/**
+ * Timestamp metadata. Timebase is the same as wpi::Now
+ */
+enum WPI_TimestampSource {
+  WPI_TIMESRC_UNKNOWN = 0,    // unknown
+  WPI_TIMESRC_FRAME_DEQUEUE,  // wpi::Now when the new frame was dequeued by
+                              // CSCore. Does not account for camera exposure
+                              // time or V4L latency.
+  WPI_TIMESRC_V4L_EOF,  // End of Frame. Same as V4L2_BUF_FLAG_TSTAMP_SRC_EOF,
+                        // translated into wpi::Now's timebase.
+  WPI_TIMESRC_V4L_SOE,  // Start of Exposure. Same as
+                        // V4L2_BUF_FLAG_TSTAMP_SRC_SOE, translated into
+                        // wpi::Now's timebase.
+};
+
 // Returns nonzero if the frame data was allocated/reallocated
 int WPI_AllocateRawFrameData(WPI_RawFrame* frame, size_t requestedSize);
 void WPI_FreeRawFrameData(WPI_RawFrame* frame);
@@ -82,6 +99,8 @@ struct RawFrame : public WPI_RawFrame {
     pixelFormat = WPI_PIXFMT_UNKNOWN;
     width = 0;
     height = 0;
+    timestamp = 0;
+    timestampSrc = WPI_TIMESRC_UNKNOWN;
   }
   RawFrame(const RawFrame&) = delete;
   RawFrame& operator=(const RawFrame&) = delete;
@@ -120,19 +139,23 @@ template <std::same_as<wpi::RawFrame> T>
 void SetFrameData(JNIEnv* env, jclass rawFrameCls, jobject jframe,
                   const T& frame, bool newData) {
   if (newData) {
-    static jmethodID setData = env->GetMethodID(rawFrameCls, "setDataJNI",
-                                                "(Ljava/nio/ByteBuffer;IIII)V");
+    static jmethodID setData = env->GetMethodID(
+        rawFrameCls, "setDataJNI", "(Ljava/nio/ByteBuffer;IIIIJI)V");
     env->CallVoidMethod(
         jframe, setData, env->NewDirectByteBuffer(frame.data, frame.size),
         static_cast<jint>(frame.width), static_cast<jint>(frame.height),
-        static_cast<jint>(frame.stride), static_cast<jint>(frame.pixelFormat));
+        static_cast<jint>(frame.stride), static_cast<jint>(frame.pixelFormat),
+        static_cast<jlong>(frame.timestamp),
+        static_cast<jint>(frame.timestampSrc));
   } else {
     static jmethodID setInfo =
-        env->GetMethodID(rawFrameCls, "setInfoJNI", "(IIII)V");
+        env->GetMethodID(rawFrameCls, "setInfoJNI", "(IIIIJI)V");
     env->CallVoidMethod(jframe, setInfo, static_cast<jint>(frame.width),
                         static_cast<jint>(frame.height),
                         static_cast<jint>(frame.stride),
-                        static_cast<jint>(frame.pixelFormat));
+                        static_cast<jint>(frame.pixelFormat),
+                        static_cast<jlong>(frame.timestamp),
+                        static_cast<jint>(frame.timestampSrc));
   }
 }
 #endif

wpiutil/src/main/native/include/wpi/Synchronization.h

@@ -407,7 +407,7 @@ class SignalObject final {
   }
   SignalObject& operator=(SignalObject&& rhs) {
     if (m_handle != 0) {
-      DestroySemaphore(m_handle);
+      DestroySignalObject(m_handle);
     }
     m_handle = rhs.m_handle;
     rhs.m_handle = 0;