Skip to content

Commit

Permalink
UI Message Passing (PhotonVision#1448)
Browse files Browse the repository at this point in the history
Bring the UI setting changes in thread with the camera.
  • Loading branch information
Juniormunk authored Oct 9, 2024
1 parent 142e22f commit 471c90e
Show file tree
Hide file tree
Showing 5 changed files with 130 additions and 33 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ public ArucoPipelineSettings() {
pipelineType = PipelineType.Aruco;
outputShowMultipleTargets = true;
targetModel = TargetModel.kAprilTag6p5in_36h11;
cameraExposureRaw = -1;
cameraExposureRaw = 20;
cameraAutoExposure = true;
ledMode = false;
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -69,8 +69,10 @@ public class VisionModule {
protected final VisionSource visionSource;
private final VisionRunner visionRunner;
private final StreamRunnable streamRunnable;
private final VisionModuleChangeSubscriber changeSubscriber;
private final LinkedList<CVPipelineResultConsumer> resultConsumers = new LinkedList<>();
// Raw result consumers run before any drawing has been done by the OutputStreamPipeline
// Raw result consumers run before any drawing has been done by the
// OutputStreamPipeline
private final LinkedList<BiConsumer<Frame, List<TrackedTarget>>> streamResultConsumers =
new LinkedList<>();
private final NTDataPublisher ntConsumer;
Expand Down Expand Up @@ -102,7 +104,8 @@ public VisionModule(PipelineManager pipelineManager, VisionSource visionSource,
if (visionSource.getCameraConfiguration().cameraQuirks == null)
visionSource.getCameraConfiguration().cameraQuirks = QuirkyCamera.DefaultCamera;

// We don't show gain if the config says it's -1. So check here to make sure it's non-negative
// We don't show gain if the config says it's -1. So check here to make sure
// it's non-negative
// if it _is_ supported
if (cameraQuirks.hasQuirk(CameraQuirk.Gain)) {
pipelineManager.userPipelineSettings.forEach(
Expand All @@ -120,16 +123,18 @@ public VisionModule(PipelineManager pipelineManager, VisionSource visionSource,

this.pipelineManager = pipelineManager;
this.visionSource = visionSource;
changeSubscriber = new VisionModuleChangeSubscriber(this);
this.visionRunner =
new VisionRunner(
this.visionSource.getFrameProvider(),
this.pipelineManager::getCurrentPipeline,
this::consumeResult,
this.cameraQuirks);
this.cameraQuirks,
getChangeSubscriber());
this.streamRunnable = new StreamRunnable(new OutputStreamPipeline());
this.moduleIndex = index;

DataChangeService.getInstance().addSubscriber(new VisionModuleChangeSubscriber(this));
DataChangeService.getInstance().addSubscriber(changeSubscriber);

createStreams();

Expand Down Expand Up @@ -315,6 +320,10 @@ private boolean isVendorCamera() {
return visionSource.isVendorCamera();
}

public VisionModuleChangeSubscriber getChangeSubscriber() {
return changeSubscriber;
}

void changePipelineType(int newType) {
pipelineManager.changePipelineType(newType);
setPipeline(pipelineManager.getRequestedIndex());
Expand Down Expand Up @@ -449,9 +458,11 @@ boolean setPipeline(int index) {
}

private boolean camShouldControlLEDs() {
// Heuristic - if the camera has a known FOV or is a piCam, assume it's in use for
// Heuristic - if the camera has a known FOV or is a piCam, assume it's in use
// for
// vision processing, and should command stuff to the LED's.
// TODO: Make LED control a property of the camera itself and controllable in the UI.
// TODO: Make LED control a property of the camera itself and controllable in
// the UI.
return isVendorCamera();
}

Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,55 @@
/*
* 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.vision.processes;

import io.javalin.websocket.WsContext;
import org.photonvision.vision.pipeline.CVPipelineSettings;

public class VisionModuleChange<T> {
private final String propName;
private final T newPropValue;
private final CVPipelineSettings currentSettings;
private final WsContext originContext;

public VisionModuleChange(
String propName,
T newPropValue,
CVPipelineSettings currentSettings,
WsContext originContext) {
this.propName = propName;
this.newPropValue = newPropValue;
this.currentSettings = currentSettings;
this.originContext = originContext;
}

public String getPropName() {
return propName;
}

public T getNewPropValue() {
return newPropValue;
}

public CVPipelineSettings getCurrentSettings() {
return currentSettings;
}

public WsContext getOriginContext() {
return originContext;
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,9 @@
package org.photonvision.vision.processes;

import java.util.ArrayList;
import java.util.List;
import java.util.Map;
import java.util.concurrent.locks.ReentrantLock;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Point;
import org.photonvision.common.dataflow.DataChangeSubscriber;
Expand All @@ -39,6 +41,8 @@
public class VisionModuleChangeSubscriber extends DataChangeSubscriber {
private final VisionModule parentModule;
private final Logger logger;
private List<VisionModuleChange<?>> settingChanges = new ArrayList<>();
private final ReentrantLock changeListLock = new ReentrantLock();

public VisionModuleChangeSubscriber(VisionModule parentModule) {
this.parentModule = parentModule;
Expand All @@ -54,28 +58,47 @@ public void onDataChangeEvent(DataChangeEvent<?> event) {
if (event instanceof IncomingWebSocketEvent) {
var wsEvent = (IncomingWebSocketEvent<?>) event;

// Camera index -1 means a "multicast event" (i.e. the event is received by all cameras)
// Camera index -1 means a "multicast event" (i.e. the event is received by all
// cameras)
if (wsEvent.cameraIndex != null
&& (wsEvent.cameraIndex == parentModule.moduleIndex || wsEvent.cameraIndex == -1)) {
logger.trace("Got PSC event - propName: " + wsEvent.propertyName);
changeListLock.lock();
try {
getSettingChanges()
.add(
new VisionModuleChange(
wsEvent.propertyName,
wsEvent.data,
parentModule.pipelineManager.getCurrentPipeline().getSettings(),
wsEvent.originContext));
} finally {
changeListLock.unlock();
}
}
}
}

var propName = wsEvent.propertyName;
var newPropValue = wsEvent.data;
var currentSettings = parentModule.pipelineManager.getCurrentPipeline().getSettings();
public List<VisionModuleChange<?>> getSettingChanges() {
return settingChanges;
}

// special case for non-PipelineSetting changes
public void processSettingChanges() {
// special case for non-PipelineSetting changes
changeListLock.lock();
try {
for (var change : settingChanges) {
var propName = change.getPropName();
var newPropValue = change.getNewPropValue();
var currentSettings = change.getCurrentSettings();
var originContext = change.getOriginContext();
switch (propName) {
// case "cameraNickname": // rename camera
// var newNickname = (String) newPropValue;
// logger.info("Changing nickname to " + newNickname);
// parentModule.setCameraNickname(newNickname);
// return;
case "pipelineName": // rename current pipeline
logger.info("Changing nick to " + newPropValue);
parentModule.pipelineManager.getCurrentPipelineSettings().pipelineNickname =
(String) newPropValue;
parentModule.saveAndBroadcastAll();
return;
continue;
case "newPipelineInfo": // add new pipeline
var typeName = (Pair<String, PipelineType>) newPropValue;
var type = typeName.getRight();
Expand All @@ -86,23 +109,23 @@ public void onDataChangeEvent(DataChangeEvent<?> event) {
var addedSettings = parentModule.pipelineManager.addPipeline(type);
addedSettings.pipelineNickname = name;
parentModule.saveAndBroadcastAll();
return;
continue;
case "deleteCurrPipeline":
var indexToDelete = parentModule.pipelineManager.getRequestedIndex();
logger.info("Deleting current pipe at index " + indexToDelete);
int newIndex = parentModule.pipelineManager.removePipeline(indexToDelete);
parentModule.setPipeline(newIndex);
parentModule.saveAndBroadcastAll();
return;
continue;
case "changePipeline": // change active pipeline
var index = (Integer) newPropValue;
if (index == parentModule.pipelineManager.getRequestedIndex()) {
logger.debug("Skipping pipeline change, index " + index + " already active");
return;
continue;
}
parentModule.setPipeline(index);
parentModule.saveAndBroadcastAll();
return;
continue;
case "startCalibration":
try {
var data =
Expand All @@ -113,25 +136,25 @@ public void onDataChangeEvent(DataChangeEvent<?> event) {
} catch (Exception e) {
logger.error("Error deserailizing start-cal request", e);
}
return;
continue;
case "saveInputSnapshot":
parentModule.saveInputSnapshot();
return;
continue;
case "saveOutputSnapshot":
parentModule.saveOutputSnapshot();
return;
continue;
case "takeCalSnapshot":
parentModule.takeCalibrationSnapshot();
return;
continue;
case "duplicatePipeline":
int idx = parentModule.pipelineManager.duplicatePipeline((Integer) newPropValue);
parentModule.setPipeline(idx);
parentModule.saveAndBroadcastAll();
return;
continue;
case "calibrationUploaded":
if (newPropValue instanceof CameraCalibrationCoefficients)
parentModule.addCalibrationToConfig((CameraCalibrationCoefficients) newPropValue);
return;
continue;
case "robotOffsetPoint":
if (currentSettings instanceof AdvancedPipelineSettings) {
var curAdvSettings = (AdvancedPipelineSettings) currentSettings;
Expand Down Expand Up @@ -176,14 +199,14 @@ public void onDataChangeEvent(DataChangeEvent<?> event) {
}
}
}
return;
continue;
case "changePipelineType":
parentModule.changePipelineType((Integer) newPropValue);
parentModule.saveAndBroadcastAll();
return;
continue;
case "isDriverMode":
parentModule.setDriverMode((Boolean) newPropValue);
return;
continue;
}

// special case for camera settables
Expand Down Expand Up @@ -218,8 +241,11 @@ public void onDataChangeEvent(DataChangeEvent<?> event) {
logger.error("Unknown exception when setting PSC prop!", e);
}

parentModule.saveAndBroadcastSelective(wsEvent.originContext, propName, newPropValue);
parentModule.saveAndBroadcastSelective(originContext, propName, newPropValue);
}
getSettingChanges().clear();
} finally {
changeListLock.unlock();
}
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@ public class VisionRunner {
private final FrameProvider frameSupplier;
private final Supplier<CVPipeline> pipelineSupplier;
private final Consumer<CVPipelineResult> pipelineResultConsumer;
private final VisionModuleChangeSubscriber changeSubscriber;
private final QuirkyCamera cameraQuirks;

private long loopCount;
Expand All @@ -53,15 +54,18 @@ public VisionRunner(
FrameProvider frameSupplier,
Supplier<CVPipeline> pipelineSupplier,
Consumer<CVPipelineResult> pipelineResultConsumer,
QuirkyCamera cameraQuirks) {
QuirkyCamera cameraQuirks,
VisionModuleChangeSubscriber changeSubscriber) {
this.frameSupplier = frameSupplier;
this.pipelineSupplier = pipelineSupplier;
this.pipelineResultConsumer = pipelineResultConsumer;
this.cameraQuirks = cameraQuirks;
this.changeSubscriber = changeSubscriber;

visionProcessThread = new Thread(this::update);
visionProcessThread.setName("VisionRunner - " + frameSupplier.getName());
logger = new Logger(VisionRunner.class, frameSupplier.getName(), LogGroup.VisionModule);
changeSubscriber.processSettingChanges();
}

public void startProcess() {
Expand All @@ -70,6 +74,7 @@ public void startProcess() {

private void update() {
while (!Thread.interrupted()) {
changeSubscriber.processSettingChanges();
var pipeline = pipelineSupplier.get();

// Tell our camera implementation here what kind of pre-processing we need it to be doing
Expand Down

0 comments on commit 471c90e

Please sign in to comment.