feat(vision): add time synchronization with Raspberry Pi

src/main/java/frc/robot/Constants.java

@@ -483,5 +483,9 @@ public static class OceanViewConstants {
         public static final int DASHBOARD_PORT_NUMBER = 5000;
         public static final int UDP_PORT_NUMBER = 5400;
         public static final int TCP_PORT_NUMBER = 5300;
+
+        // Time Synchronization
+        public static final int TIME_SYNC_PORT_NUMBER = 6000;
+        public static final int DEFAULT_NUM_SAMPLES = 10;
     }
 }

src/main/java/frc/robot/network/TimeSyncClient.java

@@ -0,0 +1,99 @@
+package frc.robot.network;
+
+import java.net.*;
+import com.google.gson.Gson;
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * TimeSyncClient performs time synchronization with the Raspberry Pi using UDP.
+ * It sends multiple time sync requests using nanosecond timestamps (via System.nanoTime())
+ * and calculates the average clock offset and round-trip delay.
+ *
+ * The offset is computed using:
+ *   offset = ((T2 - T1) + (T3 - T4)) / 2
+ * where T1 is the send time and T4 is the receive time on the RoboRIO.
+ */
+public class TimeSyncClient {
+    private final String piIp;
+    private final int piPort;
+    private final int numSamples;
+    private final Gson gson;
+
+    /**
+     * Container for time sync response data from the Raspberry Pi.
+     * All times are in nanoseconds.
+     */
+    public static class TimeSyncResponse {
+        public long t2; // Pi receive time
+        public long t3; // Pi send time
+    }
+
+    /**
+     * Constructs a TimeSyncClient.
+     * 
+     * @param piIp      The IP address of the Raspberry Pi.
+     * @param piPort    The port number on which the Pi’s time sync server listens.
+     * @param numSamples The number of UDP requests to send for averaging.
+     */
+    public TimeSyncClient(String piIp, int piPort, int numSamples) {
+        this.piIp = piIp;
+        this.piPort = piPort;
+        this.numSamples = numSamples;
+        this.gson = new Gson();
+    }
+
+    /**
+     * Sends several UDP time sync requests and computes the average clock offset and round-trip delay.
+     * Timestamps are obtained in nanoseconds.
+     *
+     * @return a double array: [average offset (ns), average round-trip delay (ns)]
+     */
+    public double[] synchronizeTime() {
+        List<Long> offsets = new ArrayList<>();
+        List<Long> delays = new ArrayList<>();
+
+        try (DatagramSocket socket = new DatagramSocket()) {
+            socket.setSoTimeout(1000); // 1 second timeout
+            byte[] receiveBuffer = new byte[2048];
+
+            for (int i = 0; i < numSamples; i++) {
+                // Record send time T1 in nanoseconds
+                long t1 = System.nanoTime();
+
+                // Send a dummy time sync request
+                String request = "TIME_SYNC";
+                byte[] sendData = request.getBytes("UTF-8");
+                DatagramPacket sendPacket = new DatagramPacket(sendData, sendData.length, InetAddress.getByName(piIp), piPort);
+                socket.send(sendPacket);
+
+                // Wait for a response and record receive time T4 in nanoseconds
+                DatagramPacket receivePacket = new DatagramPacket(receiveBuffer, receiveBuffer.length);
+                socket.receive(receivePacket);
+                long t4 = System.nanoTime();
+
+                // Parse JSON response into TimeSyncResponse object
+                String jsonResponse = new String(receivePacket.getData(), 0, receivePacket.getLength(), "UTF-8");
+                TimeSyncResponse response = gson.fromJson(jsonResponse, TimeSyncResponse.class);
+
+                // Calculate round-trip delay and clock offset (ignoring processing time)
+                long delay = (t4 - t1) - (response.t3 - response.t2);
+                long offset = ((response.t2 - t1) + (response.t3 - t4)) / 2;
+
+                offsets.add(offset);
+                delays.add(delay);
+
+                // Wait briefly before next sample
+                Thread.sleep(50);
+            }
+        } catch (Exception e) {
+            e.printStackTrace();
+        }
+
+        // Compute averages
+        double avgOffset = offsets.stream().mapToLong(Long::longValue).average().orElse(0);
+        double avgDelay = delays.stream().mapToLong(Long::longValue).average().orElse(0);
+
+        return new double[] {avgOffset, avgDelay};
+    }
+}

src/main/java/frc/robot/subsystems/vision/OceanViewManager.java

@@ -7,6 +7,7 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.OceanViewConstants;
 import frc.robot.network.TCPSender;
+import frc.robot.network.TimeSyncClient;
 import frc.robot.network.UDPReceiver;
 import frc.robot.util.upper_assembly.ScoringHeight;
 import frc.robot.util.vision.ScoringLocation;
