Adding target_receiver.cc and coms protos.
Change-Id: I5a3a9e681b2521e4d1fb379556bf276fea0730a2
diff --git a/y2017/vision/BUILD b/y2017/vision/BUILD
new file mode 100644
index 0000000..c9794fe
--- /dev/null
+++ b/y2017/vision/BUILD
@@ -0,0 +1,36 @@
+load('/aos/build/queues', 'queue_library')
+load('/tools/build_rules/gtk_dependent', 'gtk_dependent_cc_binary', 'gtk_dependent_cc_library')
+load('/tools/build_rules/protobuf', 'proto_cc_library')
+
+package(default_visibility = ["//visibility:public"])
+
+queue_library(
+ name = 'vision_queue',
+ visibility = ['//visibility:public'],
+ srcs = [
+ 'vision.q',
+ ],
+)
+
+proto_cc_library(
+ name = 'vision_data',
+ src = 'vision_data.proto',
+)
+
+cc_binary(
+ name = 'target_receiver',
+ srcs = [
+ 'target_receiver.cc',
+ ],
+ visibility = ['//visibility:public'],
+ deps = [
+ '//aos/common/logging',
+ '//aos/common/logging:queue_logging',
+ '//aos/linux_code:init',
+ '//aos/common:time',
+ '//aos/vision/events:udp',
+ ':vision_queue',
+ ':vision_data',
+ '//aos/common:mutex',
+ ],
+)
diff --git a/y2017/vision/target_receiver.cc b/y2017/vision/target_receiver.cc
new file mode 100644
index 0000000..8cabe76
--- /dev/null
+++ b/y2017/vision/target_receiver.cc
@@ -0,0 +1,64 @@
+#include <netdb.h>
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
+#include "aos/common/time.h"
+#include "aos/linux_code/init.h"
+#include "aos/vision/events/udp.h"
+#include "y2017/vision/vision.q.h"
+#include "y2017/vision/vision_data.pb.h"
+
+using aos::monotonic_clock;
+
+namespace y2017 {
+namespace vision {
+
+void ComputeDistanceAngle(const Target &target, double *distance,
+ double *angle) {
+ // TODO: fix this.
+ *distance = target.y();
+ *angle = target.x();
+}
+
+} // namespace vision
+} // namespace y2017
+
+int main() {
+ using namespace y2017::vision;
+ ::aos::events::RXUdpSocket recv(8080);
+ char raw_data[65507];
+
+ while (true) {
+ // TODO(austin): Don't malloc.
+ VisionData target;
+ int size = recv.Recv(raw_data, sizeof(raw_data));
+ monotonic_clock::time_point now = monotonic_clock::now();
+ auto target_time = now -
+ std::chrono::nanoseconds(target.send_timestamp() -
+ target.image_timestamp()) +
+ // It takes a bit to shoot a frame. Push the frame
+ // further back in time.
+ std::chrono::milliseconds(10);
+
+ if (!target.ParseFromArray(raw_data, size)) {
+ continue;
+ }
+
+ auto new_vision_status = vision_status.MakeMessage();
+ new_vision_status->image_valid = target.has_target();
+ if (new_vision_status->image_valid) {
+ new_vision_status->target_time =
+ std::chrono::duration_cast<std::chrono::nanoseconds>(
+ target_time.time_since_epoch())
+ .count();
+
+ ComputeDistanceAngle(target.target(), &new_vision_status->distance,
+ &new_vision_status->angle);
+ }
+
+ LOG_STRUCT(DEBUG, "vision", *new_vision_status);
+ if (!new_vision_status.Send()) {
+ LOG(ERROR, "Failed to send vision information\n");
+ }
+ }
+}
diff --git a/y2017/vision/vision.q b/y2017/vision/vision.q
new file mode 100644
index 0000000..b3eeee8
--- /dev/null
+++ b/y2017/vision/vision.q
@@ -0,0 +1,14 @@
+package y2017.vision;
+
+message VisionStatus {
+ bool image_valid;
+
+ // Distance to the target in meters.
+ double distance;
+ // The angle in radians of the bottom of the target.
+ double angle;
+
+ // Capture time of the angle using the clock behind monotonic_clock::now().
+ int64_t target_time;
+};
+queue VisionStatus vision_status;
diff --git a/y2017/vision/vision_data.proto b/y2017/vision/vision_data.proto
new file mode 100644
index 0000000..3190686
--- /dev/null
+++ b/y2017/vision/vision_data.proto
@@ -0,0 +1,19 @@
+syntax = "proto2";
+
+package y2017.vision;
+
+// Represents a target found by the vision processing code.
+// X is an estimate of the center of the target.
+// Y is an estimate of the top of the bottom retroreflective tape.
+message Target {
+ optional double x = 1;
+ optional double y = 2;
+}
+
+// Represents the best target in the image if there is such a target
+// along with timing information.
+message VisionData {
+ optional int64 image_timestamp = 1;
+ optional int64 send_timestamp = 2;
+ optional Target target = 3;
+}