Revert part of #288 (#306)

Fixes picam streaming/hangs

.github/workflows/main.yml

@@ -188,7 +188,7 @@ jobs:
       - run: |
           chmod +x gradlew
           ./gradlew photon-lib:build --max-workers 1
-      - run: ./gradlew photon-lib:publish
+      - run: ./gradlew photon-lib:publish photon-targeting:publish
         name: Publish
         env:
           ARTIFACTORY_API_KEY: ${{ secrets.ARTIFACTORY_API_KEY }}

build.gradle

@@ -41,5 +41,6 @@ spotless {
         target "**/*.java"
         licenseHeaderFile "$rootDir/LicenseHeader.txt"
         targetExclude("photon-core/src/main/java/org/photonvision/PhotonVersion.java")
+        targetExclude("photon-lib/src/main/java/org/photonvision/PhotonVersion.java")
     }
 }

photon-core/build.gradle

@@ -1,5 +1,4 @@
-import java.time.LocalDateTime
-import java.time.format.DateTimeFormatter
+import java.nio.file.Path
 
 apply from: "${rootDir}/shared/common.gradle"
 
@@ -30,23 +29,8 @@ dependencies {
 }
 
 task writeCurrentVersionJava {
-    String date = DateTimeFormatter.ofPattern("yyyy-M-d hh:mm:ss").format(LocalDateTime.now())
-    File versionFile = new File(java.nio.file.Path.of("$projectDir", "src", "main", "java", "org", "photonvision", "PhotonVersion.java")
-            .toAbsolutePath().toString())
-    versionFile.delete()
-    versionFile << "package org.photonvision;\n" +
-            "\n" +
-            "/*\n" +
-            " * Autogenerated file! Do not manually edit this file. This version is regenerated\n" +
-            " * any time the publish task is run, or when this file is deleted.\n" +
-            " */\n" +
-            "\n" +
-            "@SuppressWarnings(\"ALL\")\n" +
-            "public final class PhotonVersion {\n" +
-            "    public static final String versionString = \"${versionString}\";\n" +
-            "    public static final String buildDate = \"${date}\";\n" +
-            "    public static final boolean isRelease = !versionString.startsWith(\"dev\");\n" +
-            "}"
+    writePhotonVersionFile(Path.of("$projectDir", "src", "main", "java", "org", "photonvision", "PhotonVersion.java"),
+            versionString)
 }
 
 build.dependsOn writeCurrentVersionJava

photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java

@@ -21,6 +21,7 @@ import edu.wpi.first.networktables.LogMessage;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import java.util.function.Consumer;
+import org.photonvision.PhotonVersion;
 import org.photonvision.common.configuration.NetworkConfig;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
@@ -62,16 +63,23 @@ public class NetworkTablesManager {
                 hasReportedConnectionFailure = false;
                 lastConnectMessageMillis = System.currentTimeMillis();
                 ScriptManager.queueEvent(ScriptEventType.kNTConnected);
+                getInstance().broadcastVersion();
             }
         }
     }
 
+    private void broadcastVersion() {
+        kRootTable.getEntry("version").setString(PhotonVersion.versionString);
+        kRootTable.getEntry("buildDate").setString(PhotonVersion.buildDate);
+    }
+
     public void setConfig(NetworkConfig config) {
         if (config.runNTServer) {
             setServerMode();
         } else {
             setClientMode(config.teamNumber);
         }
+        broadcastVersion();
     }
 
     private void setClientMode(int teamNumber) {
@@ -86,11 +94,13 @@ public class NetworkTablesManager {
             logger.error(
                     "[NetworkTablesManager] Could not connect to the robot! Will retry in the background...");
         }
+        broadcastVersion();
     }
 
     private void setServerMode() {
         logger.info("Starting NT Server");
         ntInstance.stopClient();
         ntInstance.startServer();
+        broadcastVersion();
     }
 }

photon-core/src/main/java/org/photonvision/vision/pipe/impl/GroupContoursPipe.java

