Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-07-03 03:01:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Robustify setting pipeline index (#790)

Browse Source
This commit is contained in:
Matt
2023-02-08 21:07:12 -05:00
committed by GitHub
parent 3c53dcbb7b
commit 6886663688
8 changed files with 72 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java
View File

@@ -89,7 +89,7 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
ts.pipelineIndexPublisher.set(setIndex);
// TODO: Log
}
logger.debug("Successfully set pipeline index to " + newIndex);
logger.debug("Set pipeline index to " + newIndex);
}
private void onDriverModeChange(NetworkTableEvent entryNotification) {
@@ -102,7 +102,7 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
}
driverModeConsumer.accept(newDriverMode);
logger.debug("Successfully set driver mode to " + newDriverMode);
logger.debug("Set driver mode to " + newDriverMode);
}
private void removeEntries() {
@@ -119,7 +119,7 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
pipelineIndexListener =
new NTDataChangeListener(
ts.subTable.getInstance(), ts.pipelineIndexSubscriber, this::onPipelineIndexChange);
ts.subTable.getInstance(), ts.pipelineIndexRequestSub, this::onPipelineIndexChange);
driverModeListener =
new NTDataChangeListener(

24
photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java
View File

@@ -17,7 +17,8 @@
package org.photonvision.common.hardware;
import edu.wpi.first.networktables.IntegerEntry;
import edu.wpi.first.networktables.IntegerPublisher;
import edu.wpi.first.networktables.IntegerSubscriber;
import java.io.IOException;
import org.photonvision.common.ProgramStatus;
import org.photonvision.common.configuration.ConfigManager;
@@ -47,7 +48,9 @@ public class HardwareManager {
private final StatusLED statusLED;
@SuppressWarnings("FieldCanBeLocal")
private final IntegerEntry ledModeEntry;
private IntegerSubscriber ledModeRequest;
private IntegerPublisher ledModeState;
@SuppressWarnings({"FieldCanBeLocal", "unused"})
private final NTDataChangeListener ledModeListener;
@@ -71,6 +74,15 @@ public class HardwareManager {
this.metricsManager = new MetricsManager();
this.metricsManager.setConfig(hardwareConfig);
ledModeRequest =
NetworkTablesManager.getInstance()
.kRootTable
.getIntegerTopic("ledModeRequest")
.subscribe(-1);
ledModeState =
NetworkTablesManager.getInstance().kRootTable.getIntegerTopic("ledModeState").publish();
ledModeState.set(VisionLEDMode.kDefault.value);
CustomGPIO.setConfig(hardwareConfig);
if (Platform.isRaspberryPi()) {
@@ -92,17 +104,15 @@ public class HardwareManager {
hardwareConfig.ledPins,
hasBrightnessRange ? hardwareConfig.ledBrightnessRange.get(0) : 0,
hasBrightnessRange ? hardwareConfig.ledBrightnessRange.get(1) : 100,
pigpioSocket);
pigpioSocket,
ledModeState::set);
ledModeEntry =
NetworkTablesManager.getInstance().kRootTable.getIntegerTopic("ledMode").getEntry(0);
ledModeEntry.set(VisionLEDMode.kDefault.value);
ledModeListener =
visionLED == null
? null
: new NTDataChangeListener(
NetworkTablesManager.getInstance().kRootTable.getInstance(),
ledModeEntry,
ledModeRequest,
visionLED::onLedModeChange);
Runtime.getRuntime().addShutdownHook(new Thread(this::onJvmExit));

15
photon-core/src/main/java/org/photonvision/common/hardware/VisionLED.java
View File

@@ -21,6 +21,7 @@ import edu.wpi.first.networktables.NetworkTableEvent;
import java.util.ArrayList;
import java.util.List;
import java.util.function.BooleanSupplier;
import java.util.function.Consumer;
import org.photonvision.common.hardware.GPIO.CustomGPIO;
import org.photonvision.common.hardware.GPIO.GPIOBase;
import org.photonvision.common.hardware.GPIO.pi.PigpioException;
@@ -45,11 +46,18 @@ public class VisionLED {
private int mappedBrightnessPercentage;
private Consumer<Integer> modeConsumer;
public VisionLED(
List<Integer> ledPins, int brightnessMin, int brightnessMax, PigpioSocket pigpioSocket) {
List<Integer> ledPins,
int brightnessMin,
int brightnessMax,
PigpioSocket pigpioSocket,
Consumer<Integer> visionLEDmode) {
this.brightnessMin = brightnessMin;
this.brightnessMax = brightnessMax;
this.pigpioSocket = pigpioSocket;
this.modeConsumer = visionLEDmode;
this.ledPins = ledPins.stream().mapToInt(i -> i).toArray();
ledPins.forEach(
pin -> {
@@ -123,7 +131,8 @@ public class VisionLED {
}
void onLedModeChange(NetworkTableEvent entryNotification) {
var newLedModeRaw = (int) entryNotification.valueData.value.getDouble();
var newLedModeRaw = (int) entryNotification.valueData.value.getInteger();
logger.debug("Got LED mode " + newLedModeRaw);
if (newLedModeRaw != currentLedMode.value) {
VisionLEDMode newLedMode;
switch (newLedModeRaw) {
@@ -145,6 +154,8 @@ public class VisionLED {
break;
}
setInternal(newLedMode, true);
if (modeConsumer != null) modeConsumer.accept(newLedMode.value);
}
}

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

@@ -371,15 +371,15 @@ public class VisionModule {
return ret;
}
void setPipeline(int index) {
boolean setPipeline(int index) {
logger.info("Setting pipeline to " + index);
logger.info("Pipeline name: " + pipelineManager.getPipelineNickname(index));
pipelineManager.setIndex(index);
var pipelineSettings = pipelineManager.getPipelineSettings(index);
if (pipelineSettings == null) {
logger.error("Config for index " + index + " was null!");
return;
logger.error("Config for index " + index + " was null! Not changing pipelines");
return false;
}
visionSource.getSettables().setVideoModeInternal(pipelineSettings.cameraVideoModeIndex);
@@ -422,6 +422,8 @@ public class VisionModule {
visionSource.getSettables().getConfiguration().currentPipelineIndex =
pipelineManager.getCurrentPipelineIndex();
return true;
}
private boolean camShouldControlLEDs() {

25
photon-lib/src/main/java/org/photonvision/PhotonCamera.java
View File

@@ -29,6 +29,7 @@ import edu.wpi.first.networktables.BooleanSubscriber;
import edu.wpi.first.networktables.DoubleArrayPublisher;
import edu.wpi.first.networktables.DoublePublisher;
import edu.wpi.first.networktables.IntegerEntry;
import edu.wpi.first.networktables.IntegerPublisher;
import edu.wpi.first.networktables.IntegerSubscriber;
import edu.wpi.first.networktables.MultiSubscriber;
import edu.wpi.first.networktables.NetworkTable;
@@ -60,7 +61,8 @@ public class PhotonCamera {
DoublePublisher targetSkewEntry;
StringSubscriber versionEntry;
IntegerEntry inputSaveImgEntry, outputSaveImgEntry;
IntegerEntry pipelineIndexEntry, ledModeEntry;
IntegerPublisher pipelineIndexRequest, ledModeRequest;
IntegerSubscriber pipelineIndexState, ledModeState;
IntegerSubscriber heartbeatEntry;
public void close() {
@@ -77,8 +79,11 @@ public class PhotonCamera {
versionEntry.close();
inputSaveImgEntry.close();
outputSaveImgEntry.close();
pipelineIndexEntry.close();
ledModeEntry.close();
pipelineIndexRequest.close();
pipelineIndexState.close();
ledModeRequest.close();
ledModeState.close();
pipelineIndexRequest.close();
}
private final String path;
@@ -122,9 +127,11 @@ public class PhotonCamera {
driverModeSubscriber = rootTable.getBooleanTopic("driverModeRequest").subscribe(false);
inputSaveImgEntry = rootTable.getIntegerTopic("inputSaveImgCmd").getEntry(0);
outputSaveImgEntry = rootTable.getIntegerTopic("outputSaveImgCmd").getEntry(0);
pipelineIndexEntry = rootTable.getIntegerTopic("pipelineIndex").getEntry(0);
pipelineIndexRequest = rootTable.getIntegerTopic("pipelineIndexRequest").publish();
pipelineIndexState = rootTable.getIntegerTopic("pipelineIndexState").subscribe(0);
heartbeatEntry = rootTable.getIntegerTopic("heartbeat").subscribe(-1);
ledModeEntry = mainTable.getIntegerTopic("ledMode").getEntry(-1);
ledModeRequest = mainTable.getIntegerTopic("ledModeRequest").publish();
ledModeState = mainTable.getIntegerTopic("ledModeState").subscribe(-1);
versionEntry = mainTable.getStringTopic("version").subscribe("");
m_topicNameSubscriber =
@@ -215,7 +222,7 @@ public class PhotonCamera {
* @return The active pipeline index.
*/
public int getPipelineIndex() {
return (int) pipelineIndexEntry.get(0);
return (int) pipelineIndexState.get(0);
}
/**
@@ -224,7 +231,7 @@ public class PhotonCamera {
* @param index The active pipeline index.
*/
public void setPipelineIndex(int index) {
pipelineIndexEntry.set(index);
pipelineIndexRequest.set(index);
}
/**
@@ -233,7 +240,7 @@ public class PhotonCamera {
* @return The current LED mode.
*/
public VisionLEDMode getLEDMode() {
int value = (int) ledModeEntry.get(-1);
int value = (int) ledModeState.get(-1);
switch (value) {
case 0:
return VisionLEDMode.kOff;
@@ -253,7 +260,7 @@ public class PhotonCamera {
* @param led The mode to set to.
*/
public void setLED(VisionLEDMode led) {
ledModeEntry.set(led.value);
ledModeRequest.set(led.value);
}
/**

19
photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp
View File

@@ -50,16 +50,17 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance,
rootTable->GetIntegerTopic("outputSaveImgCmd").Publish()),
outputSaveImgSubscriber(
rootTable->GetIntegerTopic("outputSaveImgCmd").Subscribe(0)),
pipelineIndexEntry(rootTable->GetIntegerTopic("pipelineIndex").Publish()),
ledModeEntry(mainTable->GetIntegerTopic("ledMode").Publish()),
pipelineIndexPub(
rootTable->GetIntegerTopic("pipelineIndexRequest").Publish()),
pipelineIndexSub(
rootTable->GetIntegerTopic("pipelineIndexState").Subscribe(0)),
ledModePub(mainTable->GetIntegerTopic("ledMode").Publish()),
ledModeSub(mainTable->GetIntegerTopic("ledMode").Subscribe(0)),
versionEntry(mainTable->GetStringTopic("version").Subscribe("")),
driverModeSubscriber(
rootTable->GetBooleanTopic("driverMode").Subscribe(false)),
driverModePublisher(
rootTable->GetBooleanTopic("driverModeRequest").Publish()),
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) {}
@@ -107,19 +108,19 @@ void PhotonCamera::TakeOutputSnapshot() {
bool PhotonCamera::GetDriverMode() const { return driverModeSubscriber.Get(); }
void PhotonCamera::SetPipelineIndex(int index) {
pipelineIndexEntry.Set(static_cast<double>(index));
pipelineIndexPub.Set(static_cast<double>(index));
}
int PhotonCamera::GetPipelineIndex() const {
return static_cast<int>(pipelineIndexSubscriber.Get());
return static_cast<int>(pipelineIndexSub.Get());
}
LEDMode PhotonCamera::GetLEDMode() const {
return static_cast<LEDMode>(static_cast<int>(ledModeSubscriber.Get()));
return static_cast<LEDMode>(static_cast<int>(ledModeSub.Get()));
}
void PhotonCamera::SetLEDMode(LEDMode mode) {
ledModeEntry.Set(static_cast<double>(static_cast<int>(mode)));
ledModePub.Set(static_cast<double>(static_cast<int>(mode)));
}
const std::string_view PhotonCamera::GetCameraName() const {

7
photon-lib/src/main/native/include/photonlib/PhotonCamera.h
View File

@@ -172,13 +172,14 @@ class PhotonCamera {
nt::IntegerSubscriber inputSaveImgSubscriber;
nt::IntegerPublisher outputSaveImgEntry;
nt::IntegerSubscriber outputSaveImgSubscriber;
nt::IntegerPublisher pipelineIndexEntry;
nt::IntegerPublisher ledModeEntry;
nt::IntegerPublisher pipelineIndexPub;
nt::IntegerSubscriber pipelineIndexSub;
nt::IntegerPublisher ledModePub;
nt::IntegerSubscriber ledModeSub;
nt::StringSubscriber versionEntry;
nt::BooleanSubscriber driverModeSubscriber;
nt::BooleanPublisher driverModePublisher;
nt::IntegerSubscriber pipelineIndexSubscriber;
nt::IntegerSubscriber ledModeSubscriber;
nt::MultiSubscriber m_topicNameSubscriber;

10
photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java
View File

@@ -41,9 +41,8 @@ public class NTTopicSet {
public NetworkTable subTable;
public RawPublisher rawBytesEntry;
public IntegerTopic pipelineIndexTopic;
public IntegerPublisher pipelineIndexPublisher;
public IntegerSubscriber pipelineIndexSubscriber;
public IntegerSubscriber pipelineIndexRequestSub;
public BooleanTopic driverModeEntry;
public BooleanPublisher driverModePublisher;
@@ -71,9 +70,8 @@ public class NTTopicSet {
.getRawTopic("rawBytes")
.publish("rawBytes", PubSubOption.periodic(0.01), PubSubOption.sendAll(true));
pipelineIndexTopic = subTable.getIntegerTopic("pipelineIndex");
pipelineIndexPublisher = pipelineIndexTopic.publish();
pipelineIndexSubscriber = pipelineIndexTopic.subscribe(0);
pipelineIndexPublisher = subTable.getIntegerTopic("pipelineIndexState").publish();
pipelineIndexRequestSub = subTable.getIntegerTopic("pipelineIndexRequest").subscribe(0);
driverModePublisher = subTable.getBooleanTopic("driverMode").publish();
driverModeSubscriber = subTable.getBooleanTopic("driverModeRequest").subscribe(false);
@@ -101,7 +99,7 @@ public class NTTopicSet {
public void removeEntries() {
if (rawBytesEntry != null) rawBytesEntry.close();
if (pipelineIndexPublisher != null) pipelineIndexPublisher.close();
if (pipelineIndexSubscriber != null) pipelineIndexSubscriber.close();
if (pipelineIndexRequestSub != null) pipelineIndexRequestSub.close();
if (driverModePublisher != null) driverModePublisher.close();
if (driverModeSubscriber != null) driverModeSubscriber.close();