Feature/robot state #4

AlonSchwierz · 2023-09-22T09:20:44Z

Hello, I am having trouble figuring whether the calculations for the robot pose Vision subsystem in simulation are correct.
please help me.
thanks.

Yim Yum

still need to check the target's pose strategy

commit משמעותי

…dOriented

…for field oriented and target oriented position

katzuv · 2023-11-11T15:53:48Z

src/main/java/frc/robot/RobotContainer.java

+    private final Joystick leftJoystick = new Joystick(1);
+    private final Joystick rightJoystick = new Joystick(2);
+
+    private final JoystickButton joystickTrigger = new JoystickButton(leftJoystick, 1);


Use a CommandJoystick instead.

katzuv · 2023-11-11T15:55:02Z

src/main/java/frc/robot/subsystems/vision/PhotonVisionIO.java

-                    AprilTagFieldLayout.loadFromResource("2023field.fmap"),
-                    PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP,
+                    AprilTagFields.k2023ChargedUp.loadAprilTagLayoutField(),
+//                    PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP,


Don't commit commented code.

katzuv · 2023-11-11T15:55:03Z

src/main/java/frc/robot/subsystems/vision/PhotonVisionIO.java

-                    AprilTagFieldLayout.loadFromResource("2023field.fmap"),
-                    PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP,
+                    AprilTagFields.k2023ChargedUp.loadAprilTagLayoutField(),
+//                    PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP,


Don't commit commented code.

katzuv · 2023-11-11T15:55:35Z

src/main/java/frc/robot/subsystems/vision/PhotonVisionIO.java

