Finish roboRIO-side vision code

It all builds with --cpu=roborio now, and successfully starts listening
for data when run on my laptop. I think it should work...

Change-Id: I5cb107f61ad7a63b2e0926a92c3238893719c5f5
diff --git a/y2016/constants.cc b/y2016/constants.cc
index 97d253a..cb0f3fc 100644
--- a/y2016/constants.cc
+++ b/y2016/constants.cc
@@ -70,6 +70,8 @@
            {Values::kZeroingSampleSize, Values::kWristEncoderIndexDifference,
             0.0, 0.3},
           },
+
+          "practice",
       };
       break;
 
@@ -101,6 +103,8 @@
            {Values::kZeroingSampleSize, Values::kWristEncoderIndexDifference,
             -0.06138835 * M_PI / 180.0 - 0.260542, 0.3},
           },
+
+          "competition",
       };
       break;
     case kPracticeTeamNumber:
@@ -130,6 +134,8 @@
            {Values::kZeroingSampleSize, Values::kWristEncoderIndexDifference,
             -0.622423, 0.3},
           },
+
+          "practice",
       };
       break;
     default:
diff --git a/y2016/constants.h b/y2016/constants.h
index cf75ae4..e5bbc06 100644
--- a/y2016/constants.h
+++ b/y2016/constants.h
@@ -99,6 +99,8 @@
     ZeroingConstants zeroing;
   };
   Wrist wrist;
+
+  const char *vision_name;
 };
 
 // Creates (once) a Values instance for ::aos::network::GetTeamNumber() and
