Update Java Simulation Examples

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

@@ -66,6 +66,8 @@ public class VisionSystemSim {
 
     private final Field2d dbgField;
 
+    private final Transform3d kEmptyTrf = new Transform3d();
+
     /**
      * A simulated vision system involving a camera(s) and coprocessor(s) mounted on a mobile robot
      * running PhotonVision, detecting targets placed on the field. {@link VisionTargetSim}s added to
@@ -337,7 +339,7 @@ public Field2d getDebugField() {
      * Periodic update. Ensure this is called repeatedly-- camera performance is used to automatically
      * determine if a new frame should be submitted.
      *
-     * @param robotPoseMeters The current robot pose in meters
+     * @param robotPoseMeters The simulated robot pose in meters
      */
     public void update(Pose2d robotPoseMeters) {
         update(new Pose3d(robotPoseMeters));
@@ -347,7 +349,7 @@ public void update(Pose2d robotPoseMeters) {
      * Periodic update. Ensure this is called repeatedly-- camera performance is used to automatically
      * determine if a new frame should be submitted.
      *
-     * @param robotPoseMeters The current robot pose in meters
+     * @param robotPoseMeters The simulated robot pose in meters
      */
     public void update(Pose3d robotPoseMeters) {
         var targetTypes = targetSets.entrySet();
@@ -370,13 +372,15 @@ public void update(Pose3d robotPoseMeters) {
 
         var allTargets = new ArrayList<VisionTargetSim>();
         targetTypes.forEach((entry) -> allTargets.addAll(entry.getValue()));
-        var visibleTargets = new ArrayList<Pose3d>();
-        var cameraPose2ds = new ArrayList<Pose2d>();
+        var visTgtPoses2d = new ArrayList<Pose2d>();
+        var cameraPoses2d = new ArrayList<Pose2d>();
+        boolean processed = false;
         // process each camera
         for (var camSim : camSimMap.values()) {
             // check if this camera is ready to process and get latency
             var optTimestamp = camSim.consumeNextEntryTime();
             if (optTimestamp.isEmpty()) continue;
+            else processed = true;
             // when this result "was" read by NT
             long timestampNT = optTimestamp.get();
             // this result's processing latency in milliseconds
@@ -387,22 +391,20 @@ public void update(Pose3d robotPoseMeters) {
             // use camera pose from the image capture timestamp
             Pose3d lateRobotPose = getRobotPose(timestampCapture);
             Pose3d lateCameraPose = lateRobotPose.plus(getRobotToCamera(camSim, timestampCapture).get());
-            cameraPose2ds.add(lateCameraPose.toPose2d());
+            cameraPoses2d.add(lateCameraPose.toPose2d());
 
             // process a PhotonPipelineResult with visible targets
             var camResult = camSim.process(latencyMillis, lateCameraPose, allTargets);
             // publish this info to NT at estimated timestamp of receive
             camSim.submitProcessedFrame(camResult, timestampNT);
             // display debug results
             for (var target : camResult.getTargets()) {
-                visibleTargets.add(lateCameraPose.transformBy(target.getBestCameraToTarget()));
+                var trf = target.getBestCameraToTarget();
+                if (trf.equals(kEmptyTrf)) continue;
+                visTgtPoses2d.add(lateCameraPose.transformBy(trf).toPose2d());
             }
         }
-        if (visibleTargets.size() != 0) {
-            dbgField
-                    .getObject("visibleTargetPoses")
-                    .setPoses(visibleTargets.stream().map(Pose3d::toPose2d).collect(Collectors.toList()));
-        }
-        if (cameraPose2ds.size() != 0) dbgField.getObject("cameras").setPoses(cameraPose2ds);
+        if (processed) dbgField.getObject("visibleTargetPoses").setPoses(visTgtPoses2d);
+        if (cameraPoses2d.size() != 0) dbgField.getObject("cameras").setPoses(cameraPoses2d);
     }
 }

photonlib-cpp-examples/README.md

@@ -0,0 +1,7 @@
+## PhotonLib C++ Examples
+
+### Running examples
+
+For instructions on how to run these examples locally, see [Running Examples](https://docs.photonvision.org/en/latest/docs/contributing/photonvision/build-instructions.html#running-examples).
+
+---

photonlib-cpp-examples/build.gradle

@@ -27,9 +27,11 @@ spotless {
     }
 }
 
+apply from: "examples.gradle"
+
 // Task that depends on the build task for every example
 task buildAllExamples { task ->
-    new File('examples.txt').eachLine { line ->
+    exampleFolderNames.each { line ->
         task.dependsOn(line + ":build")
     }
 }

photonlib-cpp-examples/examples.gradle

@@ -0,0 +1,14 @@
+// These should be the only 2 non-project subdirectories in the examples folder
+// I could check for (it)/build.gradle to exist, but w/e
+def EXCLUDED_DIRS = ["bin", "build"]
+
+// List all non-hidden directories not in EXCUDED_DIRS
+ext.exampleFolderNames = file("${rootDir}")
+    .listFiles()
+    .findAll {
+        return (it.isDirectory()
+            && !it.isHidden()
+            && !(it.name in EXCLUDED_DIRS) && !it.name.startsWith(".")
+            && it.toPath().resolve("build.gradle").toFile().exists())
+    }
+    .collect { it.name }

photonlib-cpp-examples/settings.gradle

@@ -1 +1,3 @@
-new File('examples.txt').eachLine { line -> include line }
+apply from: "examples.gradle"
+
+exampleFolderNames.each { line -> include line }

photonlib-java-examples/README.md

@@ -0,0 +1,53 @@
+## PhotonLib Java Examples
+
+### Running examples
+
+For instructions on how to run these examples locally, see [Running Examples](https://docs.photonvision.org/en/latest/docs/contributing/photonvision/build-instructions.html#running-examples).
+
+---
+
+### [**`aimattarget`**](aimattarget)
+
+A simple demonstration of using PhotonVision's 2d target yaw to align a differential drivetrain with a target.
+
+---
+
+### [**`getinrange`**](getinrange)
+
+A simple demonstration of using PhotonVision's 2d target pitch to bring a differential drivetrain to a specific distance from a target.
+
+---
+
+### [**`aimandrange`**](aimandrange)
+
+A combination of the previous `aimattarget` and `getinrange` examples to simultaneously aim and get in range of a target.
+
+---
+
+### [**`simaimandrange`**](simaimandrange)
+
+The above `aimandrange` example with simulation support.
+
+<img src="https://github-production-user-asset-6210df.s3.amazonaws.com/7953350/268856085-432a54b9-f596-4e30-8b57-a8f38f88f985.png" width=60% height=60%>
+
+**Keyboard controls:**
+- Drive forward/backward: W/S
+- Turn left/right: A/D
+- Perform vision alignment: Z
+
+---
+
+### [**`swervedriveposeestsim`**](swervedriveposeestsim)
+
+A minimal swerve drive example demonstrating the usage of PhotonVision for AprilTag vision estimation with a swerve drive pose estimator.
+
+The example also has simulation support with an approximation of swerve drive dynamics.
+
+<img src="https://github-production-user-asset-6210df.s3.amazonaws.com/7953350/268862944-3392e69a-7705-4dbc-9eb8-0d03a6e27b9e.png" width=60% height=60%>
+
+<img src="https://github-production-user-asset-6210df.s3.amazonaws.com/7953350/268857280-bae145b8-356e-4afb-b842-597dbea60df6.png" width=60% height=60%>
+
+**Keyboard controls:**
+- Translate field-relative: WASD
+- Rotate counter/clockwise: Q/E
+- Offset pose estimate: X

photonlib-java-examples/aimandrange/README.md

@@ -0,0 +1,3 @@
+## **`aimandrange`**
+
+### See [PhotonLib Java Examples](./README.md#aimandrange)

photonlib-java-examples/aimattarget/README.md

@@ -0,0 +1,3 @@
+## **`aimattarget`**
+
+### See [PhotonLib Java Examples](./README.md#aimattarget)