Add plotter for 2022 vision & deploy to pis
This will be most useful when run on the IMU node, but will also
function on the individual pis for getting an idea of what the camera is
doing.
Change-Id: I49f0d612b0b5e945f93e730c4b120be498cf76db
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2022/BUILD b/y2022/BUILD
index 98723af..1623ec9 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -42,6 +42,9 @@
data = [
":aos_config",
],
+ dirs = [
+ "//y2022/www:www_files",
+ ],
start_binaries = [
"//aos/events/logging:logger_main",
"//aos/network:message_bridge_client",
diff --git a/y2022/vision/BUILD b/y2022/vision/BUILD
index 9e93343..290a929 100644
--- a/y2022/vision/BUILD
+++ b/y2022/vision/BUILD
@@ -1,4 +1,5 @@
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library", "flatbuffer_py_library")
+load("@npm//@bazel/typescript:index.bzl", "ts_library")
flatbuffer_cc_library(
name = "calibration_fbs",
@@ -23,6 +24,18 @@
visibility = ["//visibility:public"],
)
+ts_library(
+ name = "vision_plotter",
+ srcs = ["vision_plotter.ts"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/network/www:aos_plotter",
+ "//aos/network/www:colors",
+ "//aos/network/www:proxy",
+ ],
+)
+
py_library(
name = "camera_definition",
srcs = [
diff --git a/y2022/vision/vision_plotter.ts b/y2022/vision/vision_plotter.ts
new file mode 100644
index 0000000..bc27170
--- /dev/null
+++ b/y2022/vision/vision_plotter.ts
@@ -0,0 +1,114 @@
+import {AosPlotter} from 'org_frc971/aos/network/www/aos_plotter';
+import * as proxy from 'org_frc971/aos/network/www/proxy';
+import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from 'org_frc971/aos/network/www/colors';
+
+import Connection = proxy.Connection;
+
+const TIME = AosPlotter.TIME;
+// magenta, yellow, cyan, orange
+const PI_COLORS = [[255, 0, 255], [255, 255, 0], [0, 255, 255], [255, 165, 0]];
+
+export function plotVision(conn: Connection, element: Element): void {
+ const aosPlotter = new AosPlotter(conn);
+
+ const targets = [];
+ for (const pi of ["pi1", "pi2", "pi3", "pi4"]) {
+ targets.push(aosPlotter.addMessageSource(
+ '/' + pi + '/camera', 'y2022.vision.TargetEstimate'));
+ }
+ const localizer = aosPlotter.addMessageSource(
+ '/localizer', 'frc971.controls.LocalizerVisualization');
+ const localizerOutput = aosPlotter.addMessageSource(
+ '/localizer', 'frc971.controls.LocalizerOutput');
+ const superstructureStatus = aosPlotter.addMessageSource(
+ '/superstructure', 'y2022.control_loops.superstructure.Status');
+
+ const rejectionPlot = aosPlotter.addPlot(element);
+ rejectionPlot.plot.getAxisLabels().setTitle("Rejection Reasons");
+ rejectionPlot.plot.getAxisLabels().setXLabel(TIME);
+ rejectionPlot.plot.getAxisLabels().setYLabel("[bool, enum]");
+
+ rejectionPlot.addMessageLine(localizer, ['targets[]', 'accepted'])
+ .setDrawLine(false)
+ .setColor(BLUE);
+ rejectionPlot.addMessageLine(localizer, ['targets[]', 'rejection_reason'])
+ .setDrawLine(false)
+ .setColor(RED);
+
+ const xPlot = aosPlotter.addPlot(element);
+ xPlot.plot.getAxisLabels().setTitle("X Position");
+ xPlot.plot.getAxisLabels().setXLabel(TIME);
+ xPlot.plot.getAxisLabels().setYLabel("[m]");
+
+ xPlot.addMessageLine(localizer, ['targets[]', 'implied_robot_x'])
+ .setDrawLine(false)
+ .setColor(RED);
+ xPlot.addMessageLine(localizerOutput, ['x'])
+ .setDrawLine(false)
+ .setColor(BLUE);
+
+ const yPlot = aosPlotter.addPlot(element);
+ yPlot.plot.getAxisLabels().setTitle("X Position");
+ yPlot.plot.getAxisLabels().setXLabel(TIME);
+ yPlot.plot.getAxisLabels().setYLabel("[m]");
+
+ yPlot.addMessageLine(localizer, ['targets[]', 'implied_robot_y'])
+ .setDrawLine(false)
+ .setColor(RED);
+ yPlot.addMessageLine(localizerOutput, ['y'])
+ .setDrawLine(false)
+ .setColor(BLUE);
+
+ const turretPlot = aosPlotter.addPlot(element);
+ turretPlot.plot.getAxisLabels().setTitle("Turret Position");
+ turretPlot.plot.getAxisLabels().setXLabel(TIME);
+ turretPlot.plot.getAxisLabels().setYLabel("[m]");
+
+ turretPlot.addMessageLine(localizer, ['targets[]', 'implied_turret_goal'])
+ .setDrawLine(false)
+ .setColor(RED);
+ turretPlot.addMessageLine(superstructureStatus, ['turret', 'position'])
+ .setPointSize(0.0)
+ .setColor(BLUE);
+ turretPlot
+ .addMessageLine(
+ superstructureStatus, ['aimer', 'turret_position'])
+ .setPointSize(0.0)
+ .setColor(GREEN);
+
+ const anglePlot = aosPlotter.addPlot(element);
+ anglePlot.plot.getAxisLabels().setTitle("TargetEstimate Angle");
+ anglePlot.plot.getAxisLabels().setXLabel(TIME);
+ anglePlot.plot.getAxisLabels().setYLabel("[rad]");
+
+ for (let ii = 0; ii < targets.length; ++ii) {
+ anglePlot.addMessageLine(targets[ii], ['angle_to_target'])
+ .setDrawLine(false)
+ .setColor(PI_COLORS[ii])
+ .setLabel('pi' + ii);
+ }
+
+ const distancePlot = aosPlotter.addPlot(element);
+ distancePlot.plot.getAxisLabels().setTitle("TargetEstimate Distance");
+ distancePlot.plot.getAxisLabels().setXLabel(TIME);
+ distancePlot.plot.getAxisLabels().setYLabel("[rad]");
+
+ for (let ii = 0; ii < targets.length; ++ii) {
+ distancePlot.addMessageLine(targets[ii], ['distance'])
+ .setDrawLine(false)
+ .setColor(PI_COLORS[ii])
+ .setLabel('pi' + ii);
+ }
+
+ const confidencePlot = aosPlotter.addPlot(element);
+ confidencePlot.plot.getAxisLabels().setTitle("TargetEstimate Confidence");
+ confidencePlot.plot.getAxisLabels().setXLabel(TIME);
+ confidencePlot.plot.getAxisLabels().setYLabel("[rad]");
+
+ for (let ii = 0; ii < targets.length; ++ii) {
+ confidencePlot.addMessageLine(targets[ii], ['confidence'])
+ .setDrawLine(false)
+ .setColor(PI_COLORS[ii])
+ .setLabel('pi' + ii);
+ }
+}