Skip to content

Commit 30a6cbb

Browse files
mcm001mcm001
committed
ope
1 parent d7a2cdc commit 30a6cbb

File tree

1 file changed

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

1 file changed

+92
-128
lines changed

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

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

2020
import edu.wpi.first.hal.HAL;
21-
import edu.wpi.first.math.geometry.Rotation2d;
2221
import java.io.IOException;
2322
import java.nio.file.Path;
2423
import java.util.ArrayList;
2524
import java.util.List;
2625
import org.apache.commons.cli.*;
27-
import org.opencv.core.Size;
28-
import org.photonvision.common.LoadJNI;
29-
import org.photonvision.common.LoadJNI.JNITypes;
3026
import org.photonvision.common.configuration.CameraConfiguration;
3127
import org.photonvision.common.configuration.ConfigManager;
3228
import org.photonvision.common.configuration.NeuralNetworkModelManager;
3329
import org.photonvision.common.dataflow.networktables.NetworkTablesManager;
3430
import org.photonvision.common.hardware.HardwareManager;
35-
import org.photonvision.common.hardware.OsImageData;
31+
import org.photonvision.common.hardware.OsImageVersion;
3632
import org.photonvision.common.hardware.PiVersion;
3733
import org.photonvision.common.hardware.Platform;
38-
import org.photonvision.common.hardware.metrics.SystemMonitor;
3934
import org.photonvision.common.logging.KernelLogLogger;
4035
import org.photonvision.common.logging.LogGroup;
4136
import org.photonvision.common.logging.LogLevel;
4237
import org.photonvision.common.logging.Logger;
4338
import org.photonvision.common.logging.PvCSCoreLogger;
4439
import org.photonvision.common.networking.NetworkManager;
4540
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;
5148
import org.photonvision.vision.camera.PVCameraInfo;
52-
import org.photonvision.vision.frame.FrameDivisor;
5349
import org.photonvision.vision.opencv.CVMat;
5450
import org.photonvision.vision.pipeline.AprilTagPipelineSettings;
5551
import org.photonvision.vision.pipeline.CVPipelineSettings;
@@ -61,6 +57,7 @@ public class Main {
6157
public static final int DEFAULT_WEBPORT = 5800;
6258

6359
private static final Logger logger = new Logger(Main.class, LogGroup.General);
60+
private static final boolean isRelease = PhotonVersion.isRelease;
6461

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

80-
options.addOption("f", "folder", true, "Point test mode to a specific folder");
77+
options.addOption("p", "path", true, "Point test mode to a specific folder");
8178
options.addOption("n", "disable-networking", false, "Disables control device network settings");
8279
options.addOption(
8380
"c",
@@ -89,7 +86,6 @@ private static boolean handleArgs(String[] args) throws ParseException {
8986
"smoketest",
9087
false,
9188
"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");
9389

9490
CommandLineParser parser = new DefaultParser();
9591
CommandLine cmd = parser.parse(options, args);
@@ -126,104 +122,49 @@ private static boolean handleArgs(String[] args) throws ParseException {
126122
if (cmd.hasOption("smoketest")) {
127123
isSmoketest = true;
128124
}
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-
}
141125
}
142126
return true;
143127
}
144128

