Skip to content

Commit

Permalink
Merge branch 'master' into enable-aruco
Browse files Browse the repository at this point in the history
  • Loading branch information
@srimanachanta
srimanachanta authored Oct 9, 2023
2 parents 732a516 + 6e8e3a0 commit 724149f
Show file tree
Hide file tree
Showing 72 changed files with 1,928 additions and 2,068 deletions.
11 changes: 5 additions & 6 deletions photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.Num;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
Expand Down Expand Up @@ -142,16 +142,15 @@ public static MatOfPoint3f rotationToRvec(Rotation3d rotation) {
* @param rvecInput The rvec to create a Rotation3d from
*/
public static Rotation3d rvecToRotation(Mat rvecInput) {
// Get the 'rodriguez' (axis-angle, where the norm is the angle about the normalized direction
// of the vector)
float[] data = new float[3];
var wrapped = new Mat(rvecInput.rows(), rvecInput.cols(), CvType.CV_32F);
rvecInput.convertTo(wrapped, CvType.CV_32F);
wrapped.get(0, 0, data);
wrapped.release();
Vector<N3> axis = new Vector<>(Nat.N3());
axis.set(0, 0, data[0]);
axis.set(1, 0, data[1]);
axis.set(2, 0, data[2]);
return rotationEDNtoNWU(new Rotation3d(axis.div(axis.norm()), axis.norm()));

return rotationEDNtoNWU(new Rotation3d(VecBuilder.fill(data[0], data[1], data[2])));
}

public static Point avgPoint(Point[] points) {
Expand Down
2 changes: 1 addition & 1 deletion photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -103,7 +103,7 @@ public static PNPResults estimateCamPosePNP(
Point[] points = OpenCVHelp.cornersToPoints(corners);

// single-tag pnp
if (visTags.size() == 1) {
if (knownTags.size() == 1) {
var camToTag =
OpenCVHelp.solvePNP_SQUARE(
cameraMatrix, distCoeffs, TargetModel.kTag16h5.vertices, points);
Expand Down
26 changes: 14 additions & 12 deletions photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,6 +66,8 @@ public class VisionSystemSim {

private final Field2d dbgField;

private final Transform3d kEmptyTrf = new Transform3d();

/**
* A simulated vision system involving a camera(s) and coprocessor(s) mounted on a mobile robot
* running PhotonVision, detecting targets placed on the field. {@link VisionTargetSim}s added to
Expand Down Expand Up @@ -337,7 +339,7 @@ public Field2d getDebugField() {
* Periodic update. Ensure this is called repeatedly-- camera performance is used to automatically
* determine if a new frame should be submitted.
*
* @param robotPoseMeters The current robot pose in meters
* @param robotPoseMeters The simulated robot pose in meters
*/
public void update(Pose2d robotPoseMeters) {
update(new Pose3d(robotPoseMeters));
Expand All @@ -347,7 +349,7 @@ public void update(Pose2d robotPoseMeters) {
* Periodic update. Ensure this is called repeatedly-- camera performance is used to automatically
* determine if a new frame should be submitted.
*
* @param robotPoseMeters The current robot pose in meters
* @param robotPoseMeters The simulated robot pose in meters
*/
public void update(Pose3d robotPoseMeters) {
var targetTypes = targetSets.entrySet();
Expand All @@ -370,13 +372,15 @@ public void update(Pose3d robotPoseMeters) {

var allTargets = new ArrayList<VisionTargetSim>();
targetTypes.forEach((entry) -> allTargets.addAll(entry.getValue()));
var visibleTargets = new ArrayList<Pose3d>();
var cameraPose2ds = new ArrayList<Pose2d>();
var visTgtPoses2d = new ArrayList<Pose2d>();
var cameraPoses2d = new ArrayList<Pose2d>();
boolean processed = false;
// process each camera
for (var camSim : camSimMap.values()) {
// check if this camera is ready to process and get latency
var optTimestamp = camSim.consumeNextEntryTime();
if (optTimestamp.isEmpty()) continue;
else processed = true;
// when this result "was" read by NT
long timestampNT = optTimestamp.get();
// this result's processing latency in milliseconds
Expand All @@ -387,22 +391,20 @@ public void update(Pose3d robotPoseMeters) {
// use camera pose from the image capture timestamp
Pose3d lateRobotPose = getRobotPose(timestampCapture);
Pose3d lateCameraPose = lateRobotPose.plus(getRobotToCamera(camSim, timestampCapture).get());
cameraPose2ds.add(lateCameraPose.toPose2d());
cameraPoses2d.add(lateCameraPose.toPose2d());

// process a PhotonPipelineResult with visible targets
var camResult = camSim.process(latencyMillis, lateCameraPose, allTargets);
// publish this info to NT at estimated timestamp of receive
camSim.submitProcessedFrame(camResult, timestampNT);
// display debug results
for (var target : camResult.getTargets()) {
visibleTargets.add(lateCameraPose.transformBy(target.getBestCameraToTarget()));
var trf = target.getBestCameraToTarget();
if (trf.equals(kEmptyTrf)) continue;
visTgtPoses2d.add(lateCameraPose.transformBy(trf).toPose2d());
}
}
if (visibleTargets.size() != 0) {
dbgField
.getObject("visibleTargetPoses")
.setPoses(visibleTargets.stream().map(Pose3d::toPose2d).collect(Collectors.toList()));
}
if (cameraPose2ds.size() != 0) dbgField.getObject("cameras").setPoses(cameraPose2ds);
if (processed) dbgField.getObject("visibleTargetPoses").setPoses(visTgtPoses2d);
if (cameraPoses2d.size() != 0) dbgField.getObject("cameras").setPoses(cameraPoses2d);
}
}
4 changes: 2 additions & 2 deletions photon-server/build.gradle
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@ dependencies {
implementation project(':photon-core')
implementation project(':photon-targeting')

implementation "io.javalin:javalin:4.2.0"
implementation "io.javalin:javalin:5.6.2"

implementation wpilibTools.deps.wpilibJava("wpiutil")
implementation wpilibTools.deps.wpilibJava("wpimath")
Expand All @@ -46,7 +46,7 @@ dependencies {
implementation "org.msgpack:msgpack-core:0.9.0"
implementation "org.msgpack:jackson-dataformat-msgpack:0.9.0"

implementation "org.slf4j:slf4j-simple:1.8.0-beta4"
implementation "org.slf4j:slf4j-simple:2.0.7"
}

shadowJar {
Expand Down
13 changes: 8 additions & 5 deletions photon-server/src/main/java/org/photonvision/server/CameraSocketHandler.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,8 @@
import io.javalin.websocket.WsConnectContext;
import io.javalin.websocket.WsContext;
import io.javalin.websocket.WsMessageContext;
import java.net.InetSocketAddress;
import java.time.Duration;
import java.util.HashMap;
import java.util.List;
import java.util.concurrent.CopyOnWriteArrayList;
Expand Down Expand Up @@ -56,16 +58,17 @@ private CameraSocketHandler() {
}

public void onConnect(WsConnectContext context) {
context.session.setIdleTimeout(Long.MAX_VALUE); // TODO: determine better value
var insa = context.session.getRemote().getInetSocketAddress();
var host = insa.getAddress().toString() + ":" + insa.getPort();
context.session.setIdleTimeout(
Duration.ofMillis(Long.MAX_VALUE)); // TODO: determine better value
var remote = (InetSocketAddress) context.session.getRemoteAddress();
var host = remote.getAddress().toString() + ":" + remote.getPort();
logger.info("New camera websocket connection from " + host);
users.add(context);
}

protected void onClose(WsCloseContext context) {
var insa = context.session.getRemote().getInetSocketAddress();
var host = insa.getAddress().toString() + ":" + insa.getPort();
var remote = (InetSocketAddress) context.session.getRemoteAddress();
var host = remote.getAddress().toString() + ":" + remote.getPort();
var reason = context.reason() != null ? context.reason() : "Connection closed by client";
logger.info("Closing camera websocket connection from " + host + " for reason: " + reason);
svsManager.removeSubscription(context);
Expand Down
17 changes: 10 additions & 7 deletions photon-server/src/main/java/org/photonvision/server/DataSocketHandler.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,9 @@
import io.javalin.websocket.WsConnectContext;
import io.javalin.websocket.WsContext;
import java.io.IOException;
import java.net.InetSocketAddress;
import java.nio.ByteBuffer;
import java.time.Duration;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
Expand Down Expand Up @@ -68,9 +70,10 @@ private DataSocketHandler() {
}

public void onConnect(WsConnectContext context) {
context.session.setIdleTimeout(Long.MAX_VALUE); // TODO: determine better value
var insa = context.session.getRemote().getInetSocketAddress();
var host = insa.getAddress().toString() + ":" + insa.getPort();
context.session.setIdleTimeout(
Duration.ofMillis(Long.MAX_VALUE)); // TODO: determine better value
var remote = (InetSocketAddress) context.session.getRemoteAddress();
var host = remote.getAddress().toString() + ":" + remote.getPort();
logger.info("New websocket connection from " + host);
users.add(context);
dcService.publishEvent(
Expand All @@ -79,8 +82,8 @@ public void onConnect(WsConnectContext context) {
}

protected void onClose(WsCloseContext context) {
var insa = context.session.getRemote().getInetSocketAddress();
var host = insa.getAddress().toString() + ":" + insa.getPort();
var remote = (InetSocketAddress) context.session.getRemoteAddress();
var host = remote.getAddress().toString() + ":" + remote.getPort();
var reason = context.reason() != null ? context.reason() : "Connection closed by client";
logger.info("Closing websocket connection from " + host + " for reason: " + reason);
users.remove(context);
Expand Down Expand Up @@ -332,9 +335,9 @@ public void broadcastMessage(Object message, WsContext userToSkip)
sendMessage(message, user);
}
} else {
var skipUserPort = userToSkip.session.getRemote().getInetSocketAddress().getPort();
var skipUserPort = ((InetSocketAddress) userToSkip.session.getRemoteAddress()).getPort();
for (WsContext user : users) {
var userPort = user.session.getRemote().getInetSocketAddress().getPort();
var userPort = ((InetSocketAddress) user.session.getRemoteAddress()).getPort();
if (userPort != skipUserPort) {
sendMessage(message, user);
}
Expand Down
26 changes: 16 additions & 10 deletions photon-server/src/main/java/org/photonvision/server/RequestHandler.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,7 @@ public static void onSettingsImportRequest(Context ctx) {
return;
}

if (!file.getExtension().contains("zip")) {
if (!file.extension().contains("zip")) {
ctx.status(400);
ctx.result(
"The uploaded file was not of type 'zip'. The uploaded file should be a .zip file.");
Expand Down Expand Up @@ -132,7 +132,7 @@ public static void onHardwareConfigRequest(Context ctx) {
return;
}

if (!file.getExtension().contains("json")) {
if (!file.extension().contains("json")) {
ctx.status(400);
ctx.result(
"The uploaded file was not of type 'json'. The uploaded file should be a .json file.");
Expand Down Expand Up @@ -174,7 +174,7 @@ public static void onHardwareSettingsRequest(Context ctx) {
return;
}

if (!file.getExtension().contains("json")) {
if (!file.extension().contains("json")) {
ctx.status(400);
ctx.result(
"The uploaded file was not of type 'json'. The uploaded file should be a .json file.");
Expand Down Expand Up @@ -216,7 +216,7 @@ public static void onNetworkConfigRequest(Context ctx) {
return;
}

if (!file.getExtension().contains("json")) {
if (!file.extension().contains("json")) {
ctx.status(400);
ctx.result(
"The uploaded file was not of type 'json'. The uploaded file should be a .json file.");
Expand Down Expand Up @@ -258,7 +258,7 @@ public static void onOfflineUpdateRequest(Context ctx) {
return;
}

if (!file.getExtension().contains("jar")) {
if (!file.extension().contains("jar")) {
ctx.status(400);
ctx.result(
"The uploaded file was not of type 'jar'. The uploaded file should be a .jar file.");
Expand All @@ -273,7 +273,7 @@ public static void onOfflineUpdateRequest(Context ctx) {
File targetFile = new File(filePath.toString());
var stream = new FileOutputStream(targetFile);

file.getContent().transferTo(stream);
file.content().transferTo(stream);
stream.close();

ctx.status(200);
Expand Down Expand Up @@ -492,14 +492,20 @@ public static void onMetricsPublishRequest(Context ctx) {
*/
private static Optional<File> handleTempFileCreation(UploadedFile file) {
var tempFilePath =
new File(Path.of(System.getProperty("java.io.tmpdir"), file.getFilename()).toString());
tempFilePath.getParentFile().mkdirs();
new File(Path.of(System.getProperty("java.io.tmpdir"), file.filename()).toString());
boolean makeDirsRes = tempFilePath.getParentFile().mkdirs();

if (!makeDirsRes) {
logger.error(
"There was an error while uploading " + file.filename() + " to the temp folder!");
return Optional.empty();
}

try {
FileUtils.copyInputStreamToFile(file.getContent(), tempFilePath);
FileUtils.copyInputStreamToFile(file.content(), tempFilePath);
} catch (IOException e) {
logger.error(
"There was an error while uploading " + file.getFilename() + " to the temp folder!");
"There was an error while uploading " + file.filename() + " to the temp folder!");
return Optional.empty();
}

Expand Down
51 changes: 26 additions & 25 deletions photon-server/src/main/java/org/photonvision/server/Server.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,8 @@
package org.photonvision.server;

import io.javalin.Javalin;
import io.javalin.http.staticfiles.Location;
import io.javalin.plugin.bundled.CorsPluginConfig;
import java.net.InetSocketAddress;
import java.util.StringJoiner;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
Expand All @@ -27,45 +28,45 @@ public class Server {
private static final Logger logger = new Logger(Server.class, LogGroup.WebServer);

public static void start(int port) {
Javalin app =
var app =
Javalin.create(
config -> {
config.showJavalinBanner = false;
config.addStaticFiles("web", Location.CLASSPATH);
config.enableCorsForAllOrigins();
javalinConfig -> {
javalinConfig.showJavalinBanner = false;
javalinConfig.staticFiles.add("web");
javalinConfig.plugins.enableCors(
corsContainer -> {
corsContainer.add(CorsPluginConfig::anyHost);
});

config.requestLogger(
javalinConfig.requestLogger.http(
(ctx, ms) -> {
StringJoiner joiner =
new StringJoiner(" ")
.add("Handled HTTP request of type")
.add(ctx.req.getMethod())
.add(ctx.req().getMethod())
.add("from endpoint")
.add(ctx.path())
.add("for host")
.add(ctx.req.getRemoteHost())
.add(ctx.req().getRemoteHost())
.add("in")
.add(ms.toString())
.add("ms");

logger.debug(joiner.toString());
});

config.wsLogger(
ws ->
ws.onMessage(
ctx -> logger.debug("Got WebSockets message: " + ctx.message())));

config.wsLogger(
ws ->
ws.onBinaryMessage(
ctx ->
logger.trace(
() -> {
var insa = ctx.session.getRemote().getInetSocketAddress();
var host = insa.getAddress().toString() + ":" + insa.getPort();
return "Got WebSockets binary message from host " + host;
})));
javalinConfig.requestLogger.ws(
ws -> {
ws.onMessage(ctx -> logger.debug("Got WebSockets message: " + ctx.message()));
ws.onBinaryMessage(
ctx ->
logger.trace(
() -> {
var remote = (InetSocketAddress) ctx.session.getRemoteAddress();
var host =
remote.getAddress().toString() + ":" + remote.getPort();
return "Got WebSockets binary message from host: " + host;
}));
});
});

/*Web Socket Events for Data Exchange */
Expand Down
7 changes: 7 additions & 0 deletions photonlib-cpp-examples/README.md
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,7 @@
## PhotonLib C++ Examples

### Running examples

For instructions on how to run these examples locally, see [Running Examples](https://docs.photonvision.org/en/latest/docs/contributing/photonvision/build-instructions.html#running-examples).

---
4 changes: 3 additions & 1 deletion photonlib-cpp-examples/build.gradle
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,9 +27,11 @@ spotless {
}
}

apply from: "examples.gradle"

// Task that depends on the build task for every example
task buildAllExamples { task ->
new File('examples.txt').eachLine { line ->
exampleFolderNames.each { line ->
task.dependsOn(line + ":build")
}
}
Loading

0 comments on commit 724149f

Please sign in to comment.