Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[photon-lib] Sim multitag result #973

Merged
merged 7 commits into from
Oct 26, 2023
Merged

[photon-lib] Sim multitag result #973

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
d671236
update multitag result
amquake Oct 21, 2023
78bbd19
Merge branch 'master' into sim-multitag
amquake Oct 25, 2023
e531236
hide ATFL setter
amquake Oct 25, 2023
cfcc120
Merge branch 'master' into sim-multitag
amquake Oct 25, 2023
f2cbc90
tag layout as field
amquake Oct 25, 2023
9b4001e
Merge branch 'master' into sim-multitag
amquake Oct 26, 2023
0400cf1
Merge branch 'master' into sim-multitag
amquake Oct 26, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions .gitignore
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -157,5 +157,8 @@ photonlib-java-examples/*/vendordeps/*
photonlib-cpp-examples/*/vendordeps/*

*/networktables.json
*/networktables.json.bck
photonlib-cpp-examples/*/networktables.json.bck
photonlib-java-examples/*/networktables.json.bck
*.sqlite
photon-server/src/main/resources/web/index.html
67 changes: 43 additions & 24 deletions photon-lib/src/main/java/org/photonvision/PhotonCamera.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,11 +24,9 @@

package org.photonvision;

import com.fasterxml.jackson.core.JsonProcessingException;
import com.fasterxml.jackson.databind.ObjectMapper;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.numbers.*;
Expand Down Expand Up @@ -121,6 +119,9 @@ public static void setVersionCheckEnabled(boolean enabled) {

Packet packet = new Packet(1);

private static AprilTagFieldLayout lastSetTagLayout =
AprilTagFields.kDefaultField.loadAprilTagLayoutField();

// Existing is enough to make this multisubscriber do its thing
private final MultiSubscriber m_topicNameSubscriber;

Expand Down Expand Up @@ -331,27 +332,45 @@ public boolean isConnected() {
return (now - prevHeartbeatChangeTime) < HEARBEAT_DEBOUNCE_SEC;
}

/**
* Set the Apriltag Field Layout used by all PhotonVision coprocessors that are (or might later)
* connect to this robot. The topic is marked as persistant, so even if you only call this once
* ever, it will be saved on the RoboRIO and pushed out to all NT clients when code reboots.
* PhotonVision will also store a copy of this layout locally on the coprocessor, but subscribes
* to this topic and the local copy will be updated when this function is called.
*
* @param layout The layout to use for *all* PhotonVision cameras
* @return Success of serializing the JSON. This does *not* mean that all PhotonVision clients
* have updated their internal layouts.
*/
public boolean setApriltagFieldLayout(AprilTagFieldLayout layout) {
try {
var layout_json = new ObjectMapper().writeValueAsString(layout);
atflPublisher.set(layout_json);
return true;
} catch (JsonProcessingException e) {
MathSharedStore.reportError("Error setting ATFL in " + this.getName(), e.getStackTrace());
}
return false;
}
// TODO: Implement ATFL subscribing in backend
// /**
// * Get the last set {@link AprilTagFieldLayout}. The tag layout is shared between all
// PhotonVision
// * coprocessors on the same NT instance-- this method returns the most recent layout set. If no
// * layout has been set by the user, this is equal to {@link AprilTagFields#kDefaultField}.
// *
// * @return The last set layout
// */
// public AprilTagFieldLayout getAprilTagFieldLayout() {
// return lastSetTagLayout;
// }

// /**
// * Set the {@link AprilTagFieldLayout} used by all PhotonVision coprocessors that are (or might
// * later) connect to this robot. The topic is marked as persistent, so even if you only call
// this
// * once ever, it will be saved on the RoboRIO and pushed out to all NT clients when code
// reboots.
// * PhotonVision will also store a copy of this layout locally on the coprocessor, but
// subscribes
// * to this topic and the local copy will be updated when this function is called.
// *
// * @param layout The layout to use for *all* PhotonVision cameras
// * @return Success of serializing the JSON. This does *not* mean that all PhotonVision clients
// * have updated their internal layouts.
// */
// public boolean setAprilTagFieldLayout(AprilTagFieldLayout layout) {
// try {
// var layout_json = new ObjectMapper().writeValueAsString(layout);
// atflPublisher.set(layout_json);
// lastSetTagLayout = layout;
// return true;
// } catch (JsonProcessingException e) {
// MathSharedStore.reportError("Error setting ATFL in " + this.getName(),
// e.getStackTrace());
// }
// return false;
// }

public Optional<Matrix<N3, N3>> getCameraMatrix() {
var cameraMatrix = cameraIntrinsicsSubscriber.get();
Expand Down
25 changes: 23 additions & 2 deletions photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@

package org.photonvision.simulation;

import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.cscore.CvSource;
import edu.wpi.first.cscore.VideoMode.PixelFormat;
Expand All @@ -37,6 +38,7 @@
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;
Expand All @@ -52,6 +54,8 @@
import org.photonvision.estimation.OpenCVHelp;
import org.photonvision.estimation.RotTrlTransform3d;
import org.photonvision.estimation.TargetModel;
import org.photonvision.estimation.VisionEstimation;
import org.photonvision.targeting.MultiTargetPNPResults;
import org.photonvision.targeting.PNPResults;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
Expand Down Expand Up @@ -524,11 +528,28 @@ public PhotonPipelineResult process(
videoSimProcessed.putFrame(videoSimFrameProcessed);
} else videoSimProcessed.setConnectionStrategy(ConnectionStrategy.kForceClose);

// put this simulated data to NT
// calculate multitag results
var multitagResults = new MultiTargetPNPResults();
// TODO: Implement ATFL subscribing in backend
// var tagLayout = cam.getAprilTagFieldLayout();
var tagLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField();
amquake marked this conversation as resolved.
Show resolved Hide resolved
var visibleLayoutTags = VisionEstimation.getVisibleLayoutTags(detectableTgts, tagLayout);
if (visibleLayoutTags.size() > 1) {
List<Integer> usedIDs =
visibleLayoutTags.stream().map(t -> t.ID).sorted().collect(Collectors.toList());
var pnpResults =
VisionEstimation.estimateCamPosePNP(
prop.getIntrinsics(), prop.getDistCoeffs(), detectableTgts, tagLayout);
multitagResults = new MultiTargetPNPResults(pnpResults, usedIDs);
}

// sort target order
if (sortMode != null) {
detectableTgts.sort(sortMode.getComparator());
}
return new PhotonPipelineResult(latencyMillis, detectableTgts);

// put this simulated data to NT
return new PhotonPipelineResult(latencyMillis, detectableTgts, multitagResults);
}

/**
Expand Down