-        inputs.hasTargets = latestResult.hasTargets();
-        inputs.targetID = latestResult.getBestTarget().getFiducialId();
+        for (int i = 0; i < latestResult.targets.size(); i++) {
+        System.out.println(latestResult.targets.get(i));


You log everything, just look at the data in AdvantageScope instead of printing it.

katzuv · 2023-11-11T15:55:36Z

src/main/java/frc/robot/subsystems/vision/PhotonVisionIO.java

-        inputs.hasTargets = latestResult.hasTargets();
-        inputs.targetID = latestResult.getBestTarget().getFiducialId();
+        for (int i = 0; i < latestResult.targets.size(); i++) {
+        System.out.println(latestResult.targets.get(i));


You log everything, just look at the data in AdvantageScope instead of printing it.

katzuv · 2023-11-11T16:03:05Z

src/main/java/frc/robot/subsystems/vision/PhotonVisionIO.java

+        var latestResult = camera.getLatestResult();
+        for (int i = 0; i < latestResult.targets.size(); i++) {
+        System.out.println(latestResult.targets.get(i));
+        }
+
+        if (latestResult != null) {
+            inputs.latency = (long) latestResult.getLatencyMillis();
+            if (latestResult.getBestTarget() != null) {
+                inputs.area = latestResult.getBestTarget().getArea();
+                inputs.pitch = latestResult.getBestTarget().getPitch();
+                inputs.yaw = latestResult.getBestTarget().getYaw();
+                inputs.targetSkew = latestResult.getBestTarget().getSkew();
+                inputs.hasTargets = latestResult.hasTargets();
+                inputs.targetID = latestResult.getBestTarget().getFiducialId();
+            }
+
+            var estimatedPose = estimator.update(latestResult);
+            if (estimatedPose.isPresent()) {
+                var pose = estimatedPose.get().estimatedPose;
+                inputs.poseFieldOriented = new double[]{
+                        pose.getX(),
+                        pose.getY(),
+                        pose.getZ(),
+                        pose.getRotation().getX(),
+                        pose.getRotation().getY(),
+                        pose.getRotation().getZ()
+                };
+
+                var cameraToTarget = latestResult.getBestTarget().getBestCameraToTarget();
+                System.out.println("cameraToTarget: " + cameraToTarget);
+                inputs.cameraToTarget = new double[]{
+                        cameraToTarget.getX(),
+                        cameraToTarget.getY(),
+                        cameraToTarget.getZ(),
+                        cameraToTarget.getRotation().getX(),
+                        cameraToTarget.getRotation().getY(),
+                        cameraToTarget.getRotation().getZ()
+                };
+
+                result = new Result(latestResult.getTimestampSeconds(), estimatedPose.get().estimatedPose);
+            } else {
+                result = null;
+            }
+        } else {
+            result = null;
+        }
+    }


It looks like latestResult will never be null. But:

You don't null-check in the for loop above.

Even if you do need to null-check, it's better to check whether latestResult == null and then immediately return because it will help you reduce indentation, which helps you reduce indentation. The same can be done with the condition in line 55:

var latestResult = camera.getLatestResult(); for (int i = 0; i < latestResult.targets.size(); i++) { System.out.println(latestResult.targets.get(i)); } if (latestResult == null) { result = null; return; } inputs.latency = (long) latestResult.getLatencyMillis(); if (latestResult.getBestTarget() == null) { return; } inputs.area = latestResult.getBestTarget().getArea(); inputs.pitch = latestResult.getBestTarget().getPitch(); inputs.yaw = latestResult.getBestTarget().getYaw(); inputs.targetSkew = latestResult.getBestTarget().getSkew(); inputs.hasTargets = latestResult.hasTargets(); inputs.targetID = latestResult.getBestTarget().getFiducialId(); var estimatedPose = estimator.update(latestResult); if (estimatedPose.isEmpty()) { result = null; return; } var pose = estimatedPose.get().estimatedPose; inputs.poseFieldOriented = new double[]{ pose.getX(), pose.getY(), pose.getZ(), pose.getRotation().getX(), pose.getRotation().getY(), pose.getRotation().getZ() }; var cameraToTarget = latestResult.getBestTarget().getBestCameraToTarget(); System.out.println("cameraToTarget: " + cameraToTarget); inputs.cameraToTarget = new double[]{ cameraToTarget.getX(), cameraToTarget.getY(), cameraToTarget.getZ(), cameraToTarget.getRotation().getX(), cameraToTarget.getRotation().getY(), cameraToTarget.getRotation().getZ() }; result = new Result(latestResult.getTimestampSeconds(), estimatedPose.get().estimatedPose);

katzuv · 2023-11-11T16:04:09Z

src/main/java/frc/robot/subsystems/vision/PhotonVisionIO.java

-                    pose.getRotation().getZ()
-            };
+                var cameraToTarget = latestResult.getBestTarget().getBestCameraToTarget();
+                System.out.println("cameraToTarget: " + cameraToTarget);


Again, no need to print. Use AdvantageScope instead.

katzuv · 2023-11-11T16:04:43Z

src/main/java/frc/robot/subsystems/vision/Vision.java

+                        VisionModule.photonVisionIO("LeftPie", 0),
+                        VisionModule.photonVisionIO("RightPie", 1)


"Pi", not "Pie" 🙃.
Also, it's better to write "Left Pi" and "Right Pi" -- this is a string, no need to join words.

katzuv · 2023-11-11T16:05:34Z

src/main/java/frc/robot/subsystems/vision/VisionConstants.java

