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/frc971/analysis/BUILD b/frc971/analysis/BUILD
index 20f1a01..cd6f729 100644
--- a/frc971/analysis/BUILD
+++ b/frc971/analysis/BUILD
@@ -57,6 +57,7 @@
"//y2022/control_loops/superstructure:intake_plotter",
"//y2022/control_loops/superstructure:turret_plotter",
"//y2022/localizer:localizer_plotter",
+ "//y2022/vision:vision_plotter",
],
)
diff --git a/frc971/analysis/plot_index.ts b/frc971/analysis/plot_index.ts
index bbaa0ce..b2a3efa 100644
--- a/frc971/analysis/plot_index.ts
+++ b/frc971/analysis/plot_index.ts
@@ -50,6 +50,8 @@
'org_frc971/y2022/control_loops/superstructure/climber_plotter'
import {plotLocalizer as plot2022Localizer} from
'org_frc971/y2022/localizer/localizer_plotter'
+import {plotVision as plot2022Vision} from
+ 'org_frc971/y2022/vision/vision_plotter'
import {plotDemo} from 'org_frc971/aos/network/www/demo_plot';
import {plotData} from 'org_frc971/frc971/analysis/plot_data_utils';
@@ -116,6 +118,7 @@
['2020 Turret', new PlotState(plotDiv, plot2020Turret)],
['2020 Localizer', new PlotState(plotDiv, plot2020Localizer)],
['2022 Localizer', new PlotState(plotDiv, plot2022Localizer)],
+ ['2022 Vision', new PlotState(plotDiv, plot2022Vision)],
['2022 Catapult', new PlotState(plotDiv, plot2022Catapult)],
['2022 Intake Front', new PlotState(plotDiv, plot2022IntakeFront)],
['2022 Intake Back', new PlotState(plotDiv, plot2022IntakeBack)],
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);
+ }
+}