Only plot vision estimates with high confidences
This is all we care about because the localizer only uses these.
Also, display pi names as pi1-pi4 instead of pi0-pi3.
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I88c3e2da365404c4ef5da87c176254b842371971
diff --git a/y2022/vision/vision_plotter.ts b/y2022/vision/vision_plotter.ts
index bc27170..13dd9e5 100644
--- a/y2022/vision/vision_plotter.ts
+++ b/y2022/vision/vision_plotter.ts
@@ -1,20 +1,49 @@
+import {ByteBuffer} from 'flatbuffers';
import {AosPlotter} from 'org_frc971/aos/network/www/aos_plotter';
-import * as proxy from 'org_frc971/aos/network/www/proxy';
+import {MessageHandler, TimestampedMessage} from 'org_frc971/aos/network/www/aos_plotter';
import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from 'org_frc971/aos/network/www/colors';
+import {Connection} from 'org_frc971/aos/network/www/proxy';
+import {Table} from 'org_frc971/aos/network/www/reflection';
+import {Schema} from 'org_frc971/external/com_github_google_flatbuffers/reflection/reflection_generated';
+import {TargetEstimate} from 'org_frc971/y2022/vision/target_estimate_generated';
-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]];
+class VisionMessageHandler extends MessageHandler {
+ constructor(private readonly schema: Schema) {
+ super(schema);
+ }
+
+ private readScalar(table: Table, fieldName: string): number|BigInt|null {
+ return this.parser.readScalar(table, fieldName);
+ }
+
+ addMessage(data: Uint8Array, time: number): void {
+ const target = TargetEstimate.getRootAsTargetEstimate(new ByteBuffer(data));
+ // Copied from localizer.cc
+ const MIN_TARGET_ESTIMATE_CONFIDENCE = 0.75;
+ // Only add estimates with decent confidences - these are what the localizer
+ // uses
+ if (target.confidence() >= MIN_TARGET_ESTIMATE_CONFIDENCE) {
+ const table = Table.getNamedTable(
+ target.bb, this.schema, 'y2022.vision.TargetEstimate', target.bb_pos);
+ this.messages.push(new TimestampedMessage(table, time));
+ }
+ }
+}
+
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'));
+ for (const pi of ['pi1', 'pi2', 'pi3', 'pi4']) {
+ targets.push(aosPlotter.addRawMessageSource(
+ '/' + pi + '/camera', 'y2022.vision.TargetEstimate',
+ new VisionMessageHandler(
+ conn.getSchema('y2022.vision.TargetEstimate'))));
}
const localizer = aosPlotter.addMessageSource(
'/localizer', 'frc971.controls.LocalizerVisualization');
@@ -24,9 +53,9 @@
'/superstructure', 'y2022.control_loops.superstructure.Status');
const rejectionPlot = aosPlotter.addPlot(element);
- rejectionPlot.plot.getAxisLabels().setTitle("Rejection Reasons");
+ rejectionPlot.plot.getAxisLabels().setTitle('Rejection Reasons');
rejectionPlot.plot.getAxisLabels().setXLabel(TIME);
- rejectionPlot.plot.getAxisLabels().setYLabel("[bool, enum]");
+ rejectionPlot.plot.getAxisLabels().setYLabel('[bool, enum]');
rejectionPlot.addMessageLine(localizer, ['targets[]', 'accepted'])
.setDrawLine(false)
@@ -36,9 +65,9 @@
.setColor(RED);
const xPlot = aosPlotter.addPlot(element);
- xPlot.plot.getAxisLabels().setTitle("X Position");
+ xPlot.plot.getAxisLabels().setTitle('X Position');
xPlot.plot.getAxisLabels().setXLabel(TIME);
- xPlot.plot.getAxisLabels().setYLabel("[m]");
+ xPlot.plot.getAxisLabels().setYLabel('[m]');
xPlot.addMessageLine(localizer, ['targets[]', 'implied_robot_x'])
.setDrawLine(false)
@@ -48,9 +77,9 @@
.setColor(BLUE);
const yPlot = aosPlotter.addPlot(element);
- yPlot.plot.getAxisLabels().setTitle("X Position");
+ yPlot.plot.getAxisLabels().setTitle('Y Position');
yPlot.plot.getAxisLabels().setXLabel(TIME);
- yPlot.plot.getAxisLabels().setYLabel("[m]");
+ yPlot.plot.getAxisLabels().setYLabel('[m]');
yPlot.addMessageLine(localizer, ['targets[]', 'implied_robot_y'])
.setDrawLine(false)
@@ -60,9 +89,9 @@
.setColor(BLUE);
const turretPlot = aosPlotter.addPlot(element);
- turretPlot.plot.getAxisLabels().setTitle("Turret Position");
+ turretPlot.plot.getAxisLabels().setTitle('Turret Position');
turretPlot.plot.getAxisLabels().setXLabel(TIME);
- turretPlot.plot.getAxisLabels().setYLabel("[m]");
+ turretPlot.plot.getAxisLabels().setYLabel('[rad]');
turretPlot.addMessageLine(localizer, ['targets[]', 'implied_turret_goal'])
.setDrawLine(false)
@@ -70,45 +99,43 @@
turretPlot.addMessageLine(superstructureStatus, ['turret', 'position'])
.setPointSize(0.0)
.setColor(BLUE);
- turretPlot
- .addMessageLine(
- superstructureStatus, ['aimer', 'turret_position'])
+ 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().setTitle('TargetEstimate Angle');
anglePlot.plot.getAxisLabels().setXLabel(TIME);
- anglePlot.plot.getAxisLabels().setYLabel("[rad]");
+ 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);
+ .setLabel('pi' + (ii + 1));
}
const distancePlot = aosPlotter.addPlot(element);
- distancePlot.plot.getAxisLabels().setTitle("TargetEstimate Distance");
+ distancePlot.plot.getAxisLabels().setTitle('TargetEstimate Distance');
distancePlot.plot.getAxisLabels().setXLabel(TIME);
- distancePlot.plot.getAxisLabels().setYLabel("[rad]");
+ distancePlot.plot.getAxisLabels().setYLabel('[m]');
for (let ii = 0; ii < targets.length; ++ii) {
distancePlot.addMessageLine(targets[ii], ['distance'])
.setDrawLine(false)
.setColor(PI_COLORS[ii])
- .setLabel('pi' + ii);
+ .setLabel('pi' + (ii + 1));
}
const confidencePlot = aosPlotter.addPlot(element);
- confidencePlot.plot.getAxisLabels().setTitle("TargetEstimate Confidence");
+ confidencePlot.plot.getAxisLabels().setTitle('TargetEstimate Confidence');
confidencePlot.plot.getAxisLabels().setXLabel(TIME);
- confidencePlot.plot.getAxisLabels().setYLabel("[rad]");
+ confidencePlot.plot.getAxisLabels().setYLabel('[0-1]');
for (let ii = 0; ii < targets.length; ++ii) {
confidencePlot.addMessageLine(targets[ii], ['confidence'])
.setDrawLine(false)
.setColor(PI_COLORS[ii])
- .setLabel('pi' + ii);
+ .setLabel('pi' + (ii + 1));
}
}