Merge "Add plotter for 2022 vision & deploy to pis"
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);
+ }
+}