@@ -150,6 +151,21 @@ public class OceanViewManager extends SubsystemBase {
      */
     private final TCPSender tcpSender;
 
+    /**
+     * The server used to synchronize time between the raspberry PI and the RIO.
+     */
+    private final TimeSyncClient timeSyncClient;
+
+    /**
+     * The difference between the raspberry pi`s time and the RIO`s time, in nanoseconds.
+     */
+    private final double averageOffsetTimeNs;
+
+    /**
+     * The average latency between a round UDP trip, in nanoseconds.
+     */
+    private final double averageRoundTripTImeNs;
+
     /**
      *<p> The <strong>supplier method</strong> for getting the robots position. Used
      * to send the robot's position to the PI for operational use </p>
@@ -171,6 +187,12 @@ public OceanViewManager(UDPReceiver udpReceiver, TCPSender tcpSender, Supplier<P
         this.udpReceiver  = udpReceiver;
         this.tcpSender    = tcpSender;
         this.poseSupplier = poseSupplier;
+
+        // Synchronize Time
+        this.timeSyncClient = new TimeSyncClient(OceanViewConstants.PI_IP, OceanViewConstants.TIME_SYNC_PORT_NUMBER, OceanViewConstants.DEFAULT_NUM_SAMPLES);
+        double[] timeSyncData = this.timeSyncClient.synchronizeTime();
+        this.averageOffsetTimeNs = timeSyncData[0];
+        this.averageRoundTripTImeNs = timeSyncData[1];
     }
 
     // ------------------------------------------------------------------------
@@ -242,33 +264,43 @@ private void sendRobotPoseToPi() {
      * </p>
      */
     private void fetchDetectionData() {
-        // Retrieve a Map<String, Object> representing the entire JSON
+        // Retrieve the JSON data from the UDPReceiver as a Map.
         Map<String, Object> jsonData = udpReceiver.getTargetData();
-
-        // Clear old data from the previous cycle
+    
+        // Clear old data from the previous cycle.
         availableLocations.clear();
         algaeBlockedLocations.clear();
         algaePositions.clear();
-
-        // If there's no data, we can't parse anything
+
         if (jsonData == null || jsonData.isEmpty()) {
             System.out.println("[OceanViewManager] No data or empty JSON. Skipping parse.");
             return;
         }
-
-        // Parse each major JSON key
-        parseLocationArray(jsonData, "available",     availableLocations);
-        parseLocationArray(jsonData, "algae_blocked", algaeBlockedLocations);
+
+        // Retrieve the raw timestamp (in nanoseconds) from the JSON.
+        Object tsObj = jsonData.get("timestamp");
+        long packetTimestamp = (tsObj instanceof Number) ? ((Number) tsObj).longValue() : 0;
+
+        // Adjust the timestamp using the average offset computed from the time sync.
+        // For example, if averageOffsetTimeNs is the difference (RIO time - Pi time), then:
+        long adjustedTimestamp = packetTimestamp + (long) averageOffsetTimeNs;
+
+        // Optionally, print for debugging:
+        System.out.println("[OceanViewManager] Raw timestamp: " + packetTimestamp +
+                           ", Adjusted timestamp: " + adjustedTimestamp);
+
+        // Parse each major JSON key and pass the adjusted timestamp.
+        parseLocationArray(jsonData, "available", availableLocations, adjustedTimestamp);
+        parseLocationArray(jsonData, "algae_blocked", algaeBlockedLocations, adjustedTimestamp);
         parseAlgaePositions(jsonData, "algae_positions", algaePositions);
-
-        // Optional debug logs
+    
+        // Optional debug logs.
         System.out.println("[OceanViewManager] Data updated:");
         System.out.printf("  Available: %d, Blocked: %d, AlgaePts: %d%n",
             availableLocations.size(),
             algaeBlockedLocations.size(),
-            algaePositions.size()
-        );
-    }
+            algaePositions.size());
+    }    
 
     // ------------------------------------------------------------------------
     // JSON Parsing Helpers
