Use a second camera and switch between them.
Also, blink out the state over the beacon.
Change-Id: If606dfed9ae64137f71429f1190f04d5dac2c4ec
diff --git a/y2018/vision/image_streamer.cc b/y2018/vision/image_streamer.cc
index ce6f5e2..93f8c58 100644
--- a/y2018/vision/image_streamer.cc
+++ b/y2018/vision/image_streamer.cc
@@ -1,28 +1,41 @@
-#include "aos/vision/events/socket_types.h"
-#include "aos/vision/image/reader.h"
#include "aos/vision/image/image_stream.h"
+
+#include <sys/stat.h>
+#include <deque>
+#include <fstream>
+#include <string>
+
#include "aos/common/logging/implementations.h"
#include "aos/common/logging/logging.h"
#include "aos/vision/blob/codec.h"
-#include <fstream>
-#include <sys/stat.h>
-#include <deque>
-#include <string>
+#include "aos/vision/events/socket_types.h"
+#include "aos/vision/events/udp.h"
+#include "aos/vision/image/reader.h"
+#include "third_party/gflags/include/gflags/gflags.h"
+#include "y2018/vision.pb.h"
-using aos::events::TCPServer;
-using aos::events::DataSocket;
-using aos::vision::Int32Codec;
-using aos::vision::DataRef;
+using ::aos::events::DataSocket;
+using ::aos::events::RXUdpSocket;
+using ::aos::events::TCPServer;
+using ::aos::vision::DataRef;
+using ::aos::vision::Int32Codec;
+using ::aos::monotonic_clock;
+using ::y2018::VisionControl;
-aos::vision::DataRef mjpg_header = "HTTP/1.0 200 OK\r\n"\
- "Server: YourServerName\r\n"\
- "Connection: close\r\n"\
- "Max-Age: 0\r\n"\
- "Expires: 0\r\n"\
- "Cache-Control: no-cache, private\r\n"\
- "Pragma: no-cache\r\n"\
- "Content-Type: multipart/x-mixed-replace; "\
- "boundary=--boundary\r\n\r\n";
+DEFINE_bool(single_camera, true, "If true, only use video0");
+DEFINE_int32(camera0_exposure, 300, "Exposure for video0");
+DEFINE_int32(camera1_exposure, 300, "Exposure for video1");
+
+aos::vision::DataRef mjpg_header =
+ "HTTP/1.0 200 OK\r\n"
+ "Server: YourServerName\r\n"
+ "Connection: close\r\n"
+ "Max-Age: 0\r\n"
+ "Expires: 0\r\n"
+ "Cache-Control: no-cache, private\r\n"
+ "Pragma: no-cache\r\n"
+ "Content-Type: multipart/x-mixed-replace; "
+ "boundary=--boundary\r\n\r\n";
struct Frame {
std::string data;
@@ -58,11 +71,49 @@
std::ofstream ofst_;
};
+class UdpClient : public ::aos::events::EpollEvent {
+ public:
+ UdpClient(int port, ::std::function<void(void *, size_t)> callback)
+ : ::aos::events::EpollEvent(RXUdpSocket::SocketBindListenOnPort(port)),
+ callback_(callback) {}
+
+ private:
+ ::std::function<void(void *, size_t)> callback_;
+
+ void ReadEvent() override {
+ char data[1024];
+ size_t received_data_size = Recv(data, sizeof(data));
+ callback_(data, received_data_size);
+ }
+
+ size_t Recv(void *data, int size) {
+ return PCHECK(recv(fd(), static_cast<char *>(data), size, 0));
+ }
+};
+
+template <typename PB>
+class ProtoUdpClient : public UdpClient {
+ public:
+ ProtoUdpClient(int port, ::std::function<void(const PB &)> proto_callback)
+ : UdpClient(port, ::std::bind(&ProtoUdpClient::ReadData, this,
+ ::std::placeholders::_1,
+ ::std::placeholders::_2)),
+ proto_callback_(proto_callback) {}
+
+ private:
+ ::std::function<void(const PB &)> proto_callback_;
+
+ void ReadData(void *data, size_t size) {
+ PB pb;
+ pb.ParseFromArray(data, size);
+ proto_callback_(pb);
+ }
+};
+
class MjpegDataSocket : public aos::events::SocketConnection {
public:
-
- MjpegDataSocket(aos::events::TCPServerBase *serv, int fd)
- : aos::events::SocketConnection(serv, fd) {
+ MjpegDataSocket(aos::events::TCPServerBase *server, int fd)
+ : aos::events::SocketConnection(server, fd) {
SetEvents(EPOLLOUT | EPOLLET);
}
@@ -73,6 +124,12 @@
NewDataToSend();
events &= ~EPOLLOUT;
}
+ // Other end hung up. Ditch the connection.
+ if (events & EPOLLHUP) {
+ CloseConnection();
+ events &= ~EPOLLHUP;
+ return;
+ }
if (events) {
aos::events::EpollEvent::DirectEvent(events);
}
@@ -124,9 +181,10 @@
aos::vision::DataRef data = frame->data;
size_t n_written = snprintf(data_header_tmp_, sizeof(data_header_tmp_),
- "--boundary\r\n"\
- "Content-type: image/jpg\r\n"\
- "Content-Length: %zu\r\n\r\n", data.size());
+ "--boundary\r\n"
+ "Content-type: image/jpg\r\n"
+ "Content-Length: %zu\r\n\r\n",
+ data.size());
// This should never happen because the buffer should be properly sized.
if (n_written == sizeof(data_header_tmp_)) {
fprintf(stderr, "wrong sized buffer\n");
@@ -138,13 +196,11 @@
NewDataToSend();
}
- void NewFrame(std::shared_ptr<Frame> frame) {
- RasterFrame(std::move(frame));
- }
+ void NewFrame(std::shared_ptr<Frame> frame) { RasterFrame(std::move(frame)); }
void NewDataToSend() {
while (!output_buffer_.empty()) {
- auto& data = *output_buffer_.begin();
+ auto &data = *output_buffer_.begin();
while (!data.empty()) {
int len = send(fd(), data.data(), data.size(), MSG_NOSIGNAL);
@@ -178,16 +234,25 @@
size_t match_i_ = 0;
};
-using namespace aos::vision;
-class CameraStream : public ImageStreamEvent {
+class CameraStream : public ::aos::vision::ImageStreamEvent {
public:
- CameraStream(aos::vision::CameraParams params,
- const std::string &fname, TCPServer<MjpegDataSocket>* tcp_serv)
- : ImageStreamEvent(fname, params), tcp_serv_(tcp_serv),
- log_("./logging/blob_record_", ".dat") {}
+ CameraStream(::aos::vision::CameraParams params, const ::std::string &fname,
+ TCPServer<MjpegDataSocket> *tcp_server, bool log,
+ ::std::function<void()> frame_callback)
+ : ImageStreamEvent(fname, params),
+ tcp_server_(tcp_server),
+ frame_callback_(frame_callback) {
+ if (log) {
+ log_.reset(new BlobLog("./logging/blob_record_", ".dat"));
+ }
+ }
- void ProcessImage(DataRef data, aos::monotonic_clock::time_point tp) {
- (void)tp;
+ void set_active(bool active) { active_ = active; }
+
+ bool active() const { return active_; }
+
+ void ProcessImage(DataRef data,
+ monotonic_clock::time_point /*monotonic_now*/) {
++sampling;
// 20 is the sampling rate.
if (sampling == 20) {
@@ -199,36 +264,98 @@
buf = Int32Codec::Write(&log_record[0], tmp_size);
data.copy(buf, data.size());
}
- log_.WriteLogEntry(log_record);
+ if (log_) {
+ log_->WriteLogEntry(log_record);
+ }
sampling = 0;
}
- auto frame = std::make_shared<Frame>(Frame{std::string(data)});
- tcp_serv_->Broadcast([frame](MjpegDataSocket* event) {
- event->NewFrame(frame);
- });
+ if (active_) {
+ auto frame = std::make_shared<Frame>(Frame{std::string(data)});
+ tcp_server_->Broadcast(
+ [frame](MjpegDataSocket *event) { event->NewFrame(frame); });
+ }
+ frame_callback_();
}
+
private:
int sampling = 0;
- TCPServer<MjpegDataSocket>* tcp_serv_;
- BlobLog log_;
+ TCPServer<MjpegDataSocket> *tcp_server_;
+ ::std::unique_ptr<BlobLog> log_;
+ ::std::function<void()> frame_callback_;
+ bool active_ = false;
};
-int main() {
+int main(int argc, char ** argv) {
+ gflags::ParseCommandLineFlags(&argc, &argv, false);
::aos::logging::Init();
::aos::logging::AddImplementation(
new ::aos::logging::StreamLogImplementation(stderr));
- TCPServer<MjpegDataSocket> tcp_serv_(80);
- aos::vision::CameraParams params;
- params.set_exposure(100);
- params.set_width(320);
- params.set_height(240);
- CameraStream camera(params, "/dev/video0", &tcp_serv_);
+ TCPServer<MjpegDataSocket> tcp_server_(80);
+ aos::vision::CameraParams params0;
+ params0.set_exposure(FLAGS_camera0_exposure);
+ params0.set_brightness(-40);
+ params0.set_width(320);
+ //params0.set_fps(10);
+ params0.set_height(240);
+
+ aos::vision::CameraParams params1 = params0;
+ params1.set_exposure(FLAGS_camera1_exposure);
+
+ ::y2018::VisionStatus vision_status;
+ ::aos::events::ProtoTXUdpSocket<::y2018::VisionStatus> status_socket(
+ "10.9.71.2", 5001);
+
+ ::std::unique_ptr<CameraStream> camera1;
+ ::std::unique_ptr<CameraStream> camera0(new CameraStream(
+ params0, "/dev/video0", &tcp_server_, true,
+ [&camera0, &camera1, &status_socket, &vision_status]() {
+ vision_status.set_low_frame_count(vision_status.low_frame_count() + 1);
+ LOG(INFO, "Got a frame cam0\n");
+ if (camera0->active()) {
+ status_socket.Send(vision_status);
+ }
+ }));
+ if (!FLAGS_single_camera) {
+ camera1.reset(new CameraStream(
+ // params,
+ // "/dev/v4l/by-path/platform-tegra-xhci-usb-0:3.1:1.0-video-index0",
+ params1, "/dev/video1", &tcp_server_, false,
+ [&camera0, &camera1, &status_socket, &vision_status]() {
+ vision_status.set_high_frame_count(vision_status.high_frame_count() +
+ 1);
+ LOG(INFO, "Got a frame cam1\n");
+ if (camera1->active()) {
+ status_socket.Send(vision_status);
+ }
+ }));
+ }
+
+ ProtoUdpClient<VisionControl> udp_client(
+ 5000, [&camera0, &camera1](const VisionControl &vision_control) {
+ LOG(INFO, "Got control packet\n");
+ if (camera1) {
+ camera0->set_active(!vision_control.high_video());
+ camera1->set_active(vision_control.high_video());
+ } else {
+ camera0->set_active(true);
+ }
+ });
+
+ // Default to camera0
+ camera0->set_active(true);
+ if (camera1) {
+ camera1->set_active(false);
+ }
aos::events::EpollLoop loop;
- loop.Add(&tcp_serv_);
- loop.Add(&camera);
+ loop.Add(&tcp_server_);
+ loop.Add(camera0.get());
+ if (camera1) {
+ loop.Add(camera1.get());
+ }
+ loop.Add(&udp_client);
printf("Running Camera\n");
loop.Run();