Jetson

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

@@ -29,4 +29,10 @@ public static final class ElevatorConstants {
         public static final boolean elevatorUpIsPositive = true;
 
     }
+
+    public static final class JetsonConstants {
+        public static final int jetsonCameraPort = 1337;
+        public static final String jetsonAddress = "10.1.92.14";
+
+    }
 }

src/main/java/frc/robot/JetsonConnection.java

@@ -0,0 +1,61 @@
+package frc.robot;
+
+import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.cscore.HttpCamera;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+public class JetsonConnection {
+
+    public static String TABLE_NAME = "jetson";
+    private NetworkTable table;
+
+    public JetsonConnection() {
+        table = NetworkTableInstance.getDefault().getTable(TABLE_NAME);
+
+        // Start camera stream on Shuffleboard
+        // TODO if this HttpCamera thing works, make it work for multiple streams 
+        // "mjpg:http://%s:%d/?action=stream" % (address, port)
+        HttpCamera jetsonCamera = new HttpCamera("Jetson Camera - Port 1181", "mjpg:http://10.1.92.94:1181/?action=stream");
+        CameraServer.startAutomaticCapture(jetsonCamera);
+    }
+
+    /**
+     * Runs periodically in a Thread. Put print lines in here or whatever else you
+     * want.
+     */
+    public void periodic() {
+
+        System.out.println("test: " + getString("test"));
+        System.out.println("test exists? " + hasKey("test"));
+        System.out.println("xCentroid: " + getDouble("xCentroid"));
+        System.out.println("yCentroid: " + getDouble("yCentroid"));
+        System.out.println("pitchAngle: " + getDouble("pitchAngles"));
+        System.out.println("yawAngle: " + getDouble("yawAngle"));
+
+    }
+
+    public boolean hasKey(String key) {
+        return table.getEntry(key).exists();
+    }
+
+    public String getString(String key) {
+        return table.getEntry(key).getString("");
+    }
+
+    public double getDouble(String key) {
+        System.out.println("/" + TABLE_NAME + "/" + key + " = " + table.getEntry(key).getDouble(0));
+        return table.getEntry(key).getDouble(0);
+    }
+
+    public double[] getDoubleArray(String key) {
+        Number[] numArr = table.getEntry(key).getNumberArray(new Number[0]);
+
+        double[] arr = new double[0];
+        for (int i = 0; i < numArr.length; i++) {
+            arr[i] = numArr[i].doubleValue();
+        }
+        return arr;
+    }
+
+}

src/main/java/frc/robot/RobotContainer.java

@@ -46,7 +46,6 @@ public class RobotContainer {
 
   /**
    * The container for the robot. Contains subsystems, OI devices, and commands.
-   * 
    */
   public RobotContainer() {
 
@@ -57,7 +56,7 @@ public RobotContainer() {
       FileInputStream stream = new FileInputStream(new File(Filesystem.getDeployDirectory(), CONFIG_PATH));
       config.load(stream);
     } catch (IOException ie) {
-      System.out.println("config file not found");
+      System.out.println("Config file not found");
     }
 
     // Instantiate subsystems
@@ -68,6 +67,23 @@ public RobotContainer() {
 
     // Configure the button bindings
     configureButtonBindings();
+
+    // Configure the Jetson and run it
+    JetsonConnection jetsonObj = new JetsonConnection();
+    Runnable jetson = () -> {
+      while (true) {
+        jetsonObj.periodic();
+
+        try {
+          Thread.sleep(1000);
+        } catch (InterruptedException e) {
+          e.printStackTrace();
+        }
+      }
+    };
+
+    Thread jetsonThread = new Thread(jetson);
+    jetsonThread.start();
   }
 
   /**
@@ -106,4 +122,5 @@ public Command getAutonomousCommand() {
     // An ExampleCommand will run in autonomous
     return tankCommand;
   }
+
 }

src/main/java/frc/robot/subsystems/JetsonSubsystem.java

@@ -0,0 +1,113 @@
+package frc.robot.subsystems;
+
+import java.io.BufferedReader;
+import java.io.IOException;
+import java.io.InputStreamReader;
+import java.net.Socket;
+import java.net.UnknownHostException;
+import java.util.Arrays;
+import java.util.HashMap;
+import java.util.Map;
+import java.util.stream.Collectors;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import static frc.robot.Constants.JetsonConstants.*;
+
+public class JetsonSubsystem extends SubsystemBase {
+
+    private boolean jetsonConnected;
+
+    // Socket that is connected to jetson
+    private Socket socket;
+
+    // Reader that reads from socket
+    private BufferedReader stdIn;
+
+    private Map<String, String> cameraData;
+
+    public JetsonSubsystem() {
+        jetsonConnected = false;
+        cameraData = new HashMap<String, String>();
+    }
+
+    @Override
+    public void periodic() {
+        try {
+            // If jetson not connected
+            if (!jetsonConnected) {
+                jetsonConnected = connect();
+                if (!jetsonConnected) {
+                    System.out.println("UNABLE TO CONNECT TO CAMERA");
+
+                    // If we don't connect, wait before trying to connect again
+                    Thread.sleep(500);
+                }
+            } else { // If connected
+                cameraData();
+            }
+
+        } catch (InterruptedException e) {
+            e.printStackTrace();
+        }
+    }
+
+    public boolean connect() {
+        boolean connected = false;
+        try {
+            socket = new Socket(jetsonAddress, jetsonCameraPort);
+            stdIn = new BufferedReader(new InputStreamReader(socket.getInputStream()));
+            System.out.println("Connected to jetson address=" + jetsonAddress + " at port=" + jetsonCameraPort);
+            connected = true;
+        } catch (UnknownHostException e1) {
+            socket = null;
+            stdIn = null;
+        } catch (IOException e1) {
+            socket = null;
+            stdIn = null;
+        } catch (Exception e) {
+            socket = null;
+            stdIn = null;
+            System.out.println("UNKNOWN ERROR: SOMETHING WENT SERIOUSLY WRONG WHILE CONNECTING CAMERA!");
+        }
+        return connected;
+    }
+
+    public void cameraData() throws InterruptedException {
+        try {
+            String in = stdIn.readLine();
+            if (in != null) {
+
+                cameraData = convertWithStream(in);
+            }
+        } catch (IOException e) {
+            e.printStackTrace();
+        } catch (NullPointerException e) {
+            System.out.println("NullPointerException: unable to parse camera data.");
+        } catch (NumberFormatException e) {
+            System.out.println("NumberFormatException: unable to parse camera data.");
+        }
+    }
+
+    /**
+     * Retrieves numerical value from camera data.
+     * 
+     * @param key the key
+     * @return the corresponding double value
+     */
+    public double getDouble(String key) {
+        return Double.parseDouble((String) cameraData.get(key));
+    }
+
+    /**
+     * Converts a string formatted as key1=value1,key2=value2 (etc.) into a Map.
+     * 
+     * @param mapAsString the String to convert
+     * @return the Map
+     */
+    private static Map<String, String> convertWithStream(String mapAsString) {
+        Map<String, String> map = Arrays.stream(mapAsString.split(",")).map(entry -> entry.split("="))
+                .collect(Collectors.toMap(entry -> entry[0], entry -> entry[1]));
+        return map;
+    }
+}