Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-19 00:41:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Update sim pose estimator example to use 3d (#524)

Browse Source
This commit is contained in:
Andrew Gasser
2022-10-25 20:11:41 -05:00
committed by GitHub
parent 4f355f2749
commit 04bde1b230
3 changed files with 24 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Constants.java
View File

@@ -24,10 +24,10 @@
package org.photonlib.examples.simposeest.robot;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import org.photonvision.SimVisionTarget;
@@ -71,10 +71,10 @@ public class Constants {
// Physical location of the camera on the robot, relative to the center of the
// robot.
public static final Transform2d kCameraToRobot =
new Transform2d(
new Translation2d(-0.25, 0), // in meters
new Rotation2d());
public static final Transform3d kCameraToRobot =
new Transform3d(
new Translation3d(-0.25, 0, -.25), // in meters
new Rotation3d());
// See
// https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf
@@ -94,9 +94,15 @@ public class Constants {
public static final double kFarTgtXPos = Units.feetToMeters(54);
public static final double kFarTgtYPos =
Units.feetToMeters(27 / 2) - Units.inchesToMeters(43.75) - Units.inchesToMeters(48.0 / 2.0);
public static final Pose2d kFarTargetPose =
new Pose2d(new Translation2d(kFarTgtXPos, kFarTgtYPos), new Rotation2d(0.0));
public static final double kFarTgtZPos =
(Units.inchesToMeters(98.19) - targetHeight) / 2 + targetHeight;
public static final Pose3d kFarTargetPose =
new Pose3d(
new Translation3d(kFarTgtXPos, kFarTgtYPos, kFarTgtZPos),
new Rotation3d(0.0, 0.0, Units.degreesToRadians(180)));
public static final SimVisionTarget kFarTarget =
new SimVisionTarget(kFarTargetPose, targetHeightAboveGround, targetWidth, targetHeight);
new SimVisionTarget(
kFarTargetPose.toPose2d(), targetHeightAboveGround, targetWidth, targetHeight);
}

14
photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java
View File

@@ -28,8 +28,6 @@ import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
@@ -86,15 +84,11 @@ public class DrivetrainPoseEstimator {
var res = cam.getLatestResult();
if (res.hasTargets()) {
double imageCaptureTime = Timer.getFPGATimestamp() - res.getLatencyMillis() / 1000.0;
Transform3d camToTargetTrans = res.getBestTarget().getBestCameraToTarget();
var transform =
new Transform2d(
camToTargetTrans.getTranslation().toTranslation2d(),
camToTargetTrans.getRotation().toRotation2d());
Pose2d camPose = Constants.kFarTargetPose.transformBy(transform.inverse());
var imageCaptureTime = Timer.getFPGATimestamp() - res.getLatencyMillis() / 1000.0;
var camToTargetTrans = res.getBestTarget().getBestCameraToTarget();
var camPose = Constants.kFarTargetPose.transformBy(camToTargetTrans.inverse());
m_poseEstimator.addVisionMeasurement(
camPose.transformBy(Constants.kCameraToRobot), imageCaptureTime);
camPose.transformBy(Constants.kCameraToRobot).toPose2d(), imageCaptureTime);
}
}

4
photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/sim/DrivetrainSim.java
View File

@@ -90,7 +90,9 @@ public class DrivetrainSim {
Constants.kCamName,
camDiagFOV,
camPitch,
Constants.kCameraToRobot,
new Transform2d(
Constants.kCameraToRobot.getTranslation().toTranslation2d(),
Constants.kCameraToRobot.getRotation().toRotation2d()),
camHeightOffGround,
maxLEDRange,
camResolutionWidth,