Adding field plot for 2022
Adds some bare minimum status information, mostly focused on getting the
camera readings showing on the screen.
Note that this code is largely borrowed from the 2020 field viewing
code.
TODO: Add debug information for superstructure state machine/catapult.
Change-Id: I896a3def0d8d8b0ef2fb132086eaba1ba8ec0c61
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2022/www/2022.png b/y2022/www/2022.png
new file mode 100644
index 0000000..68087bd
--- /dev/null
+++ b/y2022/www/2022.png
Binary files differ
diff --git a/y2022/www/BUILD b/y2022/www/BUILD
index 55cbde2..9ee56e3 100644
--- a/y2022/www/BUILD
+++ b/y2022/www/BUILD
@@ -1,3 +1,5 @@
+load("@npm//@bazel/typescript:index.bzl", "ts_library")
+load("//tools/build_rules:js.bzl", "rollup_bundle")
load("//frc971/downloader:downloader.bzl", "aos_downloader_dir")
filegroup(
@@ -5,13 +7,44 @@
srcs = glob([
"**/*.html",
"**/*.css",
+ "**/*.png",
]),
visibility = ["//visibility:public"],
)
+ts_library(
+ 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:web_proxy_ts_fbs",
+ "//aos/network/www:proxy",
+ "//y2022/control_loops/superstructure:superstructure_status_ts_fbs",
+ "//y2022/localizer:localizer_output_ts_fbs",
+ "//y2022/localizer:localizer_visualization_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 = ["//y2022:__subpackages__"],
+ deps = [
+ ":field_main",
+ ],
+)
+
aos_downloader_dir(
name = "www_files",
srcs = [
+ ":field_main_bundle",
":files",
"//frc971/analysis:plot_index_bundle.min.js",
],
diff --git a/y2022/www/constants.ts b/y2022/www/constants.ts
new file mode 100644
index 0000000..b94d7a7
--- /dev/null
+++ b/y2022/www/constants.ts
@@ -0,0 +1,7 @@
+// Conversion constants to meters
+export const IN_TO_M = 0.0254;
+export const FT_TO_M = 0.3048;
+// Dimensions of the field in meters
+export const FIELD_WIDTH = 26 * FT_TO_M + 11.25 * IN_TO_M;
+export const FIELD_LENGTH = 52 * FT_TO_M + 5.25 * IN_TO_M;
+
diff --git a/y2022/www/field.html b/y2022/www/field.html
new file mode 100644
index 0000000..8b5f0ff
--- /dev/null
+++ b/y2022/www/field.html
@@ -0,0 +1,74 @@
+<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>
+
+ <table>
+ <tr>
+ <th colspan="2">Aiming</th>
+ </tr>
+ <tr>
+ <td>Shot distance</td>
+ <td id="shot_distance"> NA </td>
+ </tr>
+ <tr>
+ <td>Turret</td>
+ <td id="turret"> NA </td>
+ </tr>
+ </table>
+
+ <table>
+ <tr>
+ <th colspan="2">Intakes</th>
+ </tr>
+ <tr>
+ <td>Front Intake</td>
+ <td id="front_intake"> NA </td>
+ </tr>
+ <tr>
+ <td>Back Intake</td>
+ <td id="back_intake"> NA </td>
+ </tr>
+ </table>
+
+ <table>
+ <tr>
+ <th colspan="2">Images</th>
+ </tr>
+ <tr>
+ <td> Images Accepted </td>
+ <td id="images_accepted"> NA </td>
+ </tr>
+ <tr>
+ <td> Images Rejected </td>
+ <td id="images_rejected"> NA </td>
+ </tr>
+ </table>
+ </div>
+ <div id="vision_readouts">
+ </div>
+ </body>
+</html>
+
diff --git a/y2022/www/field_handler.ts b/y2022/www/field_handler.ts
new file mode 100644
index 0000000..7603820
--- /dev/null
+++ b/y2022/www/field_handler.ts
@@ -0,0 +1,390 @@
+import * as web_proxy from 'org_frc971/aos/network/web_proxy_generated';
+import {Connection} from 'org_frc971/aos/network/www/proxy';
+import * as flatbuffers_builder from 'org_frc971/external/com_github_google_flatbuffers/ts/builder';
+import {ByteBuffer} from 'org_frc971/external/com_github_google_flatbuffers/ts/byte-buffer';
+import * as localizer from 'org_frc971/y2022/localizer/localizer_visualization_generated';
+import * as output from 'org_frc971/y2022/localizer/localizer_output_generated';
+import * as ss from 'org_frc971/y2022/control_loops/superstructure/superstructure_status_generated'
+
+import LocalizerVisualization = localizer.frc971.controls.LocalizerVisualization;
+import LocalizerOutput = output.frc971.controls.LocalizerOutput;
+import RejectionReason = localizer.frc971.controls.RejectionReason;
+import TargetEstimateDebug = localizer.frc971.controls.TargetEstimateDebug;
+import SuperstructureStatus = ss.y2022.control_loops.superstructure.Status;
+
+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 = 34 * IN_TO_M;
+const ROBOT_LENGTH = 36 * IN_TO_M;
+
+const PI_COLORS = ['#ff00ff', '#ffff00', '#00ffff', '#ffa500'];
+
+export class FieldHandler {
+ private canvas = document.createElement('canvas');
+ private localizerOutput: LocalizerOutput|null = null;
+ private superstructureStatus: SuperstructureStatus|null = null;
+
+ // Image information indexed by timestamp (seconds since the epoch), so that
+ // we can stop displaying images after a certain amount of time.
+ private localizerImageMatches = new Map<number, LocalizerVisualization>();
+ private outerTarget: HTMLElement =
+ (document.getElementById('outer_target') as HTMLElement);
+ private innerTarget: HTMLElement =
+ (document.getElementById('inner_target') as HTMLElement);
+ private x: HTMLElement = (document.getElementById('x') as HTMLElement);
+ private y: HTMLElement = (document.getElementById('y') as HTMLElement);
+ private theta: HTMLElement =
+ (document.getElementById('theta') as HTMLElement);
+ private shotDistance: HTMLElement =
+ (document.getElementById('shot_distance') as HTMLElement);
+ private turret: HTMLElement =
+ (document.getElementById('turret') as HTMLElement);
+ private frontIntake: HTMLElement =
+ (document.getElementById('front_intake') as HTMLElement);
+ private backIntake: HTMLElement =
+ (document.getElementById('back_intake') as HTMLElement);
+ private imagesAcceptedCounter: HTMLElement =
+ (document.getElementById('images_accepted') as HTMLElement);
+ private imagesRejectedCounter: HTMLElement =
+ (document.getElementById('images_rejected') as HTMLElement);
+ private rejectionReasonCells: HTMLElement[] = [];
+ private fieldImage: HTMLImageElement = new Image();
+
+ constructor(private readonly connection: Connection) {
+ (document.getElementById('field') as HTMLElement).appendChild(this.canvas);
+
+ this.fieldImage.src = "2022.png";
+
+ for (const value in RejectionReason) {
+ // Typescript generates an iterator that produces both numbers and
+ // strings... don't do anything on the string iterations.
+ if (isNaN(Number(value))) {
+ continue;
+ }
+ const row = document.createElement('div');
+ const nameCell = document.createElement('div');
+ nameCell.innerHTML = RejectionReason[value];
+ row.appendChild(nameCell);
+ const valueCell = document.createElement('div');
+ valueCell.innerHTML = 'NA';
+ this.rejectionReasonCells.push(valueCell);
+ row.appendChild(valueCell);
+ document.getElementById('vision_readouts').appendChild(row);
+ }
+
+ for (let ii = 0; ii < PI_COLORS.length; ++ii) {
+ const legendEntry = document.createElement('div');
+ legendEntry.style.color = PI_COLORS[ii];
+ legendEntry.innerHTML = 'PI' + (ii + 1).toString()
+ document.getElementById('legend').appendChild(legendEntry);
+ }
+
+ this.connection.addConfigHandler(() => {
+ this.connection.addHandler(
+ '/localizer', LocalizerVisualization.getFullyQualifiedName(),
+ (data) => {
+ this.handleLocalizerDebug(data);
+ });
+ this.connection.addHandler(
+ '/localizer', LocalizerOutput.getFullyQualifiedName(), (data) => {
+ this.handleLocalizerOutput(data);
+ });
+ this.connection.addHandler(
+ '/superstructure', SuperstructureStatus.getFullyQualifiedName(),
+ (data) => {
+ this.handleSuperstructureStatus(data);
+ });
+ });
+ }
+
+ private handleLocalizerDebug(data: Uint8Array): void {
+ const now = Date.now() / 1000.0;
+
+ const fbBuffer = new ByteBuffer(data);
+ this.localizerImageMatches.set(
+ now,
+ LocalizerVisualization.getRootAsLocalizerVisualization(
+ fbBuffer as unknown as flatbuffers.ByteBuffer));
+
+ const debug = this.localizerImageMatches.get(now);
+
+ if (debug.statistics()) {
+ this.imagesAcceptedCounter.innerHTML =
+ debug.statistics().totalAccepted().toString();
+ this.imagesRejectedCounter.innerHTML =
+ (debug.statistics().totalCandidates() -
+ debug.statistics().totalAccepted())
+ .toString();
+ if (debug.statistics().rejectionReasonCountLength() ==
+ this.rejectionReasonCells.length) {
+ for (let ii = 0; ii < debug.statistics().rejectionReasonCountLength();
+ ++ii) {
+ this.rejectionReasonCells[ii].innerHTML =
+ debug.statistics().rejectionReasonCount(ii).toString();
+ }
+ } else {
+ console.error('Unexpected number of rejection reasons in counter.');
+ }
+ this.imagesRejectedCounter.innerHTML =
+ (debug.statistics().totalCandidates() -
+ debug.statistics().totalAccepted())
+ .toString();
+ }
+ }
+
+ private handleLocalizerOutput(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ this.localizerOutput = LocalizerOutput.getRootAsLocalizerOutput(
+ fbBuffer as unknown as flatbuffers.ByteBuffer);
+ }
+
+ private handleSuperstructureStatus(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ this.superstructureStatus = SuperstructureStatus.getRootAsStatus(
+ fbBuffer as unknown as flatbuffers.ByteBuffer);
+ }
+
+ drawField(): void {
+ const ctx = this.canvas.getContext('2d');
+ ctx.drawImage(
+ this.fieldImage, 0, 0, this.fieldImage.width, this.fieldImage.height,
+ -FIELD_EDGE_X, -FIELD_SIDE_Y, FIELD_LENGTH, FIELD_WIDTH);
+ }
+
+ drawCamera(
+ x: number, y: number, theta: number, color: string = 'blue',
+ extendLines: boolean = true): 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);
+ if (extendLines) {
+ ctx.lineTo(100.0, 0);
+ 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, turret: number|null,
+ color: string = 'blue', dashed: boolean = false,
+ extendLines: boolean = true): 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);
+ if (extendLines) {
+ ctx.lineTo(1000.0, 0);
+ } else {
+ ctx.lineTo(ROBOT_LENGTH / 2.0, 0);
+ }
+ ctx.stroke();
+
+ if (turret !== null) {
+ ctx.save();
+ ctx.rotate(turret);
+ const turretRadius = ROBOT_WIDTH / 3.0;
+ ctx.strokeStyle = 'red';
+ // Draw circle for turret.
+ ctx.beginPath();
+ ctx.arc(0, 0, turretRadius, 0, 2.0 * Math.PI);
+ ctx.stroke();
+ // Draw line in circle to show forwards.
+ ctx.beginPath();
+ ctx.moveTo(0, 0);
+ if (extendLines) {
+ ctx.lineTo(1000.0, 0);
+ } else {
+ ctx.lineTo(turretRadius, 0);
+ }
+ ctx.stroke();
+ ctx.restore();
+ }
+ ctx.restore();
+ }
+
+ setZeroing(div: HTMLElement): void {
+ div.innerHTML = 'zeroing';
+ div.classList.remove('faulted');
+ div.classList.add('zeroing');
+ div.classList.remove('near');
+ }
+
+ setEstopped(div: HTMLElement): void {
+ div.innerHTML = 'estopped';
+ div.classList.add('faulted');
+ div.classList.remove('zeroing');
+ div.classList.remove('near');
+ }
+
+ setTargetValue(
+ div: HTMLElement, target: number, val: number, tolerance: number): void {
+ div.innerHTML = val.toFixed(4);
+ div.classList.remove('faulted');
+ div.classList.remove('zeroing');
+ if (Math.abs(target - val) < tolerance) {
+ div.classList.add('near');
+ } else {
+ 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');
+ }
+
+ draw(): void {
+ this.reset();
+ this.drawField();
+
+ // Draw the matches with debugging information from the localizer.
+ const now = Date.now() / 1000.0;
+ for (const [time, value] of this.localizerImageMatches) {
+ const age = now - time;
+ const kRemovalAge = 2.0;
+ if (age > kRemovalAge) {
+ this.localizerImageMatches.delete(time);
+ continue;
+ }
+ const ageAlpha = (kRemovalAge - age) / kRemovalAge
+ for (let i = 0; i < value.targetsLength(); i++) {
+ const imageDebug = value.targets(i);
+ const x = imageDebug.impliedRobotX();
+ const y = imageDebug.impliedRobotY();
+ const theta = imageDebug.impliedRobotTheta();
+ const cameraX = imageDebug.cameraX();
+ const cameraY = imageDebug.cameraY();
+ const cameraTheta = imageDebug.cameraTheta();
+ const accepted = imageDebug.accepted();
+ // Make camera readings fade over time.
+ const alpha = Math.round(255 * ageAlpha).toString(16).padStart(2, '0');
+ const dashed = false;
+ const acceptedRgb = accepted ? '#00FF00' : '#FF0000';
+ const acceptedRgba = acceptedRgb + alpha;
+ const cameraRgb = PI_COLORS[imageDebug.camera()];
+ const cameraRgba = cameraRgb + alpha;
+ this.drawRobot(x, y, theta, null, acceptedRgba, dashed, false);
+ this.drawCamera(cameraX, cameraY, cameraTheta, cameraRgba, 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());
+ }
+
+ if (this.superstructureStatus) {
+ this.shotDistance.innerHTML = this.superstructureStatus.aimer() ?
+ this.superstructureStatus.aimer().shotDistance().toFixed(2) :
+ 'NA';
+
+ if (!this.superstructureStatus.turret() ||
+ !this.superstructureStatus.turret().zeroed()) {
+ this.setZeroing(this.turret);
+ } else if (this.superstructureStatus.turret().estopped()) {
+ this.setEstopped(this.turret);
+ } else {
+ this.setTargetValue(
+ this.turret,
+ this.superstructureStatus.turret().unprofiledGoalPosition(),
+ this.superstructureStatus.turret().estimatorState().position(),
+ 1e-3);
+ }
+
+ if (!this.superstructureStatus.intakeBack() ||
+ !this.superstructureStatus.intakeBack().zeroed()) {
+ this.setZeroing(this.backIntake);
+ } else if (this.superstructureStatus.intakeBack().estopped()) {
+ this.setEstopped(this.backIntake);
+ } else {
+ this.setValue(
+ this.backIntake,
+ this.superstructureStatus.intakeBack()
+ .estimatorState()
+ .position());
+ }
+
+ if (!this.superstructureStatus.intakeFront() ||
+ !this.superstructureStatus.intakeFront().zeroed()) {
+ this.setZeroing(this.frontIntake);
+ } else if (this.superstructureStatus.intakeFront().estopped()) {
+ this.setEstopped(this.frontIntake);
+ } else {
+ this.setValue(
+ this.frontIntake,
+ this.superstructureStatus.intakeFront()
+ .estimatorState()
+ .position());
+ }
+ }
+
+
+ this.drawRobot(
+ this.localizerOutput.x(), this.localizerOutput.y(),
+ this.localizerOutput.theta(),
+ this.superstructureStatus ?
+ this.superstructureStatus.turret().position() :
+ null);
+ }
+
+ 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/y2022/www/field_main.ts b/y2022/www/field_main.ts
new file mode 100644
index 0000000..7e2e392
--- /dev/null
+++ b/y2022/www/field_main.ts
@@ -0,0 +1,12 @@
+import {Connection} from 'org_frc971/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/y2022/www/index.html b/y2022/www/index.html
index 70442f9..e4e185e 100644
--- a/y2022/www/index.html
+++ b/y2022/www/index.html
@@ -1,5 +1,6 @@
<html>
<body>
+ <a href="field.html">Field Visualization</a><br>
<a href="plotter.html">Plots</a>
</body>
</html>
