Add time-since-last-target to server
And draw little cameras on the robot that turn green when we got a
target from that camera in the last 0.25sec.
Change-Id: Icd6107163fd3f5145fcba69ac6ebb2700f602f8d
diff --git a/y2019/vision/server/server.cc b/y2019/vision/server/server.cc
index 54eb399..235054b 100644
--- a/y2019/vision/server/server.cc
+++ b/y2019/vision/server/server.cc
@@ -125,6 +125,8 @@
auto &drivetrain_status = frc971::control_loops::drivetrain_queue.status;
std::array<LocalCameraFrame, 5> latest_frames;
+ std::array<aos::monotonic_clock::time_point, 5> last_target_time;
+ last_target_time.fill(aos::monotonic_clock::epoch());
aos::monotonic_clock::time_point last_send_time = aos::monotonic_clock::now();
while (true) {
@@ -134,6 +136,7 @@
// Try again if we don't have any drivetrain statuses.
continue;
}
+ const auto now = aos::monotonic_clock::now();
{
const auto &new_frame = *camera_frames;
@@ -142,6 +145,9 @@
aos::monotonic_clock::time_point(
std::chrono::nanoseconds(new_frame.timestamp));
latest_frames[new_frame.camera].targets.clear();
+ if (new_frame.num_targets > 0) {
+ last_target_time[new_frame.camera] = now;
+ }
for (int target = 0; target < new_frame.num_targets; ++target) {
latest_frames[new_frame.camera].targets.emplace_back();
// TODO: Do something useful.
@@ -149,8 +155,9 @@
}
}
- const auto now = aos::monotonic_clock::now();
if (now > last_send_time + std::chrono::milliseconds(100)) {
+ // TODO(james): Use protobuf or the such to generate JSON rather than
+ // doing so manually.
last_send_time = now;
std::ostringstream stream;
stream << "{\n";
@@ -171,6 +178,18 @@
<< drivetrain_status->line_follow_logging.have_target;
stream << "} ";
+ stream << ", \"last_target_age\": [";
+ bool first = true;
+ for (const auto t : last_target_time) {
+ if (!first) {
+ stream << ",";
+ }
+ first = false;
+ stream << ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+ now - t).count();
+ }
+ stream << "]";
+
stream << "}";
server->execute(
std::make_shared<UpdateData>(websocket_handler, stream.str()));
diff --git a/y2019/vision/server/www/BUILD b/y2019/vision/server/www/BUILD
index 937b495..428dd41 100644
--- a/y2019/vision/server/www/BUILD
+++ b/y2019/vision/server/www/BUILD
@@ -6,7 +6,7 @@
filegroup(
name = "files",
srcs = glob([
- "**/*",
+ "**/*.html",
]),
)
@@ -14,7 +14,20 @@
name = "visualizer",
srcs = glob([
"*.ts",
- ]),
+ ]) + ["camera_constants.ts"],
+)
+
+cc_binary(
+ name = "generate_camera",
+ srcs = ["generate_camera.cc"],
+ deps = ["//y2019:constants"],
+)
+
+genrule(
+ name = "gen_cam_ts",
+ outs = ["camera_constants.ts"],
+ cmd = "$(location :generate_camera) $@",
+ tools = [":generate_camera"],
)
rollup_bundle(
diff --git a/y2019/vision/server/www/generate_camera.cc b/y2019/vision/server/www/generate_camera.cc
new file mode 100644
index 0000000..8e516c3
--- /dev/null
+++ b/y2019/vision/server/www/generate_camera.cc
@@ -0,0 +1,33 @@
+#include "y2019/constants.h"
+#include "y2019/vision/constants.h"
+
+#include <fstream>
+#include <iostream>
+
+namespace y2019 {
+namespace vision {
+void DumpPose(std::basic_ostream<char> *o, const vision::CameraGeometry &pose) {
+ *o << "{x: " << pose.location[0] << ", y: " << pose.location[1]
+ << ", theta: " << pose.heading << "}";
+}
+void DumpTypescriptConstants(const char *fname) {
+ ::std::ofstream out_file(fname);
+ out_file << "export const CAMERA_POSES = [\n";
+ for (size_t ii = 0; ii < constants::Values::kNumCameras; ++ii) {
+ out_file << " ";
+ DumpPose(&out_file,
+ GetCamera(CameraSerialNumbers(CompBotTeensyId())[ii])->geometry);
+ out_file << ",\n";
+ }
+ out_file << "];\n";
+}
+} // namespace constants
+} // namespace y2019
+
+int main(int argc, char *argv[]) {
+ if (argc != 2) {
+ ::std::cout << "Must provide a filename for output as an argument\n";
+ return 1;
+ }
+ ::y2019::vision::DumpTypescriptConstants(argv[1]);
+}
diff --git a/y2019/vision/server/www/main.ts b/y2019/vision/server/www/main.ts
index 17b7139..a9a57bd 100644
--- a/y2019/vision/server/www/main.ts
+++ b/y2019/vision/server/www/main.ts
@@ -16,6 +16,7 @@
private targetX = 0;
private targetY = 0;
private targetTheta = 0;
+ private cameraColors = ['red', 'red', 'red', 'red', 'red'];
constructor() {
const canvas = <HTMLCanvasElement>document.getElementById('field');
@@ -37,6 +38,13 @@
this.targetY = j.target.y;
this.targetTheta = j.target.theta;
}
+ for (let ii of [0, 1, 2, 3, 4]) {
+ if (j.last_target_age[ii] > 0.25) {
+ this.cameraColors[ii] = 'red';
+ } else {
+ this.cameraColors[ii] = 'green';
+ }
+ }
});
socket.addEventListener('message', (event) => {
reader.readAsText(event.data);
@@ -63,7 +71,7 @@
this.reset(ctx);
drawField(ctx);
- drawRobot(ctx, this.x, this.y, this.theta);
+ drawRobot(ctx, this.x, this.y, this.theta, this.cameraColors);
ctx.save();
ctx.lineWidth = 2.0 * ctx.lineWidth;
if (this.targetLocked) {
diff --git a/y2019/vision/server/www/robot.ts b/y2019/vision/server/www/robot.ts
index bc1f47f..489112c 100644
--- a/y2019/vision/server/www/robot.ts
+++ b/y2019/vision/server/www/robot.ts
@@ -1,9 +1,22 @@
import {IN_TO_M, FT_TO_M} from './constants';
+import {CAMERA_POSES} from './camera_constants';
const ROBOT_WIDTH = 25 * IN_TO_M;
const ROBOT_LENGTH = 31 * IN_TO_M;
+const CAMERA_SCALE = 0.3;
-export function drawRobot(ctx : CanvasRenderingContext2D, x : number, y : number, theta : number) : void {
+function drawCamera(ctx : canvasRenderingContext2d, pose : {x : number, y : number, theta : number}) : void {
+ ctx.beginPath();
+ ctx.moveTo(pose.x, pose.y);
+ ctx.lineTo(pose.x + CAMERA_SCALE * Math.cos(pose.theta + Math.PI / 4.0),
+ pose.y + CAMERA_SCALE * Math.sin(pose.theta + Math.PI / 4.0));
+ ctx.lineTo(pose.x + CAMERA_SCALE * Math.cos(pose.theta - Math.PI / 4.0),
+ pose.y + CAMERA_SCALE * Math.sin(pose.theta - Math.PI / 4.0));
+ ctx.closePath();
+ ctx.stroke();
+}
+
+export function drawRobot(ctx : CanvasRenderingContext2D, x : number, y : number, theta : number, camera_colors : string[]) : void {
ctx.save();
ctx.translate(x, y);
ctx.rotate(theta);
@@ -11,11 +24,18 @@
ctx.fillStyle = 'blue';
ctx.fillRect(-ROBOT_LENGTH / 2, -ROBOT_WIDTH / 2, ROBOT_LENGTH, ROBOT_WIDTH);
+ ctx.beginPath();
+ ctx.strokeStyle = 'black';
ctx.moveTo(ROBOT_LENGTH / 2, -ROBOT_WIDTH/2);
ctx.lineTo(ROBOT_LENGTH / 2 + 0.1, 0);
ctx.lineTo(ROBOT_LENGTH / 2, ROBOT_WIDTH/2);
ctx.closePath();
ctx.stroke();
+ ctx.lineWidth = 3.0 * ctx.lineWidth;
+ for (let ii of [0, 1, 2, 3, 4]) {
+ ctx.strokeStyle = camera_colors[ii];
+ drawCamera(ctx, CAMERA_POSES[ii]);
+ }
ctx.restore();
}