Checking in target_sender.cc. Renaming vision_data to avoid naming collision.
Change-Id: I4802bdc361f3eaf42845cb4a3e8e27f8450735ac
diff --git a/y2017/vision/BUILD b/y2017/vision/BUILD
index b46f01d..5cd6f3f 100644
--- a/y2017/vision/BUILD
+++ b/y2017/vision/BUILD
@@ -13,8 +13,38 @@
)
proto_cc_library(
- name = 'vision_data',
- src = 'vision_data.proto',
+ name = 'vision_result',
+ src = 'vision_result.proto',
+)
+
+proto_cc_library(
+ name = 'vision_config',
+ src = 'vision_config.proto',
+)
+
+cc_binary(
+ name = 'target_sender',
+ srcs = [
+ 'target_sender.cc',
+ ],
+ deps = [
+ ':vision_result',
+ ':vision_config',
+ ':target_finder',
+ '//aos/common/logging:logging',
+ '//aos/common/logging:implementations',
+ '//aos/common:time',
+ '//aos/vision/image:reader',
+ '//aos/vision/image:jpeg_routines',
+ '//aos/vision/image:image_stream',
+ '//aos/vision/events:udp',
+ '//aos/vision/events:epoll_events',
+ '//aos/vision/events:socket_types',
+ '//aos/vision/blob:range_image',
+ '//aos/vision/blob:codec',
+ '//aos/vision/blob:find_blob',
+ '//aos/vision/blob:threshold',
+ ],
)
cc_binary(
@@ -30,7 +60,7 @@
'//aos/common:time',
'//aos/vision/events:udp',
':vision_queue',
- ':vision_data',
+ ':vision_result',
'//aos/common:mutex',
],
)
@@ -46,4 +76,3 @@
'//aos/vision/math:vector',
],
)
-)
diff --git a/y2017/vision/target_receiver.cc b/y2017/vision/target_receiver.cc
index 8cabe76..851d3a5 100644
--- a/y2017/vision/target_receiver.cc
+++ b/y2017/vision/target_receiver.cc
@@ -6,14 +6,14 @@
#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"
+#include "y2017/vision/vision_result.pb.h"
using aos::monotonic_clock;
namespace y2017 {
namespace vision {
-void ComputeDistanceAngle(const Target &target, double *distance,
+void ComputeDistanceAngle(const TargetResult &target, double *distance,
double *angle) {
// TODO: fix this.
*distance = target.y();
@@ -30,7 +30,7 @@
while (true) {
// TODO(austin): Don't malloc.
- VisionData target;
+ VisionResult target;
int size = recv.Recv(raw_data, sizeof(raw_data));
monotonic_clock::time_point now = monotonic_clock::now();
auto target_time = now -
diff --git a/y2017/vision/target_sender.cc b/y2017/vision/target_sender.cc
new file mode 100644
index 0000000..548d01d
--- /dev/null
+++ b/y2017/vision/target_sender.cc
@@ -0,0 +1,246 @@
+#include <google/protobuf/io/zero_copy_stream_impl.h>
+#include <google/protobuf/text_format.h>
+#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/blob/codec.h"
+#include "aos/vision/blob/find_blob.h"
+#include "aos/vision/blob/range_image.h"
+#include "aos/vision/blob/threshold.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 "y2017/vision/target_finder.h"
+#include "y2017/vision/vision_config.pb.h"
+#include "y2017/vision/vision_result.pb.h"
+
+namespace y2017 {
+namespace vision {
+
+using aos::events::TCPServer;
+using aos::vision::DataRef;
+using aos::vision::Int32Codec;
+using aos::vision::ImageValue;
+using aos::vision::Int64Codec;
+using aos::events::TXUdpSocket;
+using aos::events::DataSocket;
+using aos::vision::ImageFormat;
+using aos::vision::ImageStreamEvent;
+
+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 BlobLog {
+ public:
+ explicit BlobLog(const char *prefix, const char *extension) {
+ int index = 0;
+ while (true) {
+ std::string file = prefix + std::to_string(index) + extension;
+ if (FileExist(file)) {
+ index++;
+ } else {
+ printf("Logging to file (%s)\n", file.c_str());
+ ofst_.open(file);
+ assert(ofst_.is_open());
+ break;
+ }
+ }
+ }
+
+ ~BlobLog() { ofst_.close(); }
+
+ void WriteLogEntry(DataRef data) { ofst_.write(&data[0], data.size()); }
+
+ private:
+ std::ofstream ofst_;
+};
+
+ImageFormat GetImageFormat(const CameraSettings ¶ms) {
+ return ImageFormat{params.width(), params.height()};
+}
+
+class ImageSender : public ImageStreamEvent {
+ public:
+ ImageSender(camera::CameraParams params, GameSpecific game_cfg,
+ const std::string &fname, const std::string &ipadder, int port)
+ : ImageStreamEvent(fname, params),
+ game_cfg_(game_cfg),
+ udp_serv_(ipadder, 8080),
+ tcp_serv_(port),
+ log_("./logging/blob_record_", ".dat") {}
+
+ void WriteAndSendBlob(ImageFormat fmt, int64_t timestamp,
+ aos::vision::BlobList blobl) {
+ // Write blob to file for logging.
+ int blob_size = CalculateSize(blobl);
+ int tmp_size = blob_size + sizeof(int32_t) * 3 + sizeof(uint64_t);
+ char *buf;
+ std::string blob_data;
+ blob_data.resize(tmp_size, 0);
+ {
+ buf = Int32Codec::Write(&blob_data[0], tmp_size);
+ buf = Int64Codec::Write(buf, timestamp);
+ buf = Int32Codec::Write(buf, fmt.w);
+ buf = Int32Codec::Write(buf, fmt.h);
+ SerializeBlob(blobl, buf);
+ }
+ log_.WriteLogEntry(blob_data);
+
+ // Send the blob back to the debug-viewer
+ tcp_serv_.Broadcast([&](DataSocket *client) { client->Emit(blob_data); });
+ }
+
+ void ProcessImage(DataRef data, aos::monotonic_clock::time_point tp) {
+ using namespace aos::vision;
+ int64_t timestamp = std::chrono::duration_cast<std::chrono::nanoseconds>(
+ tp.time_since_epoch())
+ .count();
+ DecodeJpeg(data, &image_);
+ ImageFormat fmt = image_.fmt();
+
+ RangeImage rimg = finder_.Threshold(image_.get());
+ BlobList blobl = aos::vision::FindBlobs(rimg);
+
+ // Remove bad blobs before we log.
+ finder_.PreFilter(blobl);
+
+ // Write to the logi and stream to the debug-viewer.
+ WriteAndSendBlob(fmt, timestamp, blobl);
+
+ // Calculate each component.
+ std::vector<TargetComponent> target_component_list =
+ finder_.FillTargetComponentList(blobl);
+
+ // Put the compenents together into targets and pick the best.
+ y2017::vision::Target final_target;
+ bool found_target =
+ finder_.FindTargetFromComponents(target_component_list, &final_target);
+
+ std::string dat;
+ VisionResult result;
+ result.set_image_timestamp(timestamp);
+ result.set_send_timestamp(NowNanos());
+ if (found_target) {
+ result.mutable_target()->set_x(final_target.screen_coord.x());
+ result.mutable_target()->set_y(final_target.screen_coord.y());
+ }
+ // always send data
+ if (result.SerializeToString(&dat)) {
+ if (print_once_) {
+ printf("Beginning data streaming camera...\n");
+ print_once_ = false;
+ }
+
+ // Send only target over udp.
+ udp_serv_.Send(dat.data(), dat.size());
+ }
+ }
+
+ TCPServer<DataSocket> *GetTCPServ() { return &tcp_serv_; }
+
+ // Configuration related to the game.
+ GameSpecific game_cfg_;
+
+ // target selction code.
+ TargetFinder finder_;
+
+ // print when we start
+ bool print_once_ = true;
+
+ // udp socket on which to send to robot
+ TXUdpSocket udp_serv_;
+
+ // tcp socket on which to debug to laptop
+ TCPServer<DataSocket> tcp_serv_;
+
+ ImageValue image_;
+ BlobLog log_;
+ aos::monotonic_clock::time_point tstart;
+
+ private:
+};
+
+void RunCamera(CameraSettings settings, GameSpecific game_cfg,
+ const std::string &device, const std::string &ip_addr,
+ int port) {
+ printf("Creating camera (%dx%d).\n", settings.width(), settings.height());
+ camera::CameraParams params = {settings.width(),
+ settings.height(),
+ settings.exposure(),
+ settings.brightness(),
+ 0,
+ (int32_t)settings.fps()};
+ ImageSender strm(params, game_cfg, device, ip_addr, port);
+
+ aos::events::EpollLoop loop;
+ loop.Add(strm.GetTCPServ());
+ loop.Add(&strm);
+ printf("Running Camera\n");
+ loop.Run();
+}
+
+bool ReadConfiguration(const std::string &file_name, VisionConfig *cfg) {
+ using namespace google::protobuf::io;
+ using namespace google::protobuf;
+ if (cfg == nullptr) {
+ return false;
+ }
+
+ // fd will close when it goes out of scope.
+ std::ifstream fd(file_name);
+ if (!fd.is_open()) {
+ fprintf(stderr, "File (%s) not found.\n", file_name.c_str());
+ return false;
+ }
+ IstreamInputStream is(&fd);
+ if (!TextFormat::Parse(&is, cfg)) {
+ fprintf(stderr, "Unable to parse file (%s).\n", file_name.c_str());
+ return false;
+ }
+
+ return true;
+}
+
+} // namespace vision
+} // namespace y2017
+
+int main(int, char **) {
+ using namespace y2017::vision;
+
+ ::aos::logging::Init();
+ ::aos::logging::AddImplementation(
+ new ::aos::logging::StreamLogImplementation(stdout));
+ VisionConfig cfg;
+ if (ReadConfiguration("ConfigFile.pb.ascii", &cfg)) {
+ if (cfg.robot_configs().count("Laptop") != 1) {
+ fprintf(stderr, "Could not find config key.\n");
+ return -1;
+ }
+ const RobotConfig &rbt = cfg.robot_configs().at("Laptop");
+ RunCamera(cfg.camera_params(), cfg.game_params(), rbt.camera_device_path(),
+ rbt.roborio_ipaddr(), rbt.port());
+ }
+
+ return EXIT_SUCCESS;
+}
diff --git a/y2017/vision/vision_config.proto b/y2017/vision/vision_config.proto
new file mode 100644
index 0000000..0bb7279
--- /dev/null
+++ b/y2017/vision/vision_config.proto
@@ -0,0 +1,59 @@
+syntax = "proto2";
+
+package y2017.vision;
+
+// Stores configuration for camera related settings and specs.
+message CameraSettings {
+ // The focal length of the camera in pixels.
+ optional double focal_length = 1 [default = 1418.6];
+
+ // Width of the image.
+ optional int32 width = 2 [default = 1280];
+
+ // Height of the image.
+ optional int32 height = 3 [default = 960];
+
+ // Exposure setting.
+ optional int32 exposure = 4 [default = 10];
+
+ // Brightness setting.
+ optional int32 brightness = 5 [default = 128];
+
+ // Hardware gain multiplier on pixel values.
+ optional double gain = 6 [default = 1.0];
+
+ // Frames per second to run camera.
+ optional double fps = 7 [default = 30.0];
+}
+
+message GameSpecific {
+ // Needs more woojy.
+ optional int32 woojy = 1;
+}
+
+// Configurations that may be robot dependent.
+message RobotConfig {
+ // Device name of the camera (ie /dev/video1).
+ optional string camera_device_path = 1;
+
+ // RoboRIO IP address.
+ optional string roborio_ipaddr = 2;
+
+ // Jetson board IP address.
+ optional string jetson_ipaddr = 3;
+
+ // Port to use (both sides).
+ optional int32 port = 4;
+}
+
+// Stores configuration information for a given set of hardware.
+message VisionConfig {
+ // Map robot name to the robot dependent configuration.
+ map<string, RobotConfig> robot_configs = 1;
+
+ // Parameters for camera bringup.
+ optional CameraSettings camera_params = 2;
+
+ // Parameters for this specific game
+ optional GameSpecific game_params = 3;
+}
diff --git a/y2017/vision/vision_data.proto b/y2017/vision/vision_result.proto
similarity index 84%
rename from y2017/vision/vision_data.proto
rename to y2017/vision/vision_result.proto
index 3190686..04366db 100644
--- a/y2017/vision/vision_data.proto
+++ b/y2017/vision/vision_result.proto
@@ -5,15 +5,15 @@
// 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 {
+message TargetResult {
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 {
+message VisionResult {
optional int64 image_timestamp = 1;
optional int64 send_timestamp = 2;
- optional Target target = 3;
+ optional TargetResult target = 3;
}