Merge "Make jevois deploy script much more robust"
diff --git a/y2019/vision/BUILD b/y2019/vision/BUILD
index 186850c..344e4da 100644
--- a/y2019/vision/BUILD
+++ b/y2019/vision/BUILD
@@ -34,13 +34,6 @@
tools = [":constants_formatting"],
)
-cc_library(
- name = "image_writer",
- srcs = ["image_writer.cc"],
- hdrs = ["image_writer.h"],
- deps = ["//aos/vision/image:image_types"],
-)
-
sh_test(
name = "constants_formatting_test",
srcs = ["constants_formatting_test.sh"],
@@ -99,7 +92,6 @@
srcs = ["target_sender.cc"],
restricted_to = VISION_TARGETS,
deps = [
- ":image_writer",
":target_finder",
"//aos/logging",
"//aos/logging:implementations",
diff --git a/y2019/vision/image_writer.cc b/y2019/vision/image_writer.cc
deleted file mode 100644
index a2c5f0e..0000000
--- a/y2019/vision/image_writer.cc
+++ /dev/null
@@ -1,50 +0,0 @@
-#include <fstream>
-#include <sys/stat.h>
-
-#include "y2019/vision/image_writer.h"
-
-namespace y2019 {
-namespace vision {
-
-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::ProcessImage(::aos::vision::DataRef data,
- size_t num_targets) {
- ++debounce_count_;
- if (debounce_count_ < 10) {
- return;
- }
- // Write the image if there are fewer targets in this frame than the last.
- if (num_targets < previous_num_targets_) {
- WriteImage(previous_image_);
- WriteImage(data);
- debounce_count_ = 0;
- }
- //data.swap(previous_image_);
- data.copy(previous_image_, sizeof(previous_image_));
-}
-
-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
deleted file mode 100644
index 3d0d934..0000000
--- a/y2019/vision/image_writer.h
+++ /dev/null
@@ -1,39 +0,0 @@
-#ifndef _Y2019_VISION_IMAGE_WRITER_H_
-#define _Y2019_VISION_IMAGE_WRITER_H_
-
-#include <string>
-
-#include "aos/logging/logging.h"
-#include "aos/vision/image/image_types.h"
-
-namespace y2019 {
-namespace vision {
-
-class ImageWriter {
- public:
- ImageWriter() {
- LOG(INFO, "Initializing image writer\n");
- SetDirPath();
- }
-
- // This is destructive to data.
- void ProcessImage(::aos::vision::DataRef data, size_t num_targets);
- private:
- void SetDirPath();
-
- void WriteImage(::aos::vision::DataRef data);
-
- std::string file_prefix_ = std::string("debug_viewer_jpeg_");
- std::string dir_path_;
-
- size_t previous_num_targets_ = 0;
- char previous_image_[640 * 480 * 2];
-
- unsigned int image_count_ = 0;
- unsigned int debounce_count_ = 0;
-};
-
-} // namespace vision
-} // namespace y2017
-
-#endif // _Y2019_VISION_IMAGE_WRITER_H_
diff --git a/y2019/vision/server/BUILD b/y2019/vision/server/BUILD
index 5729776..d0a08a1 100644
--- a/y2019/vision/server/BUILD
+++ b/y2019/vision/server/BUILD
@@ -51,6 +51,8 @@
"//aos:init",
"//aos/logging",
"//aos/time",
+ "//frc971/control_loops/drivetrain:drivetrain_queue",
"//third_party/seasocks",
+ "//y2019/control_loops/drivetrain:camera_queue",
],
)
diff --git a/y2019/vision/server/server.cc b/y2019/vision/server/server.cc
index cc6d260..c322322 100644
--- a/y2019/vision/server/server.cc
+++ b/y2019/vision/server/server.cc
@@ -1,14 +1,18 @@
+#include <array>
#include <memory>
#include <set>
+#include <sstream>
#include "aos/init.h"
#include "aos/logging/logging.h"
#include "aos/time/time.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "internal/Embedded.h"
#include "seasocks/PrintfLogger.h"
#include "seasocks/Server.h"
#include "seasocks/StringUtil.h"
#include "seasocks/WebSocket.h"
+#include "y2019/control_loops/drivetrain/camera.q.h"
namespace y2019 {
namespace vision {
@@ -20,6 +24,8 @@
void onData(seasocks::WebSocket* connection, const char* data) override;
void onDisconnect(seasocks::WebSocket* connection) override;
+ void SendData(const std::string &data);
+
private:
std::set<seasocks::WebSocket *> connections_;
};
@@ -44,6 +50,13 @@
seasocks::formatAddress(connection->getRemoteAddress()).c_str());
}
+void WebsocketHandler::SendData(const std::string &data) {
+ for (seasocks::WebSocket *websocket : connections_) {
+ websocket->send(reinterpret_cast<const uint8_t *>(data.data()),
+ data.size());
+ }
+}
+
// TODO(Brian): Put this somewhere shared.
class SeasocksLogger : public seasocks::PrintfLogger {
public:
@@ -74,6 +87,84 @@
LOG(aos_level, "Seasocks: %s\n", message);
}
+struct LocalCameraTarget {
+ double x, y, theta;
+};
+
+struct LocalCameraFrame {
+ aos::monotonic_clock::time_point capture_time =
+ aos::monotonic_clock::min_time;
+ std::vector<LocalCameraTarget> targets;
+
+ bool IsValid(aos::monotonic_clock::time_point now) {
+ return capture_time + std::chrono::seconds(1) > now;
+ }
+};
+
+// Sends a new chunk of data to all the websocket clients.
+class UpdateData : public seasocks::Server::Runnable {
+ public:
+ UpdateData(WebsocketHandler *websocket_handler, std::string &&data)
+ : websocket_handler_(websocket_handler), data_(std::move(data)) {}
+ ~UpdateData() override = default;
+ UpdateData(const UpdateData &) = delete;
+ UpdateData &operator=(const UpdateData &) = delete;
+
+ void run() override { websocket_handler_->SendData(data_); }
+
+ private:
+ WebsocketHandler *const websocket_handler_;
+ const std::string data_;
+};
+
+void DataThread(seasocks::Server *server, WebsocketHandler *websocket_handler) {
+ auto &camera_frames = y2019::control_loops::drivetrain::camera_frames;
+ auto &drivetrain_status = frc971::control_loops::drivetrain_queue.status;
+
+ std::array<LocalCameraFrame, 5> latest_frames;
+ aos::monotonic_clock::time_point last_send_time = aos::monotonic_clock::now();
+
+ while (true) {
+ camera_frames.FetchNextBlocking();
+ drivetrain_status.FetchLatest();
+ if (!drivetrain_status.get()) {
+ // Try again if we don't have any drivetrain statuses.
+ continue;
+ }
+
+ {
+ const auto &new_frame = *camera_frames;
+ if (new_frame.camera < latest_frames.size()) {
+ latest_frames[new_frame.camera].capture_time =
+ aos::monotonic_clock::time_point(
+ std::chrono::nanoseconds(new_frame.timestamp));
+ latest_frames[new_frame.camera].targets.clear();
+ for (int target = 0; target < new_frame.num_targets; ++target) {
+ latest_frames[new_frame.camera].targets.emplace_back();
+ // TODO: Do something useful.
+ }
+ }
+ }
+
+ const auto now = aos::monotonic_clock::now();
+ if (now > last_send_time + std::chrono::milliseconds(100)) {
+ last_send_time = now;
+ std::ostringstream stream;
+ stream << "{\n";
+
+ stream << "'robot': {";
+ stream << "'x': " << drivetrain_status->x << ",";
+ stream << "'y': " << drivetrain_status->y << ",";
+ stream << "'theta': " << drivetrain_status->theta;
+ stream << "}\n";
+
+ stream << "}";
+ server->execute(
+ std::make_shared<UpdateData>(websocket_handler, stream.str()));
+ }
+ }
+}
+
} // namespace vision
} // namespace y2019
@@ -87,8 +178,12 @@
new y2019::vision::SeasocksLogger(seasocks::Logger::INFO)));
auto websocket_handler = std::make_shared<y2019::vision::WebsocketHandler>();
-
server.addWebSocketHandler("/ws", websocket_handler);
+
+ std::thread data_thread{[&server, websocket_handler]() {
+ y2019::vision::DataThread(&server, websocket_handler.get());
+ }};
+
server.serve("/home/admin/robot_code/www", 1180);
return 0;
diff --git a/y2019/vision/target_sender.cc b/y2019/vision/target_sender.cc
index 3c0c705..3a6fbfe 100644
--- a/y2019/vision/target_sender.cc
+++ b/y2019/vision/target_sender.cc
@@ -12,7 +12,6 @@
#include "y2019/jevois/serial.h"
#include "y2019/jevois/structures.h"
#include "y2019/jevois/uart.h"
-#include "y2019/vision/image_writer.h"
#include "y2019/vision/target_finder.h"
using ::aos::events::DataSocket;
@@ -82,7 +81,6 @@
// dup2(itsDev, 2);
TargetFinder finder_;
- ImageWriter writer_;
aos::vision::CameraParams params0;
params0.set_exposure(50);
@@ -165,8 +163,6 @@
LOG(INFO, "Some problem happened");
}
}
-
- writer_.ProcessImage(data, results.size());
});
aos::events::EpollLoop loop;
diff --git a/y2019/vision/tools/deploy.sh b/y2019/vision/tools/deploy.sh
index 0b8002b..47b724d 100755
--- a/y2019/vision/tools/deploy.sh
+++ b/y2019/vision/tools/deploy.sh
@@ -65,10 +65,10 @@
echo "OK"
echo "Copying files ..."
-sudo cp ./austin_cam.sh "${TARGET_DIR}"/
-sudo cp ./launch.sh "${TARGET_DIR}"/deploy/
+cp ./austin_cam.sh "${TARGET_DIR}"/
+cp ./launch.sh "${TARGET_DIR}"/deploy/
-sudo cp "${BAZEL_BIN}/y2019/vision/target_sender" \
+cp "${BAZEL_BIN}/y2019/vision/target_sender" \
"${BAZEL_BIN}/y2019/vision/serial_waiter" \
"${TARGET_DIR}"/deploy/