Add kLog camera command.
Change-Id: Ica73c88ca38b1fc5dc505ce61553d16772270644
diff --git a/y2019/jevois/structures.h b/y2019/jevois/structures.h
index 9c74b11..fcf1ebe 100644
--- a/y2019/jevois/structures.h
+++ b/y2019/jevois/structures.h
@@ -144,6 +144,8 @@
// Send As, which triggers the bootstrap script to drop directly into USB
// mode.
kAs,
+ // Log camera images
+ kLog,
};
// This is all the information sent from the Teensy to each camera.
diff --git a/y2019/jevois/teensy.cc b/y2019/jevois/teensy.cc
index 0f9f94d..58d1570 100644
--- a/y2019/jevois/teensy.cc
+++ b/y2019/jevois/teensy.cc
@@ -774,6 +774,10 @@
printf("Sending USB mode\n");
stdin_camera_command = CameraCommand::kUsb;
break;
+ case 'l':
+ printf("Log mode\n");
+ stdin_camera_command = CameraCommand::kLog;
+ break;
case 'n':
printf("Sending normal mode\n");
stdin_camera_command = CameraCommand::kNormal;
@@ -798,6 +802,7 @@
printf("UART board commands:\n");
printf(" p: Send passthrough mode\n");
printf(" u: Send USB mode\n");
+ printf(" l: Send Log mode\n");
printf(" n: Send normal mode\n");
printf(" a: Send all-'a' mode\n");
printf(" c: Dump camera configuration\n");
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index b403913..665714e 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -88,6 +88,8 @@
const ButtonLocation kMidCargoHint(3, 16);
const ButtonLocation kFarCargoHint(4, 2);
+const ButtonLocation kCameraLog(3, 7);
+
const ElevatorWristPosition kStowPos{0.36, 0.0};
const ElevatorWristPosition kClimbPos{0.0, M_PI / 4.0};
@@ -397,6 +399,13 @@
video_tx_->Send(vision_control_);
last_vision_control_ = monotonic_now;
}
+
+ {
+ auto camera_log_message = camera_log.MakeMessage();
+ camera_log_message->log = data.IsPressed(kCameraLog);
+ LOG_STRUCT(DEBUG, "camera_log", *camera_log_message);
+ camera_log_message.Send();
+ }
}
private:
diff --git a/y2019/status_light.q b/y2019/status_light.q
index f84ed28..c26de88 100644
--- a/y2019/status_light.q
+++ b/y2019/status_light.q
@@ -8,3 +8,9 @@
};
queue StatusLight status_light;
+
+message CameraLog {
+ bool log;
+};
+
+queue CameraLog camera_log;
diff --git a/y2019/vision/BUILD b/y2019/vision/BUILD
index bf71116..b87e2ce 100644
--- a/y2019/vision/BUILD
+++ b/y2019/vision/BUILD
@@ -34,6 +34,16 @@
tools = [":constants_formatting"],
)
+cc_library(
+ name = "image_writer",
+ srcs = ["image_writer.cc"],
+ hdrs = ["image_writer.h"],
+ deps = [
+ "//aos/logging",
+ "//aos/vision/image:image_types",
+ ],
+)
+
sh_test(
name = "constants_formatting_test",
srcs = ["constants_formatting_test.sh"],
@@ -92,6 +102,7 @@
srcs = ["target_sender.cc"],
restricted_to = VISION_TARGETS,
deps = [
+ ":image_writer",
":target_finder",
"//aos/vision/blob:codec",
"//aos/vision/blob:find_blob",
diff --git a/y2019/vision/image_writer.cc b/y2019/vision/image_writer.cc
new file mode 100644
index 0000000..ac35114
--- /dev/null
+++ b/y2019/vision/image_writer.cc
@@ -0,0 +1,40 @@
+#include <fstream>
+#include <sys/stat.h>
+
+#include "aos/logging/logging.h"
+#include "y2019/vision/image_writer.h"
+
+namespace y2019 {
+namespace vision {
+
+ImageWriter::ImageWriter() {
+ LOG(INFO, "Initializing image writer\n");
+ SetDirPath();
+}
+
+void ImageWriter::WriteImage(::aos::vision::DataRef data) {
+ LOG(INFO, "Writing image %d", image_count_);
+ std::ofstream ofs(
+ dir_path_ + file_prefix_ + std::to_string(image_count_) + ".yuyv",
+ std::ofstream::out);
+ ofs << data;
+ ofs.close();
+ ++image_count_;
+}
+
+void ImageWriter::SetDirPath() {
+ ::std::string base_path = "/jevois/data/run_";
+ for (int i = 0;; ++i) {
+ struct stat st;
+ std::string option = base_path + std::to_string(i);
+ if (stat(option.c_str(), &st) != 0) {
+ file_prefix_ = option + "/";
+ LOG(INFO, "Writing to %s\n", file_prefix_.c_str());
+ mkdir(file_prefix_.c_str(), 0777);
+ break;
+ }
+ }
+}
+
+} // namespace vision
+} // namespace y2019
diff --git a/y2019/vision/image_writer.h b/y2019/vision/image_writer.h
new file mode 100644
index 0000000..f33cca7
--- /dev/null
+++ b/y2019/vision/image_writer.h
@@ -0,0 +1,29 @@
+#ifndef Y2019_VISION_IMAGE_WRITER_H_
+#define Y2019_VISION_IMAGE_WRITER_H_
+
+#include <string>
+
+#include "aos/vision/image/image_types.h"
+
+namespace y2019 {
+namespace vision {
+
+class ImageWriter {
+ public:
+ ImageWriter();
+
+ void WriteImage(::aos::vision::DataRef data);
+
+ private:
+ void SetDirPath();
+
+ ::std::string file_prefix_ = std::string("debug_viewer_jpeg_");
+ ::std::string dir_path_;
+
+ unsigned int image_count_ = 0;
+};
+
+} // namespace vision
+} // namespace y2017
+
+#endif // Y2019_VISION_IMAGE_WRITER_H_
diff --git a/y2019/vision/target_sender.cc b/y2019/vision/target_sender.cc
index 12e0e09..4344cc9 100644
--- a/y2019/vision/target_sender.cc
+++ b/y2019/vision/target_sender.cc
@@ -8,10 +8,10 @@
#include "aos/vision/events/udp.h"
#include "y2019/jevois/camera/image_stream.h"
#include "y2019/jevois/camera/reader.h"
-
#include "y2019/jevois/serial.h"
#include "y2019/jevois/structures.h"
#include "y2019/jevois/uart.h"
+#include "y2019/vision/image_writer.h"
// This has to be last to preserve compatibility with other headers using AOS
// logging.
@@ -83,6 +83,9 @@
// dup2(itsDev, 2);
TargetFinder finder_;
+ ImageWriter writer;
+ uint32_t image_count = 0;
+ bool log_images = false;
aos::vision::CameraParams params0;
params0.set_exposure(50);
@@ -167,6 +170,13 @@
LOG(INFO) << "Some problem happened";
}
}
+
+ if (log_images) {
+ if ((image_count % 5) == 0) {
+ writer.WriteImage(data);
+ }
+ ++image_count;
+ }
});
aos::events::EpollLoop loop;
@@ -195,6 +205,10 @@
switch (calibration.camera_command) {
case CameraCommand::kNormal:
case CameraCommand::kAs:
+ log_images = false;
+ break;
+ case CameraCommand::kLog:
+ log_images = true;
break;
case CameraCommand::kUsb:
return 0;
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index e75b825..95ed28e 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -314,10 +314,13 @@
using namespace frc971::jevois;
RoborioToTeensy to_teensy{};
to_teensy.realtime_now = aos::realtime_clock::now();
+ camera_log.FetchLatest();
if (activate_usb_ && !activate_usb_->Get()) {
to_teensy.camera_command = CameraCommand::kUsb;
} else if (activate_passthrough_ && !activate_passthrough_->Get()) {
to_teensy.camera_command = CameraCommand::kCameraPassthrough;
+ } else if (camera_log.get() && camera_log->log) {
+ to_teensy.camera_command = CameraCommand::kLog;
} else {
to_teensy.camera_command = CameraCommand::kNormal;
}