Examples with Transform2d edu.wpi.first.math.geometry.Transform2d used on opensource projects

Example 6 with Transform2d

use of edu.wpi.first.math.geometry.Transform2d in project Robot-2022 by frc6995.

the class LimelightS method periodic.

/**
 * Adds the latest PhotonVision result to the filters.
 */
@Override
public void periodic() {
    // This method will be called once per scheduler run
    PhotonTrackedTarget target = limelight.getLatestResult().hasTargets() ? limelight.getLatestResult().getBestTarget() : new PhotonTrackedTarget(0, 0, 0, 0, new Transform2d(), new ArrayList<TargetCorner>());
    this.filterValues = new FilterValues(xOffsetFilter.calculate(target.getYaw()), yOffsetFilter.calculate(target.getPitch()));
    // TODO Oblog
    SmartDashboard.putNumber("x offset", this.filterValues.getFilteredXOffset());
    SmartDashboard.putNumber("y offset", this.filterValues.getFilteredYOffset());
}

Example 7 with Transform2d

use of edu.wpi.first.math.geometry.Transform2d in project SnobotSimV2 by snobotsim.

the class BaseCameraSimulatorTest method doubleTargetTest.

@Test
public void doubleTargetTest() {
    List<TargetLocation> targets = new ArrayList<>();
    targets.add(new TargetLocation(new Pose2d(10, 10, Rotation2d.fromDegrees(0))));
    targets.add(new TargetLocation(new Pose2d(10, 20, Rotation2d.fromDegrees(0))));
    BaseCameraSimulator sim = new BaseCameraSimulator(targets, new Transform2d(), 50, Double.MAX_VALUE);
    TreeMap<CameraToTargetDelta, TargetLocation> visibleTargets;
    Map.Entry<CameraToTargetDelta, TargetLocation> firstEntry;
    Map.Entry<CameraToTargetDelta, TargetLocation> secondEntry;
    // Start at origin. Should only see target 1
    visibleTargets = sim.getValidTargets(new Pose2d(0, 0, Rotation2d.fromDegrees(0)));
    firstEntry = visibleTargets.firstEntry();
    assertEquals(1, visibleTargets.size());
    assertEquals(Math.sqrt(200), firstEntry.getKey().mDistance);
    assertEquals(10, firstEntry.getValue().getPosition().getY());
    // In front of first target, should see both
    visibleTargets = sim.getValidTargets(new Pose2d(0, 10, Rotation2d.fromDegrees(0)));
    firstEntry = visibleTargets.firstEntry();
    secondEntry = visibleTargets.lastEntry();
    assertEquals(2, visibleTargets.size());
    assertEquals(Math.sqrt(100), firstEntry.getKey().mDistance);
    assertEquals(10, firstEntry.getValue().getPosition().getY());
    assertEquals(Math.sqrt(200), secondEntry.getKey().mDistance);
    assertEquals(20, secondEntry.getValue().getPosition().getY());
    // In front of Second target, should see both
    visibleTargets = sim.getValidTargets(new Pose2d(0, 20, Rotation2d.fromDegrees(0)));
    firstEntry = visibleTargets.firstEntry();
    secondEntry = visibleTargets.lastEntry();
    assertEquals(2, visibleTargets.size());
    assertEquals(Math.sqrt(100), firstEntry.getKey().mDistance);
    assertEquals(20, firstEntry.getValue().getPosition().getY());
    assertEquals(Math.sqrt(200), secondEntry.getKey().mDistance);
    assertEquals(10, secondEntry.getValue().getPosition().getY());
}

Example 8 with Transform2d

use of edu.wpi.first.math.geometry.Transform2d in project SnobotSimV2 by snobotsim.

the class TargetLocation method isInVisiblePosition.

public CameraToTargetDelta isInVisiblePosition(Pose2d robotPose, double horizontalFov, double maxDistance) {
    // Alias for readability
    double robotX = robotPose.getX();
    double robotY = robotPose.getY();
    if (robotX > mVisibleMaxX || robotX < mVisibleMinX) {
        return null;
    }
    if (robotY > mVisibleMaxY || robotY < mVisibleMinY) {
        return null;
    }
    double dx = robotPose.getX() - mPosition.getX();
    double dy = robotPose.getY() - mPosition.getY();
    double angleDelta = 0;
    if (mPosition.getRotation() != null) {
        Transform2d diff = mPosition.minus(robotPose);
        Translation2d trans = diff.getTranslation();
        Rotation2d angleFromRobot = new Rotation2d(trans.getX(), trans.getY());
        angleDelta = angleFromRobot.getDegrees();
        if (Math.abs(angleDelta) > horizontalFov) {
            return null;
        }
    }
    double distance = Math.sqrt(dx * dx + dy * dy);
    if (distance > maxDistance) {
        return null;
    }
    return new CameraToTargetDelta(distance, angleDelta);
}

Example 9 with Transform2d

use of edu.wpi.first.math.geometry.Transform2d in project SnobotSimV2 by snobotsim.

the class LimelightSimulatorTest method bestTargetTest.

@SuppressWarnings("PMD.ShortVariable")
@Test
public void bestTargetTest() {
    List<TargetLocation> targets = new ArrayList<>();
    targets.add(new TargetLocation(new Pose2d(10, 10, Rotation2d.fromDegrees(0))));
    targets.add(new TargetLocation(new Pose2d(10, 20, Rotation2d.fromDegrees(0))));
    try (NetworkTableInstance testInstance = NetworkTableInstance.create()) {
        NetworkTable networkTable = testInstance.getTable("limelight");
        NetworkTableEntry visible = networkTable.getEntry("tv");
        NetworkTableEntry tx = networkTable.getEntry("tx");
        NetworkTableEntry ty = networkTable.getEntry("ty");
        LimelightSimulator sim = new LimelightSimulator(targets, new Transform2d(), Units.feetToMeters(10), Double.MAX_VALUE, networkTable);
        testInstance.flush();
        assertEquals(0.0, visible.getNumber(-1));
        // Out of FOV
        sim.update(new Pose2d(0, 0, Rotation2d.fromDegrees(0)));
        testInstance.flush();
        assertEquals(0.0, visible.getNumber(-1));
        sim.update(new Pose2d(0, 0, Rotation2d.fromDegrees(40)));
        testInstance.flush();
        assertEquals(1.0, visible.getNumber(-1));
        assertEquals(-5.0, tx.getNumber(-100).doubleValue(), 1e-6);
        assertEquals(12.162_69, ty.getNumber(-100).doubleValue(), 1e-3);
        sim.update(new Pose2d(0, 10, Rotation2d.fromDegrees(0)));
        testInstance.flush();
        assertEquals(1.0, visible.getNumber(-1));
        assertEquals(0.0, tx.getNumber(-100).doubleValue(), 1e-6);
        assertEquals(16.951_22, ty.getNumber(-100).doubleValue(), 1e-3);
    }
}

Example 10 with Transform2d

use of edu.wpi.first.math.geometry.Transform2d in project 2022-Rapid-React by Manchester-Central.

the class SwerveDriveModule method updatePosition.

public void updatePosition(Pose2d robotPose) {
    Pose2d modulePose = robotPose.transformBy(new Transform2d(m_location, Rotation2d.fromDegrees(m_targetAngle)));
    m_field.setRobotPose(modulePose);
}