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/aos/vision/events/BUILD b/aos/vision/events/BUILD
index e5be1b0..d393377 100644
--- a/aos/vision/events/BUILD
+++ b/aos/vision/events/BUILD
@@ -1,5 +1,6 @@
cc_library(
name = 'udp',
+ visibility = ['//visibility:public'],
srcs = ['udp.cc'],
hdrs = ['udp.h'],
deps = [
diff --git a/aos/vision/events/udp.cc b/aos/vision/events/udp.cc
index febb835..2d3d1d1 100644
--- a/aos/vision/events/udp.cc
+++ b/aos/vision/events/udp.cc
@@ -9,7 +9,8 @@
TXUdpSocket::TXUdpSocket(const char *ip_addr, int port)
: fd_(PCHECK(socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))) {
- sockaddr_in destination_in{};
+ sockaddr_in destination_in;
+ memset(&destination_in, 0, sizeof(destination_in));
destination_in.sin_family = AF_INET;
destination_in.sin_port = htons(port);
if (inet_aton(ip_addr, &destination_in.sin_addr) == 0) {
@@ -26,7 +27,8 @@
RXUdpSocket::RXUdpSocket(int port)
: fd_(PCHECK(socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))) {
- sockaddr_in bind_address{};
+ sockaddr_in bind_address;
+ memset(&bind_address, 0, sizeof(bind_address));
bind_address.sin_family = AF_INET;
bind_address.sin_port = htons(port);
diff --git a/aos/vision/math/BUILD b/aos/vision/math/BUILD
index c52e963..728fa03 100644
--- a/aos/vision/math/BUILD
+++ b/aos/vision/math/BUILD
@@ -1,5 +1,6 @@
cc_library(
name = 'vector',
+ visibility = ['//visibility:public'],
hdrs = [
'vector.h',
],
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> ¢er0,
+ const ::aos::vision::Vector<2> ¢er1, 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;
+}