Skip to content

Commit bd2bb12

Browse files
mcm001mcm001
committed
ope 2
1 parent 30a6cbb commit bd2bb12

File tree

1 file changed

+122
-92
lines changed
  • photon-server/src/main/java/org/photonvision

1 file changed

+122
-92
lines changed

‎photon-server/src/main/java/org/photonvision/Main.java‎

Lines changed: 122 additions & 92 deletions
Original file line numberDiff line numberDiff line change
@@ -18,34 +18,38 @@
1818
package org.photonvision;
1919

2020
import edu.wpi.first.hal.HAL;
21+
import edu.wpi.first.math.geometry.Rotation2d;
2122
import java.io.IOException;
2223
import java.nio.file.Path;
2324
import java.util.ArrayList;
2425
import java.util.List;
2526
import org.apache.commons.cli.*;
27+
import org.opencv.core.Size;
28+
import org.photonvision.common.LoadJNI;
29+
import org.photonvision.common.LoadJNI.JNITypes;
2630
import org.photonvision.common.configuration.CameraConfiguration;
2731
import org.photonvision.common.configuration.ConfigManager;
2832
import org.photonvision.common.configuration.NeuralNetworkModelManager;
2933
import org.photonvision.common.dataflow.networktables.NetworkTablesManager;
3034
import org.photonvision.common.hardware.HardwareManager;
31-
import org.photonvision.common.hardware.OsImageVersion;
35+
import org.photonvision.common.hardware.OsImageData;
3236
import org.photonvision.common.hardware.PiVersion;
3337
import org.photonvision.common.hardware.Platform;
38+
import org.photonvision.common.hardware.metrics.SystemMonitor;
3439
import org.photonvision.common.logging.KernelLogLogger;
3540
import org.photonvision.common.logging.LogGroup;
3641
import org.photonvision.common.logging.LogLevel;
3742
import org.photonvision.common.logging.Logger;
3843
import org.photonvision.common.logging.PvCSCoreLogger;
3944
import org.photonvision.common.networking.NetworkManager;
4045
import org.photonvision.common.util.TestUtils;
41-
import org.photonvision.jni.PhotonTargetingJniLoader;
42-
import org.photonvision.jni.RknnDetectorJNI;
43-
import org.photonvision.jni.RubikDetectorJNI;
44-
import org.photonvision.mrcal.MrCalJNILoader;
45-
import org.photonvision.raspi.LibCameraJNILoader;
4646
import org.photonvision.server.Server;
4747
import org.photonvision.vision.apriltag.AprilTagFamily;
48+
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
49+
import org.photonvision.vision.calibration.CameraLensModel;
50+
import org.photonvision.vision.calibration.JsonMatOfDouble;
4851
import org.photonvision.vision.camera.PVCameraInfo;
52+
import org.photonvision.vision.frame.FrameDivisor;
4953
import org.photonvision.vision.opencv.CVMat;
5054
import org.photonvision.vision.pipeline.AprilTagPipelineSettings;
5155
import org.photonvision.vision.pipeline.CVPipelineSettings;
@@ -57,7 +61,6 @@ public class Main {
5761
public static final int DEFAULT_WEBPORT = 5800;
5862

5963
private static final Logger logger = new Logger(Main.class, LogGroup.General);
60-
private static final boolean isRelease = PhotonVersion.isRelease;
6164

6265
private static boolean isTestMode = false;
6366
private static boolean isSmoketest = false;
@@ -74,7 +77,7 @@ private static boolean handleArgs(String[] args) throws ParseException {
7477
false,
7578
"Run in test mode with 2019 and 2020 WPI field images in place of cameras");
7679

77-
options.addOption("p", "path", true, "Point test mode to a specific folder");
80+
options.addOption("f", "folder", true, "Point test mode to a specific folder");
7881
options.addOption("n", "disable-networking", false, "Disables control device network settings");
7982
options.addOption(
8083
"c",
@@ -86,6 +89,7 @@ private static boolean handleArgs(String[] args) throws ParseException {
8689
"smoketest",
8790
false,
8891
"Exit Photon after loading native libraries and camera configs, but before starting up camera runners");
92+
options.addOption("p", "platform", true, "Specify platform override, based on Platform enum");
8993

9094
CommandLineParser parser = new DefaultParser();
9195
CommandLine cmd = parser.parse(options, args);
@@ -122,49 +126,104 @@ private static boolean handleArgs(String[] args) throws ParseException {
122126
if (cmd.hasOption("smoketest")) {
123127
isSmoketest = true;
124128
}
129+
130+
if (cmd.hasOption("platform")) {
131+
String platStr = cmd.getOptionValue("platform");
132+
try {
133+
Platform plat = Platform.valueOf(platStr);
134+
Platform.overridePlatform(plat);
135+
logger.info("Overrode platform to: " + plat);
136+
} catch (IllegalArgumentException e) {
137+
logger.error("Invalid platform override: " + platStr);
138+
return false;
139+
}
140+
}
125141
}
126142
return true;
127143
}
128144

129145
private static void addTestModeSources() {
130146
ConfigManager.getInstance().load();
131147

132-
CameraConfiguration camConf2024 =
133-
ConfigManager.getInstance().getConfig().getCameraConfigurations().get("WPI2024");
134-
if (camConf2024 == null || true) {
135-
camConf2024 =
148+
CameraConfiguration camConf2026 =
149+
ConfigManager.getInstance().getConfig().getCameraConfigurations().get("WPI2026");
150+
if (camConf2026 == null) {
151+
camConf2026 =
136152
new CameraConfiguration(
137153
PVCameraInfo.fromFileInfo(
138154
TestUtils.getResourcesFolderPath(true)
139155
.resolve("testimages")
140-
.resolve(TestUtils.WPI2024Images.kSpeakerCenter_143in.path)
156+
.resolve(TestUtils.WPI2026Images.kBlueOutpostFuelSpread.path)
141157
.toString(),
142-
"WPI2024"));
143-
144-
camConf2024.FOV = TestUtils.WPI2024Images.FOV;
145-
// same camera as 2023
146-
camConf2024.calibrations.add(TestUtils.get2023LifeCamCoeffs(true));
147-
148-
var pipeline2024 = new AprilTagPipelineSettings();
149-
var path_split = Path.of(camConf2024.matchedCameraInfo.path()).getFileName().toString();
150-
pipeline2024.pipelineNickname = path_split.replace(".jpg", "");
151-
pipeline2024.targetModel = TargetModel.kAprilTag6p5in_36h11;
152-
pipeline2024.tagFamily = AprilTagFamily.kTag36h11;
153-
pipeline2024.inputShouldShow = true;
154-
pipeline2024.solvePNPEnabled = true;
155-
156-
var psList2024 = new ArrayList<CVPipelineSettings>();
157-
psList2024.add(pipeline2024);
158-
camConf2024.pipelineSettings = psList2024;
158+
"WPI2026"));
159+
160+
camConf2026.FOV = TestUtils.WPI2026Images.FOV.getDegrees();
161+
162+
// stolen from SimCameraProperties
163+
int resWidth = (int) TestUtils.WPI2026Images.resolution.width;
164+
int resHeight = (int) TestUtils.WPI2026Images.resolution.height;
165+
double cx = resWidth / 2.0 - 0.5;
166+
double cy = resHeight / 2.0 - 0.5;
167+
168+
double resDiag = Math.hypot(resWidth, resHeight);
169+
double diagRatio = Math.tan(TestUtils.WPI2026Images.FOV.getRadians() / 2);
170+
var fovWidth = new Rotation2d(Math.atan(diagRatio * (resWidth / resDiag)) * 2);
171+
var fovHeight = new Rotation2d(Math.atan(diagRatio * (resHeight / resDiag)) * 2);
172+
173+
double fx = cx / Math.tan(fovWidth.getRadians() / 2.0);
174+
double fy = cy / Math.tan(fovHeight.getRadians() / 2.0);
175+
176+
JsonMatOfDouble testCameraMatrix =
177+
new JsonMatOfDouble(3, 3, new double[] {fx, 0, cx, 0, fy, cy, 0, 0, 1});
178+
JsonMatOfDouble testDistortion = new JsonMatOfDouble(1, 5, new double[] {0, 0, 0, 0, 0});
179+
180+
camConf2026.calibrations.add(
181+
new CameraCalibrationCoefficients(
182+
new Size(4000, 1868),
183+
testCameraMatrix,
184+
testDistortion,
185+
new double[0],
186+
List.of(),
187+
new Size(),
188+
1,
189+
CameraLensModel.LENSMODEL_OPENCV));
190+
191+
logger.info("Added test camera calibration for WPI2026 " + camConf2026.calibrations);
192+
193+
var pipeline2026 = new AprilTagPipelineSettings();
194+
var path_split = Path.of(camConf2026.matchedCameraInfo.path()).getFileName().toString();
195+
pipeline2026.pipelineNickname = path_split.replace(".jpg", "");
196+
pipeline2026.targetModel = TargetModel.kAprilTag6p5in_36h11;
197+
pipeline2026.tagFamily = AprilTagFamily.kTag36h11;
198+
pipeline2026.inputShouldShow = true;
199+
pipeline2026.solvePNPEnabled = true;
200+
pipeline2026.streamingFrameDivisor = FrameDivisor.QUARTER;
201+
pipeline2026.decimate = 4;
202+
203+
var psList2026 = new ArrayList<CVPipelineSettings>();
204+
psList2026.add(pipeline2026);
205+
camConf2026.pipelineSettings = psList2026;
159206
}
160207

161-
var cameraConfigs = List.of(camConf2024);
208+
var cameraConfigs = List.of(camConf2026);
162209

163210
ConfigManager.getInstance().unloadCameraConfigs();
164211
cameraConfigs.stream().forEach(ConfigManager.getInstance()::addCameraConfiguration);
165212
VisionSourceManager.getInstance().registerLoadedConfigs(cameraConfigs);
166213
}
167214

