commit d2df965d557d46de4c2e0a6bb5fc715cd3744569
author Alex Perry <alex.perry96@gmail.com> Sat Mar 23 22:53:04 2019 -0700
committer Alex Perry <alex.perry96@gmail.com> Sat Mar 23 22:53:04 2019 -0700
tree fc908f3f7f8c99d101a1351d42e95c5e6e837c50
parent 554cec00193fad93906e7a639936bf857da871fd

Add locked target info to position visualizer.

Change-Id: I31ece9023dccd2126eb50ea6601b3a70d850906f

y2019/vision/server/server.cc

diff --git a/y2019/vision/server/server.cc b/y2019/vision/server/server.cc
index 1ed6ac9..0253a4c 100644
--- a/y2019/vision/server/server.cc
+++ b/y2019/vision/server/server.cc
@@ -159,6 +159,13 @@
       stream << "\"x\": " << drivetrain_status->x << ",";
       stream << "\"y\": " << drivetrain_status->y << ",";
       stream << "\"theta\": " << drivetrain_status->theta;
+      if (drivetrain_status->line_follow_logging.frozen) {
+        stream << "\"target\": {";
+        stream << "\"x\": " << drivetrain_status->line_follow_logging.x << ",";
+        stream << "\"y\": " << drivetrain_status->line_follow_logging.y << ",";
+        stream << "\"theta\": " << drivetrain_status->line_follow_logging.theta;
+        stream << "}\n";
+      }
       stream << "}\n";
 
       stream << "}";

y2019/vision/server/www/field.ts

diff --git a/y2019/vision/server/www/field.ts b/y2019/vision/server/www/field.ts
index 5b43c36..8cb1282 100644
--- a/y2019/vision/server/www/field.ts
+++ b/y2019/vision/server/www/field.ts
@@ -57,64 +57,38 @@
 }
 
 function drawHP(ctx : CanvasRenderingContext2D) : void {
-  ctx.save();
-  ctx.translate(0, HP_Y)
-  ctx.rotate(HP_THETA);
-  drawTarget(ctx);
-  ctx.restore();
+  drawTarget(ctx, 0, HP_Y, HP_THETA);
 }
 
 function drawRocket(ctx : CanvasRenderingContext2D) : void {
-  ctx.save();
-  ctx.translate(ROCKET_PORT_X, ROCKET_PORT_Y)
-  ctx.rotate(ROCKET_PORT_THETA);
-  drawTarget(ctx);
-  ctx.restore();
+  drawTarget(ctx, ROCKET_PORT_X, ROCKET_PORT_Y, ROCKET_PORT_THETA);
 
-  ctx.save();
-  ctx.translate(ROCKET_NEAR_X, ROCKET_HATCH_Y)
-  ctx.rotate(ROCKET_NEAR_THETA);
-  drawTarget(ctx);
-  ctx.restore();
+  drawTarget(ctx, ROCKET_NEAR_X, ROCKET_HATCH_Y, ROCKET_NEAR_THETA);
 
-  ctx.save();
-  ctx.translate(ROCKET_FAR_X, ROCKET_HATCH_Y)
-  ctx.rotate(ROCKET_FAR_THETA);
-  drawTarget(ctx);
-  ctx.restore();
+  drawTarget(ctx, ROCKET_FAR_X, ROCKET_HATCH_Y, ROCKET_FAR_THETA);
 }
 
 function drawHalfCargo(ctx : CanvasRenderingContext2D) : void {
-  ctx.save();
-  ctx.translate(FAR_CARGO_X, SIDE_CARGO_Y)
-  ctx.rotate(SIDE_CARGO_THETA);
-  drawTarget(ctx);
-  ctx.restore();
+  drawTarget(ctx, FAR_CARGO_X, SIDE_CARGO_Y, SIDE_CARGO_THETA);
 
-  ctx.save();
-  ctx.translate(MID_CARGO_X, SIDE_CARGO_Y)
-  ctx.rotate(SIDE_CARGO_THETA);
-  drawTarget(ctx);
-  ctx.restore();
+  drawTarget(ctx, MID_CARGO_X, SIDE_CARGO_Y, SIDE_CARGO_THETA);
 
-  ctx.save();
-  ctx.translate(NEAR_CARGO_X, SIDE_CARGO_Y)
-  ctx.rotate(SIDE_CARGO_THETA);
-  drawTarget(ctx);
-  ctx.restore();
+  drawTarget(ctx, NEAR_CARGO_X, SIDE_CARGO_Y, SIDE_CARGO_THETA);
 
-  ctx.save();
-  ctx.translate(FACE_CARGO_X, FACE_CARGO_Y)
-  ctx.rotate(FACE_CARGO_THETA);
-  drawTarget(ctx);
-  ctx.restore();
+  drawTarget(ctx, FACE_CARGO_X, FACE_CARGO_Y, FACE_CARGO_THETA);
 }
 
-function drawTarget(ctx : CanvasRenderingContext2D) : void {
+export function drawTarget(ctx : CanvasRenderingContext2D, x: number, y: number, theta: number) : void {
+  ctx.save();
+  ctx.translate(x, y);
+  ctx.rotate(theta);
+
+  ctx.beginPath();
   ctx.moveTo(0, -0.15);
   ctx.lineTo(0, 0.15);
   ctx.moveTo(0, 0);
   ctx.lineTo(-0.15, 0);
-  ctx.closePath();
   ctx.stroke();
+
+  ctx.restore();
 }

y2019/vision/server/www/main.ts

diff --git a/y2019/vision/server/www/main.ts b/y2019/vision/server/www/main.ts
index 0806e16..1b3bbf0 100644
--- a/y2019/vision/server/www/main.ts
+++ b/y2019/vision/server/www/main.ts
@@ -1,5 +1,5 @@
 import {FT_TO_M, FIELD_WIDTH} from './constants';
-import {drawField} from './field';
+import {drawField, drawTarget} from './field';
 import {drawRobot} from './robot';
 
 const FIELD_WIDTH = 27 * FT_TO_M;
@@ -13,6 +13,11 @@
   private y = 0;
   private theta = 0;
 
+  private drawLocked = false;
+  private lockedX = 0;
+  private lockedY = 0;
+  private lockedTheta = 0;
+
   constructor() {
     const canvas = <HTMLCanvasElement>document.getElementById('field');
     const ctx = canvas.getContext('2d');
@@ -26,6 +31,13 @@
       this.x = j.robot.x;
       this.y = j.robot.y;
       this.theta = j.robot.theta;
+
+      if(j.target) {
+        this.drawLocked = true;
+        this.lockedX = j.target.x;
+        this.lockedY = j.target.y;
+        this.lockedTheta = j.target.theta;
+      }
     });
     socket.addEventListener('message', (event) => {
       reader.readAsText(event.data);
@@ -46,8 +58,6 @@
     const M_TO_PX = size / FIELD_WIDTH
     ctx.scale(M_TO_PX, M_TO_PX);
     ctx.lineWidth = 1 / M_TO_PX;
-
-    ctx.beginPath();
   }
 
   draw(ctx : CanvasRenderingContext2D) : void {
@@ -55,6 +65,12 @@
 
     drawField(ctx);
     drawRobot(ctx, this.x, this.y, this.theta);
+    if (this.drawLocked) {
+      ctx.save();
+      ctx.strokeStyle = 'red';
+      drawTarget(ctx, this.lockedX, this.lockedY, this.lockedTheta);
+      ctx.restore();
+    }
     window.requestAnimationFrame(() => this.draw(ctx));
   }
 }