diff --git a/y2016/vision/BUILD b/y2016/vision/BUILD
new file mode 100644
index 0000000..66245e9
--- /dev/null
+++ b/y2016/vision/BUILD
@@ -0,0 +1,94 @@
+load('/tools/build_rules/protobuf', 'proto_cc_library')
+load('/aos/build/queues', 'queue_library')
+
+queue_library(
+  name = 'vision_queue',
+  srcs = [
+    'vision.q',
+  ],
+)
+
+proto_cc_library(
+  name = 'vision_data',
+  src = 'vision_data.proto',
+)
+
+proto_cc_library(
+  name = 'calibration',
+  src = 'calibration.proto',
+)
+
+_calibration_values_embedded_before = '''
+#include "y2016/vision/stereo_geometry.h"
+
+#include <string>
+
+#include "third_party/protobuf/src/google/protobuf/text_format.h"
+
+namespace y2016 {
+namespace vision {
+namespace {
+
+const ::std::string kEmbeddedContents ='''
+
+_calibration_values_embedded_after = ''';
+
+}  // namespace
+
+CalibrationFile EmbeddedCalibrationFile() {
+  CalibrationFile r;
+  ::google::protobuf::TextFormat::ParseFromString(kEmbeddedContents, &r);
+  return r;
+}
+
+}  // namespace vision
+}  // namespace y2016'''
+
+genrule(
+  name = 'embed_calibration_pb',
+  srcs = [
+    'calibration.pb',
+  ],
+  outs = [
+    'stereo_geometry_embedded.cc',
+  ],
+  cmd = '\n'.join([
+    'echo \'%s\' > $@' % _calibration_values_embedded_before,
+    'cat $(location :calibration.pb) | sed \'s/"/\\\\"/g ; s/^/"/g ; s/$$/\\\\n"/g\' >> $@',
+    'echo \'%s\' >> $@' % _calibration_values_embedded_after,
+  ]),
+)
+
+cc_library(
+  name = 'stereo_geometry',
+  hdrs = [
+    'stereo_geometry.h',
+  ],
+  srcs = [
+    'stereo_geometry.cc',
+    'stereo_geometry_embedded.cc',
+  ],
+  deps = [
+    ':calibration',
+    '//aos/common/logging',
+    '//aos/vision/math:vector',
+  ],
+)
+
+cc_binary(
+  name = 'target_receiver',
+  srcs = [
+    'target_receiver.cc',
+  ],
+  deps = [
+    '//aos/common/logging',
+    '//aos/common/logging:queue_logging',
+    '//aos/linux_code:init',
+    '//aos/common:time',
+    '//aos/vision/events:udp',
+    ':vision_queue',
+    ':vision_data',
+    ':stereo_geometry',
+    '//y2016:constants',
+  ],
+)
diff --git a/y2016/vision/stereo_geometry.cc b/y2016/vision/stereo_geometry.cc
new file mode 100644
index 0000000..b58cb82
--- /dev/null
+++ b/y2016/vision/stereo_geometry.cc
@@ -0,0 +1,18 @@
+#include "y2016/vision/stereo_geometry.h"
+
+namespace y2016 {
+namespace vision {
+
+Calibration FindCalibrationForRobotOrDie(
+    const ::std::string &robot_name, const CalibrationFile &calibration_file) {
+  for (const RobotCalibration &calibration : calibration_file.calibration()) {
+    if (calibration.robot() == robot_name) {
+      return calibration.calibration();
+    }
+  }
+  LOG(FATAL, "no calibration for %s found in %s\n", robot_name.c_str(),
+      calibration_file.ShortDebugString().c_str());
+}
+
+}  // namespace vision
+}  // namespace y2016
diff --git a/y2016/vision/stereo_geometry.h b/y2016/vision/stereo_geometry.h
new file mode 100644
index 0000000..93f3892
--- /dev/null
+++ b/y2016/vision/stereo_geometry.h
@@ -0,0 +1,68 @@
+#ifndef Y2016_VISION_STEREO_GEOMETRY_H_
+#define Y2016_VISION_STEREO_GEOMETRY_H_
+
+#include <string>
+
+#include "aos/vision/math/vector.h"
+#include "aos/common/logging/logging.h"
+
+#include "y2016/vision/calibration.pb.h"
+
+namespace y2016 {
+namespace vision {
+
+// Returns the contents of the calibration file which are embedded into the
+// code.
+CalibrationFile EmbeddedCalibrationFile();
+
+// Returns the embedded Calibration for the given robot_name or LOG(FATAL)s.
+Calibration FindCalibrationForRobotOrDie(
+    const ::std::string &robot_name, const CalibrationFile &calibration_file);
+
+class StereoGeometry {
+ public:
+  StereoGeometry(const ::std::string &robot_name)
+      : calibration_(FindCalibrationForRobotOrDie(robot_name,
+                                                  EmbeddedCalibrationFile())) {}
+
+  // Returns the forward world distance in meters corresponding to the given
+  // distance in pixels between the two cameras.
+  double PixelToWorldDistance(double pixel_distance) {
+    return (calibration_.center_center_dist() *
+            calibration_.camera_image_width() * calibration_.focal_length() /
+            (pixel_distance + calibration_.center_center_skew())) +
+           calibration_.measure_camera_offset();
+  }
+
+  // Converts locations on both cameras to the distance between the locations
+  // and both angles.
+  // Returns (via pointers) the distance to the target in meters, the horizontal
+  // angle to the target in radians, and the vertical angle to the target in
+  // radians.
+  // TODO(Brian): Figure out which way is positive on the angles.
+  void Process(const ::aos::vision::Vector<2> &center0,
+               const ::aos::vision::Vector<2> &center1, double *distance,
+               double *horizontal_angle, double *vertical_angle) {
+    *distance = PixelToWorldDistance(center1.x() - center0.x());
+    const double average_x = (center0.x() + center1.x()) / 2.0 -
+                             calibration_.camera_image_width() / 2.0;
+    const double average_y = (center0.y() + center1.y()) / 2.0 -
+                             calibration_.camera_image_height() / 2.0;
+
+    *horizontal_angle =
+        std::atan2(average_x, calibration_.camera_image_width() *
+                                  calibration_.focal_length());
+    *vertical_angle = std::atan2(average_y, calibration_.camera_image_width() *
+                                                calibration_.focal_length());
+  }
+
+  const Calibration &calibration() const { return calibration_; }
+
+ private:
+  Calibration calibration_;
+};
+
+}  // namespace vision
+}  // namespace y2016
+
+#endif  // Y2016_VISION_STEREO_GEOMETRY_H_
diff --git a/y2016/vision/target_receiver.cc b/y2016/vision/target_receiver.cc
new file mode 100644
index 0000000..d023ac8
--- /dev/null
+++ b/y2016/vision/target_receiver.cc
@@ -0,0 +1,97 @@
+#include <stdlib.h>
+#include <netdb.h>
+#include <unistd.h>
+
+#include <vector>
+#include <memory>
+
+#include "aos/linux_code/init.h"
+#include "aos/common/time.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
+#include "aos/vision/events/udp.h"
+
+#include "y2016/vision/vision.q.h"
+#include "y2016/vision/vision_data.pb.h"
+#include "y2016/vision/stereo_geometry.h"
+#include "y2016/constants.h"
+
+namespace y2016 {
+namespace vision {
+
+void Main() {
+  StereoGeometry stereo(constants::GetValues().vision_name);
+  LOG(INFO, "calibration: %s\n",
+      stereo.calibration().ShortDebugString().c_str());
+  VisionData left_target;
+  VisionData right_target;
+  // TODO(Brian): Having two sockets here like this doesn't work. They'll get
+  // out of sync.
+  ::aos::vision::RXUdpSocket left(8080);
+  ::aos::vision::RXUdpSocket right(8081);
+  char rawData[65507];
+  while (true) {
+    bool got_left = false;
+    bool got_right = false;
+    int recv = left.Recv(rawData, 65507);
+    if (left_target.ParseFromArray(rawData, recv)) {
+      LOG(DEBUG, "left_target: %s\n", left_target.ShortDebugString().c_str());
+      if (left_target.target_size() > 0) {
+        got_left = true;
+      }
+    } else {
+      LOG(WARNING, "left parse error\n");
+    }
+
+    recv = right.Recv(rawData, 65507);
+    if (right_target.ParseFromArray(rawData, recv)) {
+      LOG(DEBUG, "right_target: %s\n", right_target.ShortDebugString().c_str());
+      if (right_target.target_size() > 0) {
+        got_right = true;
+      }
+    } else {
+      LOG(WARNING, "right parse error\n");
+    }
+
+    if (got_left && got_right) {
+      const ::aos::vision::Vector<2> center0(
+          (left_target.target(0).left_corner_x() +
+           left_target.target(0).right_corner_x()) /
+              2.0,
+          (left_target.target(0).left_corner_y() +
+           left_target.target(0).right_corner_y()) /
+              2.0);
+      const ::aos::vision::Vector<2> center1(
+          (right_target.target(0).left_corner_x() +
+           right_target.target(0).right_corner_x()) /
+              2.0,
+          (right_target.target(0).left_corner_y() +
+           right_target.target(0).right_corner_y()) /
+              2.0);
+      double distance, horizontal_angle, vertical_angle;
+      stereo.Process(center0, center1, &distance, &horizontal_angle,
+                     &vertical_angle);
+      auto new_vision_status = vision_status.MakeMessage();
+      new_vision_status->left_image_timestamp = left_target.image_timestamp();
+      new_vision_status->right_image_timestamp = right_target.image_timestamp();
+      new_vision_status->left_send_timestamp = left_target.send_timestamp();
+      new_vision_status->right_send_timestamp = right_target.send_timestamp();
+      new_vision_status->horizontal_angle = horizontal_angle;
+      new_vision_status->vertical_angle = vertical_angle;
+      new_vision_status->distance = distance;
+      LOG_STRUCT(DEBUG, "vision", *new_vision_status);
+
+      if (!new_vision_status.Send()) {
+        LOG(ERROR, "Failed to send vision information\n");
+      }
+    }
+  }
+}
+
+}  // namespace vision
+}  // namespace y2016
+
+int main(int /*argc*/, char ** /*argv*/) {
+  ::aos::InitNRT();
+  ::y2016::vision::Main();
+}
diff --git a/y2016/vision/vision.q b/y2016/vision/vision.q
new file mode 100644
index 0000000..ed46874
--- /dev/null
+++ b/y2016/vision/vision.q
@@ -0,0 +1,22 @@
+package y2016.vision;
+
+message VisionStatus {
+  // Times when the images were taken as nanoseconds on CLOCK_MONOTONIC on the
+  // TK1.
+  int64_t left_image_timestamp;
+  int64_t right_image_timestamp;
+  // Times when the images were sent from the TK1 as nanoseconds on the TK1's
+  // CLOCK_MONOTONIC.
+  int64_t left_send_timestamp;
+  int64_t right_send_timestamp;
+
+  // Horizontal angle of the goal in radians.
+  // TODO(Brian): Figure out which way is positive.
+  double horizontal_angle;
+  // Vertical angle of the goal in radians.
+  // TODO(Brian): Figure out which way is positive.
+  double vertical_angle;
+  // Distance to the target in meters.
+  double distance;
+};
+queue VisionStatus vision_status;
diff --git a/y2016/vision/vision_data.proto b/y2016/vision/vision_data.proto
new file mode 100644
index 0000000..4128871
--- /dev/null
+++ b/y2016/vision/vision_data.proto
@@ -0,0 +1,23 @@
+syntax = "proto2";
+
+package y2016.vision;
+
+// Represents a target found by the vision processing code.
+message Target {
+  optional float left_corner_x = 1;
+  optional float left_corner_y = 2;
+  optional float right_corner_x = 3;
+  optional float right_corner_y = 4;
+}
+
+// Contains all of the information extracted from a single frame captured by a
+// single camera.
+// TODO(Brian): Figure out what this all means and rework it to properly handle
+// 2 cameras without assuming they will stay perfectly in sync.
+message VisionData {
+  optional int32 camera_index = 1;
+  optional int64 image_timestamp = 2;
+  optional int64 send_timestamp = 3;
+  repeated Target target = 4;
+  optional bytes raw = 5;
+}