Skip to content

Commit

Permalink
more formatting fixes
Browse files Browse the repository at this point in the history
  • Loading branch information
srimanachanta committed Nov 6, 2023
1 parent bfb84d4 commit 030f543
Show file tree
Hide file tree
Showing 5 changed files with 27 additions and 26 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -113,11 +113,10 @@ public TrackedTarget(
tvec.put(
0,
0,
new double[] {
bestPose.getTranslation().getX(),
bestPose.getTranslation().getY(),
bestPose.getTranslation().getZ()
});
bestPose.getTranslation().getX(),
bestPose.getTranslation().getY(),
bestPose.getTranslation().getZ()
);
setCameraRelativeTvec(tvec);

// Opencv expects a 3d vector with norm = angle and direction = axis
Expand All @@ -129,10 +128,10 @@ public TrackedTarget(
double[] corners = tagDetection.getCorners();
Point[] cornerPoints =
new Point[] {
new Point(corners[0], corners[1]),
new Point(corners[2], corners[3]),
new Point(corners[4], corners[5]),
new Point(corners[6], corners[7])
new Point(corners[0], corners[1]),
new Point(corners[2], corners[3]),
new Point(corners[4], corners[5]),
new Point(corners[6], corners[7])
};
m_targetCorners = List.of(cornerPoints);
MatOfPoint contourMat = new MatOfPoint(cornerPoints);
Expand Down Expand Up @@ -168,10 +167,10 @@ public TrackedTarget(

Point[] cornerPoints =
new Point[] {
new Point(xCorners[0], yCorners[0]),
new Point(xCorners[1], yCorners[1]),
new Point(xCorners[2], yCorners[2]),
new Point(xCorners[3], yCorners[3])
new Point(xCorners[0], yCorners[0]),
new Point(xCorners[1], yCorners[1]),
new Point(xCorners[2], yCorners[2]),
new Point(xCorners[3], yCorners[3])
};
m_targetCorners = List.of(cornerPoints);
MatOfPoint contourMat = new MatOfPoint(cornerPoints);
Expand Down Expand Up @@ -204,11 +203,10 @@ public TrackedTarget(
tvec.put(
0,
0,
new double[] {
bestPose.getTranslation().getX(),
bestPose.getTranslation().getY(),
bestPose.getTranslation().getZ()
});
bestPose.getTranslation().getX(),
bestPose.getTranslation().getY(),
bestPose.getTranslation().getZ()
);
setCameraRelativeTvec(tvec);

var rvec = new Mat(3, 1, CvType.CV_64FC1);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@
import edu.wpi.first.math.geometry.Pose3d;
import java.util.List;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.proto.PhotonTypes.PhotonPipelineResult.PhotonTrackedTarget;
import org.photonvision.targeting.PhotonTrackedTarget;

/** An estimated pose based on pipeline result */
public class EstimatedRobotPose {
Expand Down
8 changes: 5 additions & 3 deletions photon-lib/src/main/java/org/photonvision/PhotonCamera.java
Original file line number Diff line number Diff line change
Expand Up @@ -47,13 +47,12 @@
import edu.wpi.first.networktables.StringSubscriber;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import us.hebi.quickbuf.InvalidProtocolBufferException;

import java.util.Optional;
import java.util.Set;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.hardware.VisionLEDMode;
import org.photonvision.proto.PhotonTypes.PhotonPipelineResult;
import us.hebi.quickbuf.InvalidProtocolBufferException;

/** Represents a camera that is connected to PhotonVision. */
public class PhotonCamera implements AutoCloseable {
Expand Down Expand Up @@ -144,7 +143,10 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) {
cameraTable
.getRawTopic("result_proto")
.subscribe(
"proto:" + PhotonPipelineResult.getDescriptor().getFullName(), new byte[] {}, PubSubOption.periodic(0.01), PubSubOption.sendAll(true));
"proto:" + PhotonPipelineResult.getDescriptor().getFullName(),
new byte[] {},
PubSubOption.periodic(0.01),
PubSubOption.sendAll(true));
driverModePublisher = cameraTable.getBooleanTopic("driverModeRequest").publish();
driverModeSubscriber = cameraTable.getBooleanTopic("driverMode").subscribe(false);
inputSaveImgEntry = cameraTable.getIntegerTopic("inputSaveImgCmd").getEntry(0);
Expand Down
6 changes: 3 additions & 3 deletions photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -40,9 +40,9 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance,
const std::string_view cameraName)
: mainTable(instance.GetTable("photonvision")),
rootTable(mainTable->GetSubTable(cameraName)),
rawBytesEntry(
rootTable->GetRawTopic("result_proto")
.Subscribe("asdfasdfasdf", {}, {.periodic = 0.01, .sendAll = true})),
rawBytesEntry(rootTable->GetRawTopic("result_proto")
.Subscribe("asdfasdfasdf", {},
{.periodic = 0.01, .sendAll = true})),
inputSaveImgEntry(
rootTable->GetIntegerTopic("inputSaveImgCmd").Publish()),
inputSaveImgSubscriber(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,8 @@ class SimPhotonCamera : public PhotonCamera {
targetAreaEntry = rootTable->GetEntry("targetAreaEntry");
targetSkewEntry = rootTable->GetEntry("targetSkewEntry");
targetPoseEntry = rootTable->GetEntry("targetPoseEntry");
rawBytesPublisher = rootTable->GetRawTopic("result_proto").Publish("asdfasdfasdf");
rawBytesPublisher =
rootTable->GetRawTopic("result_proto").Publish("asdfasdfasdf");
versionEntry = instance.GetTable("photonvision")->GetEntry("version");
// versionEntry.SetString(PhotonVersion.versionString);
}
Expand Down

0 comments on commit 030f543

Please sign in to comment.