Add code for a practice defense bot
We want to use the 2nd robot drivetrain as a defense bot to use in drive
practice, this commit adds the folder for that robot.
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: I8d379c8793dffe3248f1ecef83fe18040c7e0e77
diff --git a/y2024_defense/www/BUILD b/y2024_defense/www/BUILD
new file mode 100644
index 0000000..7af68fe
--- /dev/null
+++ b/y2024_defense/www/BUILD
@@ -0,0 +1,63 @@
+load("//tools/build_rules:js.bzl", "rollup_bundle", "ts_project")
+load("//frc971/downloader:downloader.bzl", "aos_downloader_dir")
+
+filegroup(
+ name = "files",
+ srcs = glob([
+ "**/*.html",
+ "**/*.css",
+ "**/*.png",
+ ]) + ["2023.png"],
+ visibility = ["//visibility:public"],
+)
+
+# TODO(max): This needs to be changed once the season starts
+genrule(
+ name = "2023_field_png",
+ srcs = ["//third_party/y2023/field:pictures"],
+ outs = ["2023.png"],
+ cmd = "cp third_party/y2023/field/2023.png $@",
+)
+
+ts_project(
+ name = "field_main",
+ srcs = [
+ "constants.ts",
+ "field_handler.ts",
+ "field_main.ts",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos/network:connect_ts_fbs",
+ "//aos/network:message_bridge_client_ts_fbs",
+ "//aos/network:message_bridge_server_ts_fbs",
+ "//aos/network:web_proxy_ts_fbs",
+ "//aos/network/www:proxy",
+ "//frc971/control_loops:control_loops_ts_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_ts_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_ts_fbs",
+ "@com_github_google_flatbuffers//ts:flatbuffers_ts",
+ ],
+)
+
+rollup_bundle(
+ name = "field_main_bundle",
+ entry_point = "field_main.ts",
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//y2024_defense:__subpackages__"],
+ deps = [
+ ":field_main",
+ ],
+)
+
+aos_downloader_dir(
+ name = "www_files",
+ srcs = [
+ ":field_main_bundle.min.js",
+ ":files",
+ "//frc971/analysis:plot_index_bundle.min.js",
+ ],
+ dir = "www",
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
diff --git a/y2024_defense/www/constants.ts b/y2024_defense/www/constants.ts
new file mode 100644
index 0000000..d6ecfaf
--- /dev/null
+++ b/y2024_defense/www/constants.ts
@@ -0,0 +1,8 @@
+// Conversion constants to meters
+export const IN_TO_M = 0.0254;
+export const FT_TO_M = 0.3048;
+// Dimensions of the field in meters
+// Numbers are slightly hand-tuned to match the PNG that we are using.
+export const FIELD_WIDTH = 26 * FT_TO_M + 7.25 * IN_TO_M;
+export const FIELD_LENGTH = 54 * FT_TO_M + 5.25 * IN_TO_M;
+
diff --git a/y2024_defense/www/field.html b/y2024_defense/www/field.html
new file mode 100644
index 0000000..d086af7
--- /dev/null
+++ b/y2024_defense/www/field.html
@@ -0,0 +1,41 @@
+<html>
+ <head>
+ <script src="field_main_bundle.min.js" defer></script>
+ <link rel="stylesheet" href="styles.css">
+ </head>
+ <body>
+ <div id="field"> </div>
+ <div id="legend"> </div>
+ <div id="readouts">
+ <table>
+ <tr>
+ <th colspan="2">Robot State</th>
+ </tr>
+ <tr>
+ <td>X</td>
+ <td id="x"> NA </td>
+ </tr>
+ <tr>
+ <td>Y</td>
+ <td id="y"> NA </td>
+ </tr>
+ <tr>
+ <td>Theta</td>
+ <td id="theta"> NA </td>
+ </tr>
+ </table>
+ <h3>Zeroing Faults:</h3>
+ <p id="zeroing_faults"> NA </p>
+ </div>
+ <div id="middle_readouts">
+ <div id="message_bridge_status">
+ <div>
+ <div>Node</div>
+ <div>Client</div>
+ <div>Server</div>
+ </div>
+ </div>
+ </div>
+ </body>
+</html>
+
diff --git a/y2024_defense/www/field_handler.ts b/y2024_defense/www/field_handler.ts
new file mode 100644
index 0000000..8010b10
--- /dev/null
+++ b/y2024_defense/www/field_handler.ts
@@ -0,0 +1,261 @@
+import {ByteBuffer} from 'flatbuffers'
+import {ClientStatistics} from '../../aos/network/message_bridge_client_generated'
+import {ServerStatistics, State as ConnectionState} from '../../aos/network/message_bridge_server_generated'
+import {Connection} from '../../aos/network/www/proxy'
+import {ZeroingError} from '../../frc971/control_loops/control_loops_generated'
+import {Status as DrivetrainStatus} from '../../frc971/control_loops/drivetrain/drivetrain_status_generated'
+import {LocalizerOutput} from '../../frc971/control_loops/drivetrain/localization/localizer_output_generated'
+
+import {FIELD_LENGTH, FIELD_WIDTH, FT_TO_M, IN_TO_M} from './constants';
+
+// (0,0) is field center, +X is toward red DS
+const FIELD_SIDE_Y = FIELD_WIDTH / 2;
+const FIELD_EDGE_X = FIELD_LENGTH / 2;
+
+const ROBOT_WIDTH = 25 * IN_TO_M;
+const ROBOT_LENGTH = 32 * IN_TO_M;
+
+const PI_COLORS = ['#ff00ff', '#ffff00', '#00ffff', '#ffa500'];
+const PIS = ['pi1', 'pi2', 'pi3', 'pi4'];
+
+export class FieldHandler {
+ private canvas = document.createElement('canvas');
+ private localizerOutput: LocalizerOutput|null = null;
+ private drivetrainStatus: DrivetrainStatus|null = null;
+
+ private x: HTMLElement = (document.getElementById('x') as HTMLElement);
+ private y: HTMLElement = (document.getElementById('y') as HTMLElement);
+ private theta: HTMLElement =
+ (document.getElementById('theta') as HTMLElement);
+ // HTML elements for rejection reasons for individual pis. Indices
+ // corresponding to RejectionReason enum values will be for those reasons. The
+ // final row will account for images rejected by the aprilrobotics detector
+ // instead of the localizer.
+ private messageBridgeDiv: HTMLElement =
+ (document.getElementById('message_bridge_status') as HTMLElement);
+ private clientStatuses = new Map<string, HTMLElement>();
+ private serverStatuses = new Map<string, HTMLElement>();
+ private fieldImage: HTMLImageElement = new Image();
+
+ constructor(private readonly connection: Connection) {
+ (document.getElementById('field') as HTMLElement).appendChild(this.canvas);
+
+ this.fieldImage.src = '2023.png';
+
+ this.connection.addConfigHandler(() => {
+ // Visualization message is reliable so that we can see *all* the vision
+ // matches.
+ this.connection.addHandler(
+ '/drivetrain', 'frc971.control_loops.drivetrain.Status', (data) => {
+ this.handleDrivetrainStatus(data);
+ });
+ this.connection.addHandler(
+ '/localizer', 'frc971.controls.LocalizerOutput', (data) => {
+ this.handleLocalizerOutput(data);
+ });
+ this.connection.addHandler(
+ '/aos', 'aos.message_bridge.ServerStatistics',
+ (data) => {this.handleServerStatistics(data)});
+ this.connection.addHandler(
+ '/aos', 'aos.message_bridge.ClientStatistics',
+ (data) => {this.handleClientStatistics(data)});
+ });
+ }
+
+ private handleLocalizerOutput(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ this.localizerOutput = LocalizerOutput.getRootAsLocalizerOutput(fbBuffer);
+ }
+
+ private handleDrivetrainStatus(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ this.drivetrainStatus = DrivetrainStatus.getRootAsStatus(fbBuffer);
+ }
+
+ private populateNodeConnections(nodeName: string): void {
+ const row = document.createElement('div');
+ this.messageBridgeDiv.appendChild(row);
+ const nodeDiv = document.createElement('div');
+ nodeDiv.innerHTML = nodeName;
+ row.appendChild(nodeDiv);
+ const clientDiv = document.createElement('div');
+ clientDiv.innerHTML = 'N/A';
+ row.appendChild(clientDiv);
+ const serverDiv = document.createElement('div');
+ serverDiv.innerHTML = 'N/A';
+ row.appendChild(serverDiv);
+ this.serverStatuses.set(nodeName, serverDiv);
+ this.clientStatuses.set(nodeName, clientDiv);
+ }
+
+ private setCurrentNodeState(element: HTMLElement, state: ConnectionState):
+ void {
+ if (state === ConnectionState.CONNECTED) {
+ element.innerHTML = ConnectionState[state];
+ element.classList.remove('faulted');
+ element.classList.add('connected');
+ } else {
+ element.innerHTML = ConnectionState[state];
+ element.classList.remove('connected');
+ element.classList.add('faulted');
+ }
+ }
+
+ private handleServerStatistics(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ const serverStatistics =
+ ServerStatistics.getRootAsServerStatistics(fbBuffer);
+
+ for (let ii = 0; ii < serverStatistics.connectionsLength(); ++ii) {
+ const connection = serverStatistics.connections(ii);
+ const nodeName = connection.node().name();
+ if (!this.serverStatuses.has(nodeName)) {
+ this.populateNodeConnections(nodeName);
+ }
+ this.setCurrentNodeState(
+ this.serverStatuses.get(nodeName), connection.state());
+ }
+ }
+
+ private handleClientStatistics(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ const clientStatistics =
+ ClientStatistics.getRootAsClientStatistics(fbBuffer);
+
+ for (let ii = 0; ii < clientStatistics.connectionsLength(); ++ii) {
+ const connection = clientStatistics.connections(ii);
+ const nodeName = connection.node().name();
+ if (!this.clientStatuses.has(nodeName)) {
+ this.populateNodeConnections(nodeName);
+ }
+ this.setCurrentNodeState(
+ this.clientStatuses.get(nodeName), connection.state());
+ }
+ }
+
+ setZeroing(div: HTMLElement): void {
+ div.innerHTML = 'zeroing';
+ div.classList.remove('faulted');
+ div.classList.add('zeroing');
+ div.classList.remove('near');
+ }
+
+ setValue(div: HTMLElement, val: number): void {
+ div.innerHTML = val.toFixed(4);
+ div.classList.remove('faulted');
+ div.classList.remove('zeroing');
+ div.classList.remove('near');
+ }
+
+ drawField(): void {
+ const ctx = this.canvas.getContext('2d');
+ ctx.save();
+ ctx.scale(1.0, -1.0);
+ ctx.drawImage(
+ this.fieldImage, 0, 0, this.fieldImage.width, this.fieldImage.height,
+ -FIELD_EDGE_X, -FIELD_SIDE_Y, FIELD_LENGTH, FIELD_WIDTH);
+ ctx.restore();
+ }
+
+ drawCamera(x: number, y: number, theta: number, color: string = 'blue'):
+ void {
+ const ctx = this.canvas.getContext('2d');
+ ctx.save();
+ ctx.translate(x, y);
+ ctx.rotate(theta);
+ ctx.strokeStyle = color;
+ ctx.beginPath();
+ ctx.moveTo(0.5, 0.5);
+ ctx.lineTo(0, 0);
+ ctx.lineTo(0.5, -0.5);
+ ctx.stroke();
+ ctx.beginPath();
+ ctx.arc(0, 0, 0.25, -Math.PI / 4, Math.PI / 4);
+ ctx.stroke();
+ ctx.restore();
+ }
+
+ drawRobot(
+ x: number, y: number, theta: number, color: string = 'blue',
+ dashed: boolean = false): void {
+ const ctx = this.canvas.getContext('2d');
+ ctx.save();
+ ctx.translate(x, y);
+ ctx.rotate(theta);
+ ctx.strokeStyle = color;
+ ctx.lineWidth = ROBOT_WIDTH / 10.0;
+ if (dashed) {
+ ctx.setLineDash([0.05, 0.05]);
+ } else {
+ // Empty array = solid line.
+ ctx.setLineDash([]);
+ }
+ ctx.rect(-ROBOT_LENGTH / 2, -ROBOT_WIDTH / 2, ROBOT_LENGTH, ROBOT_WIDTH);
+ ctx.stroke();
+
+ // Draw line indicating which direction is forwards on the robot.
+ ctx.beginPath();
+ ctx.moveTo(0, 0);
+ ctx.lineTo(ROBOT_LENGTH / 2.0, 0);
+ ctx.stroke();
+
+ ctx.restore();
+ }
+
+ draw(): void {
+ this.reset();
+ this.drawField();
+
+ // Draw the matches with debugging information from the localizer.
+ const now = Date.now() / 1000.0;
+
+ if (this.drivetrainStatus && this.drivetrainStatus.trajectoryLogging()) {
+ this.drawRobot(
+ this.drivetrainStatus.trajectoryLogging().x(),
+ this.drivetrainStatus.trajectoryLogging().y(),
+ this.drivetrainStatus.trajectoryLogging().theta(), '#000000FF',
+ false);
+ }
+
+ if (this.localizerOutput) {
+ if (!this.localizerOutput.zeroed()) {
+ this.setZeroing(this.x);
+ this.setZeroing(this.y);
+ this.setZeroing(this.theta);
+ } else {
+ this.setValue(this.x, this.localizerOutput.x());
+ this.setValue(this.y, this.localizerOutput.y());
+ this.setValue(this.theta, this.localizerOutput.theta());
+ }
+
+ this.drawRobot(
+ this.localizerOutput.x(), this.localizerOutput.y(),
+ this.localizerOutput.theta());
+ }
+
+
+ window.requestAnimationFrame(() => this.draw());
+ }
+
+ reset(): void {
+ const ctx = this.canvas.getContext('2d');
+ ctx.setTransform(1, 0, 0, 1, 0, 0);
+ const size = window.innerHeight * 0.9;
+ ctx.canvas.height = size;
+ const width = size / 2 + 20;
+ ctx.canvas.width = width;
+ ctx.clearRect(0, 0, size, width);
+
+ // Translate to center of display.
+ ctx.translate(width / 2, size / 2);
+ // Coordinate system is:
+ // x -> forward.
+ // y -> to the left.
+ ctx.rotate(-Math.PI / 2);
+ ctx.scale(1, -1);
+
+ const M_TO_PX = (size - 10) / FIELD_LENGTH;
+ ctx.scale(M_TO_PX, M_TO_PX);
+ ctx.lineWidth = 1 / M_TO_PX;
+ }
+}
diff --git a/y2024_defense/www/field_main.ts b/y2024_defense/www/field_main.ts
new file mode 100644
index 0000000..d71a45e
--- /dev/null
+++ b/y2024_defense/www/field_main.ts
@@ -0,0 +1,12 @@
+import {Connection} from '../../aos/network/www/proxy';
+
+import {FieldHandler} from './field_handler';
+
+const conn = new Connection();
+
+conn.connect();
+
+const fieldHandler = new FieldHandler(conn);
+
+fieldHandler.draw();
+
diff --git a/y2024_defense/www/index.html b/y2024_defense/www/index.html
new file mode 100644
index 0000000..e4e185e
--- /dev/null
+++ b/y2024_defense/www/index.html
@@ -0,0 +1,6 @@
+<html>
+ <body>
+ <a href="field.html">Field Visualization</a><br>
+ <a href="plotter.html">Plots</a>
+ </body>
+</html>
diff --git a/y2024_defense/www/plotter.html b/y2024_defense/www/plotter.html
new file mode 100644
index 0000000..629ceaa
--- /dev/null
+++ b/y2024_defense/www/plotter.html
@@ -0,0 +1,7 @@
+<html>
+ <head>
+ <script src="plot_index_bundle.min.js" defer></script>
+ </head>
+ <body>
+ </body>
+</html>
diff --git a/y2024_defense/www/styles.css b/y2024_defense/www/styles.css
new file mode 100644
index 0000000..c2c44d2
--- /dev/null
+++ b/y2024_defense/www/styles.css
@@ -0,0 +1,74 @@
+.channel {
+ display: flex;
+ border-bottom: 1px solid;
+ font-size: 24px;
+}
+#field {
+ display: inline-block
+}
+
+#readouts,
+#middle_readouts
+{
+ display: inline-block;
+ vertical-align: top;
+ float: right;
+}
+
+
+#legend {
+ display: inline-block;
+}
+
+table, th, td {
+ border: 1px solid black;
+ border-collapse: collapse;
+ padding: 5px;
+ margin: 10px;
+}
+
+th, td {
+ text-align: right;
+ width: 70px;
+}
+
+td:first-child {
+ width: 150px;
+}
+
+.connected, .near {
+ background-color: LightGreen;
+ border-radius: 10px;
+}
+
+.zeroing {
+ background-color: yellow;
+ border-radius: 10px;
+}
+
+.faulted {
+ background-color: red;
+ border-radius: 10px;
+}
+
+#vision_readouts > div {
+ display: table-row;
+ padding: 5px;
+}
+
+#vision_readouts > div > div {
+ display: table-cell;
+ padding: 5px;
+ text-align: right;
+}
+
+#message_bridge_status > div {
+ display: table-row;
+ padding: 5px;
+}
+
+#message_bridge_status > div > div {
+ display: table-cell;
+ padding: 5px;
+ text-align: right;
+}