Adding camera simulator (#5)

* Adding camera simulator

* Ran formatter

* Fixing styleguide

.github/workflows/gradle.yml

@@ -5,6 +5,7 @@ on: [push]
 jobs:
   build_simulator:
     strategy:
+      fail-fast: false
       matrix:
         platform:
         - os: windows-latest

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<TargetLocation> mTargets;
+    private final double mHorizontalFov;
+    private final double mMaxDistance;
+
+    public BaseCameraSimulator(List<TargetLocation> targets, double cameraFov, double maxDistance)
+    {
+        mTargets = targets;
+        mHorizontalFov = cameraFov;
+        mMaxDistance = maxDistance;
+    }
+
+    public TreeMap<CameraToTargetDelta, TargetLocation> getValidTargets(Pose2d pose) // NOPMD
+    {
+        TreeMap<CameraToTargetDelta, TargetLocation> valid = new TreeMap<>();
+
+        for (TargetLocation target : mTargets)
+        {
+            CameraToTargetDelta delta = target.isInVisiblePosition(pose, mHorizontalFov, mMaxDistance);
+            if (delta != null)
+            {
+                valid.put(delta, target);
+            }
+        }
+
+        return valid;
+    }
+}

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<CameraToTargetDelta>
+{
+    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);
+    }
+}

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<TargetLocation> targets, double cameraToTargetHeight)
+    {
+        this(targets, cameraToTargetHeight, Double.MAX_VALUE, NetworkTableInstance.getDefault().getTable("limelight"));
+    }
+
+    public LimelightSimulator(List<TargetLocation> 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<CameraToTargetDelta, TargetLocation> targets = getValidTargets(pose);
+        if (targets.isEmpty())
+        {
+            mTV.setNumber(0);
+        }
+        else
+        {
+            Map.Entry<CameraToTargetDelta, TargetLocation> 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;
+    }
+}

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 + "]";
+    }
+}

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<TargetLocation> 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<TargetLocation> 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;
+    }
+}

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);
+    }
+}