diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java index 57989bf9e..a3668f99d 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java @@ -114,7 +114,6 @@ public class Draw3dTargetsPipe // Distort the points so they match the image they're being overlaid on distortPoints(tempMat, tempMat); } - var topPoints = tempMat.toList(); dividePointList(bottomPoints); diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index 63152043d..ecbee6c11 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -31,18 +31,23 @@ import edu.wpi.first.networktables.DoubleArrayPublisher; import edu.wpi.first.networktables.DoublePublisher; import edu.wpi.first.networktables.IntegerEntry; import edu.wpi.first.networktables.IntegerSubscriber; +import edu.wpi.first.networktables.MultiSubscriber; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.PubSubOption; import edu.wpi.first.networktables.RawSubscriber; import edu.wpi.first.networktables.StringSubscriber; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; +import java.util.Set; import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.hardware.VisionLEDMode; import org.photonvision.targeting.PhotonPipelineResult; /** Represents a camera that is connected to PhotonVision. */ public class PhotonCamera { + static final String kTableName = "photonvision"; + protected final NetworkTable rootTable; RawSubscriber rawBytesEntry; BooleanEntry driverModeEntry; @@ -96,6 +101,8 @@ public class PhotonCamera { Packet packet = new Packet(1); + private final MultiSubscriber m_topicNameSubscriber; + /** * Constructs a PhotonCamera from a root table. * @@ -106,7 +113,7 @@ public class PhotonCamera { */ public PhotonCamera(NetworkTableInstance instance, String cameraName) { name = cameraName; - var mainTable = instance.getTable("photonvision"); + var mainTable = instance.getTable(kTableName); this.rootTable = mainTable.getSubTable(cameraName); path = rootTable.getPath(); rawBytesEntry = rootTable.getRawTopic("rawBytes").subscribe("rawBytes", new byte[] {}); @@ -117,6 +124,12 @@ public class PhotonCamera { heartbeatEntry = rootTable.getIntegerTopic("heartbeat").subscribe(-1); ledModeEntry = mainTable.getIntegerTopic("ledMode").getEntry(-1); versionEntry = mainTable.getStringTopic("version").subscribe(""); + + m_topicNameSubscriber = + new MultiSubscriber( + instance, + new String[] {"/photonvision/"}, + new PubSubOption[] {PubSubOption.topicsOnly(true)}); } /** @@ -280,7 +293,7 @@ public class PhotonCamera { prevHeartbeatValue = curHeartbeat; } - return ((now - prevHeartbeatChangeTime) < HEARBEAT_DEBOUNCE_SEC); + return (now - prevHeartbeatChangeTime) < HEARBEAT_DEBOUNCE_SEC; } private void verifyVersion() { @@ -292,12 +305,25 @@ public class PhotonCamera { // Heartbeat entry is assumed to always be present. If it's not present, we // assume that a camera with that name was never connected in the first place. if (!heartbeatEntry.exists()) { - DriverStation.reportError( - "PhotonVision coprocessor at path " + path + " not found on NetworkTables!", true); + Set cameraNames = rootTable.getInstance().getTable(kTableName).getSubTables(); + if (cameraNames.isEmpty()) { + DriverStation.reportError( + "Could not find any PhotonVision coprocessors on NetworkTables. Double check that PhotonVision is running, and that your camera is connected!", + false); + } else { + DriverStation.reportError( + "PhotonVision coprocessor at path " + + path + + " not found on NetworkTables. Double check that your camera names match!", + true); + DriverStation.reportError( + "Found the following PhotonVision cameras on NetworkTables:\n" + + String.join("\n", cameraNames), + false); + } } - // Check for connection status. Warn if disconnected. - if (!isConnected()) { + else if (!isConnected()) { DriverStation.reportWarning( "PhotonVision coprocessor at path " + path + " is not sending new data.", true); } diff --git a/photon-lib/src/main/java/org/photonvision/SimPhotonCamera.java b/photon-lib/src/main/java/org/photonvision/SimPhotonCamera.java index 5d22d0a38..1823c4875 100644 --- a/photon-lib/src/main/java/org/photonvision/SimPhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/SimPhotonCamera.java @@ -48,7 +48,7 @@ public class SimPhotonCamera { */ public SimPhotonCamera(NetworkTableInstance instance, String cameraName) { ts.removeEntries(); - ts.subTable = instance.getTable("/photonvision").getSubTable(cameraName); + ts.subTable = instance.getTable(PhotonCamera.kTableName).getSubTable(cameraName); ts.updateEntries(); } diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp index 4e81d5d7e..8eb0d6344 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp @@ -33,10 +33,11 @@ namespace photonlib { constexpr const units::second_t VERSION_CHECK_INTERVAL = 5_s; +static const std::vector PHOTON_PREFIX = {"/photonvision/"}; -PhotonCamera::PhotonCamera(std::shared_ptr instance, +PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance, const std::string_view cameraName) - : mainTable(instance->GetTable("photonvision")), + : mainTable(instance.GetTable("photonvision")), rootTable(mainTable->GetSubTable(cameraName)), rawBytesEntry(rootTable->GetRawTopic("rawBytes").Subscribe("raw", {})), driverModeEntry(rootTable->GetBooleanTopic("driverMode").Publish()), @@ -56,13 +57,12 @@ PhotonCamera::PhotonCamera(std::shared_ptr instance, pipelineIndexSubscriber( rootTable->GetIntegerTopic("pipelineIndex").Subscribe(-1)), ledModeSubscriber(mainTable->GetIntegerTopic("ledMode").Subscribe(0)), + m_topicNameSubscriber(instance, PHOTON_PREFIX, {.topicsOnly = true}), path(rootTable->GetPath()), m_cameraName(cameraName) {} PhotonCamera::PhotonCamera(const std::string_view cameraName) - : PhotonCamera(std::make_shared( - nt::NetworkTableInstance::GetDefault()), - cameraName) {} + : PhotonCamera(nt::NetworkTableInstance::GetDefault(), cameraName) {} PhotonPipelineResult PhotonCamera::GetLatestResult() { if (test) return testResult; @@ -134,10 +134,29 @@ void PhotonCamera::VerifyVersion() { const std::string& versionString = versionEntry.Get(""); if (versionString.empty()) { std::string path_ = path; - FRC_ReportError( - frc::warn::Warning, - "PhotonVision coprocessor at path {} not found on NetworkTables!", - path_); + std::vector cameraNames = + rootTable->GetInstance().GetTable("photonvision")->GetSubTables(); + if (cameraNames.empty()) { + FRC_ReportError(frc::warn::Warning, + "Could not find any PhotonVision coprocessors on " + "NetworkTables. Double check that PhotonVision is " + "running, and that your camera is connected!"); + } else { + FRC_ReportError( + frc::warn::Warning, + "PhotonVision coprocessor at path {} not found on NetworkTables. " + "Double check that your camera names match!", + path_); + + std::string cameraNameOutString; + for (unsigned int i = 0; i < cameraNames.size(); i++) { + cameraNameOutString += "\n" + cameraNames[i]; + } + FRC_ReportError( + frc::warn::Warning, + "Found the following PhotonVision cameras on NetworkTables:{}", + cameraNameOutString); + } } else if (!VersionMatches(versionString)) { FRC_ReportError(frc::warn::Warning, "Photon version {} does not match coprocessor version {}!", diff --git a/photon-lib/src/main/native/include/photonlib/PhotonCamera.h b/photon-lib/src/main/native/include/photonlib/PhotonCamera.h index a1ce0ecd4..d15d888f0 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photonlib/PhotonCamera.h @@ -30,6 +30,7 @@ #include #include #include +#include #include #include #include @@ -57,7 +58,7 @@ class PhotonCamera { * @param cameraName The name of the camera, as seen in the UI. * over. */ - explicit PhotonCamera(std::shared_ptr instance, + explicit PhotonCamera(nt::NetworkTableInstance instance, const std::string_view cameraName); /** @@ -178,6 +179,8 @@ class PhotonCamera { nt::IntegerSubscriber pipelineIndexSubscriber; nt::IntegerSubscriber ledModeSubscriber; + nt::MultiSubscriber m_topicNameSubscriber; + std::string path; std::string m_cameraName; diff --git a/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h b/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h index b346acafa..08c6a8bf9 100644 --- a/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h +++ b/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h @@ -37,7 +37,7 @@ namespace photonlib { class SimPhotonCamera : public PhotonCamera { public: - SimPhotonCamera(std::shared_ptr instance, + SimPhotonCamera(nt::NetworkTableInstance instance, const std::string& cameraName) : PhotonCamera(instance, cameraName) { latencyMillisEntry = rootTable->GetEntry("latencyMillis"); @@ -48,14 +48,12 @@ class SimPhotonCamera : public PhotonCamera { targetSkewEntry = rootTable->GetEntry("targetSkewEntry"); targetPoseEntry = rootTable->GetEntry("targetPoseEntry"); rawBytesPublisher = rootTable->GetRawTopic("rawBytes").Publish("raw"); - versionEntry = instance->GetTable("photonvision")->GetEntry("version"); + versionEntry = instance.GetTable("photonvision")->GetEntry("version"); // versionEntry.SetString(PhotonVersion.versionString); } explicit SimPhotonCamera(const std::string& cameraName) - : SimPhotonCamera(std::make_shared( - nt::NetworkTableInstance::GetDefault()), - cameraName) {} + : SimPhotonCamera(nt::NetworkTableInstance::GetDefault(), cameraName) {} virtual ~SimPhotonCamera() = default;