145129
private static void addTestModeSources() {
146130
ConfigManager.getInstance().load();
147131

148-
CameraConfiguration camConf2026 =
149-
ConfigManager.getInstance().getConfig().getCameraConfigurations().get("WPI2026");
150-
if (camConf2026 == null) {
151-
camConf2026 =
132+
CameraConfiguration camConf2024 =
133+
ConfigManager.getInstance().getConfig().getCameraConfigurations().get("WPI2024");
134+
if (camConf2024 == null || true) {
135+
camConf2024 =
152136
new CameraConfiguration(
153137
PVCameraInfo.fromFileInfo(
154138
TestUtils.getResourcesFolderPath(true)
155139
.resolve("testimages")
156-
.resolve(TestUtils.WPI2026Images.kBlueOutpostFuelSpread.path)
140+
.resolve(TestUtils.WPI2024Images.kSpeakerCenter_143in.path)
157141
.toString(),
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;
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;
206159
}
207160

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

210163
ConfigManager.getInstance().unloadCameraConfigs();
211164
cameraConfigs.stream().forEach(ConfigManager.getInstance()::addCameraConfiguration);
212165
VisionSourceManager.getInstance().registerLoadedConfigs(cameraConfigs);
213166
}
214167

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-
227168
public static void main(String[] args) {
228169
var logLevel = printDebugLogs ? LogLevel.TRACE : LogLevel.DEBUG;
229170
Logger.setLevel(LogGroup.Camera, logLevel);
@@ -241,12 +182,8 @@ public static void main(String[] args) {
241182
+ Platform.getPlatformName()
242183
+ (Platform.isRaspberryPi() ? (" (Pi " + PiVersion.getPiVersion() + ")") : ""));
243184

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");
185+
if (OsImageVersion.IMAGE_VERSION.isPresent()) {
186+
logger.info("PhotonVision image version: " + OsImageVersion.IMAGE_VERSION.get());
250187
}
251188

252189
try {
@@ -257,55 +194,86 @@ public static void main(String[] args) {
257194
logger.error("Failed to parse command-line options!", e);
258195
}
259196

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

267203
try {
268-
var loadStart = System.currentTimeMillis();
269-
logger.debug("Starting LoadJNI::loadLibraries");
270-
boolean success = LoadJNI.loadLibraries();
204+
boolean success = TestUtils.loadLibraries();
271205

272206
if (!success) {
273207
logger.error("Failed to load native libraries! Giving up :(");
274208
System.exit(1);
275209
}
276-
277-
logger.info(
278-
"WPILib and photon-targeting JNI libraries loaded in "
279-
+ ((System.currentTimeMillis() - loadStart) / 1000.0)
280-
+ "seconds!");
281210
} catch (Exception e) {
282211
logger.error("Failed to load native libraries!", e);
283212
System.exit(1);
284213
}
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.");
285228

286229
if (!HAL.initialize(500, 0)) {
287230
logger.error("Failed to initialize the HAL! Giving up :(");
288231
System.exit(1);
289232
}
290233

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

295-
if (Platform.isRK3588()) {
296-
tryLoadJNI(JNITypes.RKNN_DETECTOR);
297-
} else {
298-
logger.warn("Platform does not support RKNN based machine learning!");
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);
299255
}
300256

301-
if (Platform.isQCS6490()) {
302-
tryLoadJNI(JNITypes.RUBIK_DETECTOR);
303-
} else {
304-
logger.warn("Platform does not support Rubik based machine learning!");
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);
305270
}
306-
307-
if (Platform.isWindows() || Platform.isLinux()) {
308-
tryLoadJNI(JNITypes.MRCAL);
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());
309277
}
310278

311279
CVMat.enablePrint(false);
@@ -326,6 +294,10 @@ public static void main(String[] args) {
326294
modelManager.extractModels();
327295
modelManager.discoverModels();
328296

297+
logger.debug("Loading HardwareManager...");
298+
// Force load the hardware manager
299+
HardwareManager.getInstance();
300+
329301
logger.debug("Loading NetworkManager...");
330302
NetworkManager.getInstance().reinitialize();
331303

@@ -334,19 +306,11 @@ public static void main(String[] args) {
334306
.setConfig(ConfigManager.getInstance().getConfig().getNetworkConfig());
335307
NetworkTablesManager.getInstance().registerTimedTasks();
336308

337-
logger.debug("Loading HardwareManager...");
338-
// Force load the hardware manager
339-
HardwareManager.getInstance();
340-
341309
if (isSmoketest) {
342310
logger.info("PhotonVision base functionality loaded -- smoketest complete");
343311
System.exit(0);
344312
}
345313

346-
logger.debug("Loading SystemMonitor...");
347-
SystemMonitor.getInstance().logSystemInformation();
348-
SystemMonitor.getInstance().startMonitor(500, 1000);
349-
350314
// todo - should test mode just add test mode sources, but still allow local usb cameras to be
351315
// added?
352316
if (!isTestMode) {
@@ -363,7 +327,7 @@ public static void main(String[] args) {
363327
VisionSourceManager.getInstance().registerTimedTasks();
364328

365329
logger.info("Starting server...");
366-
HardwareManager.getInstance().setError(null);
330+
HardwareManager.getInstance().setRunning(true);
367331
Server.initialize(DEFAULT_WEBPORT);
368332
}
369333
}

0 commit comments

Comments
 (0)