Use a second camera and switch between them.
Also, blink out the state over the beacon.
Change-Id: If606dfed9ae64137f71429f1190f04d5dac2c4ec
diff --git a/y2018/vision/BUILD b/y2018/vision/BUILD
index 11fb7c5..dfe5ebb 100644
--- a/y2018/vision/BUILD
+++ b/y2018/vision/BUILD
@@ -1,13 +1,43 @@
+load("//aos/build:queues.bzl", "queue_library")
+
cc_binary(
- name = "image_streamer",
- srcs = ["image_streamer.cc"],
- deps = [
- "//aos/vision/events:socket_types",
- '//aos/vision/events:epoll_events',
- '//aos/vision/image:reader',
- '//aos/vision/image:image_stream',
- '//aos/vision/blob:codec',
- '//aos/common/logging:logging',
- '//aos/common/logging:implementations',
- ],
+ name = "image_streamer",
+ srcs = ["image_streamer.cc"],
+ deps = [
+ "//aos/common/logging",
+ "//aos/common/logging:implementations",
+ "//aos/vision/blob:codec",
+ "//aos/vision/events:epoll_events",
+ "//aos/vision/events:socket_types",
+ "//aos/vision/events:udp",
+ "//aos/vision/image:image_stream",
+ "//aos/vision/image:reader",
+ "//third_party/gflags",
+ "//y2018:vision_proto",
+ ],
+)
+
+queue_library(
+ name = "vision_queue",
+ srcs = [
+ "vision.q",
+ ],
+ visibility = ["//visibility:public"],
+)
+
+cc_binary(
+ name = "vision_status",
+ srcs = [
+ "vision_status.cc",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":vision_queue",
+ "//aos/common:time",
+ "//aos/common/logging",
+ "//aos/common/logging:queue_logging",
+ "//aos/linux_code:init",
+ "//aos/vision/events:udp",
+ "//y2018:vision_proto",
+ ],
)
diff --git a/y2018/vision/exposure_2018.sh b/y2018/vision/exposure_2018.sh
index c96f5f0..bfcd31c 100755
--- a/y2018/vision/exposure_2018.sh
+++ b/y2018/vision/exposure_2018.sh
@@ -2,18 +2,20 @@
set -e
-A=`ls /dev/video*`
EXPOSURE='100'
echo $SHELL
-echo $A
# Michael added this one to try and have the exposer set correctly sooner.
sleep 1
echo Setting exposure again after 1 seconds
-v4l2-ctl --set-ctrl="exposure_absolute=$EXPOSURE" -d $A
+for CAMERA in /dev/video0 /dev/video1
+do
+ echo "${CAMERA}"
+ v4l2-ctl --set-ctrl="exposure_absolute=$EXPOSURE" -d "${CAMERA}"
+done
echo Done setting exposure again after 1 seconds
@@ -21,8 +23,11 @@
# Michael added this one to try and have the exposer set correctly sooner.
sleep 5
echo Setting exposure again after 5 seconds
-v4l2-ctl --set-ctrl="exposure_absolute=$EXPOSURE" -d $A
-
+for CAMERA in /dev/video0 /dev/video1
+do
+ echo "${CAMERA}"
+ v4l2-ctl --set-ctrl="exposure_absolute=$EXPOSURE" -d "${CAMERA}"
+done
echo Done setting exposure again after 5 seconds
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();
diff --git a/y2018/vision/startup.sh b/y2018/vision/startup.sh
index 6cb211a..7122c01 100755
--- a/y2018/vision/startup.sh
+++ b/y2018/vision/startup.sh
@@ -30,25 +30,25 @@
# echo All done setting exposure
# echo "Starting target sender now."
-A=`ls /dev/video*`
-echo $SHELL
+for CAMERA in /dev/video0 /dev/video1
+do
+ echo $CAMERA
-echo $A
+ v4l2-ctl --set-ctrl="exposure_auto=1" -d $CAMERA
+ sleep 0.5
+ v4l2-ctl --set-ctrl="exposure_absolute=100" -d $CAMERA
+ sleep 0.5
-v4l2-ctl --set-ctrl="exposure_auto=1" -d $A
-sleep 0.5
-v4l2-ctl --set-ctrl="exposure_absolute=100" -d $A
-sleep 0.5
+ PATH="./;$PATH"
-PATH="./;$PATH"
-
-/root/camera_primer $A
+ /root/camera_primer $CAMERA
+done
# Run a script to reset the exposure a few times and exit.
/root/exposure_2018.sh &
-/root/image_streamer
+exec /root/image_streamer --single_camera=false
#exec ./target_sender Practice
#exec ./target_sender Comp
#exec ./target_sender Spare
diff --git a/y2018/vision/vision.q b/y2018/vision/vision.q
new file mode 100644
index 0000000..a3b57ea
--- /dev/null
+++ b/y2018/vision/vision.q
@@ -0,0 +1,7 @@
+package y2018.vision;
+
+message VisionStatus {
+ uint32_t high_frame_count;
+ uint32_t low_frame_count;
+};
+queue VisionStatus vision_status;
diff --git a/y2018/vision/vision_status.cc b/y2018/vision/vision_status.cc
new file mode 100644
index 0000000..e8ac6db
--- /dev/null
+++ b/y2018/vision/vision_status.cc
@@ -0,0 +1,42 @@
+#include <netdb.h>
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
+#include "aos/common/time.h"
+#include "aos/linux_code/init.h"
+#include "aos/vision/events/udp.h"
+#include "y2018/vision.pb.h"
+#include "y2018/vision/vision.q.h"
+
+namespace y2018 {
+namespace vision {
+
+using aos::monotonic_clock;
+
+int Main() {
+ ::aos::events::RXUdpSocket video_rx(5001);
+ char data[65507];
+ ::y2018::VisionStatus status;
+
+ while (true) {
+ const ssize_t rx_size = video_rx.Recv(data, sizeof(data));
+ if (rx_size > 0) {
+ status.ParseFromArray(data, rx_size);
+ auto new_vision_status = vision_status.MakeMessage();
+ new_vision_status->high_frame_count = status.high_frame_count();
+ new_vision_status->low_frame_count = status.low_frame_count();
+ LOG_STRUCT(DEBUG, "vision", *new_vision_status);
+ if (!new_vision_status.Send()) {
+ LOG(ERROR, "Failed to send vision information\n");
+ }
+ }
+ }
+}
+
+} // namespace vision
+} // namespace y2018
+
+int main(int /*argc*/, char ** /*argv*/) {
+ ::aos::InitNRT();
+ ::y2018::vision::Main();
+}