generated from Team488/FRCRobotTemplate
-
Notifications
You must be signed in to change notification settings - Fork 1
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Adding XTableClient and code checked in.
- Loading branch information
Showing
33 changed files
with
7,010 additions
and
2 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
110 changes: 110 additions & 0 deletions
110
src/main/java/competition/commandgroups/DriveToWaypointsWithVisionCommand.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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; | ||
} | ||
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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); | ||
// } | ||
//} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 | ||
// } | ||
// } | ||
// } | ||
//} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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()); | ||
} | ||
} |
Oops, something went wrong.