@@ -46,7 +46,7 @@ public class GroupContoursPipe
         } else {
             int groupingCount = params.getGroup().count;
 
-            if (input.size() > groupingCount) {
+            if (input.size() >= groupingCount) {
                 input.sort(Contour.SortByMomentsX);
                 // also why reverse? shouldn't the sort comparator just get reversed?
                 // TODO: Matt, see this

photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java

@@ -221,8 +221,10 @@ public class ColoredShapePipeline
                 // If we grabbed it (in color copy mode), make a new Mat of it
                 rawInputMat = new Mat(inputMatPtr);
             } else {
-                // Otherwise, use a blank/empty mat as placeholder
-                rawInputMat = new Mat();
+                //                // Otherwise, use a blank/empty mat as placeholder
+                //                rawInputMat = new Mat();
+                // Otherwise, the input mat is frame we got from the camera
+                rawInputMat = frame.image.getMat();
             }
 
             // We can skip a few steps if the image is single channel because we've already done them on

photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java

@@ -164,8 +164,10 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
                 // If we grabbed it (in color copy mode), make a new Mat of it
                 rawInputMat = new Mat(inputMatPtr);
             } else {
-                // Otherwise, use a blank/empty mat as placeholder
-                rawInputMat = new Mat();
+                // Otherwise, the input mat is frame we got from the camera
+                rawInputMat = frame.image.getMat();
+                //                // Otherwise, use a blank/empty mat as placeholder
+                //                rawInputMat = new Mat();
             }
 
             // We can skip a few steps if the image is single channel because we've already done them on

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

@@ -19,7 +19,6 @@ package org.photonvision.vision.processes;
 
 import java.util.*;
 import java.util.stream.Collectors;
-import org.photonvision.common.configuration.CameraConfiguration;
 
 /** VisionModuleManager has many VisionModules, and provides camera configuration data to them. */
 public class VisionModuleManager {
@@ -54,9 +53,9 @@ public class VisionModuleManager {
     public List<VisionModule> addSources(List<VisionSource> visionSources) {
         var addedModules = new HashMap<Integer, VisionModule>();
 
+        assignCameraIndex(visionSources);
         for (var visionSource : visionSources) {
             var pipelineManager = new PipelineManager(visionSource.getCameraConfiguration());
-            assignCameraIndex(visionSource.getCameraConfiguration());
 
             var module = new VisionModule(pipelineManager, visionSource, visionModules.size());
             visionModules.add(module);
@@ -72,16 +71,30 @@ public class VisionModuleManager {
         return sortedModulesList;
     }
 
-    private void assignCameraIndex(CameraConfiguration config) {
-        var max =
-                visionModules.stream()
-                        .mapToInt(it -> it.visionSource.getCameraConfiguration().streamIndex)
-                        .max()
-                        .orElse(-1);
+    private void assignCameraIndex(List<VisionSource> config) {
+        // We won't necessarily have already added all of the cameras we need to at this point
+        // But by operating on the list, we have a fairly good idea of which we need to change
+        // but it's not guaranteed that we change the correct one
+        // The best we can do is try to avoid a case where the stream index runs away to infinity
+        // since we can only stream 5 cameras at once
 
-        // If the current stream index is reserved, increase by 1
-        if (config.streamIndex <= max) {
-            config.streamIndex = max + 1;
+        for (var v : config) {
+            var listNoV = new ArrayList<>(config);
+            listNoV.remove(v);
+            if (listNoV.stream()
+                    .anyMatch(
+                            it ->
+                                    it.getCameraConfiguration().streamIndex
+                                            == v.getCameraConfiguration().streamIndex)) {
+                int idx = 0;
+                while (listNoV.stream()
+                        .map(it -> it.getCameraConfiguration().streamIndex)
+                        .collect(Collectors.toList())
+                        .contains(idx)) {
+                    idx++;
+                }
+                v.getCameraConfiguration().streamIndex = idx;
+            }
         }
     }
 }

photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java

@@ -17,10 +17,14 @@
 
 package org.photonvision.vision.processes;
 
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
 import edu.wpi.cscore.VideoMode;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import java.util.Arrays;
 import java.util.HashMap;
 import java.util.List;
+import java.util.stream.Collectors;
 import org.junit.jupiter.api.*;
 import org.photonvision.common.configuration.CameraConfiguration;
 import org.photonvision.common.configuration.ConfigManager;
@@ -131,6 +135,52 @@ public class VisionModuleManagerTest {
         printTestResults(module0DataConsumer.result);
     }
 
+    @Test
+    public void testMultipleStreamIndex() {
+        ConfigManager.getInstance().load();
+
+        var conf = new CameraConfiguration("Foo", "Bar");
+        conf.streamIndex = 1;
+        var ffp =
+                new FileFrameProvider(
+                        TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes, false),
+                        TestUtils.WPI2019Image.FOV);
+        var testSource = new TestSource(ffp, conf);
+
+        var conf2 = new CameraConfiguration("Foo2", "Bar");
+        conf2.streamIndex = 0;
+        var ffp2 =
+                new FileFrameProvider(
+                        TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes, false),
+                        TestUtils.WPI2019Image.FOV);
+        var testSource2 = new TestSource(ffp2, conf2);
+
+        var conf3 = new CameraConfiguration("Foo3", "Bar");
+        conf3.streamIndex = 0;
+        var ffp3 =
+                new FileFrameProvider(
+                        TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes, false),
+                        TestUtils.WPI2019Image.FOV);
+        var testSource3 = new TestSource(ffp3, conf3);
+
+        var modules =
+                VisionModuleManager.getInstance().addSources(List.of(testSource, testSource2, testSource3));
+
+        System.out.println(
+                Arrays.toString(
+                        modules.stream()
+                                .map(it -> it.visionSource.getCameraConfiguration().streamIndex)
+                                .collect(Collectors.toList())
+                                .toArray()));
+        var idxs =
+                modules.stream()
+                        .map(it -> it.visionSource.getCameraConfiguration().streamIndex)
+                        .collect(Collectors.toList());
+        assertTrue(idxs.contains(0));
+        assertTrue(idxs.contains(1));
+        assertTrue(idxs.contains(2));
+    }
+
     private static void printTestResults(CVPipelineResult pipelineResult) {
         double fps = 1000 / pipelineResult.getLatencyMillis();
         System.out.print(

photon-lib/build.gradle

@@ -1,14 +1,19 @@
+import java.nio.file.Path
+
 apply plugin: "cpp"
 apply plugin: "java"
 apply plugin: "google-test-test-suite"
 apply plugin: "edu.wpi.first.NativeUtils"
 
 apply from: "${rootDir}/shared/config.gradle"
+apply from: "${rootDir}/versioningHelper.gradle"
 
 test {
     useJUnitPlatform()
 }
 
+def jniPlatforms = ['linuxaarch64bionic', 'linuxraspbian', 'linuxx86-64', 'osxx86-64', 'windowsx86-64']
+
 // Apply Java configuration
 dependencies {
     compile project(":photon-targeting")
@@ -19,16 +24,15 @@ dependencies {
     implementation "edu.wpi.first.wpilibj:wpilibj-java:$wpilibVersion"
     implementation "edu.wpi.first.wpiutil:wpiutil-java:$wpilibVersion"
     implementation "edu.wpi.first.wpimath:wpimath-java:$wpilibVersion"
-    implementation "edu.wpi.first.hal:hal-java:$wpilibVersion"
     implementation "edu.wpi.first.thirdparty.frc2020.opencv:opencv-java:3.4.7-2"
 
     // NTCore
     implementation "edu.wpi.first.ntcore:ntcore-java:$wpilibVersion"
-    compile "edu.wpi.first.ntcore:ntcore-jni:$wpilibVersion:linuxaarch64bionic"
-    compile "edu.wpi.first.ntcore:ntcore-jni:$wpilibVersion:linuxraspbian"
-    compile "edu.wpi.first.ntcore:ntcore-jni:$wpilibVersion:linuxx86-64"
-    compile "edu.wpi.first.ntcore:ntcore-jni:$wpilibVersion:osxx86-64"
-    compile "edu.wpi.first.ntcore:ntcore-jni:$wpilibVersion:windowsx86-64"
+    jniPlatforms.each {  compile "edu.wpi.first.ntcore:ntcore-jni:$wpilibVersion:$it" }
+
+    // HAL
+    implementation "edu.wpi.first.hal:hal-java:$wpilibVersion"
+    jniPlatforms.each {compile "edu.wpi.first.hal:hal-jni:$wpilibVersion:$it"}
 
     // Junit
     testImplementation("org.junit.jupiter:junit-jupiter-api:5.6.2")
@@ -102,4 +106,12 @@ task generateVendorJson() {
 
 build.dependsOn generateVendorJson
 
+
+task writeCurrentVersionJava {
+    writePhotonVersionFile(Path.of("$projectDir", "src", "main", "java", "org", "photonvision", "PhotonVersion.java"),
+            versionString)
+}
+
+build.dependsOn writeCurrentVersionJava
+
 apply from: "publish.gradle"

photon-lib/publish.gradle

@@ -141,6 +141,7 @@ publishing {
                 username 'ghactions'
                 password System.getenv("ARTIFACTORY_API_KEY")
             }
+            println("Publishing to " + url)
         }
     }
 }

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

@@ -20,6 +20,7 @@ package org.photonvision;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.DriverStation;
 import org.photonvision.common.dataflow.structures.Packet;
 import org.photonvision.common.hardware.VisionLEDMode;
 import org.photonvision.targeting.PhotonPipelineResult;
@@ -32,8 +33,10 @@ public class PhotonCamera {
     final NetworkTableEntry outputSaveImgEntry;
     final NetworkTableEntry pipelineIndexEntry;
     final NetworkTableEntry ledModeEntry;
+    final NetworkTableEntry versionEntry;
 
     final NetworkTable mainTable = NetworkTableInstance.getDefault().getTable("photonvision");
+    private final String path;
 
     boolean driverMode;
     int pipelineIndex;
@@ -47,12 +50,14 @@ public class PhotonCamera {
     * @param rootTable The root table that the camera is broadcasting information over.
     */
     public PhotonCamera(NetworkTable rootTable) {
+        path = rootTable.getPath();
         rawBytesEntry = rootTable.getEntry("rawBytes");
         driverModeEntry = rootTable.getEntry("driverMode");
         inputSaveImgEntry = rootTable.getEntry("inputSaveImgCmd");
         outputSaveImgEntry = rootTable.getEntry("outputSaveImgCmd");
         pipelineIndexEntry = rootTable.getEntry("pipelineIndex");
         ledModeEntry = mainTable.getEntry("ledMode");
+        versionEntry = mainTable.getEntry("version");
 
         driverMode = driverModeEntry.getBoolean(false);
         pipelineIndex = pipelineIndexEntry.getNumber(0).intValue();
@@ -74,6 +79,8 @@ public class PhotonCamera {
     * @return The latest pipeline result.
     */
     public PhotonPipelineResult getLatestResult() {
+        verifyVersion();
+
         // Clear the packet.
         packet.clear();
 
@@ -199,4 +206,20 @@ public class PhotonCamera {
     public boolean hasTargets() {
         return getLatestResult().hasTargets();
     }
+
+    private void verifyVersion() {
+        String versionString = versionEntry.getString("");
+        if (versionString.equals("")) {
+            DriverStation.reportError(
+                    "PhotonVision coprocessor at path " + path + " not found on NetworkTables!", true);
+        } else if (!PhotonVersion.versionMatches(versionString)) {
+            DriverStation.reportError(
+                    "Photon version "
+                            + PhotonVersion.versionString
+                            + " does not match coprocessor version "
+                            + versionString
+                            + "!",
+                    true);
+        }
+    }
 }

photon-lib/src/test/java/org/photonvision/PacketTest.java

@@ -95,4 +95,29 @@ class PacketTest {
 
         Assertions.assertEquals(t, target);
     }
+
+    @Test
+    void testPacketv2021_1_6() {
+        // From v2021.1.6
+        var simplified =
+                new PhotonPipelineResult(
+                        12.34,
+                        List.of(
+                                new PhotonTrackedTarget(
+                                        -23, -10, 6, 1, new Transform2d(new Translation2d(1, 2), new Rotation2d(3)))));
+        byte[] bytes = {
+            64, 40, -82, 20, 122, -31, 71, -82, 1, -64, 55, 0, 0, 0, 0, 0, 0, -64, 36, 0, 0, 0, 0, 0, 0,
+            64, 24, 0, 0, 0, 0, 0, 0, 63, -16, 0, 0, 0, 0, 0, 0, 63, -16, 0, 0, 0, 0, 0, 0, 64, 0, 0, 0,
+            0, 0, 0, 0, 64, 101, 124, 101, 19, -54, -47, 122, 0
+        };
+
+        // Let's check that those bytes still mean the same thing
+        Packet packet = new Packet(1);
+        packet.clear();
+        packet.setData(bytes);
+        var ret = new PhotonPipelineResult();
+        ret.createFromPacket(packet);
+        System.out.println(ret);
+        Assertions.assertEquals(simplified, ret);
+    }
 }

photon-lib/src/test/java/org/photonvision/PhotonVersionTest.java

@@ -0,0 +1,52 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision;
+
+import java.util.regex.Matcher;
+import java.util.regex.Pattern;
+import org.junit.jupiter.api.Assertions;
+import org.junit.jupiter.api.Test;
+
+public class PhotonVersionTest {
+    public static final boolean versionMatches(String versionString, String other) {
+        String c = versionString;
+        Pattern p = Pattern.compile("v[0-9]+.[0-9]+.[0-9]+");
+        Matcher m = p.matcher(c);
+        if (m.find()) {
+            c = m.group(0);
+        } else {
+            return false;
+        }
+        m = p.matcher(other);
+        if (m.find()) {
+            other = m.group(0);
+        } else {
+            return false;
+        }
+        return c.equals(other);
+    }
+
+    @Test
+    public void testVersion() {
+        Assertions.assertTrue(versionMatches("v2021.1.6", "v2021.1.6"));
+        Assertions.assertTrue(versionMatches("dev-v2021.1.6", "v2021.1.6"));
+        Assertions.assertTrue(versionMatches("dev-v2021.1.6-5-gca49ea50", "v2021.1.6"));
+        Assertions.assertFalse(versionMatches("", "v2021.1.6"));
+        Assertions.assertFalse(versionMatches("v2021.1.6", ""));
+    }
+}

shared/PhotonVersion.java.in

@@ -0,0 +1,51 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision;
+
+/*
+* Autogenerated file! Do not manually edit this file. This version is regenerated
+* any time the publish task is run, or when this file is deleted.
+*/
+
+import java.util.regex.Matcher;
+import java.util.regex.Pattern;
+
+@SuppressWarnings("ALL")
+public final class PhotonVersion {
+    public static final String versionString = "${version}";
+    public static final String buildDate = "${date}";
+    public static final boolean isRelease = !versionString.startsWith("dev");
+
+    public static final boolean versionMatches(String other) {
+        String c = versionString;
+        Pattern p = Pattern.compile("v[0-9]+.[0-9]+.[0-9]+");
+        Matcher m = p.matcher(c);
+        if (m.find()) {
+            c = m.group(0);
+        } else {
+            return false;
+        }
+        m = p.matcher(other);
+        if (m.find()) {
+            other = m.group(0);
+        } else {
+            return false;
+        }
+        return c.equals(other);
+    }
+}

shared/common.gradle

@@ -1,3 +1,7 @@
+import java.nio.file.Path
+import java.time.LocalDateTime
+import java.time.format.DateTimeFormatter
+
 // Plugins
 apply plugin: "jacoco"
 apply plugin: "java"

versioningHelper.gradle

@@ -1,6 +1,6 @@
+import java.nio.file.Path
 import java.time.LocalDateTime
 import java.time.format.DateTimeFormatter
-import java.nio.file.Path
 
 gradle.allprojects {
     ext.getCurrentVersion = { ->
@@ -12,16 +12,28 @@ gradle.allprojects {
                 standardOutput = stdout
             }
             tagIsh = stdout.toString().trim().toLowerCase()
-        } catch(Exception e) {
+        } catch (Exception e) {
             tagIsh = "dev-Unknown"
         }
-//        boolean isDev = tagIsh.matches(".*-[0-9]*-g[0-9a-f]*")
-//        if(isDev) tagIsh = "dev-" + tagIsh
+        // Dev tags: v2021.1.6-3-gf922466d
+        // We're specifically looking to capture the middle -3-
+        boolean isDev = tagIsh.matches(".*-[0-9]*-g[0-9a-f]*")
+        if (isDev && !tagIsh.startsWith("dev-")) tagIsh = "dev-" + tagIsh
         println("Picked up version: " + tagIsh)
         return tagIsh
     }
 
-    if(!ext.has("versionString")) {
+    if (!ext.has("versionString")) {
         ext.versionString = getCurrentVersion()
     }
+
+    ext.writePhotonVersionFile = { Path path, String version ->
+        println("Writing " + version + " to " + path.toAbsolutePath().toString())
+        String date = DateTimeFormatter.ofPattern("yyyy-M-d hh:mm:ss").format(LocalDateTime.now())
+        File versionFileOut = new File(path.toAbsolutePath().toString())
+        versionFileOut.delete()
+        def versionFileIn = file("${rootDir}/shared/PhotonVersion.java.in")
+        def read = versionFileIn.text.replace('${version}', version).replace('${date}', date)
+        versionFileOut.write(read)
+    }
 }