@@ -292,53 +324,51 @@ private void fetchDetectionData() {
      * @param jsonData   The overall JSON map from the Pi.
      * @param key        The array key, e.g. "available" or "algae_blocked".
      * @param outputList The list to store parsed <code>ScoringLocation</code> objects.
+     * @param timestamp  The time the data was detected, in RIO time.
      */
     @SuppressWarnings("unchecked")
-    private void parseLocationArray(Map<String, Object> jsonData, String key, List<ScoringLocation> outputList) {
-
-        // Extract the raw array
+    private void parseLocationArray(Map<String, Object> jsonData, String key, List<ScoringLocation> outputList, long timestamp) {
+        // Extract the raw array.
         Object rawObj = jsonData.get(key);
         if (!(rawObj instanceof List)) {
-            return; // Key not present or not a list, skip
+            return; // Key not present or not a list, skip.
         }
-
         List<Object> rawList = (List<Object>) rawObj;
 
-        // Iterate over each element in the list
+        // Iterate over each element in the list.
         for (Object o : rawList) {
             if (!(o instanceof Map)) {
-                continue; // Not the expected structure, skip
+                continue; // Not the expected structure, skip.
             }
-
-            // Each element is a map with "branch", "level", and "position"
             Map<String, Object> locMap = (Map<String, Object>) o;
             String branch = safeGetString(locMap, "branch", "UnknownBranch");
             String level  = safeGetString(locMap, "level",  "UnknownLevel");
 
-            // Nested position
+            // Nested position map.
             Map<String, Object> posMap = (Map<String, Object>) locMap.get("position");
             if (posMap == null) {
-                continue; // If no position map is found, skip
+                continue; // If no position map is found, skip.
             }
 
-            // Extract x, y, z
+            // Extract x, y, z.
             double x = safeGetDouble(posMap, "x", 0.0);
             double y = safeGetDouble(posMap, "y", 0.0);
             double z = safeGetDouble(posMap, "z", 0.0);
 
-            // Apply exponential smoothing
+            // Apply exponential smoothing.
             String id = branch + "-" + level;
             Transform3d smoothed = smoothPosition(id, x, y, z);
 
-            // Add to outputList
-            ScoringLocation location = new ScoringLocation(branch, level, smoothed);
+            // Create ScoringLocation including the packet timestamp.
+            ScoringLocation location = new ScoringLocation(branch, level, smoothed, timestamp);
             outputList.add(location);
 
-            // Also store in rolling memory
+            // Store in rolling memory.
             storeInDetectionHistory(id, smoothed);
         }
     }
 
+
     /**
      * <p>
      * Parses an array of algae positions under "algae_positions", each of which 

src/main/java/frc/robot/util/vision/ScoringLocation.java

@@ -6,39 +6,39 @@
 
 /**
  * <p>
- * <strong>ScoringLocation</strong> is a data container representing a single
- * scoring position on the field. It has:
+ * ScoringLocation is a data container representing a single scoring position on the field.
+ * It contains:
  * </p>
  * <ul>
- * <li><code>branch</code>: e.g., "B1", "B2"</li>
- * <li><code>level</code>: e.g., "L2", "L3", "L4"</li>
- * <li><code>transform</code>: The 3D transform (position + orientation).</li>
+ *   <li><code>branch</code>: e.g., "B1", "B2"</li>
+ *   <li><code>level</code>: e.g., "L2", "L3", "L4"</li>
+ *   <li><code>transform</code>: The 3D transform specifying position (x, y, z) and rotation.</li>
+ *   <li><code>timestamp</code>: The time (in nanoseconds) when this detection data was received.</li>
  * </ul>
  * <hr>
  * @author Cameron Myhre
  * @since v1.2.0
  */
 public class ScoringLocation {
 
-    /** The branch name (e.g., "B1"). */
     public final String branch;
-    /** The level (e.g., "L2"). */
     public final String level;
-    /** The 3D transform specifying position (x, y, z) and rotation. */
     public final Transform3d transform;
+    public final long timestamp;
 
     /**
-     * Constructs a new ScoringLocation with the given identifiers and transform.
+     * Constructs a new ScoringLocation with the given identifiers, transform, and timestamp.
      *
      * @param branch    The branch identifier (e.g. "B1").
      * @param level     The level identifier (e.g. "L2").
-     * @param transform A 3D transform with (x, y, z) position (and optional
-     *                  rotation).
+     * @param transform A 3D transform with (x, y, z) position and rotation.
+     * @param timestamp The time (in nanoseconds) when the detection was received.
      */
-    public ScoringLocation(String branch, String level, Transform3d transform) {
+    public ScoringLocation(String branch, String level, Transform3d transform, long timestamp) {
         this.branch = branch;
         this.level = level;
         this.transform = transform;
+        this.timestamp = timestamp;
     }
 
     /**
@@ -63,15 +63,16 @@ public ScoringHeight getLevelAsEnum() {
      * Returns a string representation for debugging, e.g. "B1-L2 @ (1.00, 0.00,
      * 2.00)".
      *
-     * @return A simple string format describing this location.
+     * @return A formatted string describing this scoring location.
      */
     @Override
     public String toString() {
-        return String.format("%s-%s @ (%.2f, %.2f, %.2f)",
+        return String.format("%s-%s @ (%.2f, %.2f, %.2f) [ts=%d]",
                 branch,
                 level,
-                transform.getX(), //
+                transform.getX(), 
                 transform.getY(),
-                transform.getZ());
+                transform.getZ(),
+                timestamp);
     }
 }