Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-20 00:51:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Bootup sprint (#18)

Browse Source 
* Did some stuff

* Fix gradle, start implementing mjpeg frame consumer

* Did some stuff

* bade changes

* rename camera config to USBCameraConfiguration, add name

* unrename cameraconfiguration

* Add pub/sub framework

* Add setResolution to mjpeg frame consumer

* add NTDataConsumer

* Add some totally broken hsv hacks

* Start refactoring UI data

* Update index.js

* Commit and push, he says

* Fix up some errors

* Fix input tab

* Fix fps

* Update index.js

* Add pipeline field setting, update PipelineManager, fix nullpointers and USBCameraSettables

* Change v-model to point to data()

* update hsv to use mutations

* Work on saving, fix hsv

* Rename shouldErode/shouldDilate to erode and dilate

* Hook all the tabs up to the Store

* Change handleData to handlePipelineData

* camera quirk redo, add ICCSub to SocketHandler

* Fix some property names

* Fixed tons of naming in UI, fix backend for multi-val PSCs, fix PSC enums

* change pipeline type to an int in store

* Fix mutation naming

* Attempt threshold fix

* Update SocketHandler.java

* Add truthy data sending

* Start adding logging support

* [UI] Add delay to slider input boxes (#1)

* [UI] [Backend] potentially fix camera settings, various logging tweaks

* Don't release raw input mat

* add setVideoModeIndex to vision settables

* Implement pipeline index in socket handler, add framework for renaming/changing pipes

* (ish) get pipeline change working

* Create index.html

* Cleanups, fix pipeline index bug, fix stream res for MJPG, add dashboard stream (unused)

* Refactor UI to use mutatePipeline, send pipeline results

* Update NetworkConfig.java

* Change double to number

* Run spotless

* Fix reversal of large/small comparators

* Fix left/right

* Fix pitch/yaw calculation bug, fix area bug

* Use Vue.set instead of assignment

This fixes {{ }}

* Update App.vue

* run spotless

* Actually add pipelines and reassign indecies

* Delete old pipeline configs

Fixes duplication on renaming pipeline

* Start working on deleting pipes

* Fix camera nickname change

* run spotless

* Fix some test stuff

* Update VisionModuleManagerTest.java

* vision source manager test is still broken

* Fix VisionSourceManager test

* Apply spotless 2 electric boogaloo

Co-authored-by: Banks Troutman <btrout.dhrs@gmail.com>
Co-authored-by: Declan Freeman-Gleason <declanfreemangleason@gmail.com>
Co-authored-by: Aaryan Agrawal <54345060+13Ducks@users.noreply.github.com>
This commit is contained in:
Matt
2020-07-07 01:01:58 -07:00
committed by GitHub
parent 01712a7396
commit 4cd2262acc
106 changed files with 3666 additions and 623 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
photon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java
View File

@@ -11,6 +11,7 @@ import com.fasterxml.jackson.annotation.JsonAutoDetect;
import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
import com.fasterxml.jackson.annotation.JsonProperty;
import java.util.HashMap;
import java.util.Objects;
/** Represents a 2d pose containing translational and rotational elements. */
@@ -228,4 +229,12 @@ public class Pose2d {
public int hashCode() {
return Objects.hash(m_translation, m_rotation);
}
public HashMap<String, Object> toHashMap() {
var ret = new HashMap<String, Object>();
ret.put("x", getTranslation().getX());
ret.put("y", getTranslation().getY());
ret.put("rot", getRotation().getDegrees());
return ret;
}
}

72
photon-server/src/main/java/org/photonvision/Main.java Normal file
View File

@@ -0,0 +1,72 @@
/*
* Copyright (C) 2020 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;
import java.util.HashMap;
import java.util.List;
import org.photonvision.common.configuration.CameraConfiguration;
import org.photonvision.common.configuration.ConfigManager;
import org.photonvision.common.datatransfer.networktables.NetworkTablesManager;
import org.photonvision.common.logging.Level;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.networking.NetworkManager;
import org.photonvision.common.util.TestUtils;
import org.photonvision.server.Server;
import org.photonvision.vision.camera.USBCameraSource;
import org.photonvision.vision.pipeline.CVPipelineSettings;
import org.photonvision.vision.processes.VisionModuleManager;
import org.photonvision.vision.processes.VisionSource;
import org.photonvision.vision.processes.VisionSourceManager;
public class Main {
private static final Logger logger = new Logger(Main.class, LogGroup.General);
public static final int DEFAULT_WEBPORT = 5800;
public static void main(String[] args) {
Logger.setLevel(LogGroup.Camera, Level.DE_PEST);
Logger.setLevel(LogGroup.Server, Level.DE_PEST);
Logger.setLevel(LogGroup.VisionProcess, Level.DE_PEST);
Logger.setLevel(LogGroup.Data, Level.DE_PEST);
Logger.setLevel(LogGroup.General, Level.DE_PEST);
TestUtils.loadLibraries();
ConfigManager.getInstance(); // init config manager
NetworkManager.getInstance().initialize(false); // basically empty. todo: link to ConfigManager?
NetworkTablesManager.setClientMode("127.0.0.1");
HashMap<String, CameraConfiguration> camConfigs =
ConfigManager.getInstance().getConfig().getCameraConfigurations();
logger.info("Loaded " + camConfigs.size() + " configs from disk!");
List<VisionSource> sources = VisionSourceManager.loadAllSources(camConfigs.values());
var collectedSources = new HashMap<VisionSource, List<CVPipelineSettings>>();
for (var src : sources) {
var usbSrc = (USBCameraSource) src;
collectedSources.put(usbSrc, usbSrc.configuration.pipelineSettings);
}
logger.info("Adding " + collectedSources.size() + " configs to VMM");
VisionModuleManager.getInstance().addSources(collectedSources);
ConfigManager.getInstance().addCameraConfigurations(collectedSources);
VisionModuleManager.getInstance().startModules();
Server.main(DEFAULT_WEBPORT);
}
}

43
photon-server/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java
View File

@@ -40,7 +40,14 @@ public class CameraConfiguration {
public String path = "";
public CameraType cameraType = CameraType.UsbCamera;
public CameraCalibrationCoefficients calibration;
public List<Integer> CameraLEDs = new ArrayList<>();
public List<Integer> cameraLeds = new ArrayList<>();
public int currentPipelineIndex = -1;
@JsonIgnore // this ignores the pipes as we serialize them to their own subfolder
public List<CVPipelineSettings> pipelineSettings = new ArrayList<>();
@JsonIgnore
public DriverModePipelineSettings driveModeSettings = new DriverModePipelineSettings();
public CameraConfiguration(String baseName, String path) {
this(baseName, baseName, baseName, path);
@@ -51,6 +58,15 @@ public class CameraConfiguration {
this.uniqueName = uniqueName;
this.nickname = nickname;
this.path = path;
logger.debug(
"Creating USB camera configuration for "
+ cameraType
+ baseName
+ " (AKA "
+ nickname
+ ") at "
+ path);
}
@JsonCreator
@@ -62,7 +78,8 @@ public class CameraConfiguration {
@JsonProperty("path") String path,
@JsonProperty("cameraType") CameraType cameraType,
@JsonProperty("calibration") CameraCalibrationCoefficients calibration,
@JsonProperty("CameraLEDs") List<Integer> cameraLEDs) {
@JsonProperty("cameraLEDs") List<Integer> cameraLeds,
@JsonProperty("currentPipelineIndex") int currentPipelineIndex) {
this.baseName = baseName;
this.uniqueName = uniqueName;
this.nickname = nickname;
@@ -70,15 +87,19 @@ public class CameraConfiguration {
this.path = path;
this.cameraType = cameraType;
this.calibration = calibration;
this.CameraLEDs = cameraLEDs;
this.cameraLeds = cameraLeds;
this.currentPipelineIndex = currentPipelineIndex;
logger.debug(
"Creating camera configuration for "
+ cameraType
+ baseName
+ " (AKA "
+ nickname
+ ") at "
+ path);
}
@JsonIgnore // this ignores the pipes as we serialize them to their own subfolder
public final List<CVPipelineSettings> pipelineSettings = new ArrayList<>();
@JsonIgnore
public DriverModePipelineSettings driveModeSettings = new DriverModePipelineSettings();
public void addPipelineSettings(List<CVPipelineSettings> settings) {
for (var setting : settings) {
addPipelineSetting(setting);
@@ -101,4 +122,8 @@ public class CameraConfiguration {
pipelineSettings.add(setting);
pipelineSettings.sort(PipelineManager.PipelineSettingsIndexComparator);
}
public void setPipelineSettings(List<CVPipelineSettings> settings) {
pipelineSettings = settings;
}
}

33
photon-server/src/main/java/org/photonvision/common/configuration/ConfigManager.java
View File

@@ -32,6 +32,7 @@ import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.file.JacksonUtils;
import org.photonvision.vision.pipeline.CVPipelineSettings;
import org.photonvision.vision.pipeline.DriverModePipelineSettings;
import org.photonvision.vision.processes.VisionSource;
public class ConfigManager {
private static final Logger logger = new Logger(ConfigManager.class, LogGroup.General);
@@ -65,10 +66,11 @@ public class ConfigManager {
this.networkConfigFile =
new File(Path.of(rootFolder.toString(), "networkSettings.json").toUri());
this.camerasFolder = new File(Path.of(rootFolder.toString(), "cameras").toUri());
load();
}
public void load() {
private void load() {
logger.info("Loading settings...");
if (!rootFolder.exists()) {
if (rootFolder.mkdirs()) {
@@ -150,6 +152,7 @@ public class ConfigManager {
var subdir = Path.of(camerasFolder.toPath().toString(), subdirName);
if (!subdir.toFile().exists()) {
// TODO: check for error
subdir.toFile().mkdirs();
}
@@ -166,10 +169,20 @@ public class ConfigManager {
logger.error("Could not save drivermode.json for " + subdir);
}
// Delete old pipe configs so that we don't get any conflicts
try {
var pipelineFolder = Path.of(subdir.toString(), "pipelines");
Files.list(pipelineFolder).forEach(it -> it.toFile().delete());
} catch (IOException e) {
logger.error("Exception while deleting old configs!");
e.printStackTrace();
}
for (var pipe : camConfig.pipelineSettings) {
var pipePath = Path.of(subdir.toString(), "pipelines", pipe.pipelineNickname + ".json");
if (!pipePath.getParent().toFile().exists()) {
// TODO: check for error
pipePath.getParent().toFile().mkdirs();
}
@@ -235,9 +248,7 @@ public class ConfigManager {
var relativizedFilePath =
getRootFolder().toAbsolutePath().relativize(p).toString();
try {
var ret = JacksonUtils.deserialize(p, CVPipelineSettings.class);
return ret;
return JacksonUtils.deserialize(p, CVPipelineSettings.class);
} catch (JsonProcessingException e) {
logger.error("Exception while deserializing " + relativizedFilePath);
e.printStackTrace();
@@ -260,4 +271,18 @@ public class ConfigManager {
}
return loadedConfigurations;
}
public void addCameraConfigurations(HashMap<VisionSource, List<CVPipelineSettings>> sources) {
List<CameraConfiguration> list =
sources.keySet().stream()
.map(it -> it.getSettables().getConfiguration())
.collect(Collectors.toList());
getConfig().addCameraConfigs(list);
save();
}
public void saveModule(CameraConfiguration config, String uniqueName) {
getConfig().addCameraConfig(uniqueName, config);
save();
}
}

13
photon-server/src/main/java/org/photonvision/common/configuration/NetworkConfig.java
View File

@@ -17,13 +17,24 @@
package org.photonvision.common.configuration;
import java.util.HashMap;
import org.photonvision.common.networking.NetworkMode;
public class NetworkConfig {
public int teamNumber = 1577;
public int teamNumber = 1;
public NetworkMode connectionType = NetworkMode.DHCP;
public String ip = "";
public String gateway = "";
public String netmask = "";
public String hostname = "photonvision";
public HashMap<String, Object> toHashMap() {
HashMap<String, Object> tmp = new HashMap<>();
tmp.put("teamNumber", teamNumber);
tmp.put("connectionType", connectionType.ordinal());
tmp.put("ip", ip);
tmp.put("gateway", gateway);
tmp.put("netmask", netmask);
return tmp;
}
}

37
photon-server/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java
View File

@@ -18,6 +18,12 @@
package org.photonvision.common.configuration;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.stream.Collectors;
import org.photonvision.common.util.SerializationUtils;
import org.photonvision.vision.processes.VisionModule;
import org.photonvision.vision.processes.VisionModuleManager;
// TODO rename this class
public class PhotonConfiguration {
@@ -33,6 +39,12 @@ public class PhotonConfiguration {
return cameraConfigurations;
}
public void addCameraConfigs(List<CameraConfiguration> config) {
for (var c : config) {
addCameraConfig(c);
}
}
public void addCameraConfig(CameraConfiguration config) {
addCameraConfig(config.uniqueName, config);
}
@@ -58,4 +70,29 @@ public class PhotonConfiguration {
this.networkConfig = networkConfig;
this.cameraConfigurations = cameraConfigurations;
}
public Map<String, Object> toHashMap() {
Map<String, Object> map = new HashMap<>();
map.put("networkSettings", networkConfig.toHashMap());
map.put(
"cameraSettings",
VisionModuleManager.getInstance().getModules().stream()
.map(VisionModule::toUICameraConfig)
.map(SerializationUtils::objectToHashMap)
.collect(Collectors.toList()));
return map;
}
public static class UICameraConfiguration {
@SuppressWarnings("unused")
public double fov, tiltDegrees;
public String nickname;
public HashMap<String, Object> currentPipelineSettings;
public int currentPipelineIndex;
public List<String> pipelineNicknames;
public HashMap<Integer, HashMap<String, Object>> videoFormatList;
public int streamPort;
}
}

35
photon-server/src/main/java/org/photonvision/common/dataflow/DataChangeDestination.java Normal file
View File

@@ -0,0 +1,35 @@
/*
* Copyright (C) 2020 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.common.dataflow;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
public enum DataChangeDestination {
DCD_ACTIVEMODULE,
DCD_ACTIVEPIPELINESETTINGS,
DCD_GENSETTINGS,
DCD_UI,
DCD_OTHER;
public static final List<DataChangeDestination> AllDestinations =
Arrays.asList(DataChangeDestination.values());
public static class DataChangeDestinationList extends ArrayList<DataChangeDestination> {}
}

109
photon-server/src/main/java/org/photonvision/common/dataflow/DataChangeService.java Normal file
View File

@@ -0,0 +1,109 @@
/*
* Copyright (C) 2020 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.common.dataflow;
import java.util.concurrent.BlockingQueue;
import java.util.concurrent.CopyOnWriteArrayList;
import java.util.concurrent.LinkedBlockingQueue;
import java.util.stream.Collectors;
import org.photonvision.common.dataflow.events.DataChangeEvent;
import org.photonvision.common.logging.Level;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
@SuppressWarnings("rawtypes")
public class DataChangeService {
private static final Logger logger = new Logger(DataChangeService.class, LogGroup.Server);
private static class ThreadSafeSingleton {
private static final DataChangeService INSTANCE = new DataChangeService();
}
public static DataChangeService getInstance() {
return ThreadSafeSingleton.INSTANCE;
}
private final CopyOnWriteArrayList<DataChangeSubscriber> subscribers;
@SuppressWarnings("FieldCanBeLocal")
private final Thread dispatchThread;
private final BlockingQueue<DataChangeEvent> eventQueue = new LinkedBlockingQueue<>();
private DataChangeService() {
subscribers = new CopyOnWriteArrayList<>();
dispatchThread = new Thread(this::dispatchFromQueue);
dispatchThread.setName("EventDispatchThread");
dispatchThread.start();
}
public boolean hasEvents() {
return !eventQueue.isEmpty();
}
private void dispatchFromQueue() {
while (true) {
try {
var taken = eventQueue.take();
for (var sub : subscribers) {
if (sub.wantedSources.contains(taken.sourceType)
&& sub.wantedDestinations.contains(taken.destType)) {
sub.onDataChangeEvent(taken);
}
}
} catch (Exception e) {
logger.error("Exception when dispatching event!");
e.printStackTrace();
}
}
}
public void addSubscriber(DataChangeSubscriber subscriber) {
if (!subscribers.addIfAbsent(subscriber)) {
logger.warn("Attempted to add already added subscriber!");
} else {
if (Logger.shouldLog(Level.TRACE, LogGroup.Data)) {
var sources =
subscriber.wantedSources.stream().map(Enum::toString).collect(Collectors.joining(", "));
var dests =
subscriber.wantedDestinations.stream()
.map(Enum::toString)
.collect(Collectors.joining(", "));
logger.trace("Added subscriber - " + "Sources: " + sources + ", Destinations: " + dests);
}
}
}
public void addSubscribers(DataChangeSubscriber... subs) {
for (var sub : subs) {
addSubscriber(sub);
}
}
public void publishEvent(DataChangeEvent event) {
eventQueue.offer(event);
}
public void publishEvents(DataChangeEvent... events) {
for (var event : events) {
publishEvent(event);
}
}
}

34
photon-server/src/main/java/org/photonvision/common/dataflow/DataChangeSource.java Normal file
View File

@@ -0,0 +1,34 @@
/*
* Copyright (C) 2020 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.common.dataflow;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
public enum DataChangeSource {
DCS_WEBSOCKET,
DCS_HTTP,
DCS_NETWORKTABLES,
DCS_VISIONMODULE,
DCS_OTHER;
public static final List<DataChangeSource> AllSources = Arrays.asList(DataChangeSource.values());
public static class DataChangeSourceList extends ArrayList<DataChangeSource> {}
}

56
photon-server/src/main/java/org/photonvision/common/dataflow/DataChangeSubscriber.java Normal file
View File

@@ -0,0 +1,56 @@
/*
* Copyright (C) 2020 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.common.dataflow;
import java.util.List;
import java.util.Objects;
import org.photonvision.common.dataflow.events.DataChangeEvent;
@SuppressWarnings("rawtypes")
public abstract class DataChangeSubscriber {
public final List<DataChangeSource> wantedSources;
public final List<DataChangeDestination> wantedDestinations;
private final int hash;
public DataChangeSubscriber(
List<DataChangeSource> wantedSources, List<DataChangeDestination> wantedDestinations) {
this.wantedSources = wantedSources;
this.wantedDestinations = wantedDestinations;
hash = Objects.hash(wantedSources, wantedDestinations);
}
public DataChangeSubscriber() {
this(DataChangeSource.AllSources, DataChangeDestination.AllDestinations);
}
public DataChangeSubscriber(DataChangeSource.DataChangeSourceList wantedSources) {
this(wantedSources, DataChangeDestination.AllDestinations);
}
public DataChangeSubscriber(DataChangeDestination.DataChangeDestinationList wantedDestinations) {
this(DataChangeSource.AllSources, wantedDestinations);
}
public abstract void onDataChangeEvent(DataChangeEvent event);
@Override
public int hashCode() {
return hash;
}
}

20
photon-server/src/main/java/org/photonvision/common/dataflow/DataProvider.java Normal file
View File

@@ -0,0 +1,20 @@
/*
* Copyright (C) 2020 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.common.dataflow;
public interface DataProvider {}

42
photon-server/src/main/java/org/photonvision/common/dataflow/camera/IncomingCameraCommandSubscriber.java Normal file
View File

@@ -0,0 +1,42 @@
/*
* Copyright (C) 2020 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.common.dataflow.camera;
import org.photonvision.common.dataflow.DataChangeSubscriber;
import org.photonvision.common.dataflow.events.DataChangeEvent;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.vision.processes.VisionModuleManager;
public class IncomingCameraCommandSubscriber extends DataChangeSubscriber {
private static final Logger logger =
new Logger(IncomingCameraCommandSubscriber.class, LogGroup.Camera);
private final VisionModuleManager vmm;
public IncomingCameraCommandSubscriber(VisionModuleManager instance) {
this.vmm = instance;
}
@Override
public void onDataChangeEvent(DataChangeEvent event) {
// logger.de_pest("Got event from [" + event.sourceType + "] and dest [" + event.destType
// + "] with property name [" + event.propertyName
// + "] and value [" + event.data + "]");
}
}

54
photon-server/src/main/java/org/photonvision/common/dataflow/events/DataChangeEvent.java Normal file
View File

@@ -0,0 +1,54 @@
/*
* Copyright (C) 2020 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.common.dataflow.events;
import org.photonvision.common.dataflow.DataChangeDestination;
import org.photonvision.common.dataflow.DataChangeSource;
public class DataChangeEvent<T> {
public final DataChangeSource sourceType;
public final DataChangeDestination destType;
public final String propertyName;
public final T data;
public DataChangeEvent(
DataChangeSource sourceType,
DataChangeDestination destType,
String propertyName,
T newValue) {
this.sourceType = sourceType;
this.destType = destType;
this.propertyName = propertyName;
this.data = newValue;
}
@Override
public String toString() {
return "DataChangeEvent{"
+ "sourceType="
+ sourceType
+ ", destType="
+ destType
+ ", propertyName='"
+ propertyName
+ '\''
+ ", data="
+ data
+ '}';
}
}

31
photon-server/src/main/java/org/photonvision/common/dataflow/events/HTTPRequestEvent.java Normal file
View File

@@ -0,0 +1,31 @@
/*
* Copyright (C) 2020 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.common.dataflow.events;
import org.photonvision.common.dataflow.DataChangeDestination;
import org.photonvision.common.dataflow.DataChangeSource;
public class HTTPRequestEvent<T> extends DataChangeEvent<T> {
public HTTPRequestEvent(
DataChangeSource sourceType,
DataChangeDestination destType,
String propertyName,
T newValue) {
super(sourceType, destType, propertyName, newValue);
}
}

60
photon-server/src/main/java/org/photonvision/common/dataflow/events/IncomingWebSocketEvent.java Normal file
View File

@@ -0,0 +1,60 @@
/*
* Copyright (C) 2020 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.common.dataflow.events;
import java.util.HashMap;
import org.photonvision.common.dataflow.DataChangeDestination;
import org.photonvision.common.dataflow.DataChangeSource;
public class IncomingWebSocketEvent<T> extends DataChangeEvent<T> {
public final Integer cameraIndex;
public IncomingWebSocketEvent(DataChangeDestination destType, String propertyName, T newValue) {
this(destType, propertyName, newValue, null);
}
public IncomingWebSocketEvent(
DataChangeDestination destType, String propertyName, T newValue, Integer cameraIndex) {
super(DataChangeSource.DCS_WEBSOCKET, destType, propertyName, newValue);
this.cameraIndex = cameraIndex;
}
@SuppressWarnings("unchecked")
public IncomingWebSocketEvent(
DataChangeDestination destType, String dataKey, HashMap<String, Object> data) {
this(destType, dataKey, (T) data.get(dataKey));
}
@Override
public String toString() {
return "IncomingWebSocketEvent{"
+ "cameraIndex="
+ cameraIndex
+ ", sourceType="
+ sourceType
+ ", destType="
+ destType
+ ", propertyName='"
+ propertyName
+ '\''
+ ", data="
+ data
+ '}';
}
}

37
photon-server/src/main/java/org/photonvision/common/dataflow/events/OutgoingUIEvent.java Normal file
View File

@@ -0,0 +1,37 @@
/*
* Copyright (C) 2020 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.common.dataflow.events;
import java.util.HashMap;
import org.photonvision.common.dataflow.DataChangeDestination;
import org.photonvision.common.dataflow.DataChangeSource;
import org.photonvision.server.UIUpdateType;
public class OutgoingUIEvent<T> extends DataChangeEvent<T> {
public final UIUpdateType updateType;
public OutgoingUIEvent(UIUpdateType updateType, String propertyName, T newValue) {
super(DataChangeSource.DCS_WEBSOCKET, DataChangeDestination.DCD_UI, propertyName, newValue);
this.updateType = updateType;
}
@SuppressWarnings("unchecked")
public OutgoingUIEvent(UIUpdateType updateType, String dataKey, HashMap<String, Object> data) {
this(updateType, dataKey, (T) data.get(dataKey));
}
}

50
photon-server/src/main/java/org/photonvision/common/dataflow/networktables/NTDataConsumer.java Normal file
View File

@@ -0,0 +1,50 @@
/*
* Copyright (C) 2020 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.common.dataflow.networktables;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import org.photonvision.common.datatransfer.DataConsumer;
import org.photonvision.vision.pipeline.result.SimplePipelineResult;
import org.photonvision.vision.processes.Data;
public class NTDataConsumer implements DataConsumer {
private final NetworkTable rootTable;
NetworkTable subTable;
NetworkTableEntry rawData;
public NTDataConsumer(NetworkTable root, String camName) {
this.rootTable = root;
this.subTable = root.getSubTable(camName);
rawData = subTable.getEntry("rawData");
}
public void setCameraName(String camName) {
this.subTable = rootTable.getSubTable(camName);
rawData = subTable.getEntry("rawData");
}
@Override
public void accept(Data data) {
var simplified = new SimplePipelineResult(data.result);
var bytes = simplified.toByteArray();
rawData.setRaw(bytes);
rootTable.getInstance().flush();
}
}

97
photon-server/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java Normal file
View File

@@ -0,0 +1,97 @@
/*
* Copyright (C) 2020 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.common.dataflow.networktables;
import edu.wpi.first.networktables.LogMessage;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import java.util.function.Consumer;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.scripting.ScriptEventType;
import org.photonvision.common.scripting.ScriptManager;
public class NetworkTablesManager {
private NetworkTablesManager() {}
private static final Logger logger = new Logger(NetworkTablesManager.class, LogGroup.General);
private static final NetworkTableInstance ntInstance = NetworkTableInstance.getDefault();
public static final String kRootTableName = "/chameleon-vision";
public static final NetworkTable kRootTable =
NetworkTableInstance.getDefault().getTable(kRootTableName);
public static boolean isServer = false;
private static int getTeamNumber() {
// TODO: FIX
return 0;
// return ConfigManager.settings.teamNumber;
}
private static class NTLogger implements Consumer<LogMessage> {
private boolean hasReportedConnectionFailure = false;
@Override
public void accept(LogMessage logMessage) {
if (!hasReportedConnectionFailure && logMessage.message.contains("timed out")) {
logger.error("NT Connection has failed! Will retry in background.");
hasReportedConnectionFailure = true;
} else if (logMessage.message.contains("connected")) {
logger.info("NT Connected!");
hasReportedConnectionFailure = false;
ScriptManager.queueEvent(ScriptEventType.kNTConnected);
}
}
}
static {
NetworkTableInstance.getDefault().addLogger(new NTLogger(), 0, 255); // to hide error messages
}
public static void setClientMode(String host) {
isServer = false;
logger.info("Starting NT Client");
ntInstance.stopServer();
if (host != null) {
ntInstance.startClient(host);
} else {
ntInstance.startClientTeam(getTeamNumber());
if (ntInstance.isConnected()) {
logger.info("[NetworkTablesManager] Connected to the robot!");
} else {
logger.info(
"[NetworkTablesManager] Could NOT to the robot! Will retry in the background...");
}
}
}
public static void setTeamClientMode() {
setClientMode(null);
}
public static void setServerMode() {
isServer = true;
logger.info("Starting NT Server");
ntInstance.stopClient();
ntInstance.startServer();
}
}

141
photon-server/src/main/java/org/photonvision/common/dataflow/structures/BytePackable.java Normal file
View File

@@ -0,0 +1,141 @@
/*
* Copyright (C) 2020 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.common.dataflow.structures;
@SuppressWarnings("PointlessBitwiseExpression")
public abstract class BytePackable {
public abstract byte[] toByteArray();
public abstract void fromByteArray(byte[] src);
protected int bufferPosition = 0;
public int getBufferPosition() {
return bufferPosition;
}
public void resetBufferPosition() {
bufferPosition = 0;
}
protected void bufferData(byte[] src, byte[] dest) {
System.arraycopy(src, 0, dest, bufferPosition, src.length);
bufferPosition += src.length;
}
protected void bufferData(byte src, byte[] dest) {
System.arraycopy(new byte[] {src}, 0, dest, bufferPosition, 1);
bufferPosition++;
}
protected void bufferData(int src, byte[] dest) {
System.arraycopy(toBytes(src), 0, dest, bufferPosition, Integer.BYTES);
bufferPosition += Integer.BYTES;
}
protected void bufferData(double src, byte[] dest) {
System.arraycopy(toBytes(src), 0, dest, bufferPosition, Double.BYTES);
bufferPosition += Double.BYTES;
}
protected void bufferData(boolean src, byte[] dest) {
System.arraycopy(toBytes(src), 0, dest, bufferPosition, 1);
bufferPosition += 1;
}
protected boolean unbufferBoolean(byte[] src) {
return toBoolean(src, bufferPosition++);
}
protected byte unbufferByte(byte[] src) {
var value = src[bufferPosition];
bufferPosition++;
return value;
}
protected byte[] unbufferBytes(byte[] src, int len) {
var bytes = new byte[len];
System.arraycopy(src, bufferPosition, bytes, 0, len);
return bytes;
}
protected int unbufferInt(byte[] src) {
var value = toInt(src, bufferPosition);
bufferPosition += Integer.BYTES;
return value;
}
protected double unbufferDouble(byte[] src) {
var value = toDouble(src, bufferPosition);
bufferPosition += Double.BYTES;
return value;
}
private static boolean toBoolean(byte[] src, int pos) {
return src[pos] != 0;
}
private static int toInt(byte[] src, int pos) {
return (0xff & src[pos]) << 24
| (0xff & src[pos + 1]) << 16
| (0xff & src[pos + 2]) << 8
| (0xff & src[pos + 3]) << 0;
}
private static long toLong(byte[] src, int pos) {
return (long) (0xff & src[pos]) << 56
| (long) (0xff & src[pos + 1]) << 48
| (long) (0xff & src[pos + 2]) << 40
| (long) (0xff & src[pos + 3]) << 32
| (long) (0xff & src[pos + 4]) << 24
| (long) (0xff & src[pos + 5]) << 16
| (long) (0xff & src[pos + 6]) << 8
| (long) (0xff & src[pos + 7]) << 0;
}
private static double toDouble(byte[] src, int pos) {
return Double.longBitsToDouble(toLong(src, pos));
}
protected byte[] toBytes(double value) {
long data = Double.doubleToRawLongBits(value);
return new byte[] {
(byte) ((data >> 56) & 0xff),
(byte) ((data >> 48) & 0xff),
(byte) ((data >> 40) & 0xff),
(byte) ((data >> 32) & 0xff),
(byte) ((data >> 24) & 0xff),
(byte) ((data >> 16) & 0xff),
(byte) ((data >> 8) & 0xff),
(byte) ((data >> 0) & 0xff),
};
}
protected byte[] toBytes(int value) {
return new byte[] {
(byte) ((value >> 24) & 0xff),
(byte) ((value >> 16) & 0xff),
(byte) ((value >> 8) & 0xff),
(byte) ((value >> 0) & 0xff)
};
}
protected byte[] toBytes(boolean value) {
return new byte[] {(byte) (value ? 1 : 0)};
}
}

110
photon-server/src/main/java/org/photonvision/common/dataflow/structures/SimplePipelineResult.java Normal file
View File

@@ -0,0 +1,110 @@
/*
* Copyright (C) 2020 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.common.dataflow.structures;
import java.util.ArrayList;
import java.util.List;
import java.util.Objects;
import org.photonvision.vision.pipeline.result.CVPipelineResult;
public class SimplePipelineResult extends BytePackable {
private double latencyMillis;
private boolean hasTargets;
public final List<SimpleTrackedTarget> targets = new ArrayList<>();
public SimplePipelineResult() {}
public SimplePipelineResult(
double latencyMillis, boolean hasTargets, List<SimpleTrackedTarget> targets) {
this.latencyMillis = latencyMillis;
this.hasTargets = hasTargets;
this.targets.addAll(targets);
}
public SimplePipelineResult(CVPipelineResult origResult) {
update(origResult);
}
public void update(CVPipelineResult origResult) {
latencyMillis = origResult.getLatencyMillis();
hasTargets = origResult.hasTargets();
targets.clear();
for (var origTarget : origResult.targets) {
targets.add(new SimpleTrackedTarget(origTarget));
}
}
public double getLatencyMillis() {
return latencyMillis;
}
public boolean hasTargets() {
return hasTargets;
}
@Override
public boolean equals(Object o) {
if (this == o) return true;
if (o == null || getClass() != o.getClass()) return false;
SimplePipelineResult that = (SimplePipelineResult) o;
boolean latencyMatch = Double.compare(that.latencyMillis, latencyMillis) == 0;
boolean hasTargetsMatch = that.hasTargets == hasTargets;
boolean targetsMatch = that.targets.equals(targets);
return latencyMatch && hasTargetsMatch && targetsMatch;
}
@Override
public int hashCode() {
return Objects.hash(latencyMillis, hasTargets, targets);
}
@Override
public byte[] toByteArray() {
bufferPosition = 0;
int bufferSize =
8 + 1 + 1 + (targets.size() * 48); // latencyMillis + hasTargets + targetCount + targets
var buff = new byte[bufferSize];
bufferData(latencyMillis, buff);
bufferData(hasTargets, buff);
bufferData((byte) targets.size(), buff);
for (var target : targets) {
bufferData(target.toByteArray(), buff);
}
return buff;
}
@Override
public void fromByteArray(byte[] src) {
bufferPosition = 0;
latencyMillis = unbufferDouble(src);
hasTargets = unbufferBoolean(src);
byte targetCount = unbufferByte(src);
targets.clear();
for (int i = 0; i < targetCount; i++) {
var target = new SimpleTrackedTarget();
target.fromByteArray(unbufferBytes(src, 48));
bufferPosition += 48;
targets.add(target);
}
}
}

97
photon-server/src/main/java/org/photonvision/common/dataflow/structures/SimpleTrackedTarget.java Normal file
View File

@@ -0,0 +1,97 @@
/*
* Copyright (C) 2020 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.common.dataflow.structures;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import java.util.Objects;
import org.photonvision.vision.target.TrackedTarget;
public class SimpleTrackedTarget extends BytePackable {
public static final int PACK_SIZE_BYTES = Double.BYTES * 6;
private double yaw;
private double pitch;
private double area;
private Pose2d robotRelativePose = new Pose2d();
public SimpleTrackedTarget() {}
public SimpleTrackedTarget(double yaw, double pitch, double area, Pose2d pose) {
this.yaw = yaw;
this.pitch = pitch;
this.area = area;
robotRelativePose = pose;
}
public SimpleTrackedTarget(TrackedTarget origTarget) {
yaw = origTarget.getYaw();
pitch = origTarget.getPitch();
area = origTarget.getArea();
robotRelativePose = origTarget.getRobotRelativePose();
}
@Override
public boolean equals(Object o) {
if (this == o) return true;
if (o == null || getClass() != o.getClass()) return false;
SimpleTrackedTarget that = (SimpleTrackedTarget) o;
return Double.compare(that.yaw, yaw) == 0
&& Double.compare(that.pitch, pitch) == 0
&& Double.compare(that.area, area) == 0
&& Objects.equals(robotRelativePose, that.robotRelativePose);
}
@Override
public int hashCode() {
return Objects.hash(yaw, pitch, area, robotRelativePose);
}
@Override
public byte[] toByteArray() {
resetBufferPosition();
byte[] data = new byte[PACK_SIZE_BYTES];
bufferData(yaw, data);
bufferData(pitch, data);
bufferData(area, data);
bufferData(robotRelativePose.getTranslation().getX(), data);
bufferData(robotRelativePose.getTranslation().getY(), data);
bufferData(robotRelativePose.getRotation().getDegrees(), data);
return data;
}
@Override
public void fromByteArray(byte[] src) {
resetBufferPosition();
if (src.length < PACK_SIZE_BYTES) {
// TODO: log error?
return;
}
yaw = unbufferDouble(src);
pitch = unbufferDouble(src);
area = unbufferDouble(src);
var poseX = unbufferDouble(src);
var poseY = unbufferDouble(src);
var poseR = unbufferDouble(src);
robotRelativePose = new Pose2d(poseX, poseY, Rotation2d.fromDegrees(poseR));
}
}

5
photon-server/src/main/java/org/photonvision/common/datatransfer/networktables/NetworkTablesManager.java
View File

@@ -21,6 +21,7 @@ import edu.wpi.first.networktables.LogMessage;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import java.util.function.Consumer;
import org.photonvision.common.configuration.ConfigManager;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.scripting.ScriptEventType;
@@ -41,9 +42,7 @@ public class NetworkTablesManager {
public static boolean isServer = false;
private static int getTeamNumber() {
// TODO: FIX
return 0;
// return ConfigManager.settings.teamNumber;
return ConfigManager.getInstance().getConfig().getNetworkConfig().teamNumber;
}
private static class NTLogger implements Consumer<LogMessage> {

1
photon-server/src/main/java/org/photonvision/common/logging/LogGroup.java
View File

@@ -21,5 +21,6 @@ public enum LogGroup {
Camera,
Server,
VisionProcess,
Data,
General
}

31
photon-server/src/main/java/org/photonvision/common/logging/Logger.java
View File

@@ -27,11 +27,12 @@ import java.util.ArrayList;
import java.util.Date;
import java.util.HashMap;
import java.util.List;
import org.photonvision.common.dataflow.DataChangeService;
import org.photonvision.common.dataflow.events.OutgoingUIEvent;
import org.photonvision.server.UIUpdateType;
public class Logger {
private final String className;
public static final String ANSI_RESET = "\u001B[0m";
public static final String ANSI_BLACK = "\u001B[30m";
public static final String ANSI_RED = "\u001B[31m";
@@ -42,7 +43,10 @@ public class Logger {
public static final String ANSI_CYAN = "\u001B[36m";
public static final String ANSI_WHITE = "\u001B[37m";
private static SimpleDateFormat simpleDateFormat = new SimpleDateFormat("yyyy-MM-dd HH:mm:ss");
private static final SimpleDateFormat simpleDateFormat =
new SimpleDateFormat("yyyy-MM-dd HH:mm:ss");
private final String className;
private final LogGroup group;
public Logger(Class<?> clazz, LogGroup group) {
@@ -50,6 +54,11 @@ public class Logger {
this.group = group;
}
public Logger(Class<?> clazz, String suffix, LogGroup group) {
this.className = clazz.getSimpleName() + " - " + suffix;
this.group = group;
}
public static String getDate() {
return simpleDateFormat.format(new Date());
}
@@ -81,11 +90,13 @@ public class Logger {
levelMap.put(LogGroup.Camera, Level.INFO);
levelMap.put(LogGroup.General, Level.INFO);
levelMap.put(LogGroup.Server, Level.INFO);
levelMap.put(LogGroup.Data, Level.INFO);
levelMap.put(LogGroup.VisionProcess, Level.INFO);
}
static {
currentAppenders.add(new ConsoleAppender());
currentAppenders.add(new UILogAppender());
}
public static void addFileAppender(Path logFilePath) {
@@ -113,7 +124,7 @@ public class Logger {
}
}
private static boolean shouldLog(Level logLevel, LogGroup group) {
public static boolean shouldLog(Level logLevel, LogGroup group) {
return logLevel.code <= levelMap.get(group).code;
}
@@ -152,8 +163,18 @@ public class Logger {
}
}
private static class UILogAppender extends Appender {
@Override
void log(String message) {
var message_ = new HashMap<>();
message_.put("logMessage", message);
DataChangeService.getInstance()
.publishEvent(new OutgoingUIEvent<>(UIUpdateType.BROADCAST, "log", message_));
}
}
private static class AsyncFileAppender extends Appender {
private Path filePath;
private final Path filePath;
public AsyncFileAppender(Path logFilePath) {
this.filePath = logFilePath;

46
photon-server/src/main/java/org/photonvision/common/util/SerializationUtils.java Normal file
View File

@@ -0,0 +1,46 @@
/*
* Copyright (C) 2020 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.common.util;
import java.util.HashMap;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
public final class SerializationUtils {
private static final Logger LOGGER = new Logger(SerializationUtils.class, LogGroup.General);
public static HashMap<String, Object> objectToHashMap(Object src) {
var ret = new HashMap<String, Object>();
for (var field : src.getClass().getFields()) {
try {
if (!field
.getType()
.isEnum()) { // if the field is not an enum, get it based on the current pipeline
ret.put(field.getName(), field.get(src));
} else {
var ordinal = (Enum) field.get(src);
ret.put(field.getName(), ordinal.ordinal());
}
} catch (IllegalArgumentException | IllegalAccessException e) {
LOGGER.error("Could not serialize " + src.getClass().getSimpleName());
e.printStackTrace();
}
}
return ret;
}
}

4
photon-server/src/main/java/org/photonvision/common/util/numbers/DoubleCouple.java
View File

@@ -25,6 +25,10 @@ public class DoubleCouple extends NumberCouple<Double> {
super(0.0, 0.0);
}
public DoubleCouple(Number first, Number second) {
super(first.doubleValue(), second.doubleValue());
}
public DoubleCouple(Double first, Double second) {
super(first, second);
}

44
photon-server/src/main/java/org/photonvision/server/Server.java
View File

@@ -18,24 +18,54 @@
package org.photonvision.server;
import io.javalin.Javalin;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
public class Server {
private static final Logger logger = new Logger(Server.class, LogGroup.Server);
public static void main(int port) {
Javalin app =
Javalin.create(
javalinConfig -> {
javalinConfig.showJavalinBanner = false;
javalinConfig.addStaticFiles("web");
javalinConfig.enableCorsForAllOrigins();
config -> {
config.showJavalinBanner = false;
config.addStaticFiles("web");
config.enableCorsForAllOrigins();
config.requestLogger(
(ctx, ms) ->
logger.debug(
"Handled HTTP "
+ ctx.req.getMethod()
+ " request from "
+ ctx.req.getRemoteHost()
+ " in "
+ ms.toString()
+ "ms"));
config.wsLogger(
ws ->
ws.onMessage(
ctx -> logger.debug("Got WebSockets message: " + ctx.message())));
config.wsLogger(
ws ->
ws.onBinaryMessage(
ctx ->
logger.debug(
"Got WebSockets binary message from host " + ctx.host())));
});
var socketHandler = SocketHandler.getInstance();
/*Web Socket Events */
app.ws(
"/websocket",
ws -> {
ws.onConnect(SocketHandler::onConnect);
ws.onClose(SocketHandler::onClose);
ws.onBinaryMessage(SocketHandler::onBinaryMessage);
ws.onConnect(socketHandler::onConnect);
ws.onClose(socketHandler::onClose);
ws.onBinaryMessage(socketHandler::onBinaryMessage);
});
/*API Events*/
app.post("/api/settings/general", RequestHandler::onGeneralSettings);

262
photon-server/src/main/java/org/photonvision/server/SocketHandler.java
View File

@@ -23,49 +23,273 @@ import com.fasterxml.jackson.databind.ObjectMapper;
import io.javalin.websocket.*;
import java.io.IOException;
import java.nio.ByteBuffer;
import java.util.ArrayList;
import java.util.List;
import java.util.Map;
import java.util.*;
import org.apache.commons.lang3.tuple.Pair;
import org.msgpack.jackson.dataformat.MessagePackFactory;
import org.photonvision.common.dataflow.DataChangeDestination;
import org.photonvision.common.dataflow.DataChangeService;
import org.photonvision.common.dataflow.camera.IncomingCameraCommandSubscriber;
import org.photonvision.common.dataflow.events.IncomingWebSocketEvent;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.vision.pipeline.PipelineType;
import org.photonvision.vision.processes.PipelineManager;
import org.photonvision.vision.processes.VisionModuleManager;
@SuppressWarnings("rawtypes")
public class SocketHandler {
static List<WsContext> users = new ArrayList<>();
static ObjectMapper objectMapper = new ObjectMapper(new MessagePackFactory());
public static void onConnect(WsConnectContext context) {
users.add(context);
private final Logger logger = new Logger(SocketHandler.class, LogGroup.Server);
private final List<WsContext> users = new ArrayList<>();
private final ObjectMapper objectMapper = new ObjectMapper(new MessagePackFactory());
private final DataChangeService dcService = DataChangeService.getInstance();
@SuppressWarnings("FieldCanBeLocal")
private final UIOutboundSubscriber uiOutboundSubscriber = new UIOutboundSubscriber(this);
private final IncomingCameraCommandSubscriber cameraChangeSubscriber =
new IncomingCameraCommandSubscriber(VisionModuleManager.getInstance());
public static class UIMap extends HashMap<String, Object> {}
abstract static class SelectiveBroadcastPair extends Pair<UIMap, WsContext> {}
private static class ThreadSafeSingleton {
private static final SocketHandler INSTANCE = new SocketHandler();
}
static void onClose(WsCloseContext context) {
public static SocketHandler getInstance() {
return SocketHandler.ThreadSafeSingleton.INSTANCE;
}
private SocketHandler() {
dcService.addSubscribers(
uiOutboundSubscriber,
cameraChangeSubscriber,
new UIInboundSubscriber()); // Subscribe outgoing messages to the data change service
}
public void onConnect(WsConnectContext context) {
context.session.setIdleTimeout(Long.MAX_VALUE); // TODO: determine better value
var host = context.session.getRemote().getInetSocketAddress().getHostName();
logger.info("New websocket connection from " + host);
users.add(context);
dcService.publishEvent(
new IncomingWebSocketEvent<>(
DataChangeDestination.DCD_GENSETTINGS, "userConnected", context));
}
protected void onClose(WsCloseContext context) {
var host = context.session.getRemote().getInetSocketAddress().getHostName();
var reason = context.reason() != null ? context.reason() : "Connection closed by client";
logger.info("Closing websocket connection from " + host + " for reason: " + reason);
users.remove(context);
}
public static void onBinaryMessage(WsBinaryMessageContext context) {
@SuppressWarnings({"unchecked"})
public void onBinaryMessage(WsBinaryMessageContext context) {
try {
Map<String, Object> data =
objectMapper.readValue(context.data(), new TypeReference<Map<String, Object>>() {});
// TODO pass data to ui data provider
Map<String, Object> deserializedData =
objectMapper.readValue(context.data(), new TypeReference<>() {});
// Special case the current camera index
var camIndexValue = deserializedData.get("cameraIndex");
Integer cameraIndex = null;
if (camIndexValue instanceof Integer) {
cameraIndex = (Integer) camIndexValue;
deserializedData.remove("cameraIndex");
}
for (Map.Entry<String, Object> entry : deserializedData.entrySet()) {
try {
var entryKey = entry.getKey();
var entryValue = entry.getValue();
var socketMessageType = SocketMessageType.fromEntryKey(entryKey);
logger.debug(
"Got WS message: ["
+ socketMessageType
+ "] ==> ["
+ entryKey
+ "], ["
+ entryValue
+ "]");
if (socketMessageType == null) {
logger.warn("Got unknown socket message type: " + entryKey);
continue;
}
switch (socketMessageType) {
case SMT_DRIVERMODE:
{
// TODO: what is this event?
var data = (HashMap<String, Object>) entryValue;
var dmExpEvent =
new IncomingWebSocketEvent<Integer>(
DataChangeDestination.DCD_ACTIVEMODULE, "driverExposure", data);
var dmBrightEvent =
new IncomingWebSocketEvent<Integer>(
DataChangeDestination.DCD_ACTIVEMODULE, "driverBrightness", data);
var dmIsDriverEvent =
new IncomingWebSocketEvent<Boolean>(
DataChangeDestination.DCD_ACTIVEMODULE, "isDriver", data);
dcService.publishEvents(dmExpEvent, dmBrightEvent, dmIsDriverEvent);
break;
}
case SMT_CHANGECAMERANAME:
{
var ccnEvent =
new IncomingWebSocketEvent<>(
DataChangeDestination.DCD_ACTIVEMODULE,
"cameraNickname",
(String) entryValue,
cameraIndex);
dcService.publishEvent(ccnEvent);
break;
}
case SMT_CHANGEPIPELINENAME:
{
var cpnEvent =
new IncomingWebSocketEvent<>(
DataChangeDestination.DCD_ACTIVEMODULE,
"pipelineName",
(String) entryValue,
cameraIndex);
dcService.publishEvent(cpnEvent);
break;
}
case SMT_ADDNEWPIPELINE:
{
// HashMap<String, Object> data = (HashMap<String,
// Object>) entryValue;
// var type = (PipelineType)
// data.get("pipelineType");
// var name = (String) data.get("pipelineName");
var type = PipelineType.Reflective;
var name = (String) entryValue;
var newPipelineEvent =
new IncomingWebSocketEvent<>(
DataChangeDestination.DCD_ACTIVEMODULE,
"newPipelineInfo",
Pair.of(name, type),
cameraIndex);
dcService.publishEvent(newPipelineEvent);
break;
}
case SMT_COMMAND:
{
var cmd = SocketMessageCommandType.fromEntryKey((String) entryValue);
switch (cmd) {
case SMCT_DELETECURRENTPIPELINE:
{
var deleteCurrentPipelineEvent =
new IncomingWebSocketEvent<>(
DataChangeDestination.DCD_ACTIVEMODULE,
"deleteCurrPipeline",
0,
cameraIndex);
dcService.publishEvent(deleteCurrentPipelineEvent);
break;
}
case SMCT_SAVE:
{
var saveEvent =
new IncomingWebSocketEvent<>(DataChangeDestination.DCD_OTHER, "save", 0);
dcService.publishEvent(saveEvent);
break;
}
}
break;
}
case SMT_CURRENTCAMERA:
{
var changeCurrentCameraEvent =
new IncomingWebSocketEvent<>(
DataChangeDestination.DCD_OTHER, "changeUICamera", (Integer) entryValue);
dcService.publishEvent(changeCurrentCameraEvent);
break;
}
case SMT_CURRENTPIPELINE:
{
var changePipelineEvent =
new IncomingWebSocketEvent<>(
DataChangeDestination.DCD_ACTIVEMODULE,
"changePipeline",
(Integer) entryValue,
cameraIndex);
dcService.publishEvent(changePipelineEvent);
break;
}
case SMT_ISPNPCALIBRATION:
{
var changePipelineEvent =
new IncomingWebSocketEvent<>(
DataChangeDestination.DCD_ACTIVEMODULE,
"changePipeline",
PipelineManager.CAL_3D_INDEX,
cameraIndex);
dcService.publishEvent(changePipelineEvent);
break;
}
case SMT_TAKECALIBRATIONSNAPSHOT:
{
var takeCalSnapshotEvent =
new IncomingWebSocketEvent<>(
DataChangeDestination.DCD_ACTIVEMODULE, "takeCalSnapshot", 0, cameraIndex);
dcService.publishEvent(takeCalSnapshotEvent);
break;
}
case SMT_PIPELINESETTINGCHANGE:
{
HashMap<String, Object> data = (HashMap<String, Object>) entryValue;
if (data.size() >= 2) {
var cameraIndex2 = (int) data.get("cameraIndex");
for (var dataEntry : data.entrySet()) {
if (dataEntry.getKey().equals("cameraIndex")) {
continue;
}
var pipelineSettingChangeEvent =
new IncomingWebSocketEvent(
DataChangeDestination.DCD_ACTIVEPIPELINESETTINGS,
dataEntry.getKey(),
dataEntry.getValue(),
cameraIndex2);
dcService.publishEvent(pipelineSettingChangeEvent);
}
} else {
logger.warn("Unknown message for PSC: " + data.keySet().iterator().next());
}
break;
}
}
} catch (Exception ex) {
logger.error("unknown booboo");
ex.printStackTrace();
}
}
} catch (IOException e) {
// TODO: log
e.printStackTrace();
}
}
public static void sendMessage(Object message, WsContext user) throws JsonProcessingException {
// TODO: change to use the DataFlow system
private void sendMessage(Object message, WsContext user) throws JsonProcessingException {
ByteBuffer b = ByteBuffer.wrap(objectMapper.writeValueAsBytes(message));
user.send(b);
}
public static void broadcastMessage(Object message, WsContext userToSkip)
// TODO: change to use the DataFlow system
public void broadcastMessage(Object message, WsContext userToSkip)
throws JsonProcessingException {
for (WsContext user : users) {
if (user != userToSkip) {
sendMessage(message, user);
}
}
return;
}
public static void broadcastMessage(Object message) throws JsonProcessingException {
broadcastMessage(message, null);
}
}

34
photon-server/src/main/java/org/photonvision/server/SocketMessageCommandType.java Normal file
View File

@@ -0,0 +1,34 @@
/*
* Copyright (C) 2020 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.server;
public enum SocketMessageCommandType {
SMCT_DELETECURRENTPIPELINE("deleteCurrentPipeline"),
SMCT_SAVE("save");
public final String entryValue;
SocketMessageCommandType(String entryValue) {
this.entryValue = entryValue;
}
public static SocketMessageCommandType fromEntryKey(String entryValue) {
if (entryValue.equalsIgnoreCase(SMCT_SAVE.entryValue)) return SMCT_SAVE;
else return SMCT_DELETECURRENTPIPELINE;
}
}

54
photon-server/src/main/java/org/photonvision/server/SocketMessageType.java Normal file
View File

@@ -0,0 +1,54 @@
/*
* Copyright (C) 2020 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.server;
import java.util.EnumSet;
import java.util.HashMap;
import java.util.Map;
@SuppressWarnings("unused")
public enum SocketMessageType {
SMT_DRIVERMODE("driverMode"),
SMT_CHANGECAMERANAME("changeCameraName"),
SMT_CHANGEPIPELINENAME("changePipelineName"),
SMT_ADDNEWPIPELINE("addNewPipeline"),
SMT_COMMAND("command"),
SMT_CURRENTCAMERA("currentCamera"),
SMT_PIPELINESETTINGCHANGE("changePipelineSetting"),
SMT_CURRENTPIPELINE("currentPipeline"),
SMT_ISPNPCALIBRATION("isPNPCalibration"),
SMT_TAKECALIBRATIONSNAPSHOT("takeCalibrationSnapshot");
public final String entryKey;
SocketMessageType(String entryKey) {
this.entryKey = entryKey;
}
private static final Map<String, SocketMessageType> entryKeyToValueMap = new HashMap<>();
static {
for (var value : EnumSet.allOf(SocketMessageType.class)) {
entryKeyToValueMap.put(value.entryKey, value);
}
}
public static SocketMessageType fromEntryKey(String entryKey) {
return entryKeyToValueMap.get(entryKey);
}
}

51
photon-server/src/main/java/org/photonvision/server/UIInboundSubscriber.java Normal file
View File

@@ -0,0 +1,51 @@
/*
* Copyright (C) 2020 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.server;
import java.util.Collections;
import org.photonvision.common.configuration.ConfigManager;
import org.photonvision.common.dataflow.DataChangeDestination;
import org.photonvision.common.dataflow.DataChangeService;
import org.photonvision.common.dataflow.DataChangeSource;
import org.photonvision.common.dataflow.DataChangeSubscriber;
import org.photonvision.common.dataflow.events.DataChangeEvent;
import org.photonvision.common.dataflow.events.IncomingWebSocketEvent;
import org.photonvision.common.dataflow.events.OutgoingUIEvent;
public class UIInboundSubscriber extends DataChangeSubscriber {
public UIInboundSubscriber() {
super(
Collections.singletonList(DataChangeSource.DCS_WEBSOCKET),
Collections.singletonList(DataChangeDestination.DCD_GENSETTINGS));
}
@Override
public void onDataChangeEvent(DataChangeEvent event) {
if (event instanceof IncomingWebSocketEvent) {
var incomingWSEvent = (IncomingWebSocketEvent) event;
if (incomingWSEvent.propertyName.equals("userConnected")
|| incomingWSEvent.propertyName.equals("sendFullSettings")) {
// Send full settings
var settings = ConfigManager.getInstance().getConfig().toHashMap();
var message = new OutgoingUIEvent<>(UIUpdateType.BROADCAST, "fullsettings", settings);
DataChangeService.getInstance().publishEvent(message);
}
}
}
}

79
photon-server/src/main/java/org/photonvision/server/UIOutboundSubscriber.java Normal file
View File

@@ -0,0 +1,79 @@
/*
* Copyright (C) 2020 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.server;
import com.fasterxml.jackson.core.JsonProcessingException;
import java.util.Collections;
import java.util.HashMap;
import org.apache.commons.lang3.tuple.Pair;
import org.photonvision.common.dataflow.DataChangeDestination;
import org.photonvision.common.dataflow.DataChangeSource;
import org.photonvision.common.dataflow.DataChangeSubscriber;
import org.photonvision.common.dataflow.events.DataChangeEvent;
import org.photonvision.common.dataflow.events.OutgoingUIEvent;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
@SuppressWarnings("rawtypes")
/*
* DO NOT use logging in this class. If you do, the logs will recuse forever!
*/
class UIOutboundSubscriber extends DataChangeSubscriber {
Logger logger = new Logger(UIOutboundSubscriber.class, LogGroup.Server);
private final SocketHandler socketHandler;
public UIOutboundSubscriber(SocketHandler socketHandler) {
super(DataChangeSource.AllSources, Collections.singletonList(DataChangeDestination.DCD_UI));
this.socketHandler = socketHandler;
}
@Override
public void onDataChangeEvent(DataChangeEvent event) {
if (event instanceof OutgoingUIEvent) {
var thisEvent = (OutgoingUIEvent) event;
try {
switch (thisEvent.updateType) {
case BROADCAST:
{
// logger.debug("Broadcasting message");
if (event.data instanceof HashMap) {
var data = (HashMap) event.data;
socketHandler.broadcastMessage(data, null);
} else {
socketHandler.broadcastMessage(event.data, null);
}
break;
}
case SINGLEUSER:
{
// logger.debug("Sending single user message");
if (event.data instanceof Pair) {
var pair = (SocketHandler.SelectiveBroadcastPair) event.data;
socketHandler.broadcastMessage(pair.getLeft(), pair.getRight());
}
break;
}
}
} catch (JsonProcessingException e) {
// TODO: Log
e.printStackTrace();
}
}
}
}

26
photon-server/src/main/java/org/photonvision/server/UISettings.java Normal file
View File

@@ -0,0 +1,26 @@
/*
* Copyright (C) 2020 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.server;
import org.photonvision.common.configuration.CameraConfiguration;
import org.photonvision.common.configuration.NetworkConfig;
public class UISettings {
public NetworkConfig networkConfig;
public CameraConfiguration currentCameraConfiguration;
}

23
photon-server/src/main/java/org/photonvision/server/UIUpdateType.java Normal file
View File

@@ -0,0 +1,23 @@
/*
* Copyright (C) 2020 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.server;
public enum UIUpdateType {
BROADCAST,
SINGLEUSER
}

25
photon-server/src/main/java/org/photonvision/vision/camera/CameraQuirk.java Normal file
View File

@@ -0,0 +1,25 @@
/*
* Copyright (C) 2020 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.camera;
public enum CameraQuirk {
/** Camera settable for controllable image gain */
Gain,
/** For cameras that need a bit of encouragement for settings to stick */
DoubleSet
}

67
photon-server/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java
View File

@@ -17,36 +17,65 @@
package org.photonvision.vision.camera;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Objects;
public class QuirkyCamera {
public static final List<QuirkyCamera> quirkyCameras =
private static final List<QuirkyCamera> quirkyCameras =
List.of(
// ps3 eye
new QuirkyCamera(0x1415, 0x2000, "PS3Eye", List.of(CameraQuirks.Gain)));
new QuirkyCamera(0x1415, 0x2000, "PS3Eye", CameraQuirk.Gain),
new QuirkyCamera(0x72E, 0x45D, "LifeCam VX-5500", CameraQuirk.DoubleSet));
public final int usbVid;
public final int usbPid;
public final String name;
public List<CameraQuirks> quirks;
public final String baseName;
public final HashMap<CameraQuirk, Boolean> quirks;
public QuirkyCamera(int usbVid, int usbPid, String baseName, List<CameraQuirks> quirks) {
private QuirkyCamera(int usbVid, int usbPid, String baseName, CameraQuirk quirk) {
this(usbVid, usbPid, baseName, List.of(quirk));
}
private QuirkyCamera(int usbVid, int usbPid, String baseName, List<CameraQuirk> quirks) {
this.usbVid = usbVid;
this.usbPid = usbPid;
this.name = baseName;
this.quirks = quirks;
}
this.baseName = baseName;
public QuirkyCamera(int usbVid, int usbPid, String baseName) {
this(usbVid, usbPid, baseName, new ArrayList<>());
QuirkyCamera quirky =
quirkyCameras.stream()
.filter(quirkyCamera -> quirkyCamera.usbPid == usbPid && quirkyCamera.usbVid == usbVid)
.findFirst()
.orElse(null);
if (quirky != null) {
this.quirks = quirky.quirks;
this.quirks = new HashMap<>();
for (var q : quirks) {
this.quirks.put(q, true);
}
for (var q : CameraQuirk.values()) {
this.quirks.putIfAbsent(q, false);
}
}
public static QuirkyCamera getQuirkyCamera(int usbVid, int usbPid, String baseName) {
for (var qc : quirkyCameras) {
if (qc.usbVid == usbVid && qc.usbPid == usbPid) {
return qc;
}
}
return new QuirkyCamera(usbVid, usbPid, baseName, List.of());
}
public boolean hasQuirk(CameraQuirk quirk) {
return quirks.get(quirk);
}
@Override
public boolean equals(Object o) {
if (this == o) return true;
if (o == null || getClass() != o.getClass()) return false;
QuirkyCamera that = (QuirkyCamera) o;
return usbVid == that.usbVid
&& usbPid == that.usbPid
&& Objects.equals(baseName, that.baseName)
&& Objects.equals(quirks, that.quirks);
}
@Override
public int hashCode() {
return Objects.hash(usbVid, usbPid, baseName, quirks);
}
}

82
photon-server/src/main/java/org/photonvision/vision/camera/USBCameraSource.java
View File

@@ -23,47 +23,34 @@ import edu.wpi.cscore.VideoMode;
import edu.wpi.first.cameraserver.CameraServer;
import java.util.*;
import org.photonvision.common.configuration.CameraConfiguration;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.vision.frame.FrameProvider;
import org.photonvision.vision.frame.FrameStaticProperties;
import org.photonvision.vision.frame.provider.USBFrameProvider;
import org.photonvision.vision.processes.VisionSource;
import org.photonvision.vision.processes.VisionSourceSettables;
public class USBCameraSource implements VisionSource {
private final Logger logger;
private final UsbCamera camera;
private final USBCameraSettables usbCameraSettables;
private final USBFrameProvider usbFrameProvider;
private final CameraConfiguration configuration;
public final CameraConfiguration configuration;
private final CvSink cvSink;
private final QuirkyCamera cameraQuirks;
public USBCameraSource(CameraConfiguration config) {
this.configuration = config;
this.camera = new UsbCamera(config.nickname, config.path);
this.cameraQuirks =
new QuirkyCamera(camera.getInfo().productId, camera.getInfo().vendorId, config.baseName);
CvSink cvSink = CameraServer.getInstance().getVideo(this.camera);
this.usbCameraSettables = new USBCameraSettables(config);
this.usbFrameProvider =
new USBFrameProvider(cvSink, usbCameraSettables.getFrameStaticProperties());
}
@Override
public boolean equals(Object obj) {
if (obj.getClass() != USBCameraSource.class) {
return false;
}
USBCameraSource tmp = (USBCameraSource) obj;
boolean i = this.cameraQuirks.quirks.equals(tmp.cameraQuirks.quirks);
boolean r = this.configuration.uniqueName.equals(tmp.configuration.uniqueName);
boolean c = this.configuration.baseName.equals(tmp.configuration.baseName);
boolean j = this.configuration.nickname.equals(tmp.configuration.nickname);
boolean k = this.camera.getInfo().name.equals(tmp.camera.getInfo().name);
boolean x = this.camera.getInfo().productId == tmp.camera.getInfo().productId;
boolean y = this.camera.getInfo().vendorId == tmp.camera.getInfo().vendorId;
var t = i && r && c && j && k && x && y;
return t;
logger = new Logger(USBCameraSource.class, config.nickname, LogGroup.Camera);
configuration = config;
camera = new UsbCamera(config.nickname, config.path);
cameraQuirks =
QuirkyCamera.getQuirkyCamera(
camera.getInfo().productId, camera.getInfo().vendorId, config.baseName);
cvSink = CameraServer.getInstance().getVideo(this.camera);
usbCameraSettables = new USBCameraSettables(config);
usbFrameProvider = new USBFrameProvider(cvSink, usbCameraSettables.getFrameStaticProperties());
}
@Override
@@ -79,6 +66,9 @@ public class USBCameraSource implements VisionSource {
public class USBCameraSettables extends VisionSourceSettables {
protected USBCameraSettables(CameraConfiguration configuration) {
super(configuration);
getAllVideoModes();
setCurrentVideoMode(videoModes.get(0));
frameStaticProperties = new FrameStaticProperties(getCurrentVideoMode(), getFOV());
}
@Override
@@ -89,6 +79,7 @@ public class USBCameraSource implements VisionSource {
@Override
public void setExposure(int exposure) {
camera.setExposureManual(exposure);
camera.setExposureManual(exposure);
}
@Override
@@ -99,16 +90,17 @@ public class USBCameraSource implements VisionSource {
@Override
public void setBrightness(int brightness) {
camera.setBrightness(brightness);
camera.setBrightness(brightness);
}
@Override
public int getGain() {
return camera.getProperty("gain").get();
return !cameraQuirks.hasQuirk(CameraQuirk.Gain) ? -1 : camera.getProperty("gain").get();
}
@Override
public void setGain(int gain) {
if (cameraQuirks.quirks.contains(CameraQuirks.Gain)) {
if (cameraQuirks.hasQuirk(CameraQuirk.Gain)) {
camera.getProperty("gain_automatic").set(0);
camera.getProperty("gain").set(gain);
}
@@ -116,19 +108,31 @@ public class USBCameraSource implements VisionSource {
@Override
public VideoMode getCurrentVideoMode() {
return camera.getVideoMode();
return camera.isConnected() ? camera.getVideoMode() : null;
}
@Override
public void setCurrentVideoMode(VideoMode videoMode) {
if (videoMode == null) {
logger.error("Got a null video mode! Doing nothing...");
return;
}
camera.setVideoMode(videoMode);
this.frameStaticProperties = new FrameStaticProperties(getCurrentVideoMode(), getFOV());
}
@Override
public HashMap<Integer, VideoMode> getAllVideoModes() {
if (videoModes == null) {
videoModes = new HashMap<>();
List<VideoMode> videoModesList = Arrays.asList(camera.enumerateVideoModes());
List<VideoMode> videoModesList;
try {
videoModesList = Arrays.asList(camera.enumerateVideoModes());
} catch (Exception e) {
logger.error("Exception while enumerating video modes!");
e.printStackTrace();
videoModesList = List.of();
}
for (VideoMode videoMode : videoModesList) {
videoModes.put(videoModesList.indexOf(videoMode), videoMode);
}
@@ -136,4 +140,18 @@ public class USBCameraSource implements VisionSource {
return videoModes;
}
}
@Override
public boolean equals(Object o) {
if (this == o) return true;
if (o == null || getClass() != o.getClass()) return false;
USBCameraSource that = (USBCameraSource) o;
return cameraQuirks.equals(that.cameraQuirks);
}
@Override
public int hashCode() {
return Objects.hash(
camera, usbCameraSettables, usbFrameProvider, configuration, cvSink, cameraQuirks);
}
}

2
photon-server/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java
View File

@@ -41,7 +41,7 @@ public class FrameStaticProperties {
* @param fov The fov of the image.
*/
public FrameStaticProperties(VideoMode mode, double fov) {
this(mode.width, mode.height, fov);
this(mode != null ? mode.width : 1, mode != null ? mode.height : 1, fov);
}
/**

48
photon-server/src/main/java/org/photonvision/vision/frame/consumer/MJPGFrameConsumer.java
View File

@@ -17,13 +17,57 @@
package org.photonvision.vision.frame.consumer;
import org.apache.commons.lang3.NotImplementedException;
import edu.wpi.cscore.CvSource;
import edu.wpi.cscore.MjpegServer;
import edu.wpi.cscore.VideoMode;
import edu.wpi.first.cameraserver.CameraServer;
import org.opencv.core.Mat;
import org.opencv.core.Size;
import org.opencv.imgproc.Imgproc;
import org.photonvision.vision.frame.Frame;
import org.photonvision.vision.frame.FrameConsumer;
import org.photonvision.vision.frame.FrameDivisor;
public class MJPGFrameConsumer implements FrameConsumer {
private final CvSource cvSource;
private final MjpegServer mjpegServer;
private FrameDivisor divisor = FrameDivisor.NONE;
public MJPGFrameConsumer(String sourceName, int width, int height) {
// TODO h264?
this.cvSource = new CvSource(sourceName, VideoMode.PixelFormat.kMJPEG, width, height, 30);
this.mjpegServer = CameraServer.getInstance().startAutomaticCapture(cvSource);
}
public MJPGFrameConsumer(String name) {
this(name, 320, 240);
}
public void setFrameDivisor(FrameDivisor divisor) {
this.divisor = divisor;
}
@Override
public void accept(Frame frame) {
throw new NotImplementedException("");
if (!frame.image.getMat().empty()) {
if (divisor != FrameDivisor.NONE) {
var tempMat = new Mat();
Imgproc.resize(
frame.image.getMat(), tempMat, getScaledSize(frame.image.getMat().size(), divisor));
cvSource.putFrame(tempMat);
tempMat.release();
} else {
cvSource.putFrame(frame.image.getMat());
}
}
}
private Size getScaledSize(Size orig, FrameDivisor divisor) {
return new Size(orig.width / divisor.value, orig.height / divisor.value);
}
public int getCurrentStreamPort() {
return mjpegServer.getPort();
}
}

12
photon-server/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java
View File

@@ -24,20 +24,20 @@ import org.photonvision.vision.frame.FrameStaticProperties;
import org.photonvision.vision.opencv.CVMat;
public class USBFrameProvider implements FrameProvider {
private static int count = 0;
private CvSink cvSink;
private FrameStaticProperties frameStaticProperties;
private CVMat mat;
private final CvSink cvSink;
private final FrameStaticProperties frameStaticProperties;
private final CVMat mat;
public USBFrameProvider(CvSink sink, FrameStaticProperties frameStaticProperties) {
cvSink = sink;
cvSink.setEnabled(true);
this.frameStaticProperties = frameStaticProperties;
mat = new CVMat();
}
@Override
public Frame get() {
if (mat != null && mat.getMat() != null) {
if (mat.getMat() != null) {
mat.release();
}
long time = cvSink.grabFrame(mat.getMat());
@@ -46,6 +46,6 @@ public class USBFrameProvider implements FrameProvider {
@Override
public String getName() {
return "USBFrameProvider" + count++;
return "USBFrameProvider - " + cvSink.getName();
}
}

6
photon-server/src/main/java/org/photonvision/vision/opencv/ContourSortMode.java
View File

@@ -22,11 +22,13 @@ import org.apache.commons.math3.util.FastMath;
import org.photonvision.vision.target.PotentialTarget;
public enum ContourSortMode {
Largest(Comparator.comparingDouble(PotentialTarget::getArea)),
Largest(
Comparator.comparingDouble(PotentialTarget::getArea)
.reversed()), // reversed so that zero index has the largest size
Smallest(Largest.getComparator().reversed()),
Highest(Comparator.comparingDouble(rect -> rect.getMinAreaRect().center.y)),
Lowest(Highest.getComparator().reversed()),
Leftmost(Comparator.comparingDouble(target -> target.getMinAreaRect().center.x)),
Leftmost(Comparator.comparingDouble(target -> target.getMinAreaRect().center.x * -1)),
Rightmost(Leftmost.getComparator().reversed()),
Centermost(
Comparator.comparingDouble(

2
photon-server/src/main/java/org/photonvision/vision/pipeline/AdvancedPipelineSettings.java
View File

@@ -35,7 +35,7 @@ public class AdvancedPipelineSettings extends CVPipelineSettings {
public IntegerCouple hsvSaturation = new IntegerCouple(50, 255);
public IntegerCouple hsvValue = new IntegerCouple(50, 255);
public boolean outputShowThresholded = false;
public boolean outputShowThresholded = true;
public boolean outputShowMultipleTargets = false;
public boolean erode = false;

6
photon-server/src/main/java/org/photonvision/vision/pipeline/CVPipeline.java
View File

@@ -17,9 +17,11 @@
package org.photonvision.vision.pipeline;
import java.util.List;
import org.photonvision.common.util.math.MathUtils;
import org.photonvision.vision.frame.Frame;
import org.photonvision.vision.frame.FrameStaticProperties;
import org.photonvision.vision.pipeline.result.CVPipelineResult;
public abstract class CVPipeline<R extends CVPipelineResult, S extends CVPipelineSettings> {
protected S settings;
@@ -32,6 +34,7 @@ public abstract class CVPipeline<R extends CVPipelineResult, S extends CVPipelin
return settings;
}
// TODO (BANKS) ACTUALLY SET THE CAMERA RESOLUTION
public R run(Frame frame) {
long pipelineStartNanos = System.nanoTime();
@@ -40,6 +43,9 @@ public abstract class CVPipeline<R extends CVPipelineResult, S extends CVPipelin
}
setPipeParams(frame.frameStaticProperties, settings);
if (frame.image.getMat().empty()) {
return (R) new CVPipelineResult(0, List.of(), frame);
}
R result = process(frame, settings);
result.setLatencyMillis(MathUtils.nanosToMillis(System.nanoTime() - pipelineStartNanos));

6
photon-server/src/main/java/org/photonvision/vision/pipeline/CVPipelineSettings.java
View File

@@ -39,9 +39,9 @@ public class CVPipelineSettings {
public ImageFlipMode inputImageFlipMode = ImageFlipMode.NONE;
public ImageRotationMode inputImageRotationMode = ImageRotationMode.DEG_0;
public String pipelineNickname = "New Pipeline";
public double cameraExposure = 50.0;
public double cameraBrightness = 50.0;
public double cameraGain = 50.0;
public int cameraExposure = 50;
public int cameraBrightness = 50;
public int cameraGain = 50;
public int cameraVideoModeIndex = 0;
public FrameDivisor inputFrameDivisor = FrameDivisor.NONE;
public FrameDivisor outputFrameDivisor = FrameDivisor.NONE;

3
photon-server/src/main/java/org/photonvision/vision/pipeline/Calibration3dPipeline.java
View File

@@ -19,6 +19,8 @@ package org.photonvision.vision.pipeline;
import org.photonvision.vision.frame.Frame;
import org.photonvision.vision.frame.FrameStaticProperties;
import org.photonvision.vision.pipeline.result.CVPipelineResult;
import org.photonvision.vision.processes.PipelineManager;
public class Calibration3dPipeline extends CVPipeline<CVPipelineResult, CVPipelineSettings> {
@@ -26,6 +28,7 @@ public class Calibration3dPipeline extends CVPipeline<CVPipelineResult, CVPipeli
public Calibration3dPipeline() {
settings = new CVPipelineSettings();
settings.pipelineIndex = PipelineManager.CAL_3D_INDEX;
}
@Override

5
photon-server/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java
View File

@@ -29,6 +29,7 @@ import org.photonvision.vision.frame.FrameStaticProperties;
import org.photonvision.vision.opencv.*;
import org.photonvision.vision.pipe.CVPipeResult;
import org.photonvision.vision.pipe.impl.*;
import org.photonvision.vision.pipeline.result.CVPipelineResult;
import org.photonvision.vision.target.PotentialTarget;
import org.photonvision.vision.target.TrackedTarget;
@@ -64,6 +65,10 @@ public class ColoredShapePipeline
settings = new ColoredShapePipelineSettings();
}
public ColoredShapePipeline(ColoredShapePipelineSettings settings) {
this.settings = settings;
}
@Override
protected void setPipeParams(
FrameStaticProperties frameStaticProperties, ColoredShapePipelineSettings settings) {

2
photon-server/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipelineSettings.java
View File

@@ -52,7 +52,7 @@ public class ColoredShapePipelineSettings extends AdvancedPipelineSettings {
public boolean solvePNPEnabled = false;
public CameraCalibrationCoefficients cameraCalibration;
public TargetModel targetModel;
public Rotation2d cameraPitch = Rotation2d.fromDegrees(0.0);
public Rotation2d cameraPitch = Rotation2d.fromDegrees(0.0); // TODO where should pitch live?
// Corner detection settings
public CornerDetectionPipe.DetectionStrategy cornerDetectionStrategy =

1
photon-server/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java
View File

@@ -26,6 +26,7 @@ import org.photonvision.vision.opencv.CVMat;
import org.photonvision.vision.pipe.impl.Draw2dCrosshairPipe;
import org.photonvision.vision.pipe.impl.ResizeImagePipe;
import org.photonvision.vision.pipe.impl.RotateImagePipe;
import org.photonvision.vision.pipeline.result.DriverModePipelineResult;
public class DriverModePipeline
extends CVPipeline<DriverModePipelineResult, DriverModePipelineSettings> {

3
photon-server/src/main/java/org/photonvision/vision/pipeline/DriverModePipelineSettings.java
View File

@@ -19,6 +19,7 @@ package org.photonvision.vision.pipeline;
import com.fasterxml.jackson.annotation.JsonTypeName;
import org.photonvision.common.util.numbers.DoubleCouple;
import org.photonvision.vision.processes.PipelineManager;
import org.photonvision.vision.target.RobotOffsetPointMode;
@JsonTypeName("DriverModePipelineSettings")
@@ -28,6 +29,8 @@ public class DriverModePipelineSettings extends CVPipelineSettings {
public DriverModePipelineSettings() {
super();
pipelineNickname = "Driver Mode";
pipelineIndex = PipelineManager.DRIVERMODE_INDEX;
pipelineType = PipelineType.DriverMode;
}
}

17
photon-server/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java
View File

@@ -42,6 +42,7 @@ import org.photonvision.vision.pipe.impl.RotateImagePipe;
import org.photonvision.vision.pipe.impl.SolvePNPPipe;
import org.photonvision.vision.pipe.impl.SortContoursPipe;
import org.photonvision.vision.pipe.impl.SpeckleRejectPipe;
import org.photonvision.vision.pipeline.result.CVPipelineResult;
import org.photonvision.vision.target.PotentialTarget;
import org.photonvision.vision.target.TrackedTarget;
@@ -71,6 +72,10 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
settings = new ReflectivePipelineSettings();
}
public ReflectivePipeline(ReflectivePipelineSettings settings) {
this.settings = settings;
}
@Override
protected void setPipeParams(
FrameStaticProperties frameStaticProperties, ReflectivePipelineSettings settings) {
@@ -162,6 +167,7 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
long sumPipeNanosElapsed = 0L;
rawInputMat.release();
frame.image.getMat().copyTo(rawInputMat);
CVPipeResult<Mat> rotateImageResult = rotateImagePipe.apply(frame.image.getMat());
@@ -187,8 +193,12 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
speckleRejectPipe.apply(findContoursResult.result);
sumPipeNanosElapsed += speckleRejectResult.nanosElapsed;
CVPipeResult<List<Contour>> filterContoursResult =
filterContoursPipe.apply(speckleRejectResult.result);
sumPipeNanosElapsed += filterContoursResult.nanosElapsed;
CVPipeResult<List<PotentialTarget>> groupContoursResult =
groupContoursPipe.apply(speckleRejectResult.result);
groupContoursPipe.apply(filterContoursResult.result);
sumPipeNanosElapsed += groupContoursResult.nanosElapsed;
CVPipeResult<List<PotentialTarget>> sortContoursResult =
@@ -234,11 +244,6 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
result = draw2dContoursResult;
}
// TODO: better way?
if (settings.outputShowThresholded) {
rawInputMat.release();
}
// TODO: Implement all the things
return new CVPipelineResult(
MathUtils.nanosToMillis(sumPipeNanosElapsed),

141
photon-server/src/main/java/org/photonvision/vision/pipeline/result/BytePackable.java Normal file
View File

@@ -0,0 +1,141 @@
/*
* Copyright (C) 2020 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.pipeline.result;
@SuppressWarnings("PointlessBitwiseExpression")
public abstract class BytePackable {
public abstract byte[] toByteArray();
public abstract void fromByteArray(byte[] src);
protected int bufferPosition = 0;
public int getBufferPosition() {
return bufferPosition;
}
public void resetBufferPosition() {
bufferPosition = 0;
}
protected void bufferData(byte[] src, byte[] dest) {
System.arraycopy(src, 0, dest, bufferPosition, src.length);
bufferPosition += src.length;
}
protected void bufferData(byte src, byte[] dest) {
System.arraycopy(new byte[] {src}, 0, dest, bufferPosition, 1);
bufferPosition++;
}
protected void bufferData(int src, byte[] dest) {
System.arraycopy(toBytes(src), 0, dest, bufferPosition, Integer.BYTES);
bufferPosition += Integer.BYTES;
}
protected void bufferData(double src, byte[] dest) {
System.arraycopy(toBytes(src), 0, dest, bufferPosition, Double.BYTES);
bufferPosition += Double.BYTES;
}
protected void bufferData(boolean src, byte[] dest) {
System.arraycopy(toBytes(src), 0, dest, bufferPosition, 1);
bufferPosition += 1;
}
protected boolean unbufferBoolean(byte[] src) {
return toBoolean(src, bufferPosition++);
}
protected byte unbufferByte(byte[] src) {
var value = src[bufferPosition];
bufferPosition++;
return value;
}
protected byte[] unbufferBytes(byte[] src, int len) {
var bytes = new byte[len];
System.arraycopy(src, bufferPosition, bytes, 0, len);
return bytes;
}
protected int unbufferInt(byte[] src) {
var value = toInt(src, bufferPosition);
bufferPosition += Integer.BYTES;
return value;
}
protected double unbufferDouble(byte[] src) {
var value = toDouble(src, bufferPosition);
bufferPosition += Double.BYTES;
return value;
}
private static boolean toBoolean(byte[] src, int pos) {
return src[pos] != 0;
}
private static int toInt(byte[] src, int pos) {
return (0xff & src[pos]) << 24
| (0xff & src[pos + 1]) << 16
| (0xff & src[pos + 2]) << 8
| (0xff & src[pos + 3]) << 0;
}
private static long toLong(byte[] src, int pos) {
return (long) (0xff & src[pos]) << 56
| (long) (0xff & src[pos + 1]) << 48
| (long) (0xff & src[pos + 2]) << 40
| (long) (0xff & src[pos + 3]) << 32
| (long) (0xff & src[pos + 4]) << 24
| (long) (0xff & src[pos + 5]) << 16
| (long) (0xff & src[pos + 6]) << 8
| (long) (0xff & src[pos + 7]) << 0;
}
private static double toDouble(byte[] src, int pos) {
return Double.longBitsToDouble(toLong(src, pos));
}
protected byte[] toBytes(double value) {
long data = Double.doubleToRawLongBits(value);
return new byte[] {
(byte) ((data >> 56) & 0xff),
(byte) ((data >> 48) & 0xff),
(byte) ((data >> 40) & 0xff),
(byte) ((data >> 32) & 0xff),
(byte) ((data >> 24) & 0xff),
(byte) ((data >> 16) & 0xff),
(byte) ((data >> 8) & 0xff),
(byte) ((data >> 0) & 0xff),
};
}
protected byte[] toBytes(int value) {
return new byte[] {
(byte) ((value >> 24) & 0xff),
(byte) ((value >> 16) & 0xff),
(byte) ((value >> 8) & 0xff),
(byte) ((value >> 0) & 0xff)
};
}
protected byte[] toBytes(boolean value) {
return new byte[] {(byte) (value ? 1 : 0)};
}
}

4
photon-server/src/main/java/org/photonvision/vision/pipeline/CVPipelineResult.java → photon-server/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java
View File

@@ -15,7 +15,7 @@
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package org.photonvision.vision.pipeline;
package org.photonvision.vision.pipeline.result;
import java.util.List;
import org.photonvision.vision.frame.Frame;
@@ -50,7 +50,7 @@ public class CVPipelineResult implements Releasable {
return latencyMillis;
}
protected void setLatencyMillis(double latencyMillis) {
public void setLatencyMillis(double latencyMillis) {
this.latencyMillis = latencyMillis;
}
}

2
photon-server/src/main/java/org/photonvision/vision/pipeline/DriverModePipelineResult.java → photon-server/src/main/java/org/photonvision/vision/pipeline/result/DriverModePipelineResult.java
View File

@@ -15,7 +15,7 @@
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package org.photonvision.vision.pipeline;
package org.photonvision.vision.pipeline.result;
import java.util.List;
import org.photonvision.vision.frame.Frame;

109
photon-server/src/main/java/org/photonvision/vision/pipeline/result/SimplePipelineResult.java Normal file
View File

@@ -0,0 +1,109 @@
/*
* Copyright (C) 2020 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.pipeline.result;
import java.util.ArrayList;
import java.util.List;
import java.util.Objects;
import org.photonvision.vision.target.TrackedTarget;
public class SimplePipelineResult extends BytePackable {
private double latencyMillis;
private boolean hasTargets;
public final List<SimpleTrackedTarget> targets = new ArrayList<>();
public SimplePipelineResult() {}
public SimplePipelineResult(
double latencyMillis, boolean hasTargets, List<SimpleTrackedTarget> targets) {
this.latencyMillis = latencyMillis;
this.hasTargets = hasTargets;
this.targets.addAll(targets);
}
public SimplePipelineResult(CVPipelineResult r) {
this(r.processingMillis, r.hasTargets(), simpleFromTrackedTargets(r.targets));
}
public double getLatencyMillis() {
return latencyMillis;
}
public boolean hasTargets() {
return hasTargets;
}
@Override
public boolean equals(Object o) {
if (this == o) return true;
if (o == null || getClass() != o.getClass()) return false;
SimplePipelineResult that = (SimplePipelineResult) o;
boolean latencyMatch = Double.compare(that.latencyMillis, latencyMillis) == 0;
boolean hasTargetsMatch = that.hasTargets == hasTargets;
boolean targetsMatch = that.targets.equals(targets);
return latencyMatch && hasTargetsMatch && targetsMatch;
}
@Override
public int hashCode() {
return Objects.hash(latencyMillis, hasTargets, targets);
}
@Override
public byte[] toByteArray() {
bufferPosition = 0;
int bufferSize =
8 + 1 + 1 + (targets.size() * 48); // latencyMillis + hasTargets + targetCount + targets
var buff = new byte[bufferSize];
bufferData(latencyMillis, buff);
bufferData(hasTargets, buff);
bufferData((byte) targets.size(), buff);
for (var target : targets) {
bufferData(target.toByteArray(), buff);
}
return buff;
}
@Override
public void fromByteArray(byte[] src) {
bufferPosition = 0;
latencyMillis = unbufferDouble(src);
hasTargets = unbufferBoolean(src);
byte targetCount = unbufferByte(src);
targets.clear();
for (int i = 0; i < targetCount; i++) {
var target = new SimpleTrackedTarget();
target.fromByteArray(unbufferBytes(src, 48));
bufferPosition += 48;
targets.add(target);
}
}
private static List<SimpleTrackedTarget> simpleFromTrackedTargets(List<TrackedTarget> targets) {
var ret = new ArrayList<SimpleTrackedTarget>();
for (var t : targets) {
ret.add(new SimpleTrackedTarget(t));
}
return ret;
}
}

116
photon-server/src/main/java/org/photonvision/vision/pipeline/result/SimpleTrackedTarget.java Normal file
View File

@@ -0,0 +1,116 @@
/*
* Copyright (C) 2020 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.pipeline.result;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import java.util.Objects;
import org.photonvision.vision.target.TrackedTarget;
public class SimpleTrackedTarget extends BytePackable {
public static final int PACK_SIZE_BYTES = Double.BYTES * 6;
private double yaw;
private double pitch;
private double area;
private Pose2d robotRelativePose = new Pose2d();
public SimpleTrackedTarget() {}
public SimpleTrackedTarget(double yaw, double pitch, double area, Pose2d pose) {
this.yaw = yaw;
this.pitch = pitch;
this.area = area;
robotRelativePose = pose;
}
public SimpleTrackedTarget(TrackedTarget t) {
this(t.getYaw(), t.getPitch(), t.getArea(), t.getRobotRelativePose());
}
public double getYaw() {
return yaw;
}
public double getPitch() {
return pitch;
}
public double getArea() {
return area;
}
public Pose2d getRobotRelativePose() {
return robotRelativePose;
}
@Override
public boolean equals(Object o) {
if (this == o) return true;
if (o == null || getClass() != o.getClass()) return false;
SimpleTrackedTarget that = (SimpleTrackedTarget) o;
return Double.compare(that.yaw, yaw) == 0
&& Double.compare(that.pitch, pitch) == 0
&& Double.compare(that.area, area) == 0
&& Objects.equals(robotRelativePose, that.robotRelativePose);
}
@Override
public int hashCode() {
return Objects.hash(yaw, pitch, area, robotRelativePose);
}
@Override
public byte[] toByteArray() {
resetBufferPosition();
byte[] data = new byte[PACK_SIZE_BYTES];
bufferData(yaw, data);
bufferData(pitch, data);
bufferData(area, data);
if (robotRelativePose != null) {
bufferData(robotRelativePose.getTranslation().getX(), data);
bufferData(robotRelativePose.getTranslation().getY(), data);
bufferData(robotRelativePose.getRotation().getDegrees(), data);
} else {
bufferData((double) 0, data);
bufferData((double) 0, data);
bufferData((double) 0, data);
}
return data;
}
@Override
public void fromByteArray(byte[] src) {
resetBufferPosition();
if (src.length < PACK_SIZE_BYTES) {
// TODO: log error?
return;
}
yaw = unbufferDouble(src);
pitch = unbufferDouble(src);
area = unbufferDouble(src);
var poseX = unbufferDouble(src);
var poseY = unbufferDouble(src);
var poseR = unbufferDouble(src);
robotRelativePose = new Pose2d(poseX, poseY, Rotation2d.fromDegrees(poseR));
}
}

10
photon-server/src/main/java/org/photonvision/vision/processes/Data.java
View File

@@ -17,9 +17,15 @@
package org.photonvision.vision.processes;
import org.photonvision.vision.pipeline.CVPipelineResult;
import org.photonvision.vision.opencv.Releasable;
import org.photonvision.vision.pipeline.result.CVPipelineResult;
// TODO replace with CTT's data class
public class Data {
public class Data implements Releasable {
public CVPipelineResult result;
@Override
public void release() {
result.release();
}
}

174
photon-server/src/main/java/org/photonvision/vision/processes/PipelineManager.java
View File

@@ -17,26 +17,30 @@
package org.photonvision.vision.processes;
import java.util.ArrayList;
import java.util.Comparator;
import java.util.List;
import org.photonvision.vision.pipeline.CVPipeline;
import org.photonvision.vision.pipeline.CVPipelineSettings;
import org.photonvision.vision.pipeline.Calibration3dPipeline;
import org.photonvision.vision.pipeline.DriverModePipeline;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.vision.pipeline.*;
@SuppressWarnings({"rawtypes", "unused"})
public class PipelineManager {
private static final Logger logger = new Logger(PipelineManager.class, LogGroup.VisionProcess);
private static final int DRIVERMODE_INDEX = -1;
private static final int CAL_3D_INDEX = -2;
public static final int DRIVERMODE_INDEX = -1;
public static final int CAL_3D_INDEX = -2;
public final List<CVPipeline> userPipelines;
private final Calibration3dPipeline calibration3dPipeline = new Calibration3dPipeline();
private final DriverModePipeline driverModePipeline = new DriverModePipeline();
protected final List<CVPipelineSettings> userPipelineSettings;
protected final Calibration3dPipeline calibration3dPipeline = new Calibration3dPipeline();
protected final DriverModePipeline driverModePipeline = new DriverModePipeline();
/** Index of the currently active pipeline. */
private int currentPipelineIndex = DRIVERMODE_INDEX;
/** The currently active pipeline. */
private CVPipeline currentPipeline = driverModePipeline;
/**
* Index of the last active user-created pipeline. <br>
* <br>
@@ -50,32 +54,8 @@ public class PipelineManager {
*
* @param userPipelines Pipelines to add to the manager.
*/
public PipelineManager(List<CVPipeline> userPipelines) {
this.userPipelines = userPipelines;
}
/** Creates a PipelineManager with a DriverModePipeline, and a Calibration3dPipeline. */
public PipelineManager() {
this(List.of());
}
/**
* Get a pipeline by index.
*
* @param index Index of desired pipeline.
* @return The desired pipeline.
*/
public CVPipeline getPipeline(int index) {
if (index < 0) {
switch (index) {
case DRIVERMODE_INDEX:
return driverModePipeline;
case CAL_3D_INDEX:
return calibration3dPipeline;
}
}
return userPipelines.get(index);
public PipelineManager(List<CVPipelineSettings> userPipelines) {
this.userPipelineSettings = userPipelines;
}
/**
@@ -85,7 +65,41 @@ public class PipelineManager {
* @return The gotten settings of the pipeline whose index was provided.
*/
public CVPipelineSettings getPipelineSettings(int index) {
return getPipeline(index).getSettings();
if (index < 0) {
switch (index) {
case DRIVERMODE_INDEX:
return driverModePipeline.getSettings();
case CAL_3D_INDEX:
return calibration3dPipeline.getSettings();
}
}
for (var setting : userPipelineSettings) {
if (setting.pipelineIndex == index) return setting;
}
return null;
}
/**
* Gets a list of nicknames for all user pipelines
*
* @return The list of nicknames for all user pipelines
*/
public List<String> getPipelineNicknames() {
List<String> ret = new ArrayList<>();
for (var p : userPipelineSettings) {
ret.add(p.pipelineNickname);
}
return ret;
}
/**
* Gets the index of the currently active pipeline
*
* @return The index of the currently active pipeline
*/
public int getCurrentPipelineIndex() {
return currentPipelineIndex;
}
/**
@@ -94,7 +108,30 @@ public class PipelineManager {
* @return The currently active pipeline.
*/
public CVPipeline getCurrentPipeline() {
return getPipeline(currentPipelineIndex);
if (currentPipelineIndex < 0) {
switch (currentPipelineIndex) {
case CAL_3D_INDEX:
return calibration3dPipeline;
case DRIVERMODE_INDEX:
return driverModePipeline;
}
}
var desiredPipelineSettings = userPipelineSettings.get(currentPipelineIndex);
if (currentPipeline.getSettings().pipelineIndex != desiredPipelineSettings.pipelineIndex) {
switch (desiredPipelineSettings.pipelineType) {
case Reflective:
currentPipeline =
new ReflectivePipeline((ReflectivePipelineSettings) desiredPipelineSettings);
break;
case ColoredShape:
currentPipeline =
new ColoredShapePipeline((ColoredShapePipelineSettings) desiredPipelineSettings);
break;
}
}
return currentPipeline;
}
/**
@@ -133,20 +170,7 @@ public class PipelineManager {
}
public static final Comparator<CVPipelineSettings> PipelineSettingsIndexComparator =
(o1, o2) -> {
int o1Index = o1.pipelineIndex;
int o2Index = o2.pipelineIndex;
if (o1Index == o2Index) {
return 0;
} else if (o1Index < o2Index) {
return -1;
}
return 1;
};
public static final Comparator<CVPipeline> PipelineIndexComparator =
(o1, o2) -> PipelineSettingsIndexComparator.compare(o1.getSettings(), o2.getSettings());
Comparator.comparingInt(o -> o.pipelineIndex);
/**
* Sorts the pipeline list by index, and reassigns their indexes to match the new order. <br>
@@ -154,11 +178,53 @@ public class PipelineManager {
* I don't like this but I have no other ideas, and it works so ¯\_(ツ)_/¯
*/
private void reassignIndexes() {
userPipelines.sort(PipelineIndexComparator);
for (int i = 0; i < userPipelines.size(); i++) {
userPipelineSettings.sort(PipelineSettingsIndexComparator);
for (int i = 0; i < userPipelineSettings.size(); i++) {
getPipelineSettings(i).pipelineIndex = i;
}
}
// TODO: adding/removing pipelines
public CVPipelineSettings addPipeline(PipelineType type) {
switch (type) {
case Reflective:
{
var added = new ReflectivePipelineSettings();
addPipelineInternal(added);
return added;
}
case ColoredShape:
{
var added = new ColoredShapePipelineSettings();
addPipelineInternal(added);
return added;
}
}
reassignIndexes();
return null;
}
private void addPipelineInternal(CVPipelineSettings settings) {
settings.pipelineIndex = userPipelineSettings.size();
userPipelineSettings.add(settings);
reassignIndexes();
}
private void removePipelineInternal(int index) {
userPipelineSettings.remove(index);
reassignIndexes();
}
public void setIndex(int index) {
this.setPipelineInternal(index);
}
public void removePipeline(int index) {
if (index < 0) {
logger.debug("Could not remove preset pipes!");
return;
}
// TODO should we block/lock on a mutex?
removePipelineInternal(index);
currentPipelineIndex = Math.max(userPipelineSettings.size() - 1, currentPipelineIndex);
}
}

333
photon-server/src/main/java/org/photonvision/vision/processes/VisionModule.java
View File

@@ -17,11 +17,32 @@
package org.photonvision.vision.processes;
import java.util.LinkedList;
import com.fasterxml.jackson.core.JsonProcessingException;
import edu.wpi.first.networktables.NetworkTableInstance;
import java.util.*;
import org.apache.commons.lang3.tuple.Pair;
import org.photonvision.common.configuration.CameraConfiguration;
import org.photonvision.common.configuration.ConfigManager;
import org.photonvision.common.configuration.PhotonConfiguration;
import org.photonvision.common.dataflow.DataChangeService;
import org.photonvision.common.dataflow.DataChangeSubscriber;
import org.photonvision.common.dataflow.events.DataChangeEvent;
import org.photonvision.common.dataflow.events.IncomingWebSocketEvent;
import org.photonvision.common.dataflow.events.OutgoingUIEvent;
import org.photonvision.common.dataflow.networktables.NTDataConsumer;
import org.photonvision.common.datatransfer.DataConsumer;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.SerializationUtils;
import org.photonvision.common.util.numbers.DoubleCouple;
import org.photonvision.common.util.numbers.IntegerCouple;
import org.photonvision.server.SocketHandler;
import org.photonvision.server.UIUpdateType;
import org.photonvision.vision.frame.Frame;
import org.photonvision.vision.frame.FrameConsumer;
import org.photonvision.vision.pipeline.CVPipelineResult;
import org.photonvision.vision.frame.consumer.MJPGFrameConsumer;
import org.photonvision.vision.pipeline.PipelineType;
import org.photonvision.vision.pipeline.result.CVPipelineResult;
/**
* This is the God Class
@@ -31,13 +52,25 @@ import org.photonvision.vision.pipeline.CVPipelineResult;
*/
public class VisionModule {
private final Logger logger;
private final PipelineManager pipelineManager;
private final VisionSource visionSource;
private final VisionRunner visionRunner;
private final LinkedList<DataConsumer> dataConsumers = new LinkedList<>();
private final LinkedList<FrameConsumer> frameConsumers = new LinkedList<>();
private final NTDataConsumer ntConsumer;
private final int moduleIndex;
private final MJPGFrameConsumer uiStreamer;
private long lastUpdateTimestamp = -1;
public VisionModule(PipelineManager pipelineManager, VisionSource visionSource) {
private MJPGFrameConsumer dashboardStreamer;
public VisionModule(PipelineManager pipelineManager, VisionSource visionSource, int index) {
logger =
new Logger(
VisionModule.class,
visionSource.getSettables().getConfiguration().nickname,
LogGroup.VisionProcess);
this.pipelineManager = pipelineManager;
this.visionSource = visionSource;
this.visionRunner =
@@ -45,12 +78,296 @@ public class VisionModule {
this.visionSource.getFrameProvider(),
this.pipelineManager::getCurrentPipeline,
this::consumeResult);
this.moduleIndex = index;
DataChangeService.getInstance().addSubscriber(new VisionSettingChangeSubscriber());
dashboardStreamer =
new MJPGFrameConsumer(visionSource.getSettables().getConfiguration().uniqueName);
uiStreamer = new MJPGFrameConsumer(visionSource.getSettables().getConfiguration().nickname);
addFrameConsumer(dashboardStreamer);
addFrameConsumer(uiStreamer);
ntConsumer =
new NTDataConsumer(
NetworkTableInstance.getDefault().getTable("photonvision"),
visionSource.getSettables().getConfiguration().nickname);
addDataConsumer(ntConsumer);
addDataConsumer(
data -> {
var now = System.currentTimeMillis();
if (lastUpdateTimestamp + 1000.0 / 15.0 > now) return;
var uiMap = new HashMap<Integer, HashMap<String, Object>>();
var dataMap = new HashMap<String, Object>();
dataMap.put("fps", 1000.0 / data.result.getLatencyMillis());
var targets = data.result.targets;
var uiTargets = new ArrayList<HashMap<String, Object>>();
for (var t : targets) {
uiTargets.add(t.toHashMap());
}
dataMap.put("targets", uiTargets);
uiMap.put(index, dataMap);
var retMap = new HashMap<String, Object>();
retMap.put("updatePipelineResult", uiMap);
try {
SocketHandler.getInstance().broadcastMessage(retMap, null);
} catch (JsonProcessingException e) {
logger.error(e.getMessage());
logger.error(Arrays.toString(e.getStackTrace()));
}
lastUpdateTimestamp = now;
});
setPipeline(visionSource.getSettables().getConfiguration().currentPipelineIndex);
dashboardStreamer.setFrameDivisor(
pipelineManager.getCurrentPipelineSettings().outputFrameDivisor);
}
public void start() {
visionRunner.startProcess();
}
private class VisionSettingChangeSubscriber extends DataChangeSubscriber {
private VisionSettingChangeSubscriber() {
super();
}
@SuppressWarnings({"unchecked", "rawtypes"})
@Override
public void onDataChangeEvent(DataChangeEvent event) {
if (event instanceof IncomingWebSocketEvent) {
var wsEvent = (IncomingWebSocketEvent<?>) event;
// TODO: remove?
if (wsEvent.propertyName.equals("save")) {
logger.debug("UI-based saving deprecated, ignoring");
// saveAndBroadcast();
return;
}
if (wsEvent.cameraIndex != null && wsEvent.cameraIndex == moduleIndex) {
logger.debug("Got PSC event - propName: " + wsEvent.propertyName);
var propName = wsEvent.propertyName;
var newPropValue = wsEvent.data;
var currentSettings = pipelineManager.getCurrentPipeline().getSettings();
// special case for non-PipelineSetting changes
switch (propName) {
case "cameraNickname": // rename camera
var newNickname = (String) newPropValue;
logger.info("Changing nickname to " + newNickname);
setCameraNickname(newNickname);
saveAndBroadcast();
return;
case "pipelineName": // rename current pipeline
logger.info("Changing nick to " + newPropValue);
pipelineManager.getCurrentPipelineSettings().pipelineNickname = (String) newPropValue;
// TODO rename config file
saveAndBroadcast();
return;
case "newPipelineInfo": // add new pipeline
var typeName = (Pair<String, PipelineType>) newPropValue;
var type = typeName.getRight();
var name = typeName.getLeft();
logger.info("Adding a " + type + " pipeline with name " + name);
var addedSettings = pipelineManager.addPipeline(type);
addedSettings.pipelineNickname = name;
saveAndBroadcast();
return;
case "deleteCurrPipeline":
var indexToDelete = pipelineManager.getCurrentPipelineIndex();
logger.info("Deleting current pipe at index " + indexToDelete);
pipelineManager.removePipeline(indexToDelete);
saveAndBroadcast();
return;
case "changePipeline": // change active pipeline
var index = (Integer) newPropValue;
if (index == pipelineManager.getCurrentPipelineIndex()) {
logger.debug("Skipping pipeline change, index " + index + " already active");
return;
}
logger.debug("Setting pipeline index to " + index);
setPipeline(index);
saveAndBroadcast();
return;
}
// special case for camera settables
if (propName.startsWith("camera")) {
var propMethodName = "set" + propName.replace("camera", "");
var methods = visionSource.getSettables().getClass().getMethods();
for (var method : methods) {
if (method.getName().equalsIgnoreCase(propMethodName)) {
try {
method.invoke(visionSource.getSettables(), newPropValue);
} catch (Exception e) {
logger.error("Failed to invoke camera settable method: " + method.getName());
e.printStackTrace();
}
}
}
}
try {
var propField = currentSettings.getClass().getField(propName);
var propType = propField.getType();
if (propType.isEnum()) {
var actual = propType.getEnumConstants()[(int) newPropValue];
propField.set(currentSettings, actual);
} else if (propType.isAssignableFrom(DoubleCouple.class)) {
var orig = (ArrayList<Number>) newPropValue;
var actual = new DoubleCouple(orig.get(0), orig.get(1));
propField.set(currentSettings, actual);
} else if (propType.isAssignableFrom(IntegerCouple.class)) {
var orig = (ArrayList<Integer>) newPropValue;
var actual = new IntegerCouple(orig.get(0), orig.get(1));
propField.set(currentSettings, actual);
} else if (propType.equals(Double.TYPE)) {
propField.setDouble(currentSettings, (Double) newPropValue);
} else if (propType.equals(Integer.TYPE)) {
propField.setInt(currentSettings, (Integer) newPropValue);
} else if (propType.equals(Boolean.TYPE)) {
if (newPropValue instanceof Integer) {
propField.setBoolean(currentSettings, (Integer) newPropValue != 0);
} else {
propField.setBoolean(currentSettings, (Boolean) newPropValue);
}
} else {
propField.set(newPropValue, newPropValue);
}
logger.trace("Set prop " + propName + " to value " + newPropValue);
// special case for extra tasks to perform after setting PipelineSettings
if (propName.equals("outputFrameDivisor")) {
dashboardStreamer.setFrameDivisor(
pipelineManager.getCurrentPipelineSettings().outputFrameDivisor);
}
} catch (NoSuchFieldException | IllegalAccessException e) {
logger.error(
"Could not set prop "
+ propName
+ " with value "
+ newPropValue
+ " on "
+ currentSettings);
e.printStackTrace();
} catch (Exception e) {
logger.error("Unknown exception when setting PSC prop!");
e.printStackTrace();
}
}
saveAndBroadcast();
}
}
}
private void setPipeline(int index) {
logger.info("Setting pipeline to " + index);
pipelineManager.setIndex(index);
var config = pipelineManager.getPipelineSettings(index);
if (config == null) {
logger.error("Config for index " + index + " was null!");
return;
}
visionSource.getSettables().setCurrentVideoMode(config.cameraVideoModeIndex);
visionSource.getSettables().setBrightness(config.cameraBrightness);
visionSource.getSettables().setExposure(config.cameraExposure);
visionSource.getSettables().setGain(config.cameraGain);
visionSource.getSettables().getConfiguration().currentPipelineIndex =
pipelineManager.getCurrentPipelineIndex();
}
private void saveAndBroadcast() {
ConfigManager.getInstance()
.saveModule(
getStateAsCameraConfig(), visionSource.getSettables().getConfiguration().uniqueName);
DataChangeService.getInstance()
.publishEvent(
new OutgoingUIEvent<>(
UIUpdateType.BROADCAST,
"fullsettings",
ConfigManager.getInstance().getConfig().toHashMap()));
}
private void save() {
ConfigManager.getInstance()
.saveModule(
getStateAsCameraConfig(), visionSource.getSettables().getConfiguration().uniqueName);
}
private void setCameraNickname(String newName) {
visionSource.getSettables().getConfiguration().nickname = newName;
ntConsumer.setCameraName(newName);
frameConsumers.remove(dashboardStreamer);
dashboardStreamer =
new MJPGFrameConsumer(visionSource.getSettables().getConfiguration().nickname);
frameConsumers.add(dashboardStreamer);
saveAndBroadcast();
}
public PhotonConfiguration.UICameraConfiguration toUICameraConfig() {
var ret = new PhotonConfiguration.UICameraConfiguration();
ret.fov = visionSource.getSettables().getFOV();
// ret.tiltDegrees = this.visionSource.getSettables() // TODO implement tilt in camera
// settings
ret.nickname = visionSource.getSettables().getConfiguration().nickname;
ret.currentPipelineSettings =
SerializationUtils.objectToHashMap(pipelineManager.getCurrentPipelineSettings());
ret.currentPipelineIndex = pipelineManager.getCurrentPipelineIndex();
ret.pipelineNicknames = pipelineManager.getPipelineNicknames();
// TODO refactor into helper method
var temp = new HashMap<Integer, HashMap<String, Object>>();
var videoModes = visionSource.getSettables().getAllVideoModes();
for (var k : videoModes.keySet()) {
var internalMap = new HashMap<String, Object>();
internalMap.put("width", videoModes.get(k).width);
internalMap.put("height", videoModes.get(k).height);
internalMap.put("fps", videoModes.get(k).fps);
internalMap.put("pixelFormat", videoModes.get(k).pixelFormat.toString());
temp.put(k, internalMap);
}
ret.videoFormatList = temp;
ret.streamPort = dashboardStreamer.getCurrentStreamPort();
// ret.uiStreamPort = uiStreamer.getCurrentStreamPort();
return ret;
}
public CameraConfiguration getStateAsCameraConfig() {
var config = visionSource.getSettables().getConfiguration();
config.setPipelineSettings(pipelineManager.userPipelineSettings);
config.driveModeSettings = pipelineManager.driverModePipeline.getSettings();
config.currentPipelineIndex = pipelineManager.getCurrentPipelineIndex();
return config;
}
public void addDataConsumer(DataConsumer dataConsumer) {
dataConsumers.add(dataConsumer);
}
void addFrameConsumer(FrameConsumer consumer) {
frameConsumers.add(consumer);
}
void consumeResult(CVPipelineResult result) {
// TODO: put result in to Data (not this way!)
var data = new Data();
@@ -59,6 +376,8 @@ public class VisionModule {
var frame = result.outputFrame;
consumeFrame(frame);
data.release();
}
void consumeData(Data data) {
@@ -67,14 +386,6 @@ public class VisionModule {
}
}
public void addDataConsumer(DataConsumer dataConsumer) {
dataConsumers.add(dataConsumer);
}
public void addFrameConsumer(FrameConsumer frameConsumer) {
frameConsumers.add(frameConsumer);
}
void consumeFrame(Frame frame) {
for (var frameConsumer : frameConsumers) {
frameConsumer.accept(frame);

32
photon-server/src/main/java/org/photonvision/vision/processes/VisionModuleManager.java
View File

@@ -18,25 +18,47 @@
package org.photonvision.vision.processes;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import org.photonvision.vision.pipeline.CVPipelineSettings;
/** VisionModuleManager has many VisionModules, and provides camera configuration data to them. */
public class VisionModuleManager {
private static class ThreadSafeSingleton {
private static final VisionModuleManager INSTANCE = new VisionModuleManager();
}
public static VisionModuleManager getInstance() {
return VisionModuleManager.ThreadSafeSingleton.INSTANCE;
}
protected final List<VisionModule> visionModules = new ArrayList<>();
public VisionModuleManager(List<VisionSource> visionSources) {
for (var visionSource : visionSources) {
private VisionModuleManager() {}
// TODO: loading existing pipelines from config
var pipelineManager = new PipelineManager();
public List<VisionModule> getModules() {
return visionModules;
}
visionModules.add(new VisionModule(pipelineManager, visionSource));
public VisionModule getModule(int i) {
return visionModules.get(i);
}
public void addSources(HashMap<VisionSource, List<CVPipelineSettings>> visionSources) {
for (var entry : visionSources.entrySet()) {
var visionSource = entry.getKey();
var pipelineManager = new PipelineManager(entry.getValue());
var module = new VisionModule(pipelineManager, visionSource, visionModules.size());
visionModules.add(module);
// todo: logging
}
}
public void startModules() {
for (var visionModule : visionModules) {
visionModule.start();
// todo: logging
}
}
}

22
photon-server/src/main/java/org/photonvision/vision/processes/VisionRunner.java
View File

@@ -19,15 +19,18 @@ package org.photonvision.vision.processes;
import java.util.function.Consumer;
import java.util.function.Supplier;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.vision.frame.Frame;
import org.photonvision.vision.frame.FrameProvider;
import org.photonvision.vision.pipeline.CVPipeline;
import org.photonvision.vision.pipeline.CVPipelineResult;
import org.photonvision.vision.pipeline.result.CVPipelineResult;
/** VisionRunner has a frame supplier, a pipeline supplier, and a result consumer */
@SuppressWarnings("rawtypes")
public class VisionRunner {
private final Logger logger;
private final Thread visionProcessThread;
private final Supplier<Frame> frameSupplier;
private final Supplier<CVPipeline> pipelineSupplier;
@@ -51,19 +54,17 @@ public class VisionRunner {
this.pipelineSupplier = pipelineSupplier;
this.pipelineResultConsumer = pipelineResultConsumer;
this.visionProcessThread = new Thread(this::update);
this.visionProcessThread.setName("VisionRunner - " + frameSupplier.getName());
visionProcessThread = new Thread(this::update);
visionProcessThread.setName("VisionRunner - " + frameSupplier.getName());
logger = new Logger(VisionRunner.class, frameSupplier.getName(), LogGroup.VisionProcess);
}
public void startProcess() {
visionProcessThread.start();
}
private boolean hasThrown;
private void update() {
while (!Thread.interrupted()) {
loopCount++;
var pipeline = pipelineSupplier.get();
var frame = frameSupplier.get();
@@ -71,13 +72,10 @@ public class VisionRunner {
var pipelineResult = pipeline.run(frame);
pipelineResultConsumer.accept(pipelineResult);
} catch (Exception ex) {
if (hasThrown) {
System.err.println(
"Exception in thread \"" + visionProcessThread.getName() + "\", loop " + loopCount);
ex.printStackTrace();
hasThrown = true;
}
logger.error("Exception on loop " + loopCount);
ex.printStackTrace();
}
loopCount++;
}
}
}

1
photon-server/src/main/java/org/photonvision/vision/processes/VisionSource.java
View File

@@ -20,7 +20,6 @@ package org.photonvision.vision.processes;
import org.photonvision.vision.frame.FrameProvider;
public interface VisionSource {
FrameProvider getFrameProvider();
VisionSourceSettables getSettables();

18
photon-server/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java
View File

@@ -21,6 +21,7 @@ import edu.wpi.cscore.UsbCamera;
import edu.wpi.cscore.UsbCameraInfo;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collection;
import java.util.List;
import java.util.stream.Collectors;
import org.apache.commons.lang3.NotImplementedException;
@@ -31,12 +32,13 @@ import org.photonvision.vision.camera.USBCameraSource;
import org.photonvision.vision.frame.provider.NetworkFrameProvider;
public class VisionSourceManager {
public List<VisionSource> LoadAllSources(List<CameraConfiguration> camerasConfiguration) {
public static List<VisionSource> loadAllSources(
Collection<CameraConfiguration> camerasConfiguration) {
return LoadAllSources(camerasConfiguration, Arrays.asList(UsbCamera.enumerateUsbCameras()));
}
public List<VisionSource> LoadAllSources(
List<CameraConfiguration> camerasConfiguration, List<UsbCameraInfo> usbCameraInfos) {
public static List<VisionSource> LoadAllSources(
Collection<CameraConfiguration> camerasConfiguration, List<UsbCameraInfo> usbCameraInfos) {
var UsbCamerasConfiguration =
camerasConfiguration.stream()
.filter(configuration -> configuration.cameraType == CameraType.UsbCamera)
@@ -47,11 +49,11 @@ public class VisionSourceManager {
return loadUSBCameraSources(matchedCameras);
}
private NetworkFrameProvider loadHTTPCamera(CameraConfiguration config) {
private static NetworkFrameProvider loadHTTPCamera(CameraConfiguration config) {
throw new NotImplementedException("");
}
private List<CameraConfiguration> matchUSBCameras(
private static List<CameraConfiguration> matchUSBCameras(
List<UsbCameraInfo> infos, List<CameraConfiguration> cameraConfigurationList) {
ArrayList<UsbCameraInfo> loopableInfo = new ArrayList<>(infos);
List<CameraConfiguration> cameraConfigurations = new ArrayList<>();
@@ -91,21 +93,21 @@ public class VisionSourceManager {
}
CameraConfiguration configuration =
new CameraConfiguration(name, uniqueName, uniqueName, ((Integer) info.dev).toString());
new CameraConfiguration(name, uniqueName, uniqueName, info.path);
cameraConfigurations.add(configuration);
}
return cameraConfigurations;
}
private List<VisionSource> loadUSBCameraSources(List<CameraConfiguration> configurations) {
private static List<VisionSource> loadUSBCameraSources(List<CameraConfiguration> configurations) {
List<VisionSource> usbCameraSources = new ArrayList<>();
configurations.forEach(
configuration -> usbCameraSources.add(new USBCameraSource(configuration)));
return usbCameraSources;
}
private boolean containsName(final List<CameraConfiguration> list, final String name) {
private static boolean containsName(final List<CameraConfiguration> list, final String name) {
return list.stream().anyMatch(configuration -> configuration.uniqueName.equals(name));
}
}

17
photon-server/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java
View File

@@ -23,15 +23,19 @@ import org.photonvision.common.configuration.CameraConfiguration;
import org.photonvision.vision.frame.FrameStaticProperties;
public abstract class VisionSourceSettables {
private CameraConfiguration configuration;
private final CameraConfiguration configuration;
protected VisionSourceSettables(CameraConfiguration configuration) {
this.configuration = configuration;
}
FrameStaticProperties frameStaticProperties;
protected FrameStaticProperties frameStaticProperties;
protected HashMap<Integer, VideoMode> videoModes;
public CameraConfiguration getConfiguration() {
return configuration;
}
public abstract int getExposure();
public abstract void setExposure(int exposure);
@@ -46,8 +50,17 @@ public abstract class VisionSourceSettables {
public abstract VideoMode getCurrentVideoMode();
public void setCurrentVideoMode(int index) {
setCurrentVideoMode(getAllVideoModes().get(index));
}
public abstract void setCurrentVideoMode(VideoMode videoMode);
@SuppressWarnings("unused")
public void setVideoModeIndex(int index) {
setCurrentVideoMode(videoModes.get(index));
}
public abstract HashMap<Integer, VideoMode> getAllVideoModes();
public double getFOV() {

6
photon-server/src/main/java/org/photonvision/vision/target/TargetCalculations.java
View File

@@ -25,12 +25,14 @@ import org.photonvision.common.util.numbers.DoubleCouple;
public class TargetCalculations {
public static double calculateYaw(
double offsetCenterX, double targetCenterX, double horizontalFocalLength) {
return FastMath.toDegrees(FastMath.atan(offsetCenterX - targetCenterX) / horizontalFocalLength);
return FastMath.toDegrees(
FastMath.atan((offsetCenterX - targetCenterX) / horizontalFocalLength));
}
public static double calculatePitch(
double offsetCenterY, double targetCenterY, double verticalFocalLength) {
return -FastMath.toDegrees(FastMath.atan(offsetCenterY - targetCenterY) / verticalFocalLength);
return -FastMath.toDegrees(
FastMath.atan((offsetCenterY - targetCenterY) / verticalFocalLength));
}
public static Point calculateTargetOffsetPoint(

24
photon-server/src/main/java/org/photonvision/vision/target/TrackedTarget.java
View File

@@ -18,6 +18,7 @@
package org.photonvision.vision.target;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import java.util.HashMap;
import java.util.List;
import org.opencv.core.Mat;
import org.opencv.core.MatOfPoint2f;
@@ -78,6 +79,11 @@ public class TrackedTarget implements Releasable {
return m_yaw;
}
private double getSkew() {
// TODO skew;
return 5940;
}
public double getArea() {
return m_area;
}
@@ -161,6 +167,24 @@ public class TrackedTarget implements Releasable {
cameraRelativeRvec.copyTo(this.m_cameraRelativeRvec);
}
public HashMap<String, Object> toHashMap() {
// pitch: 0,
// yaw: 0,
// skew: 0,
// area: 0,
// // 3D only
// pose: {x: 0, y: 0, rot: 0},
var ret = new HashMap<String, Object>();
ret.put("pitch", getPitch());
ret.put("yaw", getYaw());
ret.put("skew", getSkew());
ret.put("area", getArea());
if (getRobotRelativePose() != null) {
ret.put("pose", getRobotRelativePose().toHashMap());
}
return ret;
}
public static class TargetCalculationParameters {
// TargetOffset calculation values
final boolean isLandscape;