215+
private static void tryLoadJNI(JNITypes type) {
216+
try {
217+
LoadJNI.forceLoad(type);
218+
logger.info("Loaded " + type.name() + "-JNI");
219+
} catch (IOException e) {
220+
logger.error("Failed to load " + type.name() + "-JNI!", e);
221+
if (isSmoketest) {
222+
System.exit(1);
223+
}
224+
}
225+
}
226+
168227
public static void main(String[] args) {
169228
var logLevel = printDebugLogs ? LogLevel.TRACE : LogLevel.DEBUG;
170229
Logger.setLevel(LogGroup.Camera, logLevel);
@@ -182,8 +241,12 @@ public static void main(String[] args) {
182241
+ Platform.getPlatformName()
183242
+ (Platform.isRaspberryPi() ? (" (Pi " + PiVersion.getPiVersion() + ")") : ""));
184243

185-
if (OsImageVersion.IMAGE_VERSION.isPresent()) {
186-
logger.info("PhotonVision image version: " + OsImageVersion.IMAGE_VERSION.get());
244+
if (OsImageData.IMAGE_METADATA.isPresent()) {
245+
logger.info("PhotonVision image data: " + OsImageData.IMAGE_METADATA.get());
246+
} else if (OsImageData.IMAGE_VERSION.isPresent()) {
247+
logger.info("PhotonVision image version: " + OsImageData.IMAGE_VERSION.get());
248+
} else {
249+
logger.info("PhotonVision image version: unknown");
187250
}
188251

189252
try {
@@ -194,14 +257,15 @@ public static void main(String[] args) {
194257
logger.error("Failed to parse command-line options!", e);
195258
}
196259

197-
// We don't want to trigger an exit in test mode or smoke test. This is specifically for MacOS.
260+
// We don't want to trigger an exit in test mode or smoke test. This is
261+
// specifically for MacOS.
198262
if (!(Platform.isSupported() || isSmoketest || isTestMode)) {
199263
logger.error("This platform is unsupported!");
200264
System.exit(1);
201265
}
202266

203267
try {
204-
boolean success = TestUtils.loadLibraries();
268+
boolean success = LoadJNI.loadLibraries();
205269

206270
if (!success) {
207271
logger.error("Failed to load native libraries! Giving up :(");
@@ -211,69 +275,31 @@ public static void main(String[] args) {
211275
logger.error("Failed to load native libraries!", e);
212276
System.exit(1);
213277
}
214-
logger.info("WPI JNI libraries loaded.");
215-
216-
try {
217-
boolean success = PhotonTargetingJniLoader.load();
218-
219-
if (!success) {
220-
logger.error("Failed to load photon-targeting JNI! Giving up :(");
221-
System.exit(1);
222-
}
223-
} catch (Exception e) {
224-
logger.error("Failed to load photon-targeting JNI!", e);
225-
System.exit(1);
226-
}
227-
logger.info("photon-targeting JNI libraries loaded.");
278+
logger.info("WPILib and photon-targeting JNI libraries loaded.");
228279

229280
if (!HAL.initialize(500, 0)) {
230281
logger.error("Failed to initialize the HAL! Giving up :(");
231282
System.exit(1);
232283
}
233284

234-
try {
235-
if (Platform.isRaspberryPi()) {
236-
LibCameraJNILoader.forceLoad();
237-
}
238-
} catch (IOException e) {
239-
logger.error("Failed to load libcamera-JNI!", e);
285+
if (Platform.isRaspberryPi()) {
286+
tryLoadJNI(JNITypes.LIBCAMERA);
240287
}
241288

242-
try {
243-
if (Platform.isRK3588()) {
244-
RknnDetectorJNI.forceLoad();
245-
if (RknnDetectorJNI.getInstance().isLoaded()) {
246-
logger.info("RknnDetectorJNI loaded successfully.");
247-
} else {
248-
logger.error("Failed to load RknnDetectorJNI!");
249-
}
250-
} else {
251-
logger.error("Platform does not support RKNN based machine learning!");
252-
}
253-
} catch (IOException e) {
254-
logger.error("Failed to load rknn-JNI!", e);
289+
if (Platform.isRK3588()) {
290+
tryLoadJNI(JNITypes.RKNN_DETECTOR);
291+
} else {
292+
logger.warn("Platform does not support RKNN based machine learning!");
255293
}
256294

257-
try {
258-
if (Platform.isQCS6490()) {
259-
RubikDetectorJNI.forceLoad();
260-
if (RubikDetectorJNI.getInstance().isLoaded()) {
261-
logger.info("RubikDetectorJNI loaded successfully.");
262-
} else {
263-
logger.error("Failed to load RubikDetectorJNI!");
264-
}
265-
} else {
266-
logger.error("Platform does not support Rubik based machine learning!");
267-
}
268-
} catch (IOException e) {
269-
logger.error("Failed to load rubik-JNI!", e);
295+
if (Platform.isQCS6490()) {
296+
tryLoadJNI(JNITypes.RUBIK_DETECTOR);
297+
} else {
298+
logger.warn("Platform does not support Rubik based machine learning!");
270299
}
271-
try {
272-
MrCalJNILoader.forceLoad();
273-
} catch (IOException e) {
274-
logger.warn(
275-
"Failed to load mrcal-JNI! Camera calibration will fall back to opencv\n"
276-
+ e.getMessage());
300+
301+
if (Platform.isWindows() || Platform.isLinux()) {
302+
tryLoadJNI(JNITypes.MRCAL);
277303
}
278304

279305
CVMat.enablePrint(false);
@@ -294,10 +320,6 @@ public static void main(String[] args) {
294320
modelManager.extractModels();
295321
modelManager.discoverModels();
296322

297-
logger.debug("Loading HardwareManager...");
298-
// Force load the hardware manager
299-
HardwareManager.getInstance();
300-
301323
logger.debug("Loading NetworkManager...");
302324
NetworkManager.getInstance().reinitialize();
303325

@@ -306,11 +328,19 @@ public static void main(String[] args) {
306328
.setConfig(ConfigManager.getInstance().getConfig().getNetworkConfig());
307329
NetworkTablesManager.getInstance().registerTimedTasks();
308330

331+
logger.debug("Loading HardwareManager...");
332+
// Force load the hardware manager
333+
HardwareManager.getInstance();
334+
309335
if (isSmoketest) {
310336
logger.info("PhotonVision base functionality loaded -- smoketest complete");
311337
System.exit(0);
312338
}
313339

340+
logger.debug("Loading SystemMonitor...");
341+
SystemMonitor.getInstance().logSystemInformation();
342+
SystemMonitor.getInstance().startMonitor(500, 1000);
343+
314344
// todo - should test mode just add test mode sources, but still allow local usb cameras to be
315345
// added?
316346
if (!isTestMode) {
@@ -327,7 +357,7 @@ public static void main(String[] args) {
327357
VisionSourceManager.getInstance().registerTimedTasks();
328358

329359
logger.info("Starting server...");
330-
HardwareManager.getInstance().setRunning(true);
360+
HardwareManager.getInstance().setError(null);
331361
Server.initialize(DEFAULT_WEBPORT);
332362
}
333363
}

0 commit comments

Comments
 (0)