Use a second camera and switch between them.
Also, blink out the state over the beacon.
Change-Id: If606dfed9ae64137f71429f1190f04d5dac2c4ec
diff --git a/aos/vision/events/udp.cc b/aos/vision/events/udp.cc
index 4c0437b..660342c 100644
--- a/aos/vision/events/udp.cc
+++ b/aos/vision/events/udp.cc
@@ -26,8 +26,8 @@
return send(fd_.get(), data, size, 0);
}
-RXUdpSocket::RXUdpSocket(int port)
- : fd_(PCHECK(socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))) {
+int RXUdpSocket::SocketBindListenOnPort(int port) {
+ int fd = PCHECK(socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP));
sockaddr_in bind_address;
memset(&bind_address, 0, sizeof(bind_address));
@@ -35,10 +35,13 @@
bind_address.sin_port = htons(port);
bind_address.sin_addr.s_addr = htonl(INADDR_ANY);
- PCHECK(bind(fd_.get(), reinterpret_cast<sockaddr *>(&bind_address),
+ PCHECK(bind(fd, reinterpret_cast<sockaddr *>(&bind_address),
sizeof(bind_address)));
+ return fd;
}
+RXUdpSocket::RXUdpSocket(int port) : fd_(SocketBindListenOnPort(port)) {}
+
int RXUdpSocket::Recv(void *data, int size) {
return PCHECK(recv(fd_.get(), static_cast<char *>(data), size, 0));
}
diff --git a/aos/vision/events/udp.h b/aos/vision/events/udp.h
index ed4a8e7..9a23dab 100644
--- a/aos/vision/events/udp.h
+++ b/aos/vision/events/udp.h
@@ -30,6 +30,24 @@
DISALLOW_COPY_AND_ASSIGN(TXUdpSocket);
};
+// Send a protobuf. Not RT (mallocs on send).
+template <typename PB>
+class ProtoTXUdpSocket {
+ public:
+ ProtoTXUdpSocket(const std::string &ip_addr, int port)
+ : socket_(ip_addr, port) {}
+
+ void Send(const PB &pb) {
+ ::std::string serialized_data;
+ pb.SerializeToString(&serialized_data);
+ socket_.Send(serialized_data.data(), serialized_data.size());
+ }
+
+ private:
+ TXUdpSocket socket_;
+ DISALLOW_COPY_AND_ASSIGN(ProtoTXUdpSocket);
+};
+
// Simple wrapper around a receiving UDP socket.
//
// LOG(FATAL)s for all errors, including from Recv.
@@ -40,6 +58,8 @@
// Returns the number of bytes received.
int Recv(void *data, int size);
+ static int SocketBindListenOnPort(int port);
+
private:
ScopedFD fd_;
diff --git a/y2018/BUILD b/y2018/BUILD
index 67ad583..4edb57e 100644
--- a/y2018/BUILD
+++ b/y2018/BUILD
@@ -1,5 +1,6 @@
load("//aos/downloader:downloader.bzl", "aos_downloader")
load("//aos/build:queues.bzl", "queue_library")
+load("//tools/build_rules:protobuf.bzl", "proto_cc_library")
aos_downloader(
name = "download",
@@ -10,6 +11,7 @@
start_srcs = [
":joystick_reader",
":wpilib_interface",
+ "//y2018/vision:vision_status",
"//aos:prime_start_binaries",
"//y2018/control_loops/drivetrain:drivetrain",
"//y2018/actors:autonomous_action",
@@ -26,6 +28,7 @@
start_srcs = [
":joystick_reader.stripped",
":wpilib_interface.stripped",
+ "//y2018/vision:vision_status.stripped",
"//aos:prime_start_binaries_stripped",
"//y2018/actors:autonomous_action.stripped",
"//y2018/control_loops/drivetrain:drivetrain.stripped",
@@ -39,7 +42,8 @@
"joystick_reader.cc",
],
deps = [
- ":status_light",
+ ":vision_proto",
+ "//aos/common:stl_mutex",
"//aos/common:time",
"//aos/common/actions:action_lib",
"//aos/common/logging",
@@ -47,6 +51,7 @@
"//aos/input:drivetrain_input",
"//aos/input:joystick_input",
"//aos/linux_code:init",
+ "//aos/vision/events:udp",
"//frc971/autonomous:auto_queue",
"//frc971/autonomous:base_autonomous_actor",
"//frc971/control_loops/drivetrain:drivetrain_queue",
@@ -116,6 +121,7 @@
"//third_party/Phoenix-frc-lib:phoenix",
"//y2018:constants",
"//y2018/control_loops/superstructure:superstructure_queue",
+ "//y2018/vision:vision_queue",
],
)
@@ -124,4 +130,11 @@
srcs = [
"status_light.q",
],
+ visibility = ["//visibility:public"],
+)
+
+proto_cc_library(
+ name = "vision_proto",
+ src = "vision.proto",
+ visibility = ["//visibility:public"],
)
diff --git a/y2018/control_loops/superstructure/BUILD b/y2018/control_loops/superstructure/BUILD
index 8fcba95..cc179d5 100644
--- a/y2018/control_loops/superstructure/BUILD
+++ b/y2018/control_loops/superstructure/BUILD
@@ -25,9 +25,12 @@
":superstructure_queue",
"//aos/common/controls:control_loop",
"//frc971/control_loops:queues",
+ "//frc971/control_loops/drivetrain:drivetrain_queue",
"//y2018:constants",
+ "//y2018:status_light",
"//y2018/control_loops/superstructure/arm",
"//y2018/control_loops/superstructure/intake",
+ "//y2018/vision:vision_queue",
],
)
diff --git a/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
index 757b166..2098cd0 100644
--- a/y2018/control_loops/superstructure/superstructure.cc
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -5,8 +5,11 @@
#include "aos/common/controls/control_loops.q.h"
#include "aos/common/logging/logging.h"
#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2018/constants.h"
#include "y2018/control_loops/superstructure/intake/intake.h"
+#include "y2018/status_light.q.h"
+#include "y2018/vision/vision.q.h"
namespace y2018 {
namespace control_loops {
@@ -21,6 +24,17 @@
constexpr double kMaxIntakeRollerVoltage = 12.0;
} // namespace
+void SendColors(float red, float green, float blue) {
+ auto new_status_light = status_light.MakeMessage();
+ new_status_light->red = red;
+ new_status_light->green = green;
+ new_status_light->blue = blue;
+
+ if (!new_status_light.Send()) {
+ LOG(ERROR, "Failed to send lights.\n");
+ }
+}
+
Superstructure::Superstructure(
control_loops::SuperstructureQueue *superstructure_queue)
: aos::controls::ControlLoop<control_loops::SuperstructureQueue>(
@@ -237,6 +251,34 @@
stuck_count_ = 0;
}
status->rotation_state = static_cast<uint32_t>(rotation_state_);
+
+ ::frc971::control_loops::drivetrain_queue.output.FetchLatest();
+
+ ::y2018::vision::vision_status.FetchLatest();
+ if (status->estopped) {
+ SendColors(0.5, 0.0, 0.0);
+ } else if (!y2018::vision::vision_status.get() ||
+ y2018::vision::vision_status.Age() > chrono::seconds(1)) {
+ SendColors(0.5, 0.5, 0.0);
+ } else if (rotation_state_ == RotationState::ROTATING_LEFT ||
+ rotation_state_ == RotationState::ROTATING_RIGHT) {
+ SendColors(0.5, 0.20, 0.0);
+ } else if (rotation_state_ == RotationState::STUCK) {
+ SendColors(0.5, 0.0, 0.5);
+ } else if (position->box_back_beambreak_triggered) {
+ SendColors(0.0, 0.0, 0.5);
+ } else if (position->box_distance < 0.2) {
+ SendColors(0.0, 0.5, 0.0);
+ } else if (::frc971::control_loops::drivetrain_queue.output.get() &&
+ ::std::max(::std::abs(::frc971::control_loops::drivetrain_queue
+ .output->left_voltage),
+ ::std::abs(::frc971::control_loops::drivetrain_queue
+ .output->right_voltage)) > 11.5) {
+ SendColors(0.5, 0.0, 0.5);
+ } else {
+ SendColors(0.0, 0.0, 0.0);
+ }
+
last_box_distance_ = clipped_box_distance;
}
diff --git a/y2018/control_loops/superstructure/superstructure_lib_test.cc b/y2018/control_loops/superstructure/superstructure_lib_test.cc
index 639a683..d21d1ce 100644
--- a/y2018/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2018/control_loops/superstructure/superstructure_lib_test.cc
@@ -7,6 +7,7 @@
#include "aos/common/controls/control_loop_test.h"
#include "aos/common/queue.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
@@ -14,6 +15,8 @@
#include "y2018/control_loops/superstructure/arm/dynamics.h"
#include "y2018/control_loops/superstructure/arm/generated_graph.h"
#include "y2018/control_loops/superstructure/intake/intake_plant.h"
+#include "y2018/status_light.q.h"
+#include "y2018/vision/vision.q.h"
using ::frc971::control_loops::PositionSensorSimulator;
@@ -271,6 +274,9 @@
".y2018.control_loops.superstructure.output",
".y2018.control_loops.superstructure.status"),
superstructure_(&superstructure_queue_) {
+ status_light.Clear();
+ ::y2018::vision::vision_status.Clear();
+ ::frc971::control_loops::drivetrain_queue.output.Clear();
set_team_id(::frc971::control_loops::testing::kTeamNumber);
}
diff --git a/y2018/joystick_reader.cc b/y2018/joystick_reader.cc
index 4e3103b..79056e2 100644
--- a/y2018/joystick_reader.cc
+++ b/y2018/joystick_reader.cc
@@ -2,26 +2,31 @@
#include <stdio.h>
#include <string.h>
#include <unistd.h>
+#include <mutex>
#include "aos/common/actions/actions.h"
#include "aos/common/input/driver_station_data.h"
#include "aos/common/logging/logging.h"
+#include "aos/common/stl_mutex.h"
#include "aos/common/time.h"
#include "aos/common/util/log_interval.h"
#include "aos/input/drivetrain_input.h"
#include "aos/input/joystick_input.h"
#include "aos/linux_code/init.h"
+#include "aos/vision/events/udp.h"
#include "frc971/autonomous/auto.q.h"
#include "frc971/autonomous/base_autonomous_actor.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2018/control_loops/drivetrain/drivetrain_base.h"
#include "y2018/control_loops/superstructure/arm/generated_graph.h"
#include "y2018/control_loops/superstructure/superstructure.q.h"
-#include "y2018/status_light.q.h"
+#include "y2018/vision.pb.h"
+using ::aos::monotonic_clock;
using ::frc971::control_loops::drivetrain_queue;
using ::y2018::control_loops::superstructure_queue;
-
+using ::aos::events::ProtoTXUdpSocket;
+using ::aos::events::RXUdpSocket;
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::ControlBit;
using ::aos::input::driver_station::JoystickAxis;
@@ -78,7 +83,7 @@
class Reader : public ::aos::input::JoystickInput {
public:
- Reader() {
+ Reader() : video_tx_("10.9.71.179", 5000) {
drivetrain_input_reader_ = DrivetrainInputReader::Make(
DrivetrainInputReader::InputType::kPistol,
::y2018::control_loops::drivetrain::GetDrivetrainConfig());
@@ -110,17 +115,6 @@
drivetrain_input_reader_->HandleDrivetrain(data);
}
- void SendColors(float red, float green, float blue) {
- auto new_status_light = status_light.MakeMessage();
- new_status_light->red = red;
- new_status_light->green = green;
- new_status_light->blue = blue;
-
- if (!new_status_light.Send()) {
- LOG(ERROR, "Failed to send lights.\n");
- }
- }
-
void HandleTeleop(const ::aos::input::driver_station::Data &data) {
if (!data.GetControlBit(ControlBit::kEnabled)) {
action_queue_.CancelAllActions();
@@ -140,6 +134,11 @@
new_superstructure_goal->intake.left_intake_angle = intake_goal_;
new_superstructure_goal->intake.right_intake_angle = intake_goal_;
+ if (data.PosEdge(kIntakeIn) || data.PosEdge(kSmallBox) ||
+ data.PosEdge(kIntakeClosed)) {
+ vision_control_.set_high_video(false);
+ }
+
if (data.IsPressed(kIntakeIn)) {
// Turn on the rollers.
new_superstructure_goal->intake.roller_voltage = 8.0;
@@ -151,14 +150,6 @@
new_superstructure_goal->intake.roller_voltage = 0.0;
}
- if (superstructure_queue.position->box_back_beambreak_triggered) {
- SendColors(0.0, 0.5, 0.0);
- } else if (superstructure_queue.position->box_distance < 0.2) {
- SendColors(0.0, 0.0, 0.5);
- } else {
- SendColors(0.0, 0.0, 0.0);
- }
-
if (data.IsPressed(kSmallBox)) {
// Deploy the intake.
if (superstructure_queue.position->box_back_beambreak_triggered) {
@@ -251,24 +242,33 @@
}
}
} else if (data.PosEdge(kArmPickupBoxFromIntake)) {
+ vision_control_.set_high_video(false);
arm_goal_position_ = arm::NeutralIndex();
} else if (data.IsPressed(kDuck)) {
+ vision_control_.set_high_video(false);
arm_goal_position_ = arm::DuckIndex();
} else if (data.IsPressed(kArmNeutral)) {
+ vision_control_.set_high_video(false);
arm_goal_position_ = arm::NeutralIndex();
} else if (data.IsPressed(kArmUp)) {
+ vision_control_.set_high_video(true);
arm_goal_position_ = arm::UpIndex();
} else if (data.IsPressed(kArmFrontSwitch)) {
arm_goal_position_ = arm::FrontSwitchIndex();
} else if (data.IsPressed(kArmFrontExtraHighBox)) {
+ vision_control_.set_high_video(true);
arm_goal_position_ = arm::FrontHighBoxIndex();
} else if (data.IsPressed(kArmFrontHighBox)) {
+ vision_control_.set_high_video(true);
arm_goal_position_ = arm::FrontMiddle2BoxIndex();
} else if (data.IsPressed(kArmFrontMiddle2Box)) {
+ vision_control_.set_high_video(true);
arm_goal_position_ = arm::FrontMiddle3BoxIndex();
} else if (data.IsPressed(kArmFrontMiddle1Box)) {
+ vision_control_.set_high_video(true);
arm_goal_position_ = arm::FrontMiddle1BoxIndex();
} else if (data.IsPressed(kArmFrontLowBox)) {
+ vision_control_.set_high_video(true);
arm_goal_position_ = arm::FrontLowBoxIndex();
} else if (data.IsPressed(kArmBackExtraHighBox)) {
arm_goal_position_ = arm::BackHighBoxIndex();
@@ -324,6 +324,8 @@
if (!new_superstructure_goal.Send()) {
LOG(ERROR, "Sending superstructure goal failed.\n");
}
+
+ video_tx_.Send(vision_control_);
}
private:
@@ -358,11 +360,14 @@
bool never_disabled_ = true;
uint32_t arm_goal_position_ = 0;
+ VisionControl vision_control_;
decltype(FrontPoints()) front_points_ = FrontPoints();
decltype(BackPoints()) back_points_ = BackPoints();
::aos::common::actions::ActionQueue action_queue_;
+
+ ProtoTXUdpSocket<VisionControl> video_tx_;
};
} // namespace joysticks
diff --git a/y2018/vision.proto b/y2018/vision.proto
new file mode 100644
index 0000000..04b3181
--- /dev/null
+++ b/y2018/vision.proto
@@ -0,0 +1,12 @@
+syntax = "proto2";
+
+package y2018;
+
+message VisionControl {
+ optional bool high_video = 1;
+}
+
+message VisionStatus {
+ optional uint32 high_frame_count = 1;
+ optional uint32 low_frame_count = 2;
+}
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();
+}
diff --git a/y2018/wpilib_interface.cc b/y2018/wpilib_interface.cc
index f204605..ec54da8 100644
--- a/y2018/wpilib_interface.cc
+++ b/y2018/wpilib_interface.cc
@@ -52,6 +52,7 @@
#include "y2018/constants.h"
#include "y2018/control_loops/superstructure/superstructure.q.h"
#include "y2018/status_light.q.h"
+#include "y2018/vision/vision.q.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
@@ -660,35 +661,66 @@
LOG_STRUCT(DEBUG, "pneumatics info", to_log);
}
- static double last_red = -1.0;
- static double last_green = -1.0;
- static double last_blue = -1.0;
status_light.FetchLatest();
- if (status_light.get()) {
- LOG_STRUCT(DEBUG, "writing", *status_light);
- // Not sure which of these is red vs green. We're not ready to use
- // either, so just turn them off.
- if (status_light->green != last_green) {
- canifier_.SetLEDOutput(1.0 - status_light->green,
- ::ctre::phoenix::CANifier::LEDChannelA);
- last_green = status_light->green;
+ // If we don't have a light request (or it's an old one), we are borked.
+ // Flash the red light slowly.
+ if (!status_light.get() ||
+ status_light.Age() > chrono::milliseconds(100)) {
+ StatusLight color;
+ color.red = 0.0;
+ color.green = 0.0;
+ color.blue = 0.0;
+
+ ::y2018::vision::vision_status.FetchLatest();
+ ++light_flash_;
+ if (light_flash_ > 10) {
+ color.red = 0.5;
+ } else if (!y2018::vision::vision_status.get() ||
+ y2018::vision::vision_status.Age() > chrono::seconds(1)) {
+ color.red = 0.5;
+ color.green = 0.5;
}
- if (status_light->blue != last_blue) {
- canifier_.SetLEDOutput(1.0 - status_light->blue,
- ::ctre::phoenix::CANifier::LEDChannelB);
- last_blue = status_light->blue;
+ if (light_flash_ > 20) {
+ light_flash_ = 0;
}
- if (status_light->red != last_red) {
- canifier_.SetLEDOutput(1.0 - status_light->red,
- ::ctre::phoenix::CANifier::LEDChannelC);
- last_red = status_light->red;
- }
+ LOG_STRUCT(DEBUG, "color", color);
+ SetColor(color);
+ } else {
+ LOG_STRUCT(DEBUG, "color", *status_light);
+ SetColor(*status_light);
}
}
}
+ void SetColor(const StatusLight &status_light) {
+ // Save CAN bandwidth and CPU at the cost of RT. Only change the light when
+ // it actually changes. This is pretty low priority anyways.
+ static int time_since_last_send = 0;
+ ++time_since_last_send;
+ if (time_since_last_send > 10) {
+ time_since_last_send = 0;
+ }
+ if (status_light.green != last_green_ || time_since_last_send == 0) {
+ canifier_.SetLEDOutput(1.0 - status_light.green,
+ ::ctre::phoenix::CANifier::LEDChannelB);
+ last_green_ = status_light.green;
+ }
+
+ if (status_light.blue != last_blue_ || time_since_last_send == 0) {
+ canifier_.SetLEDOutput(1.0 - status_light.blue,
+ ::ctre::phoenix::CANifier::LEDChannelA);
+ last_blue_ = status_light.blue;
+ }
+
+ if (status_light.red != last_red_ || time_since_last_send == 0) {
+ canifier_.SetLEDOutput(1.0 - status_light.red,
+ ::ctre::phoenix::CANifier::LEDChannelC);
+ last_red_ = status_light.red;
+ }
+ }
+
void Quit() { run_ = false; }
private:
@@ -705,6 +737,12 @@
::ctre::phoenix::CANifier canifier_{0};
::std::atomic<bool> run_{true};
+
+ double last_red_ = -1.0;
+ double last_green_ = -1.0;
+ double last_blue_ = -1.0;
+
+ int light_flash_ = 0;
};
class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler {