Adding XTableClient and code checked in.

src/main/java/competition/auto_programs/TestVisionAuto.java

@@ -5,6 +5,7 @@
 import competition.subsystems.pose.PoseSubsystem;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import org.kobe.xbot.Client;
 
 import javax.inject.Inject;
 
@@ -14,7 +15,7 @@ public class TestVisionAuto extends SequentialCommandGroup {
     public TestVisionAuto(
             DriveSubsystem drive,
             PoseSubsystem pose,
-            DriveToGivenNoteWithVisionCommand driveToGivenNoteWithVisionCommand
+            DriveToWaypointsWithVisionCommand driveToWaypointsWithVisionCommand
     ) {
 
 
@@ -28,6 +29,6 @@ public TestVisionAuto(
                 })
         );
 
-        this.addCommands(driveToGivenNoteWithVisionCommand);
+        this.addCommands(driveToWaypointsWithVisionCommand);
     }
 }

src/main/java/competition/commandgroups/DriveToWaypointsWithVisionCommand.java

@@ -0,0 +1,110 @@
+package competition.commandgroups;
+
+import competition.subsystems.collector.CollectorSubsystem;
+import competition.subsystems.drive.DriveSubsystem;
+import competition.subsystems.drive.commands.DriveToGivenNoteCommand;
+import competition.subsystems.oracle.DynamicOracle;
+import competition.subsystems.pose.PoseSubsystem;
+import competition.subsystems.vision.NoteAcquisitionMode;
+import competition.subsystems.vision.VisionSubsystem;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import xbot.common.properties.PropertyFactory;
+import xbot.common.subsystems.drive.control_logic.HeadingModule;
+import xbot.common.trajectory.XbotSwervePoint;
+
+import javax.inject.Inject;
+import java.util.ArrayList;
+
+public class DriveToWaypointsWithVisionCommand extends SwerveSimpleTrajectoryCommand {
+
+    DynamicOracle oracle;
+    DriveSubsystem drive;
+    public Translation2d[] waypoints = null;
+    double maximumSpeedOverride = 0;
+    DynamicOracle oracle;
+    PoseSubsystem pose;
+    DriveSubsystem drive;
+    VisionSubsystem vision;
+    CollectorSubsystem collector;
+    boolean hasDoneVisionCheckYet = false;
+    XTablesClient xclient;
+
+    private NoteAcquisitionMode noteAcquisitionMode = NoteAcquisitionMode.BlindApproach;
+
+    @Inject
+    DriveToWaypointsWithVisionCommand(PoseSubsystem pose, DriveSubsystem drive, DynamicOracle oracle,
+                                      PropertyFactory pf, HeadingModule.HeadingModuleFactory headingModuleFactory,
+                                      VisionSubsystem vision, CollectorSubsystem collector) {
+        super(drive, oracle, pose, pf, headingModuleFactory);
+        this.oracle = oracle;
+        this.pose = pose;
+        this.drive = drive;
+        this.vision = vision;
+        this.collector = collector;
+    }
+
+    @Override
+    public void initialize() {
+        // The init here takes care of going to the initially given "static" note position.
+        super.initialize();
+        noteAcquisitionMode = NoteAcquisitionMode.BlindApproach;
+        hasDoneVisionCheckYet = false;
+        xclient = new XTablesClient();
+    }
+
+    //allows for driving not in a straight line
+    public void prepareToDriveAtGivenNoteWithWaypoints(Translation2d... waypoints){
+        if (waypoints == null){
+            return;
+        }
+        ArrayList<XbotSwervePoint> swervePoints = new ArrayList<>();
+        for (Translation2d waypoint : waypoints){
+            swervePoints.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint(waypoint,Rotation2d.fromDegrees(180),5));
+        }
+        // when driving to a note, the robot must face backwards, as the robot's intake is on the back
+        this.logic.setKeyPoints(swervePoints);
+        this.logic.setAimAtGoalDuringFinalLeg(true);
+        this.logic.setDriveBackwards(true);
+        this.logic.setEnableConstantVelocity(true);
+
+        double suggestedSpeed = drive.getSuggestedAutonomousMaximumSpeed();
+        if (maximumSpeedOverride > suggestedSpeed) {
+            log.info("Using maximum speed override");
+            suggestedSpeed = maximumSpeedOverride;
+        } else {
+            log.info("Not using max speed override");
+        }
+
+        this.logic.setConstantVelocity(suggestedSpeed);
+
+        reset();
+    }
+
+    //allows for driving not in a straight line
+    public void retrieveWaypointsFromVision() {
+        ArrayList<Coordinate> coordinates = xclient.getArray("target_waypoints", Coordinate);
+        ArrayList<Translation2d> waypoints = new ArrayList<Translation2d>();
+        for (Coordinate coordinate : coordinates) {
+            waypoints.add(new Translation2d(coordinate.x, coordinate.y));
+        }
+
+        this.prepareToDriveAtGivenNoteWithWaypoints(waypoints);
+    }
+
+    @Override
+    public void execute() {
+        this.retrieveWaypointsFromVision();
+        super.execute();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return super.isFinished();
+    }
+
+    private class Coordinate {
+        double x;
+        double y;
+    }
+}

src/main/java/org/kobe/xbot/Client/ImageStreamClient.java

@@ -0,0 +1,71 @@
+//package org.kobe.xbot.Client;
+//
+//import org.bytedeco.javacpp.BytePointer;
+//import org.bytedeco.opencv.global.opencv_imgcodecs;
+//import org.bytedeco.opencv.opencv_core.Mat;
+//import org.kobe.xbot.Utilities.Logger.XTablesLogger;
+//
+//import java.io.BufferedInputStream;
+//import java.io.IOException;
+//import java.io.InputStream;
+//import java.net.HttpURLConnection;
+//import java.net.URL;
+//import java.util.concurrent.ExecutorService;
+//import java.util.concurrent.Executors;
+//import java.util.concurrent.atomic.AtomicBoolean;
+//import java.util.function.Consumer;
+//
+//public class ImageStreamClient {
+//    private static final XTablesLogger logger = XTablesLogger.getLogger();
+//    private final String serverUrl;
+//    private final Consumer<Mat> consumer;
+//    private final AtomicBoolean running = new AtomicBoolean(false);
+//
+//    public ImageStreamClient(String serverUrl, Consumer<Mat> onFrame) {
+//        this.serverUrl = serverUrl;
+//        this.consumer = onFrame;
+//    }
+//
+//    public void start(ExecutorService service) {
+//        running.set(true);
+//        service.execute(() -> {
+//            try {
+//                while (running.get() && !Thread.currentThread().isInterrupted()) {
+//                    HttpURLConnection connection = (HttpURLConnection) new URL(serverUrl).openConnection();
+//                    connection.setRequestMethod("GET");
+//
+//                    try (InputStream inputStream = new BufferedInputStream(connection.getInputStream())) {
+//                        byte[] byteArray = inputStream.readAllBytes();
+//                        if (byteArray.length == 0) {
+//                            logger.warning("Received empty byte array.");
+//                            continue;
+//                        }
+//
+//                        Mat frame = byteArrayToMat(byteArray);
+//                        if (frame.empty()) {
+//                            logger.severe("Failed to decode frame.");
+//                            continue;
+//                        }
+//                        consumer.accept(frame);
+//                    } catch (IOException e) {
+//                        logger.severe("Error reading stream: " + e.getMessage());
+//                        Thread.sleep(1000);
+//                    } finally {
+//                        connection.disconnect();
+//                    }
+//                }
+//            } catch (IOException | InterruptedException e) {
+//                logger.severe("Error connecting to server: " + e.getMessage());
+//            }
+//        });
+//    }
+//
+//    public void stop() {
+//        running.set(false);
+//    }
+//
+//    private Mat byteArrayToMat(byte[] byteArray) {
+//        Mat mat = new Mat(new BytePointer(byteArray));
+//        return opencv_imgcodecs.imdecode(mat, opencv_imgcodecs.IMREAD_COLOR);
+//    }
+//}

src/main/java/org/kobe/xbot/Client/ImageStreamServer.java

@@ -0,0 +1,67 @@
+//package org.kobe.xbot.Client;
+//
+//import com.sun.net.httpserver.HttpExchange;
+//import com.sun.net.httpserver.HttpHandler;
+//import com.sun.net.httpserver.HttpServer;
+//import org.kobe.xbot.Utilities.Logger.XTablesLogger;
+//
+//import java.io.IOException;
+//import java.io.OutputStream;
+//import java.net.InetSocketAddress;
+//import java.util.concurrent.Executors;
+//
+//public class ImageStreamServer {
+//    private static final XTablesLogger logger = XTablesLogger.getLogger();
+//    private static HttpServer server;
+//    private static boolean isServerRunning = false;
+//    private byte[] currentFrame;
+//    private final String endpoint;
+//
+//    public ImageStreamServer(String name) {
+//        this.endpoint = "/" + name;
+//    }
+//
+//    public ImageStreamServer start() throws IOException {
+//        if (server == null) {
+//            server = HttpServer.create(new InetSocketAddress(4888), 0);
+//            server.setExecutor(Executors.newCachedThreadPool());
+//        }
+//
+//        server.createContext(endpoint, new StreamHandler());
+//        if (!isServerRunning) {
+//            server.start();
+//            isServerRunning = true;
+//            logger.info(("Server started at: http://localhost:" + server.getAddress().getPort()) + endpoint);
+//        } else {
+//            logger.info(("Server added endpoint: http://localhost:" + server.getAddress().getPort()) + endpoint);
+//        }
+//        return this;
+//    }
+//
+//    public void stop() {
+//        if (server != null && isServerRunning) {
+//            server.stop(0);
+//            isServerRunning = false;
+//            logger.info("Camera streaming server stopped.");
+//        }
+//    }
+//
+//    public void updateFrame(byte[] frame) {
+//        this.currentFrame = frame;
+//    }
+//
+//    private class StreamHandler implements HttpHandler {
+//        @Override
+//        public void handle(HttpExchange exchange) throws IOException {
+//            if (currentFrame != null) {
+//                exchange.getResponseHeaders().set("Content-Type", "image/jpeg");
+//                exchange.sendResponseHeaders(200, currentFrame.length);
+//                OutputStream os = exchange.getResponseBody();
+//                os.write(currentFrame);
+//                os.close();
+//            } else {
+//                exchange.sendResponseHeaders(503, -1); // Service unavailable if no frame available
+//            }
+//        }
+//    }
+//}

src/main/java/org/kobe/xbot/Client/Main.java

@@ -0,0 +1,119 @@
+package org.kobe.xbot.Client;
+// EXAMPLE SETUP
+
+import org.kobe.xbot.Server.XTablesData;
+import org.kobe.xbot.Utilities.ResponseStatus;
+import org.kobe.xbot.Utilities.ScriptResponse;
+
+import java.util.List;
+import java.util.function.Consumer;
+
+public class Main {
+    private static final String SERVER_ADDRESS = "localhost"; // Server address
+    private static final int SERVER_PORT = 1735; // Server port
+
+    public static void main(String[] args) {
+        // Initialize a new client with address and port
+        XTablesClient client = new XTablesClient(SERVER_ADDRESS, SERVER_PORT, 3, true);
+        // Thread blocks until connection is successful
+
+        // Get raw JSON from server
+        String response = client.getRawJSON().complete();
+        System.out.println(response);
+
+        // Variable for connection status, updates on every message
+        System.out.println("Connected? " + client.getSocketClient().isConnected);
+        // Get latency from server
+        XTablesClient.LatencyInfo info = client.ping_latency().complete();
+        System.out.println("Network Latency: " + info.getNetworkLatencyMS() + "ms");
+        System.out.println("Round Trip Latency: " + info.getRoundTripLatencyMS() + "ms");
+        System.out.println("CPU Usage: " + info.getSystemStatistics().getProcessCpuLoadPercentage());
+
+        // -------- PUT VALUES --------
+        // "OK" - Value updated
+        // "FAIL" - Failed to update
+
+        // Put a string value into server
+        ResponseStatus status = client.putString("SmartDashboard", "Some Value1" + Math.random()).complete();
+        System.out.println(status);
+
+        // Put a string value into a sub table
+        status = client.putString("SmartDashboard.sometable", "Some Value").complete();
+        System.out.println(status);
+
+        // Put a string value into server asynchronously
+        client.putString("SmartDashboard", "Some Value").queue();
+
+        // Put a string value into server asynchronously with response
+        client.putString("SmartDashboard", "Some Value").queue(System.out::println);
+
+        // Put a string value into server asynchronously with response & fail
+        client.putString("SmartDashboard", "Some Value").queue(System.out::println, System.err::println);
+
+        // Put an object value into server thread block
+        status = client.putObject("SmartDashboard", new String("OK")).complete();
+        System.out.println(status);
+
+        // Put an integer value on sub table asynchronously
+        client.putInteger("SmartDashboard.somevalue.awdwadawd", 488).queue();
+
+        // Put an integer array value on sub table asynchronously
+        client.putArray("SmartDashboard.somearray", List.of("Test", "TEAM XBOT")).queue();
+
+        // -------- GET VALUES --------
+        // value - Value retrieved
+        // null - No value
+
+        // Get all tables on server
+        List<String> tables = client.getTables("").complete();
+        System.out.println(tables);
+
+        // Get sub tables of table on server
+        List<String> sub_tables = client.getTables("SmartDashboard").complete();
+        System.out.println(sub_tables);
+
+        // Get string from sub-table
+        String value = client.getString("SmartDashboard.somevalue").complete();
+        System.out.println(value);
+
+        // Get integer from sub-table
+        Integer integer = client.getInteger("SmartDashboard.somevalue").complete();
+        System.out.println(integer);
+
+        // Subscribe to an update event on a key
+        status = client.subscribeUpdateEvent("SmartDashboard", Integer.class, new_value ->
+                        System.out.println("New Value: " + new_value)
+                )
+                .complete();
+        System.out.println("Subscribe status: " + status);
+
+        // Define a consumer for update events
+        Consumer<SocketClient.KeyValuePair<String>> updateConsumer = update -> {
+            System.out.println("Update received: " + update);
+        };
+        // Subscribe to updates for a specific key
+        status = client.subscribeUpdateEvent("SmartDashboard", String.class, updateConsumer).complete();
+        System.out.println("Subscribe status: " + status);
+        // Unsubscribe after a certain condition is met
+        status = client.unsubscribeUpdateEvent("SmartDashboard", String.class, updateConsumer).complete();
+        System.out.println("Unsubscribe status: " + status);
+
+        // Check weather or not the cache is set up and ready to be used.
+        // Any updates/retrievals from cache does not make any request to server.
+        // Cache updates on any update server side.
+        System.out.println("Is cache ready? " + client.isCacheReady());
+        if (client.isCacheReady()) {
+            XTablesData cache = client.getCache();
+
+            // Example: Get data from cache
+            String valueFromCache = cache.get("SmartDashboard.sometable");
+
+            System.out.println("Value from cache: " + valueFromCache);
+        }
+
+
+        ScriptResponse scriptResponse = client.runScript("test", null).complete();
+        System.out.println("Status: " + scriptResponse.getStatus());
+        System.out.println("Script Response: " + scriptResponse.getResponse());
+    }
+}