Fix OpenCV load in simuated robot projects (#1001)

Add Camera Control tab to Cameras for the button to live in

photon-client/src/assets/styles/variables.scss

@@ -7,3 +7,14 @@ $heading-font-family: $default-font;
 .v-application {
   font-family: $default-font !important;
 }
+
+.v-row-group__header {
+  background: #005281 !important;
+}
+.theme--dark.v-data-table
+  > .v-data-table__wrapper
+  > table
+  > tbody
+  > tr:hover:not(.v-data-table__expanded__content):not(.v-data-table__empty-wrapper) {
+  background: #005281 !important;
+}

photon-client/src/components/cameras/CameraCalibrationCard.vue

@@ -218,7 +218,7 @@ const endCalibration = () => {
 
 <template>
   <div>
-    <v-card class="pr-6 pb-3" color="primary" dark>
+    <v-card class="mb-3 pr-6 pb-3" color="primary" dark>
       <v-card-title>Camera Calibration</v-card-title>
       <div class="ml-5">
         <v-row>

photon-client/src/components/cameras/CameraControlCard.vue

@@ -0,0 +1,202 @@
+<script setup lang="ts">
+import { ref } from "vue";
+import axios from "axios";
+import { useStateStore } from "@/stores/StateStore";
+
+interface SnapshotMetadata {
+  snapshotName: string;
+  cameraNickname: string;
+  streamType: "input" | "output";
+  timeCreated: Date;
+}
+const getSnapshotMetadataFromName = (snapshotName: string): SnapshotMetadata => {
+  snapshotName = snapshotName.replace(/\.[^/.]+$/, "");
+
+  const data = snapshotName.split("_");
+
+  const cameraName = data.slice(0, data.length - 2).join("_");
+  const streamType = data[data.length - 2] as "input" | "output";
+  const dateStr = data[data.length - 1];
+
+  const year = parseInt(dateStr.substring(0, 4), 10);
+  const month = parseInt(dateStr.substring(5, 7), 10) - 1; // Months are zero-based
+  const day = parseInt(dateStr.substring(8, 10), 10);
+  const hours = parseInt(dateStr.substring(11, 13), 10);
+  const minutes = parseInt(dateStr.substring(13, 15), 10);
+  const seconds = parseInt(dateStr.substring(15, 17), 10);
+  const milliseconds = parseInt(dateStr.substring(17), 10);
+
+  return {
+    snapshotName: snapshotName,
+    cameraNickname: cameraName,
+    streamType: streamType,
+    timeCreated: new Date(year, month, day, hours, minutes, seconds, milliseconds)
+  };
+};
+
+interface Snapshot {
+  index: number;
+  snapshotName: string;
+  snapshotShortName: string;
+  cameraUniqueName: string;
+  cameraNickname: string;
+  streamType: "input" | "output";
+  timeCreated: Date;
+  snapshotSrc: string;
+}
+const imgData = ref<Snapshot[]>([]);
+const fetchSnapshots = () => {
+  axios
+    .get("/utils/getImageSnapshots")
+    .then((response) => {
+      imgData.value = response.data.map(
+        (snapshotData: { snapshotName: string; cameraUniqueName: string; snapshotData: string }, index) => {
+          const metadata = getSnapshotMetadataFromName(snapshotData.snapshotName);
+
+          return {
+            index: index,
+            snapshotName: snapshotData.snapshotName,
+            snapshotShortName: metadata.snapshotName,
+            cameraUniqueName: snapshotData.cameraUniqueName,
+            cameraNickname: metadata.cameraNickname,
+            streamType: metadata.streamType,
+            timeCreated: metadata.timeCreated,
+            snapshotSrc: "data:image/jpg;base64," + snapshotData.snapshotData
+          };
+        }
+      );
+      showSnapshotViewerDialog.value = true;
+    })
+    .catch((error) => {
+      if (error.response) {
+        useStateStore().showSnackbarMessage({
+          color: "error",
+          message: error.response.data.text || error.response.data
+        });
+      } else if (error.request) {
+        useStateStore().showSnackbarMessage({
+          color: "error",
+          message: "Error while trying to process the request! The backend didn't respond."
+        });
+      } else {
+        useStateStore().showSnackbarMessage({
+          color: "error",
+          message: "An error occurred while trying to process the request."
+        });
+      }
+    });
+};
+const showSnapshotViewerDialog = ref(false);
+const expanded = ref([]);
+</script>
+
+<template>
+  <v-card dark class="pr-6 pb-3" style="background-color: #006492">
+    <v-card-title>Camera Control</v-card-title>
+    <v-row class="pl-6">
+      <v-col>
+        <v-btn color="secondary" @click="fetchSnapshots">
+          <v-icon left> mdi-folder </v-icon>
+          Show Saved Snapshots
+        </v-btn>
+      </v-col>
+    </v-row>
+    <v-dialog v-model="showSnapshotViewerDialog">
+      <v-card dark class="pt-3 pl-5 pr-5" color="primary" flat>
+        <v-card-title> View Saved Frame Snapshots </v-card-title>
+        <v-divider />
+        <v-card-text v-if="imgData.length === 0" style="font-size: 18px; font-weight: 600" class="pt-4">
+          There are no snapshots saved
+        </v-card-text>
+        <div v-else class="pb-2">
+          <v-data-table
+            v-model:expanded="expanded"
+            :headers="[
+              { text: 'Snapshot Name', value: 'snapshotShortName', sortable: false },
+              { text: 'Camera Unique Name', value: 'cameraUniqueName' },
+              { text: 'Camera Nickname', value: 'cameraNickname' },
+              { text: 'Stream Type', value: 'streamType' },
+              { text: 'Time Created', value: 'timeCreated' },
+              { text: 'Actions', value: 'actions', sortable: false }
+            ]"
+            :items="imgData"
+            group-by="cameraUniqueName"
+            class="elevation-0"
+            item-key="index"
+            show-expand
+            expand-icon="mdi-eye"
+          >
+            <template #expanded-item="{ headers, item }">
+              <td :colspan="headers.length">
+                <div style="display: flex; justify-content: center; width: 100%">
+                  <img :src="item.snapshotSrc" alt="snapshot-image" class="snapshot-preview pt-2 pb-2" />
+                </div>
+              </td>
+            </template>
+            <!-- eslint-disable-next-line vue/valid-v-slot-->
+            <template #item.actions="{ item }">
+              <div style="display: flex; justify-content: center">
+                <a :download="item.snapshotName" :href="item.snapshotSrc">
+                  <v-icon small> mdi-download </v-icon>
+                </a>
+              </div>
+            </template>
+          </v-data-table>
+          <span
+            >Snapshot Timestamps may be incorrect as they depend on when the coprocessor was last connected to the
+            internet</span
+          >
+        </div>
+      </v-card>
+    </v-dialog>
+  </v-card>
+</template>
+
+<style scoped lang="scss">
+.v-divider {
+  border-color: white !important;
+}
+.v-btn {
+  width: 100%;
+}
+.v-data-table {
+  text-align: center;
+  background-color: #006492 !important;
+
+  th,
+  td {
+    background-color: #005281 !important;
+    font-size: 1rem !important;
+  }
+
+  tbody :hover tr {
+    background-color: #005281 !important;
+  }
+
+  ::-webkit-scrollbar {
+    width: 0;
+    height: 0.55em;
+    border-radius: 5px;
+  }
+
+  ::-webkit-scrollbar-track {
+    -webkit-box-shadow: inset 0 0 6px rgba(0, 0, 0, 0.3);
+    border-radius: 10px;
+  }
+
+  ::-webkit-scrollbar-thumb {
+    background-color: #ffd843;
+    border-radius: 10px;
+  }
+}
+
+.snapshot-preview {
+  max-width: 55%;
+}
+
+@media only screen and (max-width: 512px) {
+  .snapshot-preview {
+    max-width: 100%;
+  }
+}
+</style>

photon-client/src/views/CameraSettingsView.vue

@@ -5,6 +5,7 @@ import { useCameraSettingsStore } from "@/stores/settings/CameraSettingsStore";
 import { computed } from "vue";
 import CamerasView from "@/components/cameras/CamerasView.vue";
 import { useStateStore } from "@/stores/StateStore";
+import CameraControlCard from "@/components/cameras/CameraControlCard.vue";
 
 const cameraViewType = computed<number[]>({
   get: (): number[] => {
@@ -40,6 +41,7 @@ const cameraViewType = computed<number[]>({
       <v-col cols="12" md="7">
         <CamerasCard />
         <CalibrationCard />
+        <CameraControlCard />
       </v-col>
       <v-col class="pl-md-3 pt-3 pt-md-0" cols="12" md="5">
         <CamerasView v-model="cameraViewType" />

photon-core/src/main/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumer.java

@@ -47,14 +47,16 @@ public class FileSaveFrameConsumer implements Consumer<CVMat> {
     private final String ntEntryName;
     private IntegerEntry saveFrameEntry;
 
+    private final String cameraUniqueName;
     private String cameraNickname;
     private final String streamType;
 
     private long savedImagesCount = 0;
 
-    public FileSaveFrameConsumer(String camNickname, String streamPrefix) {
+    public FileSaveFrameConsumer(String camNickname, String cameraUniqueName, String streamPrefix) {
         this.ntEntryName = streamPrefix + NT_SUFFIX;
         this.cameraNickname = camNickname;
+        this.cameraUniqueName = cameraUniqueName;
         this.streamType = streamPrefix;
 
         this.rootTable = NetworkTablesManager.getInstance().kRootTable;
@@ -74,7 +76,15 @@ public class FileSaveFrameConsumer implements Consumer<CVMat> {
 
                 String fileName =
                         cameraNickname + "_" + streamType + "_" + df.format(now) + "T" + tf.format(now);
-                String saveFilePath = FILE_PATH + File.separator + fileName + FILE_EXTENSION;
+
+                // Check if the Unique Camera directory exists and create it if it doesn't
+                String cameraPath = FILE_PATH + File.separator + this.cameraUniqueName;
+                var cameraDir = new File(cameraPath);
+                if (!cameraDir.exists()) {
+                    cameraDir.mkdir();
+                }
+
+                String saveFilePath = cameraPath + File.separator + fileName + FILE_EXTENSION;
 
                 Imgcodecs.imwrite(saveFilePath, image.getMat());
 

photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java

@@ -176,10 +176,15 @@ public class VisionModule {
         this.outputStreamPort = 1181 + (camStreamIdx * 2) + 1;
 
         inputFrameSaver =
-                new FileSaveFrameConsumer(visionSource.getSettables().getConfiguration().nickname, "input");
+                new FileSaveFrameConsumer(
+                        visionSource.getSettables().getConfiguration().nickname,
+                        visionSource.getSettables().getConfiguration().uniqueName,
+                        "input");
         outputFrameSaver =
                 new FileSaveFrameConsumer(
-                        visionSource.getSettables().getConfiguration().nickname, "output");
+                        visionSource.getSettables().getConfiguration().nickname,
+                        visionSource.getSettables().getConfiguration().uniqueName,
+                        "output");
 
         String camHostname = CameraServerJNI.getHostname();
         inputVideoStreamer =

photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java

@@ -100,7 +100,8 @@ public class PhotonPoseEstimator {
      * @param fieldTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs to Pose3d objects
      *     with respect to the FIRST field using the <a href=
      *     "https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#field-coordinate-system">Field
-     *     Coordinate System</a>.
+     *     Coordinate System</a>. Note that setting the origin of this layout object will affect the
+     *     results from this class.
      * @param strategy The strategy it should use to determine the best pose.
      * @param camera PhotonCamera
      * @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie,
@@ -141,6 +142,8 @@ public class PhotonPoseEstimator {
     /**
      * Get the AprilTagFieldLayout being used by the PositionEstimator.
      *
+     * <p>Note: Setting the origin of this layout will affect the results from this class.
+     *
      * @return the AprilTagFieldLayout
      */
     public AprilTagFieldLayout getFieldTags() {
@@ -150,6 +153,8 @@ public class PhotonPoseEstimator {
     /**
      * Set the AprilTagFieldLayout being used by the PositionEstimator.
      *
+     * <p>Note: Setting the origin of this layout will affect the results from this class.
+     *
      * @param fieldTags the AprilTagFieldLayout
      */
     public void setFieldTags(AprilTagFieldLayout fieldTags) {
@@ -415,6 +420,7 @@ public class PhotonPoseEstimator {
             var best =
                     new Pose3d()
                             .plus(best_tf) // field-to-camera
+                            .relativeTo(fieldTags.getOrigin())
                             .plus(robotToCamera.inverse()); // field-to-robot
             return Optional.of(
                     new EstimatedRobotPose(

photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java

@@ -33,13 +33,11 @@ import edu.wpi.first.cscore.VideoSource.ConnectionStrategy;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.util.CombinedRuntimeLoader;
 import edu.wpi.first.util.WPIUtilJNI;
 import java.util.ArrayList;
 import java.util.List;
 import java.util.Optional;
 import java.util.stream.Collectors;
-import org.opencv.core.Core;
 import org.opencv.core.CvType;
 import org.opencv.core.Mat;
 import org.opencv.core.Point;
@@ -95,11 +93,7 @@ public class PhotonCameraSim implements AutoCloseable {
     private boolean videoSimProcEnabled = true;
 
     static {
-        try {
-            CombinedRuntimeLoader.loadLibraries(OpenCVHelp.class, Core.NATIVE_LIBRARY_NAME, "cscorejni");
-        } catch (Exception e) {
-            throw new RuntimeException("Failed to load native libraries!", e);
-        }
+        OpenCVHelp.forceLoadOpenCV();
     }
 
     @Override

photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java

@@ -28,7 +28,6 @@ import edu.wpi.first.cscore.CvSource;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.util.CombinedRuntimeLoader;
 import java.awt.image.BufferedImage;
 import java.io.IOException;
 import java.util.ArrayList;
@@ -67,11 +66,7 @@ public class VideoSimUtil {
     private static double fieldWidth = 8.0137;
 
     static {
-        try {
-            CombinedRuntimeLoader.loadLibraries(OpenCVHelp.class, Core.NATIVE_LIBRARY_NAME, "cscorejni");
-        } catch (Exception e) {
-            throw new RuntimeException("Failed to load native libraries!", e);
-        }
+        OpenCVHelp.forceLoadOpenCV();
 
         // create Mats of 8x8 apriltag images
         for (int i = 0; i < VideoSimUtil.kNumTags16h5; i++) {

photon-server/src/main/java/org/photonvision/server/RequestHandler.java

@@ -21,15 +21,14 @@ import com.fasterxml.jackson.core.JsonProcessingException;
 import com.fasterxml.jackson.databind.ObjectMapper;
 import io.javalin.http.Context;
 import io.javalin.http.UploadedFile;
-import java.io.File;
-import java.io.FileInputStream;
-import java.io.FileNotFoundException;
-import java.io.FileOutputStream;
-import java.io.IOException;
+import java.io.*;
 import java.nio.file.Files;
 import java.nio.file.Path;
 import java.nio.file.Paths;
+import java.util.ArrayList;
+import java.util.HashMap;
 import java.util.Optional;
+import javax.imageio.ImageIO;
 import org.apache.commons.io.FileUtils;
 import org.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.configuration.NetworkConfig;
@@ -526,6 +525,43 @@ public class RequestHandler {
         ctx.status(204);
     }
 
+    public static void onImageSnapshotsRequest(Context ctx) {
+        var snapshots = new ArrayList<HashMap<String, Object>>();
+        var cameraDirs = ConfigManager.getInstance().getImageSavePath().toFile().listFiles();
+
+        if (cameraDirs != null) {
+            try {
+                for (File cameraDir : cameraDirs) {
+                    var cameraSnapshots = cameraDir.listFiles();
+                    if (cameraSnapshots == null) continue;
+
+                    String cameraUniqueName = cameraDir.getName();
+
+                    for (File snapshot : cameraSnapshots) {
+                        var snapshotData = new HashMap<String, Object>();
+
+                        var bufferedImage = ImageIO.read(snapshot);
+                        var buffer = new ByteArrayOutputStream();
+                        ImageIO.write(bufferedImage, "jpg", buffer);
+                        byte[] data = buffer.toByteArray();
+
+                        snapshotData.put("snapshotName", snapshot.getName());
+                        snapshotData.put("cameraUniqueName", cameraUniqueName);
+                        snapshotData.put("snapshotData", data);
+
+                        snapshots.add(snapshotData);
+                    }
+                }
+            } catch (IOException e) {
+                ctx.status(500);
+                ctx.result("Unable to read saved images");
+            }
+        }
+
+        ctx.status(200);
+        ctx.json(snapshots);
+    }
+
     /**
      * Create a temporary file using the UploadedFile from Javalin.
      *

photon-server/src/main/java/org/photonvision/server/Server.java

@@ -97,6 +97,7 @@ public class Server {
         app.post("/api/utils/restartProgram", RequestHandler::onProgramRestartRequest);
         app.post("/api/utils/restartDevice", RequestHandler::onDeviceRestartRequest);
         app.post("/api/utils/publishMetrics", RequestHandler::onMetricsPublishRequest);
+        app.get("/api/utils/getImageSnapshots", RequestHandler::onImageSnapshotsRequest);
 
         // Calibration
         app.post("/api/calibration/end", RequestHandler::onCalibrationEndRequest);

photon-targeting/build.gradle

@@ -8,6 +8,7 @@ apply from: "${rootDir}/shared/common.gradle"
 dependencies {
     implementation wpilibTools.deps.wpilibJava("wpimath")
     implementation wpilibTools.deps.wpilibJava("apriltag")
+    implementation wpilibTools.deps.wpilibJava("cscore")
     implementation wpilibTools.deps.wpilibOpenCvJava("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get())
 
     implementation group: "org.ejml", name: "ejml-simple", version: wpi.versions.ejmlVersion.get()

photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java

@@ -17,6 +17,7 @@
 
 package org.photonvision.estimation;
 
+import edu.wpi.first.cscore.CvSink;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.Num;
@@ -26,7 +27,6 @@ import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.numbers.*;
-import edu.wpi.first.util.CombinedRuntimeLoader;
 import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.List;
@@ -52,13 +52,17 @@ public final class OpenCVHelp {
     private static Rotation3d NWU_TO_EDN;
     private static Rotation3d EDN_TO_NWU;
 
-    static {
-        try {
-            CombinedRuntimeLoader.loadLibraries(OpenCVHelp.class, Core.NATIVE_LIBRARY_NAME, "cscorejni");
-        } catch (Exception e) {
-            throw new RuntimeException("Failed to load native libraries!", e);
-        }
+    // Creating a cscore object is sufficient to load opencv, per
+    // https://www.chiefdelphi.com/t/unsatisfied-link-error-when-simulating-java-robot-code-using-opencv/426731/4
+    private static CvSink dummySink = null;
 
+    public static void forceLoadOpenCV() {
+        if (dummySink != null) return;
+        dummySink = new CvSink("ignored");
+        dummySink.close();
+    }
+
+    static {
         NWU_TO_EDN = new Rotation3d(Matrix.mat(Nat.N3(), Nat.N3()).fill(0, -1, 0, 0, 0, -1, 1, 0, 0));
         EDN_TO_NWU = new Rotation3d(Matrix.mat(Nat.N3(), Nat.N3()).fill(0, 0, 1, -1, 0, 0, 0, -1, 0));
     }