Checking in debug_view, some extra missing utils, and the y2016 target_sender code.
Change-Id: I241947265da8f332c39862f4d0ddcdc2d29c7b68
diff --git a/y2016/vision/target_sender.cc b/y2016/vision/target_sender.cc
new file mode 100644
index 0000000..706c348
--- /dev/null
+++ b/y2016/vision/target_sender.cc
@@ -0,0 +1,242 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <fstream>
+#include <iostream>
+#include <memory>
+#include <thread>
+#include <vector>
+
+#include "aos/common/logging/implementations.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/time.h"
+#include "aos/vision/events/socket_types.h"
+#include "aos/vision/events/udp.h"
+#include "aos/vision/image/image_stream.h"
+#include "aos/vision/image/jpeg_routines.h"
+#include "aos/vision/image/reader.h"
+#include "y2016/vision/blob_filters.h"
+#include "y2016/vision/stereo_geometry.h"
+#include "y2016/vision/vision_data.pb.h"
+
+namespace y2016 {
+namespace vision {
+using aos::vision::ImageStreamEvent;
+using aos::vision::DataRef;
+using aos::events::TCPServer;
+using aos::vision::BlobLRef;
+using aos::vision::Vector;
+using aos::vision::Int32Codec;
+using aos::vision::BlobList;
+using aos::vision::RangeImage;
+using aos::vision::PixelRef;
+using aos::vision::ImageValue;
+using aos::vision::HistogramBlobFilter;
+using aos::vision::CornerFinder;
+using aos::vision::Int64Codec;
+using aos::events::TXUdpSocket;
+using aos::events::DataSocket;
+using aos::vision::ImageFormat;
+
+::camera::CameraParams GetCameraParams(const Calibration &calibration) {
+ return ::camera::CameraParams{.width = calibration.camera_image_width(),
+ .height = calibration.camera_image_height(),
+ .exposure = calibration.camera_exposure(),
+ .brightness = calibration.camera_brightness(),
+ .gain = calibration.camera_gain(),
+ .fps = calibration.camera_fps()};
+}
+
+int64_t Nanos(aos::monotonic_clock::duration time_diff) {
+ return std::chrono::duration_cast<std::chrono::nanoseconds>(time_diff)
+ .count();
+}
+
+int64_t NowNanos() {
+ return Nanos(aos::monotonic_clock::now().time_since_epoch());
+}
+
+inline bool FileExist(const std::string &name) {
+ struct stat buffer;
+ return (stat(name.c_str(), &buffer) == 0);
+}
+
+class ImageSender : public ImageStreamEvent {
+ public:
+ ImageSender(int camera_index, camera::CameraParams params,
+ const std::string &fname, const std::string &ipadder, int port)
+ : ImageStreamEvent(fname, params),
+ camera_index_(camera_index),
+ udp_serv_(ipadder, 8080),
+ tcp_serv_(port),
+ blob_filt_(ImageFormat(params.width, params.height), 40, 750, 250000),
+ finder_(0.25, 35) {
+ int index = 0;
+ while (true) {
+ std::string file = "./logging/blob_record_" + std::to_string(index) +
+ "_" + std::to_string(camera_index_) + ".dat";
+ if (FileExist(file)) {
+ index++;
+ } else {
+ printf("Logging to file (%s)\n", file.c_str());
+ ofst_.open(file);
+ assert(ofst_.is_open());
+ break;
+ }
+ }
+ }
+
+ ~ImageSender() { ofst_.close(); }
+
+ void ProcessImage(DataRef data, aos::monotonic_clock::time_point tp) {
+ int64_t timestamp = std::chrono::duration_cast<std::chrono::nanoseconds>(
+ tp.time_since_epoch())
+ .count();
+ DecodeJpeg(data, &image_);
+ auto fmt = image_.fmt();
+
+ RangeImage rimg = DoThreshold(image_.get(), [](PixelRef &px) {
+ return (px.g > 88);
+ });
+
+ // flip the right image as this camera is mount backward
+ if (camera_index_ == 0) {
+ rimg.Flip(fmt.w, fmt.h);
+ }
+
+ BlobList blobl = aos::vision::FindBlobs(rimg);
+ auto whatever = blob_filt_.FilterBlobs(blobl);
+
+ VisionData target;
+ target.set_camera_index(camera_index_);
+ target.set_image_timestamp(timestamp);
+
+ if (!whatever.empty()) {
+ std::vector<std::pair<Vector<2>, Vector<2>>> corners =
+ finder_.Find(whatever);
+
+ if (!corners.empty()) {
+ for (int i = 0; i < (int)corners.size(); i++) {
+ Target *pos = target.add_target();
+ pos->set_left_corner_x(corners[i].first.x());
+ pos->set_left_corner_y(corners[i].first.y());
+ pos->set_right_corner_x(corners[i].second.x());
+ pos->set_right_corner_y(corners[i].second.y());
+ }
+ }
+ }
+ target.set_send_timestamp(NowNanos());
+
+ // always send data
+ std::string dat;
+ if (target.SerializeToString(&dat)) {
+ if (print_once_) {
+ printf("Beginning data streaming camera %d...\n", camera_index_);
+ print_once_ = false;
+ }
+
+ // Send only target over udp.
+ udp_serv_.Send(dat.data(), dat.size());
+ }
+
+ // Write blob to file for logging.
+ int blob_size = CalculateSize(blobl);
+ int tmp_size = blob_size + sizeof(int32_t) + sizeof(uint64_t);
+ char *buf;
+ blob_data_.resize(tmp_size, 0);
+ {
+ buf = Int32Codec::Write(&blob_data_[0], tmp_size);
+ buf = Int64Codec::Write(buf, timestamp);
+ SerializeBlob(blobl, buf);
+ }
+ ofst_.write(&blob_data_[0], tmp_size);
+
+ // Add blob debug
+ bool debug = true;
+ if (debug) {
+ target.set_raw(buf, blob_size);
+ if (target.SerializeToString(&dat)) {
+ tcp_serv_.Broadcast([&](DataSocket *client) { client->Emit(dat); });
+ }
+ }
+
+ bool timing = false;
+ if (timing) {
+ if (n_time > 0) {
+ auto now = aos::monotonic_clock::now();
+ printf("%g, %g\n",
+ (((double)Nanos(now - tstart)) / (double)(n_time)) / 1e6,
+ (double)Nanos(now - tp) / 1e6);
+ } else {
+ tstart = aos::monotonic_clock::now();
+ }
+ ++n_time;
+ }
+ }
+
+ TCPServer<DataSocket> *GetTCPServ() { return &tcp_serv_; }
+
+ // print when we start
+ bool print_once_ = true;
+
+ // left or right camera
+ int camera_index_;
+
+ // udp socket on which to send to robot
+ TXUdpSocket udp_serv_;
+
+ // tcp socket on which to debug to laptop
+ TCPServer<DataSocket> tcp_serv_;
+
+ // our blob processing object
+ HistogramBlobFilter blob_filt_;
+
+ // corner finder to align aiming
+ CornerFinder finder_;
+
+ ImageValue image_;
+ std::string blob_data_;
+ std::ofstream ofst_;
+ aos::monotonic_clock::time_point tstart;
+ int n_time = 0;
+
+ private:
+};
+
+void RunCamera(int camera_index, camera::CameraParams params,
+ const std::string &device, const std::string &ip_addr,
+ int port) {
+ printf("Creating camera %d (%d, %d).\n", camera_index, params.width,
+ params.height);
+ ImageSender strm(camera_index, params, device, ip_addr, port);
+
+ aos::events::EpollLoop loop;
+ loop.Add(strm.GetTCPServ());
+ loop.Add(&strm);
+ printf("Running Camera (%d)\n", camera_index);
+ loop.Run();
+}
+
+} // namespace vision
+} // namespace y2016
+
+int main(int, char **) {
+ using namespace y2016::vision;
+ StereoGeometry stereo("./stereo_rig.calib");
+ ::aos::logging::Init();
+ ::aos::logging::AddImplementation(
+ new ::aos::logging::StreamLogImplementation(stdout));
+ std::thread cam0([stereo]() {
+ RunCamera(0, GetCameraParams(stereo.calibration()),
+ stereo.calibration().right_camera_name(),
+ stereo.calibration().roborio_ip_addr(), 8082);
+ });
+ std::thread cam1([stereo]() {
+ RunCamera(1, GetCameraParams(stereo.calibration()),
+ stereo.calibration().left_camera_name(),
+ stereo.calibration().roborio_ip_addr(), 8082);
+ });
+ cam0.join();
+ cam1.join();
+
+ return EXIT_SUCCESS;
+}