diff --git a/.github/workflows/gradle.yml b/.github/workflows/gradle.yml index 4b9f02c..042aae2 100644 --- a/.github/workflows/gradle.yml +++ b/.github/workflows/gradle.yml @@ -5,6 +5,7 @@ on: [push] jobs: build_simulator: strategy: + fail-fast: false matrix: platform: - os: windows-latest diff --git a/snobot_sim_java/src/main/java/org/snobotv2/camera/BaseCameraSimulator.java b/snobot_sim_java/src/main/java/org/snobotv2/camera/BaseCameraSimulator.java new file mode 100644 index 0000000..49a8ec7 --- /dev/null +++ b/snobot_sim_java/src/main/java/org/snobotv2/camera/BaseCameraSimulator.java @@ -0,0 +1,36 @@ +package org.snobotv2.camera; + +import edu.wpi.first.wpilibj.geometry.Pose2d; + +import java.util.List; +import java.util.TreeMap; + +public class BaseCameraSimulator +{ + private final List mTargets; + private final double mHorizontalFov; + private final double mMaxDistance; + + public BaseCameraSimulator(List targets, double cameraFov, double maxDistance) + { + mTargets = targets; + mHorizontalFov = cameraFov; + mMaxDistance = maxDistance; + } + + public TreeMap getValidTargets(Pose2d pose) // NOPMD + { + TreeMap valid = new TreeMap<>(); + + for (TargetLocation target : mTargets) + { + CameraToTargetDelta delta = target.isInVisiblePosition(pose, mHorizontalFov, mMaxDistance); + if (delta != null) + { + valid.put(delta, target); + } + } + + return valid; + } +} diff --git a/snobot_sim_java/src/main/java/org/snobotv2/camera/CameraToTargetDelta.java b/snobot_sim_java/src/main/java/org/snobotv2/camera/CameraToTargetDelta.java new file mode 100644 index 0000000..a880245 --- /dev/null +++ b/snobot_sim_java/src/main/java/org/snobotv2/camera/CameraToTargetDelta.java @@ -0,0 +1,49 @@ +package org.snobotv2.camera; + +import java.util.Objects; + +public class CameraToTargetDelta implements Comparable +{ + public final double mDeltaAngle; + public final double mDistance; + + public CameraToTargetDelta(double distance, double angleDelta) + { + mDistance = distance; + mDeltaAngle = angleDelta; + } + + @Override + public int compareTo(CameraToTargetDelta other) + { + int result = Double.compare(mDistance, other.mDistance); + if (result != 0) + { + return result; + } + + return Double.compare(mDeltaAngle, other.mDeltaAngle); + } + + @Override + public boolean equals(Object o) + { + if (this == o) + { + return true; + } + if (o == null || getClass() != o.getClass()) + { + return false; + } + + CameraToTargetDelta that = (CameraToTargetDelta) o; + return Double.compare(that.mDeltaAngle, mDeltaAngle) == 0 && Double.compare(that.mDistance, mDistance) == 0; + } + + @Override + public int hashCode() + { + return Objects.hash(mDeltaAngle, mDistance); + } +} diff --git a/snobot_sim_java/src/main/java/org/snobotv2/camera/LimelightSimulator.java b/snobot_sim_java/src/main/java/org/snobotv2/camera/LimelightSimulator.java new file mode 100644 index 0000000..1a175fb --- /dev/null +++ b/snobot_sim_java/src/main/java/org/snobotv2/camera/LimelightSimulator.java @@ -0,0 +1,74 @@ +package org.snobotv2.camera; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.geometry.Pose2d; + +import java.util.List; +import java.util.Map; +import java.util.TreeMap; + +public class LimelightSimulator extends BaseCameraSimulator +{ + private static final double HORIZONTAL_FOV = 59.6 / 2; + + private final double mCameraToTargetHeight; + private final NetworkTableEntry mTV; + private final NetworkTableEntry mTX; + private final NetworkTableEntry mTY; + private final NetworkTableEntry mTA; + private double mTargetArea; + + public LimelightSimulator(List targets, double cameraToTargetHeight) + { + this(targets, cameraToTargetHeight, Double.MAX_VALUE, NetworkTableInstance.getDefault().getTable("limelight")); + } + + public LimelightSimulator(List targets, double cameraToTargetHeight, double maxDistance, NetworkTable limelightTable) + { + super(targets, HORIZONTAL_FOV, maxDistance); + + mCameraToTargetHeight = cameraToTargetHeight; + mTV = limelightTable.getEntry("tv"); + mTX = limelightTable.getEntry("tx"); + mTY = limelightTable.getEntry("ty"); + mTA = limelightTable.getEntry("ta"); + + mTargetArea = 1.0; + + mTV.setNumber(0); + } + + public void setTargetArea(double targetArea) + { + mTargetArea = targetArea; + } + + public TargetLocation update(Pose2d pose) + { + TargetLocation bestTarget = null; + + TreeMap targets = getValidTargets(pose); + if (targets.isEmpty()) + { + mTV.setNumber(0); + } + else + { + Map.Entry bestPair = targets.firstEntry(); + bestTarget = bestPair.getValue(); + double distance = bestPair.getKey().mDistance; + + double el = Math.toDegrees(Math.atan2(mCameraToTargetHeight, distance)); + double az = bestPair.getKey().mDeltaAngle; + + mTV.setNumber(1); + mTX.setNumber(-az); + mTY.setNumber(el); + mTA.setNumber(mTargetArea); + } + + return bestTarget; + } +} diff --git a/snobot_sim_java/src/main/java/org/snobotv2/camera/TargetLocation.java b/snobot_sim_java/src/main/java/org/snobotv2/camera/TargetLocation.java new file mode 100644 index 0000000..7e8c288 --- /dev/null +++ b/snobot_sim_java/src/main/java/org/snobotv2/camera/TargetLocation.java @@ -0,0 +1,114 @@ +package org.snobotv2.camera; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Transform2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; + +public class TargetLocation +{ + private final Pose2d mPosition; + + private String mName; + private double mVisibleMinX; + private double mVisibleMinY; + private double mVisibleMaxX; + private double mVisibleMaxY; + + public TargetLocation(Pose2d pose) + { + mPosition = pose; + mName = ""; + + mVisibleMinX = -Double.MAX_VALUE; + mVisibleMinY = -Double.MAX_VALUE; + mVisibleMaxX = Double.MAX_VALUE; + mVisibleMaxY = Double.MAX_VALUE; + } + + public TargetLocation setName(String name) + { + mName = name; + return this; + } + + public TargetLocation setVisibleMinX(double minX) + { + mVisibleMinX = minX; + return this; + } + + public TargetLocation setVisibleMaxX(double maxX) + { + mVisibleMaxX = maxX; + return this; + } + + public TargetLocation setVisibleMinY(double minY) + { + mVisibleMinY = minY; + return this; + } + + public TargetLocation setVisibleMaxY(double maxY) + { + mVisibleMaxY = maxY; + return this; + } + + + public CameraToTargetDelta isInVisiblePosition(Pose2d robotPose, double horizontalFov, double maxDistance) + { + // Alias for readability + double robotX = robotPose.getX(); + double robotY = robotPose.getY(); + double angleDelta = 0; + + 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(); + + 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); + } + + + public Pose2d getPosition() + { + return mPosition; + } + + @Override + public String toString() + { + return "TargetLocation [mName=" + mName + ", mPosition=" + mPosition + "]"; + } +} diff --git a/snobot_sim_java/src/main/java/org/snobotv2/camera/games/InfiniteRechargeTargets.java b/snobot_sim_java/src/main/java/org/snobotv2/camera/games/InfiniteRechargeTargets.java new file mode 100644 index 0000000..6b8d34c --- /dev/null +++ b/snobot_sim_java/src/main/java/org/snobotv2/camera/games/InfiniteRechargeTargets.java @@ -0,0 +1,40 @@ +package org.snobotv2.camera.games; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.util.Units; +import org.snobotv2.camera.TargetLocation; + +import java.util.ArrayList; +import java.util.List; + +public final class InfiniteRechargeTargets +{ + private InfiniteRechargeTargets() + { + + } + + public static List getTargets() + { + double fieldLongDim = Units.feetToMeters(52) + Units.inchesToMeters(5.25); + double fieldShortDim = Units.feetToMeters(26) + Units.inchesToMeters(11.25); + double targetOffset = Units.feetToMeters(5.5); + + List targets = new ArrayList<>(); + + targets.add(new TargetLocation(new Pose2d(0, fieldShortDim / 2 + targetOffset, Rotation2d.fromDegrees(-180))) + .setVisibleMinX(0) + .setVisibleMaxX(fieldLongDim) + .setName("Left Side")); + + targets.add(new TargetLocation(new Pose2d(fieldLongDim, fieldShortDim / 2 - targetOffset, Rotation2d.fromDegrees(0))) + .setVisibleMinX(0.0) + .setVisibleMaxX(fieldLongDim) + .setName("Right Side")); + + + + return targets; + } +} diff --git a/snobot_sim_java/src/main/java/org/snobotv2/coordinate_gui/CameraRayPublisher.java b/snobot_sim_java/src/main/java/org/snobotv2/coordinate_gui/CameraRayPublisher.java new file mode 100644 index 0000000..83ac848 --- /dev/null +++ b/snobot_sim_java/src/main/java/org/snobotv2/coordinate_gui/CameraRayPublisher.java @@ -0,0 +1,45 @@ +package org.snobotv2.coordinate_gui; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.geometry.Pose2d; + +public class CameraRayPublisher +{ + private final NetworkTableEntry mPositions; + + public CameraRayPublisher() + { + this(NetworkTableInstance.getDefault().getTable("CoordinateGui")); + } + + public CameraRayPublisher(NetworkTable coordinateGuiTable) + { + coordinateGuiTable.getEntry(".type").setString("CoordinateGui"); + + NetworkTable limelightRayTable = coordinateGuiTable.getSubTable("CameraSim"); + limelightRayTable.getEntry(".type").setString("CameraSim"); + mPositions = limelightRayTable.getEntry("Positions"); + } + + public void clear() + { + mPositions.setDoubleArray(new double[]{}); + } + + public void publish(Pose2d robotPose, Pose2d... targets) + { + double[] data = new double[targets.length * 4]; + + for (int i = 0; i < targets.length; ++i) + { + data[i * 4 + 0] = robotPose.getX(); + data[i * 4 + 1] = robotPose.getY(); + data[i * 4 + 2] = targets[i].getX(); + data[i * 4 + 3] = targets[i].getY(); + } + + mPositions.setDoubleArray(data); + } +} diff --git a/snobot_sim_java/src/main/java/org/snobotv2/coordinate_gui/RamsetePublisher.java b/snobot_sim_java/src/main/java/org/snobotv2/coordinate_gui/RamsetePublisher.java new file mode 100644 index 0000000..d2f9170 --- /dev/null +++ b/snobot_sim_java/src/main/java/org/snobotv2/coordinate_gui/RamsetePublisher.java @@ -0,0 +1,71 @@ +package org.snobotv2.coordinate_gui; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; +import edu.wpi.first.wpilibj.trajectory.Trajectory; + +public class RamsetePublisher +{ + private final double[] mMeasurementData; + private final NetworkTableEntry mIdealTableEntry; + private final NetworkTableEntry mMeasuredTableEntry; + private final Timer mTimer; + + public RamsetePublisher() + { + this(NetworkTableInstance.getDefault().getTable("CoordinateGui")); + } + + public RamsetePublisher(NetworkTable coordinateGuiTable) + { + coordinateGuiTable.getEntry(".type").setString("CoordinateGui"); + + NetworkTable trajectoryTable = coordinateGuiTable.getSubTable("Ramsete Namespace"); + trajectoryTable.getEntry(".type").setString("Ramsete Namespace"); + + mMeasuredTableEntry = trajectoryTable.getEntry("Measured"); + mIdealTableEntry = trajectoryTable.getEntry("Ideal"); + + mTimer = new Timer(); + + mMeasurementData = new double[8]; + } + + public void initialize(Trajectory trajectory) + { + mTimer.reset(); + mTimer.start(); + + double[] data = new double[trajectory.getStates().size() * 5]; + + int index = 0; + for (Trajectory.State state : trajectory.getStates()) + { + data[index++] = state.timeSeconds; + data[index++] = state.velocityMetersPerSecond; + data[index++] = state.poseMeters.getTranslation().getX(); + data[index++] = state.poseMeters.getTranslation().getY(); + data[index++] = state.poseMeters.getRotation().getDegrees(); + } + + mIdealTableEntry.setDoubleArray(data); + } + + public void addMeasurement(Pose2d pose, DifferentialDriveWheelSpeeds goalVelocity, DifferentialDriveWheelSpeeds measuredVelocity) + { + mMeasurementData[0] = mTimer.get(); + mMeasurementData[1] = pose.getX(); + mMeasurementData[2] = pose.getY(); + mMeasurementData[3] = pose.getRotation().getDegrees(); + mMeasurementData[4] = goalVelocity.leftMetersPerSecond; + mMeasurementData[5] = goalVelocity.rightMetersPerSecond; + mMeasurementData[6] = measuredVelocity.leftMetersPerSecond; + mMeasurementData[7] = measuredVelocity.rightMetersPerSecond; + + mMeasuredTableEntry.setDoubleArray(mMeasurementData); + } +} diff --git a/snobot_sim_java/src/main/java/org/snobotv2/coordinate_gui/RobotPositionPublisher.java b/snobot_sim_java/src/main/java/org/snobotv2/coordinate_gui/RobotPositionPublisher.java new file mode 100644 index 0000000..52c1e4e --- /dev/null +++ b/snobot_sim_java/src/main/java/org/snobotv2/coordinate_gui/RobotPositionPublisher.java @@ -0,0 +1,40 @@ +package org.snobotv2.coordinate_gui; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.geometry.Pose2d; + +public class RobotPositionPublisher +{ + private final double[] mData; + private final NetworkTableEntry mEntry; + private final int mPublishingFrequency; + private int mCounter; + + public RobotPositionPublisher() + { + this(NetworkTableInstance.getDefault().getTable("CoordinateGui"), 5); // Publish every 5 loops, or 100ms + } + + public RobotPositionPublisher(NetworkTable coordinateGuiTable, int publishingFrequency) + { + coordinateGuiTable.getEntry(".type").setString("CoordinateGui"); + + mPublishingFrequency = publishingFrequency; + mEntry = coordinateGuiTable.getSubTable("RobotPosition").getEntry("position"); + mData = new double[3]; + } + + public void publish(Pose2d pose) + { + ++mCounter; + if (mCounter % mPublishingFrequency == 0) + { + mData[0] = pose.getX(); + mData[1] = pose.getY(); + mData[2] = pose.getRotation().getDegrees(); + mEntry.setDoubleArray(mData); + } + } +} diff --git a/snobot_sim_java/src/test/java/org/snobotv2/camera/BaseCameraSimulatorTest.java b/snobot_sim_java/src/test/java/org/snobotv2/camera/BaseCameraSimulatorTest.java new file mode 100644 index 0000000..e3087a4 --- /dev/null +++ b/snobot_sim_java/src/test/java/org/snobotv2/camera/BaseCameraSimulatorTest.java @@ -0,0 +1,57 @@ +package org.snobotv2.camera; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import org.junit.jupiter.api.Test; + +import java.util.ArrayList; +import java.util.List; +import java.util.Map; +import java.util.TreeMap; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +public class BaseCameraSimulatorTest +{ + @Test + public void doubleTargetTest() + { + List 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, 50, Double.MAX_VALUE); + + TreeMap visibleTargets; + Map.Entry firstEntry; + Map.Entry 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()); + } +} diff --git a/snobot_sim_java/src/test/java/org/snobotv2/camera/LimelightSimulatorTest.java b/snobot_sim_java/src/test/java/org/snobotv2/camera/LimelightSimulatorTest.java new file mode 100644 index 0000000..d5fbb9e --- /dev/null +++ b/snobot_sim_java/src/test/java/org/snobotv2/camera/LimelightSimulatorTest.java @@ -0,0 +1,55 @@ +package org.snobotv2.camera; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.util.Units; +import org.junit.jupiter.api.Test; + +import java.util.ArrayList; +import java.util.List; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +public class LimelightSimulatorTest +{ + @Test + public void bestTargetTest() + { + List 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, 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.16269, 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.95122, ty.getNumber(-100).doubleValue(), 1e-3); + } + } +} diff --git a/snobot_sim_java/src/test/java/org/snobotv2/camera/TargetLocationTest.java b/snobot_sim_java/src/test/java/org/snobotv2/camera/TargetLocationTest.java new file mode 100644 index 0000000..c995fcf --- /dev/null +++ b/snobot_sim_java/src/test/java/org/snobotv2/camera/TargetLocationTest.java @@ -0,0 +1,110 @@ +package org.snobotv2.camera; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertNull; +import static org.junit.jupiter.api.Assertions.assertNotNull; + +public class TargetLocationTest +{ + private Pose2d pose(double x, double y, double angle) + { + return new Pose2d(x, y, Rotation2d.fromDegrees(angle)); + } + + @Test + @SuppressWarnings("PMD") + public void testLimitlessTarget() + { + double fov = 40; + double maxDistance = Double.MAX_VALUE; + + TargetLocation location = new TargetLocation(new Pose2d(0, 0, null)); + + for (double x : new double[]{-100, 0, 100}) + { + for (double y : new double[]{-100, 0, 100}) + { + for (double angle : new double[]{-179, -90, 0, 90, 179}) + { + assertNotNull(location.isInVisiblePosition(pose(x, y, angle), fov, maxDistance)); + } + } + } + } + + @Test + public void testLimitedX() + { + double fov = 40; + double maxDistance = Double.MAX_VALUE; + + TargetLocation location = new TargetLocation(new Pose2d(0, 0, null)) + .setVisibleMinX(-20) + .setVisibleMaxX(30); + + assertNotNull(location.isInVisiblePosition(pose(0, 0, 0), fov, maxDistance)); + assertNotNull(location.isInVisiblePosition(pose(0, 100, 0), fov, maxDistance)); + assertNotNull(location.isInVisiblePosition(pose(-10, 50, 0), fov, maxDistance)); + assertNotNull(location.isInVisiblePosition(pose(-20, 50, 0), fov, maxDistance)); + assertNotNull(location.isInVisiblePosition(pose(17, 50, 0), fov, maxDistance)); + assertNotNull(location.isInVisiblePosition(pose(30, 50, 0), fov, maxDistance)); + + + assertNull(location.isInVisiblePosition(pose(-30, 50, 0), fov, maxDistance)); + assertNull(location.isInVisiblePosition(pose(31, 50, 0), fov, maxDistance)); + } + + + @Test + public void testInTargetWithAngle0InFov() + { + double fov = 40; + double maxDistance = Double.MAX_VALUE; + + TargetLocation location = new TargetLocation(new Pose2d(10, 10, Rotation2d.fromDegrees(0))); + + // Robot directly in front of the target. The robots angle should be the only thing that matters + assertNull(location.isInVisiblePosition(pose(0, 10, -45), fov, maxDistance)); + assertNotNull(location.isInVisiblePosition(pose(0, 10, -40), fov, maxDistance)); + assertNotNull(location.isInVisiblePosition(pose(0, 10, -20), fov, maxDistance)); + assertNotNull(location.isInVisiblePosition(pose(0, 10, 0), fov, maxDistance)); + assertNotNull(location.isInVisiblePosition(pose(0, 10, 20), fov, maxDistance)); + assertNotNull(location.isInVisiblePosition(pose(0, 10, 40), fov, maxDistance)); + assertNull(location.isInVisiblePosition(pose(0, 10, 45), fov, maxDistance)); + + assertNull(location.isInVisiblePosition(pose(0, 0, 0), fov, maxDistance)); + assertNotNull(location.isInVisiblePosition(pose(0, 0, 45), fov, maxDistance)); + assertNull(location.isInVisiblePosition(pose(0, 0, 90), fov, maxDistance)); + } + + @Test + public void testInTargetWithAngle180InFov() + { + double fov = 40; + double maxDistance = Double.MAX_VALUE; + + TargetLocation location45 = new TargetLocation(new Pose2d(10, 10, Rotation2d.fromDegrees(180))); + assertNotNull(location45.isInVisiblePosition(pose(0, 10, 0), fov, maxDistance)); + assertNull(location45.isInVisiblePosition(pose(0, 10, 160), fov, maxDistance)); + assertNull(location45.isInVisiblePosition(pose(0, 10, 200), fov, maxDistance)); + assertNull(location45.isInVisiblePosition(pose(0, 10, -179), fov, maxDistance)); + } + + @Test + public void testFovWraparound() + { + double fov = 40; + double maxDistance = Double.MAX_VALUE; + + TargetLocation locationZero = new TargetLocation(new Pose2d(10, 10, Rotation2d.fromDegrees(0))); + assertNotNull(locationZero.isInVisiblePosition(pose(0, 10, 20 - 360 * 4), fov, maxDistance)); + assertNotNull(locationZero.isInVisiblePosition(pose(0, 10, 20 + 360 * 4), fov, maxDistance)); + + TargetLocation location45 = new TargetLocation(new Pose2d(10, 10, Rotation2d.fromDegrees(45))); + assertNotNull(location45.isInVisiblePosition(pose(0, 10, 20 - 360 * 4), fov, maxDistance)); + assertNotNull(location45.isInVisiblePosition(pose(0, 10, 20 + 360 * 4), fov, maxDistance)); + } +}