send the camera centric positions of where we think the nodes are

src/main/java/frc/robot/Constants.java

@@ -299,6 +299,26 @@ public enum KinematicLimits {
     public static final float ROBOT_LENGTH = 0.84f;
     public static final float HALF_ROBOT_LENGTH = ROBOT_LENGTH / 2;
 
+    /**
+     * meters
+     */
+    public static final double CONE_MID_NODE_X_OFFSET_FROM_WALL = 0.7959;
+
+    /**
+     * meters
+     */
+    public static final double CONE_HIGH_NODE_X_OFFSET_FROM_WALL = 0.3646;
+
+    /**
+     * meters
+     */
+    public static final double CONE_HIGH_NODE_HEIGHT = Units.inchesToMeters(46);
+
+    /**
+     * meters
+     */
+    public static final double CONE_MID_NODE_HEIGHT = Units.inchesToMeters(34);
+
 
     /**
      * How far away from the edge of the grids do we want to create a line for the intersection test to choose the best scoring

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

@@ -1035,9 +1035,6 @@ private void updateTeleopDrivingTarget(boolean recalculateGridPosition) {
         } else {
             // We're on the opposite side as our alliance
             // Try to go to the pickup position
-            var predictedPoseForPickup = robotTracker.getLatestPose().getTranslation().plus(
-                    robotTracker.getVelocity().times(0));
-
             if ((isRed() && robotTracker.getLatestPose().getRotation()
                     .getDegrees() < SINGLE_SUBSTATION_PICKUP_ANGLE_CUTOFF_DEGREES)
                     || (!isRed()

src/main/java/frc/robot/ScoringPositionManager.java

@@ -1,17 +1,17 @@
 package frc.robot;
 
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
 import frc.utility.Controller;
 import org.jetbrains.annotations.Contract;
 import org.jetbrains.annotations.NotNull;
 import org.littletonrobotics.junction.networktables.LoggedDashboardBoolean;
 import org.littletonrobotics.junction.networktables.LoggedDashboardString;
 
-import java.util.HashMap;
-import java.util.Map;
+import java.util.*;
 
-import static frc.robot.Constants.IS_PRACTICE;
+import static frc.robot.Constants.*;
 
 public class ScoringPositionManager {
 
@@ -337,7 +337,12 @@ enum ScoringDirection {
 
 
         for (int i = 0; i < 3; i++) {
-            possibleYs[i] = y + CUBE_SCORING_Y_CENTER[i];
+            possibleYs[i] = CUBE_SCORING_Y_CENTER[i] +
+                    y +
+                    yScoringOffsets
+                            .get(isRedAlliance ? AllianceSide.RED : AllianceSide.BLUE)
+                            .get(i)
+                            .get(selectedPosition.getScoringDirection());
         }
         return possibleYs;
     }
@@ -382,14 +387,60 @@ public static BestFieldY getBestFieldY(@NotNull SelectedPosition selectedPositio
             }
         }
 
-        bestY = bestY
-                + getGridRelativeY(selectedPosition, isRedAlliance)
-                + yScoringOffsets
-                .get(isRedAlliance ? AllianceSide.RED : AllianceSide.BLUE)
-                .get(chosenGridIndex)
-                .get(selectedPosition.getScoringDirection());
+        bestY = bestY +
+                getGridRelativeY(selectedPosition, isRedAlliance) +
+                yScoringOffsets
+                        .get(isRedAlliance ? AllianceSide.RED : AllianceSide.BLUE)
+                        .get(chosenGridIndex)
+                        .get(selectedPosition.getScoringDirection());
 
 
         return new BestFieldY(bestY, chosenGridIndex);
     }
+
+    private static final List<SelectedPosition> selectedPositionsToGenerateLinesFor = List.of(
+            SelectedPosition.MIDDLE_LEFT,
+            SelectedPosition.MIDDLE_RIGHT
+    );
+
+    public static final List<Translation3d> redNodeLinePoints;
+    public static final List<Translation3d> blueNodeLinePoints;
+
+    static {
+        List<Translation3d> redNodeLines = new ArrayList<>();
+        getYs(AllianceSide.RED).forEach(y -> {
+            redNodeLines.add(new Translation3d(CONE_MID_NODE_X_OFFSET_FROM_WALL, y, 0));
+            redNodeLines.add(new Translation3d(CONE_MID_NODE_X_OFFSET_FROM_WALL, y, CONE_MID_NODE_HEIGHT));
+
+            redNodeLines.add(new Translation3d(CONE_HIGH_NODE_X_OFFSET_FROM_WALL, y, 0));
+            redNodeLines.add(new Translation3d(CONE_HIGH_NODE_X_OFFSET_FROM_WALL, y, CONE_HIGH_NODE_HEIGHT));
+        });
+
+        List<Translation3d> blueNodeLines = new ArrayList<>();
+        getYs(AllianceSide.BLUE).forEach(y -> {
+            blueNodeLines.add(
+                    new Translation3d(FIELD_WIDTH_METERS - CONE_MID_NODE_X_OFFSET_FROM_WALL, y, 0));
+            blueNodeLines.add(
+                    new Translation3d(FIELD_WIDTH_METERS - CONE_MID_NODE_X_OFFSET_FROM_WALL, y, CONE_MID_NODE_HEIGHT));
+
+            blueNodeLines.add(
+                    new Translation3d(FIELD_WIDTH_METERS - CONE_HIGH_NODE_X_OFFSET_FROM_WALL, y, 0));
+            blueNodeLines.add(
+                    new Translation3d(FIELD_WIDTH_METERS - CONE_HIGH_NODE_X_OFFSET_FROM_WALL, y, CONE_HIGH_NODE_HEIGHT));
+        });
+
+        redNodeLinePoints = Collections.unmodifiableList(redNodeLines);
+        blueNodeLinePoints = Collections.unmodifiableList(blueNodeLines);
+    }
+
+    private static List<Double> getYs(AllianceSide allianceSide) {
+        List<Double> redYs = new ArrayList<>();
+
+        for (SelectedPosition selectedPosition : selectedPositionsToGenerateLinesFor) {
+            Arrays.stream(getPossibleFieldYs(selectedPosition, allianceSide == AllianceSide.RED))
+                    .forEach(redYs::add);
+        }
+
+        return redYs;
+    }
 }

src/main/java/frc/subsytem/vision/VisionHandler.java

@@ -17,6 +17,8 @@
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.numbers.N4;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.networktables.DoubleArrayPublisher;
+import edu.wpi.first.networktables.DoubleArrayTopic;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEvent.Kind;
 import edu.wpi.first.networktables.NetworkTableInstance;
@@ -83,6 +85,8 @@ public class VisionHandler extends AbstractSubsystem {
 
     private static String[] limelightNames = new String[]{"limelight-left", "limelight-right"};
 
+    private final DoubleArrayPublisher linesPublisher;
+
     private static final @NotNull Pose3d[] fieldTags;
 
     private static final List<Integer> redTags = List.of(1, 2, 3, 4);
@@ -136,7 +140,6 @@ public class VisionHandler extends AbstractSubsystem {
             cameraPose.getRotation().unaryMinus()
     );
 
-
     VisionInputs visionInputs = new VisionInputs();
 
     private boolean isVisionEnabled = true;
@@ -154,6 +157,9 @@ public VisionHandler() {
         @NotNull var visionMiscTable = networkTableInstance.getTable("Vision Misc");
         @NotNull var configTable = networkTableInstance.getTable("Vision Config");
 
+
+        linesPublisher = new DoubleArrayTopic(visionMiscTable.getEntry("Lines").getTopic()).publish();
+
         // Send out connection flag to april tags processor
         visionMiscTable.getEntry("Connection Flag").setBoolean(true);
 
@@ -350,6 +356,19 @@ private void processNewTagPosition(VisionUpdate data) {
     }
 
 
+    private Translation3d transformToCameraSpace(Translation3d translation) {
+        var latestPose = Robot.getRobotTracker().getLatestPose3d();
+        return translation
+                // move the origin to the center of the robot
+                .minus(latestPose.getTranslation())
+                // make it relative to the robot's rotation
+                .rotateBy(Robot.getRobotTracker().getGyroAngleAtTime(Timer.getFPGATimestamp()))
+                // move the origin to the center of the camera
+                .minus(cameraPose.getTranslation())
+                // make it relative to the camera's rotation
+                .rotateBy(negativeCameraPose.getRotation());
+    }
+
     private boolean recordingWanted = false;
 
     public void forceRecord(boolean record) {
@@ -468,6 +487,19 @@ public synchronized void update() {
         Logger.getInstance().recordOutput("VisionManager/Limelight Updates Sent", limelightUpdatesSent);
 
         visionInputs.limelightUpdates.clear();
+
+        var nodeLinePoints = Robot.isRed() ? ScoringPositionManager.redNodeLinePoints : ScoringPositionManager.blueNodeLinePoints;
+
+        double[] nodeLinePointsToSend = new double[nodeLinePoints.size() * 3];
+
+        for (int i = 0; i < nodeLinePoints.size(); i++) {
+            var point = transformToCameraSpace(nodeLinePoints.get(i));
+            nodeLinePointsToSend[i * 3] = point.getX();
+            nodeLinePointsToSend[i * 3 + 1] = point.getY();
+            nodeLinePointsToSend[i * 3 + 2] = point.getZ();
+        }
+
+        linesPublisher.set(nodeLinePointsToSend);
     }
 
     record VisionUpdate(double posX, double posY, double posZ, double rotX, double rotY, double rotZ, double rotW,