@@ -3,7 +3,14 @@
 import edu.wpi.first.math.geometry.*;

 public class VisionConstants {
-    public static final Transform3d[] ROBOT_TO_CAM = new Transform3d[]{new Transform3d(new Translation3d(0, 0, 0.45), new Rotation3d(0, 0, 180)), new Transform3d(new Translation3d(0, 0, 0.45), new Rotation3d(0, 0, 0))};
+    public static final Transform3d[] ROBOT_TO_CAM = new Transform3d[]{


No need to shorten, use the full form of the word.

Use the word "cameras" instead of "camera" so it will be clear there are multiple in this constant.

…im-bot-2023 into feature/RobotState

katzuv · 2023-11-24T17:19:19Z

src/main/java/frc/robot/Robot.java

@@ -74,6 +74,8 @@ public void robotPeriodic() {

        enabledTrigger.update(isEnabled());

+        RobotState.getInstance().update();


Why is that needed? this is not done for any other system. Perhaps another way is possible (hint: commands)

katzuv · 2023-11-24T17:19:45Z

src/main/java/frc/robot/subsystems/vision/VisionConstants.java

                    new Rotation3d(Math.toRadians(0)+0.16, Math.toRadians(-28)-0.085, Math.toRadians(-151)))};
+//     new Transform3d( //right cam


Don't commit commented code.

katzuv · 2023-12-18T11:41:35Z

src/main/java/frc/robot/commandgroups/GoToPose.java

+import frc.robot.RobotState;
+import swerve.SwerveDrive;
+
+public class GoToPose extends CommandBase {


What is the purpose of this class? Does PathPlanner lack the functionality you need?

katzuv · 2023-12-21T23:55:14Z

src/main/java/frc/robot/subsystems/vision/SImVisionIO.java

+import org.photonvision.SimVisionSystem;
+import swerve.SwerveDrive;
+
+public class SImVisionIO implements VisionIO{


Sim, not SIm.

katzuv · 2023-12-21T23:56:12Z

src/main/java/frc/robot/subsystems/vision/SImVisionIO.java

This implements specifically PhotonVision simulation, and its name should emphasize that.

katzuv · 2023-12-21T23:56:42Z

src/main/java/frc/robot/subsystems/vision/SImVisionIO.java

+
+
+
+
+
+
+
+


Why all that whitespace? One line is enogh.

katzuv · 2023-12-22T00:01:37Z

src/main/java/frc/robot/subsystems/vision/SImVisionIO.java

+        for (int i = 1; i < VisionConstants.TARGET_POSITION_SIM.length; i++) {
+            coprocessorSim.addSimVisionTarget(VisionConstants.SIM_VISION_TARGETS[i]);
+            coprocessorSim.processFrame(SwerveDrive.getInstance(false).getBotPose());
+        }


Suggested change

for (int i = 1; i < VisionConstants.TARGET_POSITION_SIM.length; i++) {

coprocessorSim.addSimVisionTarget(VisionConstants.SIM_VISION_TARGETS[i]);

coprocessorSim.processFrame(SwerveDrive.getInstance(false).getBotPose());

}

for (SimVisionTarget target : VisionConstants.SIM_VISION_TARGETS) {

coprocessorSim.addSimVisionTarget(target);

coprocessorSim.processFrame(SwerveDrive.getInstance(false).getBotPose());

}

katzuv · 2023-12-22T00:02:39Z

src/main/java/frc/robot/subsystems/vision/SImVisionIO.java

+        if (res.hasTargets()) {
+            var imageCaptureTime = res.getTimestampSeconds();
+            var camToTargetTrans = res.getBestTarget().getBestCameraToTarget();
+            var camPose = Constants.kFarTargetPose.transformBy(camToTargetTrans.inverse());
+            m_poseEstimator.addVisionMeasurement(
+                    camPose.transformBy(Constants.kCameraToRobot).toPose2d(), imageCaptureTime);
+        }


Suggested change

if (res.hasTargets()) {

var imageCaptureTime = res.getTimestampSeconds();

var camToTargetTrans = res.getBestTarget().getBestCameraToTarget();

var camPose = Constants.kFarTargetPose.transformBy(camToTargetTrans.inverse());

m_poseEstimator.addVisionMeasurement(

camPose.transformBy(Constants.kCameraToRobot).toPose2d(), imageCaptureTime);

}

if (!res.hasTargets()) {

// Exit early if there are no targets

return;

}

var imageCaptureTime = res.getTimestampSeconds();

var camToTargetTrans = res.getBestTarget().getBestCameraToTarget();

var camPose = Constants.kFarTargetPose.transformBy(camToTargetTrans.inverse());

m_poseEstimator.addVisionMeasurement(

camPose.transformBy(Constants.kCameraToRobot).toPose2d(), imageCaptureTime);

It's good practice to reduce indentation whenever possible.

katzuv · 2023-12-22T00:06:46Z

src/main/java/frc/robot/subsystems/vision/VisionConstants.java

@@ -24,6 +25,7 @@ public class VisionConstants {
    public static final double TARGET_LENGTH = 0.27;
    public static final Pose3d[] TARGET_POSITION_REAL = new Pose3d[]{new Pose3d() ,new Pose3d(new Translation3d( 7.24310, -2.93659, 0.46272), new Rotation3d(0,0,180)),new Pose3d(new Translation3d(7.24310, 1.26019, 0.46272), new Rotation3d(0,0,180)), new Pose3d(new Translation3d(7.24310, 0.41621, 0.41621), new Rotation3d(0,0,180)), new Pose3d(new Translation3d(7.90832, 2.74161, 0.695452), new Rotation3d(0,0,180)), new Pose3d(new Translation3d(-7.90832, 2.74161, 0.695452), new Rotation3d()), new Pose3d(new Translation3d(-7.24310, 0.41621, 0.46272), new Rotation3d()), new Pose3d(new Translation3d(-7.24310, 1.26019, 0.46272), new Rotation3d()), new Pose3d(new Translation3d(-7.24310, -2.93659, 0.46272), new Rotation3d())};
    public static final Pose3d[] TARGET_POSITION_LIME = new Pose3d[]{new Pose3d() ,new Pose3d(new Translation3d( 7.24310, -2.93659, 0.46272), new Rotation3d(0,0,180)),new Pose3d(new Translation3d(7.24310, 1.26019, 0.46272), new Rotation3d(0,0,180)), new Pose3d(new Translation3d(7.24310, 0.41621, 0.41621), new Rotation3d(0,0,180)), new Pose3d(new Translation3d(7.90832, 2.74161, 0.695452), new Rotation3d(0,0,180)), new Pose3d(new Translation3d(-7.90832, 2.74161, 0.695452), new Rotation3d()), new Pose3d(new Translation3d(-7.24310, 0.41621, 0.46272), new Rotation3d()), new Pose3d(new Translation3d(-7.24310, 1.26019, 0.46272), new Rotation3d()), new Pose3d(new Translation3d(-7.24310, -2.93659, 0.46272), new Rotation3d())};
-    public static final Pose3d[] TARGET_POSITION_SIM = new Pose3d[]{new Pose3d() ,new Pose3d(new Translation3d( 7.24310, -2.93659, 0.46272), new Rotation3d(0,0,180)),new Pose3d(new Translation3d(7.24310, 1.26019, 0.46272), new Rotation3d(0,0,180)), new Pose3d(new Translation3d(7.24310, 0.41621, 0.41621), new Rotation3d(0,0,180)), new Pose3d(new Translation3d(7.90832, 2.74161, 0.695452), new Rotation3d(0,0,180)), new Pose3d(new Translation3d(-7.90832, 2.74161, 0.695452), new Rotation3d()), new Pose3d(new Translation3d(-7.24310, 0.41621, 0.46272), new Rotation3d()), new Pose3d(new Translation3d(-7.24310, 1.26019, 0.46272), new Rotation3d()), new Pose3d(new Translation3d(-7.24310, -2.93659, 0.46272), new Rotation3d())};
+    public static final Pose3d[] TARGET_POSITION_SIM = new Pose3d[]{new Pose3d() ,new Pose3d(new Translation3d( 7.24310, -2.93659, 0.46272), new Rotation3d(0,0,180)),new Pose3d(new Translation3d(7.24310, -1.26019, 0.46272), new Rotation3d(0,0,180)), new Pose3d(new Translation3d(7.24310, 0.41621, 0.46272), new Rotation3d(0,0,180)), new Pose3d(new Translation3d(7.90832, 2.74161, 0.695452), new Rotation3d(0,0,180)), new Pose3d(new Translation3d(-7.90832, 2.74161, 0.695452), new Rotation3d()), new Pose3d(new Translation3d(-7.24310, 0.41621, 0.46272), new Rotation3d()), new Pose3d(new Translation3d(-7.24310, 1.26019, 0.46272), new Rotation3d()), new Pose3d(new Translation3d(-7.24310, -2.93659, 0.46272), new Rotation3d())};


You can shorten this line a bit by using this constructor that gets the x,y,z values directly without needing to create a Translation3d manually, so new Pose3d(new Translation3d(7.24310, -2.93659, 0.46272), new Rotation3d(0,0,180)) is the same as new Pose3d(7.24310, -2.93659, 0.46272, new Rotation3d(0,0,180)).

katzuv · 2023-12-22T00:13:02Z

src/main/java/frc/robot/subsystems/vision/VisionConstants.java

@@ -24,6 +25,7 @@ public class VisionConstants {
    public static final double TARGET_LENGTH = 0.27;
    public static final Pose3d[] TARGET_POSITION_REAL = new Pose3d[]{new Pose3d() ,new Pose3d(new Translation3d( 7.24310, -2.93659, 0.46272), new Rotation3d(0,0,180)),new Pose3d(new Translation3d(7.24310, 1.26019, 0.46272), new Rotation3d(0,0,180)), new Pose3d(new Translation3d(7.24310, 0.41621, 0.41621), new Rotation3d(0,0,180)), new Pose3d(new Translation3d(7.90832, 2.74161, 0.695452), new Rotation3d(0,0,180)), new Pose3d(new Translation3d(-7.90832, 2.74161, 0.695452), new Rotation3d()), new Pose3d(new Translation3d(-7.24310, 0.41621, 0.46272), new Rotation3d()), new Pose3d(new Translation3d(-7.24310, 1.26019, 0.46272), new Rotation3d()), new Pose3d(new Translation3d(-7.24310, -2.93659, 0.46272), new Rotation3d())};
    public static final Pose3d[] TARGET_POSITION_LIME = new Pose3d[]{new Pose3d() ,new Pose3d(new Translation3d( 7.24310, -2.93659, 0.46272), new Rotation3d(0,0,180)),new Pose3d(new Translation3d(7.24310, 1.26019, 0.46272), new Rotation3d(0,0,180)), new Pose3d(new Translation3d(7.24310, 0.41621, 0.41621), new Rotation3d(0,0,180)), new Pose3d(new Translation3d(7.90832, 2.74161, 0.695452), new Rotation3d(0,0,180)), new Pose3d(new Translation3d(-7.90832, 2.74161, 0.695452), new Rotation3d()), new Pose3d(new Translation3d(-7.24310, 0.41621, 0.46272), new Rotation3d()), new Pose3d(new Translation3d(-7.24310, 1.26019, 0.46272), new Rotation3d()), new Pose3d(new Translation3d(-7.24310, -2.93659, 0.46272), new Rotation3d())};
-    public static final Pose3d[] TARGET_POSITION_SIM = new Pose3d[]{new Pose3d() ,new Pose3d(new Translation3d( 7.24310, -2.93659, 0.46272), new Rotation3d(0,0,180)),new Pose3d(new Translation3d(7.24310, 1.26019, 0.46272), new Rotation3d(0,0,180)), new Pose3d(new Translation3d(7.24310, 0.41621, 0.41621), new Rotation3d(0,0,180)), new Pose3d(new Translation3d(7.90832, 2.74161, 0.695452), new Rotation3d(0,0,180)), new Pose3d(new Translation3d(-7.90832, 2.74161, 0.695452), new Rotation3d()), new Pose3d(new Translation3d(-7.24310, 0.41621, 0.46272), new Rotation3d()), new Pose3d(new Translation3d(-7.24310, 1.26019, 0.46272), new Rotation3d()), new Pose3d(new Translation3d(-7.24310, -2.93659, 0.46272), new Rotation3d())};
+    public static final Pose3d[] TARGET_POSITION_SIM = new Pose3d[]{new Pose3d() ,new Pose3d(new Translation3d( 7.24310, -2.93659, 0.46272), new Rotation3d(0,0,180)),new Pose3d(new Translation3d(7.24310, -1.26019, 0.46272), new Rotation3d(0,0,180)), new Pose3d(new Translation3d(7.24310, 0.41621, 0.46272), new Rotation3d(0,0,180)), new Pose3d(new Translation3d(7.90832, 2.74161, 0.695452), new Rotation3d(0,0,180)), new Pose3d(new Translation3d(-7.90832, 2.74161, 0.695452), new Rotation3d()), new Pose3d(new Translation3d(-7.24310, 0.41621, 0.46272), new Rotation3d()), new Pose3d(new Translation3d(-7.24310, 1.26019, 0.46272), new Rotation3d()), new Pose3d(new Translation3d(-7.24310, -2.93659, 0.46272), new Rotation3d())};
+    public static final SimVisionTarget[] SIM_VISION_TARGETS = new SimVisionTarget[]{new SimVisionTarget(new Pose3d().toPose2d(), 0,0, 0), new SimVisionTarget(TARGET_POSITION_SIM[1].toPose2d(), TARGET_WIDTH,TARGET_LENGTH, 1), new SimVisionTarget(TARGET_POSITION_SIM[2].toPose2d(), TARGET_WIDTH,TARGET_LENGTH, 2), new SimVisionTarget(TARGET_POSITION_SIM[3].toPose2d(), TARGET_WIDTH,TARGET_LENGTH, 3), new SimVisionTarget(TARGET_POSITION_SIM[4].toPose2d(), TARGET_WIDTH,TARGET_LENGTH, 4), new SimVisionTarget(TARGET_POSITION_SIM[5].toPose2d(), TARGET_WIDTH,TARGET_LENGTH, 5), new SimVisionTarget(TARGET_POSITION_SIM[6].toPose2d(), TARGET_WIDTH,TARGET_LENGTH, 6), new SimVisionTarget(TARGET_POSITION_SIM[7].toPose2d(), TARGET_WIDTH,TARGET_LENGTH, 7), new SimVisionTarget(TARGET_POSITION_SIM[8].toPose2d(), TARGET_WIDTH,TARGET_LENGTH, 8), new SimVisionTarget(new Pose3d().toPose2d(),0,0,0)};


Why do you cast to Pose2d? This class accepts Pose3d.

katzuv · 2023-12-25T23:08:55Z

src/main/java/frc/robot/subsystems/vision/SImVisionIO.java

    @Override
    public void setPipeLine(int pipeLineIndex) {
        simCamera.getCamera().setPipelineIndex(pipeLineIndex);
    }

    @Override
    public Result getLatestResult() {
-        return new Result(simCamera.getCamera().getLatestResult().getTimestampSeconds(), new Pose3d(simCamera.getCamera().getLatestResult().getBestTarget().getBestCameraToTarget().getTranslation(),simCamera.getCamera().getLatestResult().getBestTarget().getBestCameraToTarget().getRotation()));
+        return new Result(simCamera.getCamera().getLatestResult().getTimestampSeconds(), new Pose3d(simCamera.getCamera().getLatestResult().getBestTarget().getBestCameraToTarget().getTranslation(), simCamera.getCamera().getLatestResult().getBestTarget().getBestCameraToTarget().getRotation()));


simCamera.getCamera().getLatestResult() should be extracted into a variable, for two reasons:

Reduce code repittion.

It's possible that between the first and the second/third call of getLatestResult() another result will come in. This is problematic because the returned Result should have data from exactly one measurement -- we don't want the Result to have a coordinate of t1 but timestamp of t2.

But I must ask, why is this function even needed? Why not just use getLatestResult() directly? All your function does is remove information from the value returned from that function.

katzuv · 2023-12-25T23:12:39Z

src/main/java/frc/robot/subsystems/vision/SImVisionIO.java

+        inputs.hasTargets = simCamera.getCamera().getLatestResult().hasTargets();
+        inputs.yaw = simCamera.getCamera().getLatestResult().getBestTarget().getYaw();
+        inputs.pitch = simCamera.getCamera().getLatestResult().getBestTarget().getPitch();
+        inputs.area = simCamera.getCamera().getLatestResult().getBestTarget().getArea();
+        inputs.targetSkew = simCamera.getCamera().getLatestResult().getBestTarget().getSkew();
+        inputs.targetID = simCamera.getCamera().getLatestResult().getBestTarget().getFiducialId();


Here too simCamera.getCamera().getLatestResult().getBestTarget() needs to be extracted in a variable. In IntelliJ IDEA, you can select this text and use Ctrl+Alt+V to replace all usages with the new variable.

katzuv · 2023-12-25T23:17:25Z

src/main/java/frc/robot/subsystems/vision/SImVisionIO.java

+        inputs.cameraToTarget = new double[]{bestCameraToTarget.getX(), bestCameraToTarget.getY(), bestCameraToTarget.getZ(), bestCameraToTarget.getRotation().getX(), bestCameraToTarget.getRotation().getY(), bestCameraToTarget.getRotation().getZ()};
+        inputs.poseFieldOriented = new double[]{estimatedPose.getX(), estimatedPose.getY(), estimatedPose.getZ(), estimatedPose.getRotation().getQuaternion().getW(), estimatedPose.getRotation().getQuaternion().getX(), estimatedPose.getRotation().getQuaternion().getY(), estimatedPose.getRotation().getQuaternion().getZ()};
+        inputs.fieldOrientedRotationRad = new double[]{estimatedPose.getRotation().getX(), estimatedPose.getRotation().getY(), estimatedPose.getRotation().getZ()};


Pose3d and Transform3d have struct support, so you can just log them directly without needing to convert them to an array.

katzuv · 2023-12-27T23:31:29Z

src/main/java/frc/robot/subsystems/vision/PhotonVisionIO.java

@@ -18,7 +18,6 @@ public PhotonVisionIO(PhotonCamera camera, Transform3d robotToCamera) {
        try {
            estimator = new PhotonPoseEstimator(
                    AprilTagFields.k2023ChargedUp.loadAprilTagLayoutField(),
-//                    PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP,


Don't commit commented commit.

katzuv · 2023-12-27T23:31:56Z

src/main/java/frc/robot/subsystems/vision/VisionIO.java

@@ -7,21 +7,8 @@ public interface VisionIO {

    void setPipeLine(int pipeLineIndex);

-    void updateInputs(VisionInputsAutoLogged inputs);
+    void updateInputs(VisionInputsLogged inputs);


Why did you remove @AutoLog?

Alon Schwierz added 30 commits June 9, 2023 12:31

Add PhotonLib Vendor dep + create Vision Class

0978034

Start working on Vision of AprilTag

bacf51a

create redandent function in Vision and add poseEstimation

9eff9f7

still need to check the target's pose strategy

Add Vision logging

7e7c47f

add logging for second camera

ec61697

fix stupid

90c72b8

add logging for EstimatedRobotPose

ef18563

commit משמעותי

Create RobotState class

3b5f429

Update RobotState in Periodic

92c3e16

create VisionIO and IOPhotonVIsion

9fd9d82

Create IOLimeLight - not finished!!!!

b2ca7b4

Add poseFieldOriented and poseTargetOriented

cbc4483

Add TargetPoseFieldOriented

1d44e49

Replace estimated pose with targetPoseFieldOriented and robotPoseFiel…

3bd8104

…dOriented

fix stupidm

5b1563e

Comment problematic lines + Eitan's formating changes

54f8031

pull the getEstimatedPoseField to Interface and add pose3d variables …

d375416

…for field oriented and target oriented position

make Vision class use both io's and both of the io's inputs

752d72b

Think of a Idea for aprilChooser that supports modularity

11b2622

aprilChooser done

a83f6c7

delete redandent lines

21052a1

Create IO for simCamera

aac92ca

Finish logging on IOCameraSim

6ac437b

add getInstance method to Vision class

c513c33

IDK

a09a2d0

add a April chooser specified for each april index

ddd07ac

debugging

8363eb5

minor changes to fix no worky fuckn simulation

67920dd

LEAVE VISIONSIM FOR NOW, CHECK LATER WITH BUGRIM

cc78637

simulation finally working, now completing reobotState

61037dc

rakrakon and others added 3 commits November 10, 2023 12:08

Update PhotonVisionIO.java

0087540

Update camera locations

f88204d

Change strategy

1281fd0

katzuv requested changes Nov 11, 2023

View reviewed changes

GaiaZano05 and others added 11 commits November 14, 2023 18:39

Move hasTargets update and cmaeraToTarget update to correct if loops

1c38de8

Remove common

aa5634e

Add button to reset pose

0a62be7

Change pose rotation to return quaternion and add a log for rad rotation

2260aa0

Convert degrees to radians

3478466

Merge branch 'feature/RobotState' of https://github.com/Galaxia5987/S…

d1c3a5c

…im-bot-2023 into feature/RobotState

Log estimatedPose

5f49d8d

Add config timeout

8795405

Update pose estimation in robot periodic

a37a23c

Fix????? idk but it works🤷‍♀️

d62407e

Add button that uses the reset function in RobotState

452fb4f

katzuv requested changes Nov 24, 2023

View reviewed changes

vichik123 and others added 6 commits November 27, 2023 16:22

Implement common as package

94df0f2

Use common as library

b1cb48b

Change common library to latest commit

3a3e7f3

Add GoToPose command

6165211

Add limelight vision module

e0005e0

Add limelight cameraPos

1760726

katzuv reviewed Dec 18, 2023

View reviewed changes

this time it will work

0eeaaae

katzuv requested changes Dec 22, 2023

View reviewed changes

Alon Schwierz added 2 commits December 22, 2023 13:38

change Vision initialization according to new update

a6bd047

update loggings

01b48a9

katzuv requested changes Dec 25, 2023

View reviewed changes

Delete AutoLog

f8a6d3f

katzuv requested changes Dec 27, 2023

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Feature/robot state #4

Feature/robot state #4

AlonSchwierz commented Sep 22, 2023

katzuv Nov 11, 2023

katzuv Nov 11, 2023

katzuv Nov 11, 2023

katzuv Nov 11, 2023

katzuv Nov 11, 2023

katzuv Nov 11, 2023

katzuv Nov 11, 2023

katzuv Nov 11, 2023

katzuv Nov 11, 2023

katzuv Nov 24, 2023

katzuv Nov 24, 2023

katzuv Dec 18, 2023

katzuv Dec 21, 2023

katzuv Dec 21, 2023

katzuv Dec 21, 2023

katzuv Dec 22, 2023

katzuv Dec 22, 2023

katzuv Dec 22, 2023

katzuv Dec 22, 2023

katzuv Dec 25, 2023

katzuv Dec 25, 2023

katzuv Dec 25, 2023

katzuv Dec 27, 2023

katzuv Dec 27, 2023

		VisionModule.photonVisionIO("LeftPie", 0),
		VisionModule.photonVisionIO("RightPie", 1)

		@@ -74,6 +74,8 @@ public void robotPeriodic() {

		enabledTrigger.update(isEnabled());

		RobotState.getInstance().update();

		new Rotation3d(Math.toRadians(0)+0.16, Math.toRadians(-28)-0.085, Math.toRadians(-151)))};
		// new Transform3d( //right cam

Feature/robot state #4

Are you sure you want to change the base?

Feature/robot state #4

Conversation

AlonSchwierz commented Sep 22, 2023

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment