Merge "Use protobuf to generate websocket JSON"
diff --git a/aos/input/action_joystick_input.cc b/aos/input/action_joystick_input.cc
index 81e277d..061f4dc 100644
--- a/aos/input/action_joystick_input.cc
+++ b/aos/input/action_joystick_input.cc
@@ -43,7 +43,8 @@
void ActionJoystickInput::StartAuto() {
LOG(INFO, "Starting auto mode\n");
- action_queue_.EnqueueAction(::frc971::autonomous::MakeAutonomousAction(0));
+ action_queue_.EnqueueAction(
+ ::frc971::autonomous::MakeAutonomousAction(GetAutonomousMode()));
}
void ActionJoystickInput::StopAuto() {
diff --git a/aos/input/action_joystick_input.h b/aos/input/action_joystick_input.h
index 87b980e..dca4d39 100644
--- a/aos/input/action_joystick_input.h
+++ b/aos/input/action_joystick_input.h
@@ -40,6 +40,10 @@
void StartAuto();
void StopAuto();
+ // Returns the current autonomous mode which has been selected by robot
+ // inputs.
+ virtual uint32_t GetAutonomousMode() { return 0; }
+
// True if the internal state machine thinks auto is running right now.
bool auto_running_ = false;
// True if an action was running last cycle.
diff --git a/aos/scoped/BUILD b/aos/scoped/BUILD
index 002026e..f5b3832 100644
--- a/aos/scoped/BUILD
+++ b/aos/scoped/BUILD
@@ -2,6 +2,9 @@
cc_library(
name = "scoped_fd",
+ srcs = [
+ "scoped_fd.cc",
+ ],
hdrs = [
"scoped_fd.h",
],
diff --git a/aos/scoped/scoped_fd.cc b/aos/scoped/scoped_fd.cc
new file mode 100644
index 0000000..a570df0
--- /dev/null
+++ b/aos/scoped/scoped_fd.cc
@@ -0,0 +1,15 @@
+#include "aos/scoped/scoped_fd.h"
+
+#include "aos/logging/logging.h"
+
+namespace aos {
+
+void ScopedFD::Close() {
+ if (fd_ != -1) {
+ if (close(fd_) == -1) {
+ PLOG(WARNING, "close(%d) failed", fd_);
+ }
+ }
+}
+
+} // namespace aos
diff --git a/aos/scoped/scoped_fd.h b/aos/scoped/scoped_fd.h
index 696cf3b..e098d2c 100644
--- a/aos/scoped/scoped_fd.h
+++ b/aos/scoped/scoped_fd.h
@@ -1,9 +1,8 @@
-#ifndef _AOS_SCOPED_FD_
-#define _AOS_SCOPED_FD_
+#ifndef AOS_SCOPED_SCOPED_FD_H_
+#define AOS_SCOPED_SCOPED_FD_H_
#include <unistd.h>
-#include "aos/logging/logging.h"
#include "aos/macros.h"
namespace aos {
@@ -29,16 +28,10 @@
private:
int fd_;
- void Close() {
- if (fd_ != -1) {
- if (close(fd_) == -1) {
- PLOG(WARNING, "close(%d) failed", fd_);
- }
- }
- }
+ void Close();
DISALLOW_COPY_AND_ASSIGN(ScopedFD);
};
} // namespace aos
-#endif // _AOS_SCOPED_FD_
+#endif // AOS_SCOPED_SCOPED_FD_H_
diff --git a/aos/util/BUILD b/aos/util/BUILD
index af6faf5..7b50f49 100644
--- a/aos/util/BUILD
+++ b/aos/util/BUILD
@@ -295,6 +295,7 @@
"file.h",
],
deps = [
+ "//aos/logging",
"//aos/scoped:scoped_fd",
],
)
diff --git a/aos/util/file.cc b/aos/util/file.cc
index dc31ddd..f952955 100644
--- a/aos/util/file.cc
+++ b/aos/util/file.cc
@@ -3,6 +3,7 @@
#include <fcntl.h>
#include <unistd.h>
+#include "aos/logging/logging.h"
#include "aos/scoped/scoped_fd.h"
namespace aos {
diff --git a/aos/vision/blob/BUILD b/aos/vision/blob/BUILD
index 81afb93..7d5f930 100644
--- a/aos/vision/blob/BUILD
+++ b/aos/vision/blob/BUILD
@@ -18,9 +18,6 @@
cc_library(
name = "region_alloc",
hdrs = ["region_alloc.h"],
- deps = [
- "//aos/logging",
- ],
)
cc_library(
diff --git a/aos/vision/blob/region_alloc.h b/aos/vision/blob/region_alloc.h
index 8c7bc57..df33fed 100644
--- a/aos/vision/blob/region_alloc.h
+++ b/aos/vision/blob/region_alloc.h
@@ -9,8 +9,6 @@
#include <utility>
#include <vector>
-#include "aos/logging/logging.h"
-
namespace aos {
namespace vision {
@@ -22,12 +20,12 @@
T *cons_obj(Args &&... args) {
uint8_t *ptr = NULL;
if (sizeof(T) + alignof(T) > block_size_) {
- LOG(FATAL, "allocating %d too much\n", (int)sizeof(T));
+ __builtin_trap();
}
while (ptr == NULL) {
if (next_free_ >= memory_.size()) {
if (next_free_ >= 1024) {
- LOG(FATAL, "too much alloc\n");
+ __builtin_trap();
}
memory_.emplace_back(new uint8_t[block_size_]);
} else if ((used_size_ % alignof(T)) != 0) {
diff --git a/aos/vision/debug/BUILD b/aos/vision/debug/BUILD
index c550de7..05d93ad 100644
--- a/aos/vision/debug/BUILD
+++ b/aos/vision/debug/BUILD
@@ -26,8 +26,8 @@
gtk_dependent_cc_library(
name = "debug_framework",
srcs = [
- "blob_log-source.cc",
"aveugle-source.cc",
+ "blob_log-source.cc",
"camera-source.cc",
"debug_framework.cc",
"jpeg_list-source.cc",
@@ -45,10 +45,11 @@
"//aos/vision/events:epoll_events",
"//aos/vision/events:gtk_event",
"//aos/vision/events:tcp_client",
- "//aos/vision/image:image_stream",
"//aos/vision/image:image_dataset",
+ "//aos/vision/image:image_stream",
"//aos/vision/image:image_types",
"//aos/vision/image:jpeg_routines",
+ "@com_github_google_glog//:glog",
"@usr_repo//:gtk+-3.0",
],
alwayslink = 1,
diff --git a/aos/vision/debug/blob_log-source.cc b/aos/vision/debug/blob_log-source.cc
index 337a764..26706bb 100644
--- a/aos/vision/debug/blob_log-source.cc
+++ b/aos/vision/debug/blob_log-source.cc
@@ -12,6 +12,8 @@
#include "aos/vision/blob/stream_view.h"
#include "aos/vision/debug/overlay.h"
+#include "glog/logging.h"
+
namespace aos {
namespace vision {
@@ -52,7 +54,7 @@
InputFile(const std::string &fname)
: ifs_(fname, std::ifstream::in), len_(GetFileSize(fname)) {
if (len_ <= 0) {
- LOG(FATAL, "File (%s) not found. Size (%d)\n", fname.c_str(), (int)len_);
+ LOG(FATAL) << "File (" << fname << ") not found. Size (" << len_ << ")";
}
// assert(len_ > 0);
tmp_buf_.resize(len_, 0);
diff --git a/aos/vision/events/BUILD b/aos/vision/events/BUILD
index c05fc09..b887872 100644
--- a/aos/vision/events/BUILD
+++ b/aos/vision/events/BUILD
@@ -69,6 +69,7 @@
srcs = ["gtk_event.cc"],
deps = [
":epoll_events",
+ "//aos/logging",
"@usr_repo//:gtk+-3.0",
],
)
diff --git a/aos/vision/events/epoll_events.cc b/aos/vision/events/epoll_events.cc
index 0043b78..f6cba76 100644
--- a/aos/vision/events/epoll_events.cc
+++ b/aos/vision/events/epoll_events.cc
@@ -12,6 +12,18 @@
namespace aos {
namespace events {
+void EpollEvent::DirectEvent(uint32_t events) {
+ if ((events & ~(EPOLLIN | EPOLLPRI | EPOLLERR)) != 0) {
+ LOG(FATAL, "unexpected epoll events set in %x on %d\n", events, fd());
+ }
+ ReadEvent();
+}
+
+void EpollEvent::SetEvents(uint32_t events) {
+ events_ |= events;
+ CHECK(!loop_);
+}
+
EpollLoop::EpollLoop() : epoll_fd_(PCHECK(epoll_create1(0))) {}
void EpollLoop::Add(EpollEvent *event) {
diff --git a/aos/vision/events/epoll_events.h b/aos/vision/events/epoll_events.h
index 19851ed..c7aa1c6 100644
--- a/aos/vision/events/epoll_events.h
+++ b/aos/vision/events/epoll_events.h
@@ -75,20 +75,11 @@
virtual void ReadEvent() = 0;
// Handle Events directly from epoll.
- virtual void DirectEvent(uint32_t events) {
- if ((events & ~(EPOLLIN | EPOLLPRI | EPOLLERR)) != 0) {
- LOG(FATAL, "unexpected epoll events set in %x on %d\n",
- events, fd());
- }
- ReadEvent();
- }
+ virtual void DirectEvent(uint32_t events);
EpollLoop *loop() { return loop_; }
- void SetEvents(uint32_t events) {
- events_ |= events;
- CHECK(!loop_);
- }
+ void SetEvents(uint32_t events);
uint32_t events() const { return events_; }
diff --git a/aos/vision/events/gtk_event.cc b/aos/vision/events/gtk_event.cc
index 0c518e0..4ff9fde 100644
--- a/aos/vision/events/gtk_event.cc
+++ b/aos/vision/events/gtk_event.cc
@@ -6,6 +6,7 @@
#include <thread>
#include "aos/vision/events/epoll_events.h"
+#include "aos/logging/logging.h"
namespace aos {
namespace events {
diff --git a/aos/vision/image/BUILD b/aos/vision/image/BUILD
index be702d7..21dbbb8 100644
--- a/aos/vision/image/BUILD
+++ b/aos/vision/image/BUILD
@@ -5,9 +5,6 @@
cc_library(
name = "image_types",
hdrs = ["image_types.h"],
- deps = [
- "//aos/logging",
- ],
)
cc_proto_library(
@@ -52,8 +49,10 @@
cc_library(
name = "image_stream",
+ srcs = ["image_stream.cc"],
hdrs = ["image_stream.h"],
deps = [
+ "//aos/logging",
"//aos/vision/events:epoll_events",
"//aos/vision/image:reader",
],
diff --git a/aos/vision/image/image_stream.cc b/aos/vision/image/image_stream.cc
new file mode 100644
index 0000000..1dba674
--- /dev/null
+++ b/aos/vision/image/image_stream.cc
@@ -0,0 +1,18 @@
+#include "aos/vision/image/image_stream.h"
+
+#include "aos/logging/logging.h"
+
+namespace aos {
+namespace vision {
+
+void ImageStreamEvent::ProcessHelper(
+ DataRef data, aos::monotonic_clock::time_point timestamp) {
+ if (data.size() < 300) {
+ LOG(INFO, "got bad img of size(%d)\n", static_cast<int>(data.size()));
+ return;
+ }
+ ProcessImage(data, timestamp);
+}
+
+} // namespace vision
+} // namespace aos
diff --git a/aos/vision/image/image_stream.h b/aos/vision/image/image_stream.h
index 308d3ec..57e06cb 100644
--- a/aos/vision/image/image_stream.h
+++ b/aos/vision/image/image_stream.h
@@ -1,5 +1,5 @@
-#ifndef _AOS_VISION_IMAGE_IMAGE_STREAM_H_
-#define _AOS_VISION_IMAGE_IMAGE_STREAM_H_
+#ifndef AOS_VISION_IMAGE_IMAGE_STREAM_H_
+#define AOS_VISION_IMAGE_IMAGE_STREAM_H_
#include "aos/vision/events/epoll_events.h"
#include "aos/vision/image/camera_params.pb.h"
@@ -32,23 +32,18 @@
aos::vision::CameraParams params)
: ImageStreamEvent(GetCamera(fname, this, params)) {}
- void ProcessHelper(DataRef data, aos::monotonic_clock::time_point timestamp) {
- if (data.size() < 300) {
- LOG(INFO, "got bad img of size(%d)\n", static_cast<int>(data.size()));
- return;
- }
- ProcessImage(data, timestamp);
- }
virtual void ProcessImage(DataRef data,
aos::monotonic_clock::time_point timestamp) = 0;
void ReadEvent() override { reader_->HandleFrame(); }
private:
+ void ProcessHelper(DataRef data, aos::monotonic_clock::time_point timestamp);
+
std::unique_ptr<::camera::Reader> reader_;
};
} // namespace vision
} // namespace aos
-#endif // _AOS_VISION_DEBUG_IMAGE_STREAM_H_
+#endif // AOS_VISION_IMAGE_IMAGE_STREAM_H_
diff --git a/aos/vision/image/image_types.h b/aos/vision/image/image_types.h
index c50400a..a620d8a 100644
--- a/aos/vision/image/image_types.h
+++ b/aos/vision/image/image_types.h
@@ -7,7 +7,6 @@
#include <sstream>
#include <experimental/string_view>
-#include "aos/logging/logging.h"
namespace aos {
namespace vision {
@@ -65,7 +64,7 @@
ImageType &get_px(int x, int y) const {
#ifndef NDEBUG
if (x < 0 || x >= fmt_.w || y < 0 || y >= fmt_.h) {
- LOG(FATAL, "%d, %d out of range [%dx %d]\n", x, y, fmt_.w, fmt_.h);
+ __builtin_trap();
}
#endif // NBOUNDSCHECK
return data_[(x + y * fmt_.w)];
diff --git a/frc971/wpilib/ADIS16448.h b/frc971/wpilib/ADIS16448.h
index 1846723..49c4fb2 100644
--- a/frc971/wpilib/ADIS16448.h
+++ b/frc971/wpilib/ADIS16448.h
@@ -45,6 +45,8 @@
// readings.
void operator()();
+ // Sets a function to be called immediately after each time this class uses
+ // the SPI bus. This is a good place to do other things on the bus.
void set_spi_idle_callback(std::function<void()> spi_idle_callback) {
spi_idle_callback_ = std::move(spi_idle_callback);
}
diff --git a/y2019/actors/autonomous_actor.cc b/y2019/actors/autonomous_actor.cc
index 1647f04..41a70fd 100644
--- a/y2019/actors/autonomous_actor.cc
+++ b/y2019/actors/autonomous_actor.cc
@@ -27,10 +27,6 @@
.count();
}
-constexpr bool is_left = false;
-
-constexpr double turn_scalar = is_left ? 1.0 : -1.0;
-
} // namespace
AutonomousActor::AutonomousActor(
@@ -38,7 +34,8 @@
: frc971::autonomous::BaseAutonomousActor(
s, control_loops::drivetrain::GetDrivetrainConfig()) {}
-void AutonomousActor::Reset() {
+void AutonomousActor::Reset(bool is_left) {
+ const double turn_scalar = is_left ? 1.0 : -1.0;
elevator_goal_ = 0.01;
wrist_goal_ = -M_PI / 2.0;
intake_goal_ = -1.2;
@@ -86,8 +83,15 @@
bool AutonomousActor::RunAction(
const ::frc971::autonomous::AutonomousActionParams ¶ms) {
monotonic_clock::time_point start_time = monotonic_clock::now();
- LOG(INFO, "Starting autonomous action with mode %" PRId32 "\n", params.mode);
- Reset();
+ const bool is_left = params.mode == 0;
+
+ {
+ LOG(INFO, "Starting autonomous action with mode %" PRId32 " %s\n",
+ params.mode, is_left ? "left" : "right");
+ }
+ const double turn_scalar = is_left ? 1.0 : -1.0;
+
+ Reset(is_left);
// Grab the disk, wait until we have vacuum, then jump
set_elevator_goal(0.01);
diff --git a/y2019/actors/autonomous_actor.h b/y2019/actors/autonomous_actor.h
index 82e1aeb..38db070 100644
--- a/y2019/actors/autonomous_actor.h
+++ b/y2019/actors/autonomous_actor.h
@@ -26,7 +26,7 @@
const ::frc971::autonomous::AutonomousActionParams ¶ms) override;
private:
- void Reset();
+ void Reset(bool is_left);
double elevator_goal_ = 0.0;
double wrist_goal_ = 0.0;
diff --git a/y2019/image_streamer/flip_image.cc b/y2019/image_streamer/flip_image.cc
index 2db20b2..48a18c5 100644
--- a/y2019/image_streamer/flip_image.cc
+++ b/y2019/image_streamer/flip_image.cc
@@ -10,7 +10,9 @@
::cimg_library::CImg<unsigned char> image;
image.load_jpeg_buffer((JOCTET *)(input), input_size);
if (flip) {
- image.mirror("xy");
+ image.rotate(90);
+ } else {
+ image.rotate(270);
}
image.save_jpeg_buffer(buffer, *buffer_size, 80);
diff --git a/y2019/jevois/BUILD b/y2019/jevois/BUILD
index afca4ef..599bcba 100644
--- a/y2019/jevois/BUILD
+++ b/y2019/jevois/BUILD
@@ -189,7 +189,6 @@
"cobs.h",
],
deps = [
- "//aos/logging",
"//third_party/GSL",
],
)
diff --git a/y2019/jevois/camera/BUILD b/y2019/jevois/camera/BUILD
index e6e33c5..bf716a0 100644
--- a/y2019/jevois/camera/BUILD
+++ b/y2019/jevois/camera/BUILD
@@ -5,19 +5,21 @@
srcs = ["reader.cc"],
hdrs = ["reader.h"],
deps = [
- "//aos/vision/image:reader",
+ "//aos/time",
"//aos/vision/image:camera_params",
"//aos/vision/image:image_types",
- "//aos/logging",
- "//aos/time",
+ "//aos/vision/image:reader",
+ "@com_github_google_glog//:glog",
],
)
cc_library(
name = "image_stream",
+ srcs = ["image_stream.cc"],
hdrs = ["image_stream.h"],
deps = [
- "//aos/vision/events:epoll_events",
":reader",
+ "//aos/logging",
+ "//aos/vision/events:epoll_events",
],
)
diff --git a/y2019/jevois/camera/image_stream.cc b/y2019/jevois/camera/image_stream.cc
new file mode 100644
index 0000000..29a1513
--- /dev/null
+++ b/y2019/jevois/camera/image_stream.cc
@@ -0,0 +1,18 @@
+#include "y2019/jevois/camera/image_stream.h"
+
+#include "aos/logging/logging.h"
+
+namespace y2019 {
+namespace camera {
+
+void ImageStreamEvent::ProcessHelper(
+ aos::vision::DataRef data, aos::monotonic_clock::time_point timestamp) {
+ if (data.size() < 300) {
+ LOG(INFO, "got bad img of size(%d)\n", static_cast<int>(data.size()));
+ return;
+ }
+ ProcessImage(data, timestamp);
+}
+
+} // namespace camera
+} // namespace y2019
diff --git a/y2019/jevois/camera/image_stream.h b/y2019/jevois/camera/image_stream.h
index 62661c6..578e24f 100644
--- a/y2019/jevois/camera/image_stream.h
+++ b/y2019/jevois/camera/image_stream.h
@@ -1,5 +1,5 @@
-#ifndef _AOS_VISION_IMAGE_IMAGE_STREAM_H_
-#define _AOS_VISION_IMAGE_IMAGE_STREAM_H_
+#ifndef Y2019_JEVOIS_CAMERA_IMAGE_STREAM_H_
+#define Y2019_JEVOIS_CAMERA_IMAGE_STREAM_H_
#include "aos/vision/events/epoll_events.h"
#include "aos/vision/image/camera_params.pb.h"
@@ -33,13 +33,7 @@
: ImageStreamEvent(GetCamera(fname, this, params)) {}
void ProcessHelper(aos::vision::DataRef data,
- aos::monotonic_clock::time_point timestamp) {
- if (data.size() < 300) {
- LOG(INFO, "got bad img of size(%d)\n", static_cast<int>(data.size()));
- return;
- }
- ProcessImage(data, timestamp);
- }
+ aos::monotonic_clock::time_point timestamp);
virtual void ProcessImage(aos::vision::DataRef data,
aos::monotonic_clock::time_point timestamp) = 0;
@@ -49,7 +43,7 @@
std::unique_ptr<Reader> reader_;
};
-} // namespace vision
-} // namespace aos
+} // namespace camera
+} // namespace y2019
-#endif // _AOS_VISION_DEBUG_IMAGE_STREAM_H_
+#endif // Y2019_JEVOIS_CAMERA_IMAGE_STREAM_H_
diff --git a/y2019/jevois/camera/reader.cc b/y2019/jevois/camera/reader.cc
index 25b5a3f..14faa24 100644
--- a/y2019/jevois/camera/reader.cc
+++ b/y2019/jevois/camera/reader.cc
@@ -12,8 +12,8 @@
#include <sys/types.h>
#include <unistd.h>
-#include "aos/logging/logging.h"
#include "aos/time/time.h"
+#include "glog/logging.h"
#define CLEAR(x) memset(&(x), 0, sizeof(x))
@@ -45,29 +45,27 @@
: dev_name_(dev_name), process_(std::move(process)), params_(params) {
struct stat st;
if (stat(dev_name.c_str(), &st) == -1) {
- PLOG(FATAL, "Cannot identify '%s'", dev_name.c_str());
+ PLOG(FATAL) << "Cannot identify '" << dev_name << "'";
}
if (!S_ISCHR(st.st_mode)) {
- PLOG(FATAL, "%s is no device\n", dev_name.c_str());
+ PLOG(FATAL) << dev_name << " is no device";
}
fd_ = open(dev_name.c_str(), O_RDWR /* required */ | O_NONBLOCK, 0);
if (fd_ == -1) {
- PLOG(FATAL, "Cannot open '%s'", dev_name.c_str());
+ PLOG(FATAL) << "Cannot open '" << dev_name << "'";
}
Init();
InitMMap();
- LOG(INFO, "Bat Vision Successfully Initialized.\n");
+ LOG(INFO) << "Bat Vision Successfully Initialized.";
}
void Reader::QueueBuffer(v4l2_buffer *buf) {
if (xioctl(fd_, VIDIOC_QBUF, buf) == -1) {
- PLOG(WARNING,
- "ioctl VIDIOC_QBUF(%d, %p)."
- " losing buf #%" PRIu32 "\n",
- fd_, &buf, buf->index);
+ PLOG(WARNING) << "ioctl VIDIOC_QBUF(" << fd_ << ", " << &buf
+ << "). losing buf #" << buf->index;
} else {
++queued_;
}
@@ -81,7 +79,7 @@
if (xioctl(fd_, VIDIOC_DQBUF, &buf) == -1) {
if (errno != EAGAIN) {
- PLOG(ERROR, "ioctl VIDIOC_DQBUF(%d, %p)", fd_, &buf);
+ PLOG(ERROR) << "ioctl VIDIOC_DQBUF(" << fd_ << ", " << &buf << ")";
}
return;
}
@@ -110,15 +108,15 @@
buf.memory = V4L2_MEMORY_MMAP;
buf.index = n;
if (xioctl(fd_, VIDIOC_QUERYBUF, &buf) == -1) {
- PLOG(FATAL, "ioctl VIDIOC_QUERYBUF(%d, %p)", fd_, &buf);
+ PLOG(FATAL) << "ioctl VIDIOC_QUERYBUF(" << fd_ << ", " << &buf << ")";
}
buffers_[n].length = buf.length;
buffers_[n].start = mmap(NULL, buf.length, PROT_READ | PROT_WRITE,
MAP_SHARED, fd_, buf.m.offset);
if (buffers_[n].start == MAP_FAILED) {
- PLOG(FATAL,
- "mmap(NULL, %zd, PROT_READ | PROT_WRITE, MAP_SHARED, %d, %jd)",
- (size_t)buf.length, fd_, static_cast<intmax_t>(buf.m.offset));
+ PLOG(FATAL) << "mmap(NULL, " << buf.length
+ << ", PROT_READ | PROT_WRITE, MAP_SHARED, " << fd_ << ", "
+ << buf.m.offset << ")";
}
}
}
@@ -131,14 +129,14 @@
req.memory = V4L2_MEMORY_MMAP;
if (xioctl(fd_, VIDIOC_REQBUFS, &req) == -1) {
if (EINVAL == errno) {
- LOG(FATAL, "%s does not support memory mapping\n", dev_name_.c_str());
+ LOG(FATAL) << dev_name_ << " does not support memory mapping\n";
} else {
- PLOG(FATAL, "ioctl VIDIOC_REQBUFS(%d, %p)\n", fd_, &req);
+ LOG(FATAL) << "ioctl VIDIOC_REQBUFS(" << fd_ << ", " << &req << ")";
}
}
queued_ = kNumBuffers;
if (req.count != kNumBuffers) {
- LOG(FATAL, "Insufficient buffer memory on %s\n", dev_name_.c_str());
+ LOG(FATAL) << "Insufficient buffer memory on " << dev_name_;
}
}
@@ -153,11 +151,11 @@
r = xioctl(fd_, 0xc00c561b, &getArg);
if (r == 0) {
if (getArg.value == value) {
- LOG(DEBUG, "Camera control %s was already %d\n", name, getArg.value);
+ VLOG(1) << "Camera control " << name << " was already " << getArg.value;
return true;
}
} else if (errno == EINVAL) {
- LOG(DEBUG, "Camera control %s is invalid\n", name);
+ VLOG(1) << "Camera control " << name << " is invalid";
errno = 0;
return false;
}
@@ -167,12 +165,12 @@
// Jevois wants this incorrect number below:.
r = xioctl(fd_, 0xc00c561c, &setArg);
if (r == 0) {
- LOG(DEBUG, "Set camera control %s from %d to %d\n", name, getArg.value,
- value);
+ VLOG(1) << "Set camera control " << name << " from " << getArg.value
+ << " to " << value;
return true;
}
- LOG(DEBUG, "Couldn't set camera control %s to %d", name, value);
+ VLOG(1) << "Couldn't set camera control " << name << " to " << value;
errno = 0;
return false;
}
@@ -181,16 +179,16 @@
v4l2_capability cap;
if (xioctl(fd_, VIDIOC_QUERYCAP, &cap) == -1) {
if (EINVAL == errno) {
- LOG(FATAL, "%s is no V4L2 device\n", dev_name_.c_str());
+ LOG(FATAL) << dev_name_ << " is no V4L2 device";
} else {
- PLOG(FATAL, "ioctl VIDIOC_QUERYCAP(%d, %p)", fd_, &cap);
+ PLOG(FATAL) << "ioctl VIDIOC_QUERYCAP(" << fd_ << ", " << &cap << ")";
}
}
if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
- LOG(FATAL, "%s is no video capture device\n", dev_name_.c_str());
+ LOG(FATAL) << dev_name_ << " is no video capture device";
}
if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
- LOG(FATAL, "%s does not support streaming i/o\n", dev_name_.c_str());
+ LOG(FATAL) << dev_name_ << " does not support streaming i/o";
}
int camidx = -1;
@@ -208,8 +206,7 @@
}
if (xioctl(fd_, VIDIOC_S_INPUT, &camidx) == -1) {
- LOG(FATAL, "ioctl VIDIOC_S_INPUT(%d) failed with %d: %s\n", fd_, errno,
- strerror(errno));
+ PLOG(FATAL) << "ioctl VIDIOC_S_INPUT(" << fd_ << ") failed";
}
printf("camera idx: %d\n", camidx);
@@ -220,8 +217,7 @@
fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (xioctl(fd_, VIDIOC_G_FMT, &fmt) == -1) {
- LOG(FATAL, "ioctl VIDIC_G_FMT(%d, %p) failed with %d: %s\n", fd_, &fmt,
- errno, strerror(errno));
+ PLOG(FATAL) << "ioctl VIDIC_G_FMT(" << fd_ << ", " << &fmt << ") failed";
}
fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
@@ -231,8 +227,7 @@
fmt.fmt.pix.field = V4L2_FIELD_NONE;
fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
if (xioctl(fd_, VIDIOC_S_FMT, &fmt) == -1) {
- LOG(FATAL, "ioctl VIDIC_S_FMT(%d, %p) failed with %d: %s\n", fd_, &fmt,
- errno, strerror(errno));
+ PLOG(FATAL) << "ioctl VIDIC_S_FMT(" << fd_ << ", " << &fmt << ") failed";
}
/* Note VIDIOC_S_FMT may change width and height. */
@@ -250,21 +245,21 @@
setfps->parm.capture.timeperframe.numerator = 1;
setfps->parm.capture.timeperframe.denominator = params_.fps();
if (xioctl(fd_, VIDIOC_S_PARM, setfps) == -1) {
- PLOG(FATAL, "ioctl VIDIOC_S_PARM(%d, %p)\n", fd_, setfps);
+ PLOG(FATAL) << "ioctl VIDIOC_S_PARM(" << fd_ << ", " << setfps << ")";
}
- LOG(INFO, "framerate ended up at %d/%d\n",
- setfps->parm.capture.timeperframe.numerator,
- setfps->parm.capture.timeperframe.denominator);
+ LOG(INFO) << "framerate ended up at "
+ << setfps->parm.capture.timeperframe.numerator << "/"
+ << setfps->parm.capture.timeperframe.denominator;
for (int j = 0; j < 2; ++j) {
if (!SetCameraControl(V4L2_CID_EXPOSURE_AUTO, "V4L2_CID_EXPOSURE_AUTO",
V4L2_EXPOSURE_MANUAL)) {
- LOG(FATAL, "Failed to set exposure\n");
+ LOG(FATAL) << "Failed to set exposure";
}
if (!SetCameraControl(V4L2_CID_EXPOSURE_ABSOLUTE,
"V4L2_CID_EXPOSURE_ABSOLUTE", params_.exposure())) {
- LOG(FATAL, "Failed to set exposure\n");
+ LOG(FATAL) << "Failed to set exposure";
}
sleep(1);
}
@@ -274,7 +269,7 @@
struct v4l2_format fmt;
fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (xioctl(fd_, VIDIOC_G_FMT, &fmt) == -1) {
- PLOG(FATAL, "ioctl VIDIC_G_FMT(%d, %p)\n", fd_, &fmt);
+ PLOG(FATAL) << "ioctl VIDIC_G_FMT(" << fd_ << ", " << &fmt << ")";
}
return aos::vision::ImageFormat{(int)fmt.fmt.pix.width,
@@ -282,7 +277,7 @@
}
void Reader::Start() {
- LOG(DEBUG, "queueing buffers for the first time\n");
+ VLOG(1) << "queueing buffers for the first time";
v4l2_buffer buf;
for (unsigned int i = 0; i < kNumBuffers; ++i) {
CLEAR(buf);
@@ -291,11 +286,11 @@
buf.index = i;
QueueBuffer(&buf);
}
- LOG(DEBUG, "done with first queue\n");
+ VLOG(1) << "done with first queue";
v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (xioctl(fd_, VIDIOC_STREAMON, &type) == -1) {
- PLOG(FATAL, "ioctl VIDIOC_STREAMON(%d, %p)\n", fd_, &type);
+ PLOG(FATAL) << "ioctl VIDIOC_STREAMON(" << fd_ << ", " << &type << ")";
}
}
diff --git a/y2019/jevois/cobs.h b/y2019/jevois/cobs.h
index 112873d..0e92ed6 100644
--- a/y2019/jevois/cobs.h
+++ b/y2019/jevois/cobs.h
@@ -7,12 +7,6 @@
#include <array>
#include "third_party/GSL/include/gsl/gsl"
-#ifdef __linux__
-#include "aos/logging/logging.h"
-#else
-#define CHECK(...)
-#define CHECK_LE(...)
-#endif
// This file contains code for encoding and decoding Consistent Overhead Byte
// Stuffing data. <http://www.stuartcheshire.org/papers/cobsforton.pdf> has
@@ -101,14 +95,18 @@
gsl::span<const char> input,
std::array<char, CobsMaxEncodedSize(max_decoded_size)> *output_buffer) {
static_assert(max_decoded_size > 0, "Empty buffers not supported");
- CHECK_LE(static_cast<size_t>(input.size()), max_decoded_size);
+ if (static_cast<size_t>(input.size()) > max_decoded_size) {
+ __builtin_trap();
+ }
auto input_pointer = input.begin();
auto output_pointer = output_buffer->begin();
auto code_pointer = output_pointer;
++output_pointer;
uint8_t code = 1;
while (input_pointer < input.end()) {
- CHECK(output_pointer < output_buffer->end());
+ if (output_pointer >= output_buffer->end()) {
+ __builtin_trap();
+ }
if (*input_pointer == 0u) {
*code_pointer = code;
code_pointer = output_pointer;
@@ -128,7 +126,9 @@
++input_pointer;
}
*code_pointer = code;
- CHECK(output_pointer <= output_buffer->end());
+ if (output_pointer > output_buffer->end()) {
+ __builtin_trap();
+ }
return gsl::span<char>(*output_buffer)
.subspan(0, output_pointer - output_buffer->begin());
}
@@ -137,8 +137,10 @@
gsl::span<char> CobsDecode(gsl::span<const char> input,
std::array<char, max_decoded_size> *output_buffer) {
static_assert(max_decoded_size > 0, "Empty buffers not supported");
- CHECK_LE(static_cast<size_t>(input.size()),
- CobsMaxEncodedSize(max_decoded_size));
+ if (static_cast<size_t>(input.size()) >
+ CobsMaxEncodedSize(max_decoded_size)) {
+ __builtin_trap();
+ }
auto input_pointer = input.begin();
auto output_pointer = output_buffer->begin();
while (input_pointer < input.end()) {
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index 9d7117b..b403913 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -400,6 +400,15 @@
}
private:
+ uint32_t GetAutonomousMode() override {
+ ::frc971::autonomous::auto_mode.FetchLatest();
+ if (::frc971::autonomous::auto_mode.get() == nullptr) {
+ LOG(WARNING, "no auto mode values\n");
+ return 0;
+ }
+ return ::frc971::autonomous::auto_mode->mode;
+ }
+
// Current goals here.
ElevatorWristPosition elevator_wrist_pos_ = kStowPos;
bool grab_piece_ = false;
diff --git a/y2019/vision/BUILD b/y2019/vision/BUILD
index 344e4da..bf71116 100644
--- a/y2019/vision/BUILD
+++ b/y2019/vision/BUILD
@@ -93,8 +93,6 @@
restricted_to = VISION_TARGETS,
deps = [
":target_finder",
- "//aos/logging",
- "//aos/logging:implementations",
"//aos/vision/blob:codec",
"//aos/vision/blob:find_blob",
"//aos/vision/events:epoll_events",
@@ -105,6 +103,7 @@
"//y2019/jevois:uart",
"//y2019/jevois/camera:image_stream",
"//y2019/jevois/camera:reader",
+ "@com_github_google_glog//:glog",
"@com_google_ceres_solver//:ceres",
],
)
diff --git a/y2019/vision/debug_viewer.cc b/y2019/vision/debug_viewer.cc
index 9f1d4dd..4f0e747 100644
--- a/y2019/vision/debug_viewer.cc
+++ b/y2019/vision/debug_viewer.cc
@@ -80,6 +80,7 @@
}
bool HandleBlobs(BlobList imgs, ImageFormat fmt) override {
+ const CameraGeometry camera_geometry = GetCamera(FLAGS_camera)->geometry;
imgs_last_ = imgs;
fmt_last_ = fmt;
// reset for next drawing cycle
@@ -174,8 +175,23 @@
// Check that our current results match possible solutions.
results = target_finder_.FilterResults(results, 0, draw_results_);
if (draw_results_) {
- for (const IntermediateResult &res : results) {
- DrawTarget(res, {0, 255, 0});
+ for (const IntermediateResult &result : results) {
+ ::std::cout << "Found target x: "
+ << camera_geometry.location[0] +
+ ::std::cos(camera_geometry.heading +
+ result.extrinsics.r2) *
+ result.extrinsics.z
+ << ::std::endl;
+ ::std::cout << "Found target y: "
+ << camera_geometry.location[1] +
+ ::std::sin(camera_geometry.heading +
+ result.extrinsics.r2) *
+ result.extrinsics.z
+ << ::std::endl;
+ ::std::cout << "Found target z: "
+ << camera_geometry.location[2] + result.extrinsics.y
+ << ::std::endl;
+ DrawTarget(result, {0, 255, 0});
}
}
diff --git a/y2019/vision/target_sender.cc b/y2019/vision/target_sender.cc
index 04e52ac..12e0e09 100644
--- a/y2019/vision/target_sender.cc
+++ b/y2019/vision/target_sender.cc
@@ -1,7 +1,7 @@
+#include "y2019/vision/target_finder.h"
+
#include <fstream>
-#include "aos/logging/implementations.h"
-#include "aos/logging/logging.h"
#include "aos/vision/blob/codec.h"
#include "aos/vision/blob/find_blob.h"
#include "aos/vision/events/socket_types.h"
@@ -12,7 +12,10 @@
#include "y2019/jevois/serial.h"
#include "y2019/jevois/structures.h"
#include "y2019/jevois/uart.h"
-#include "y2019/vision/target_finder.h"
+
+// This has to be last to preserve compatibility with other headers using AOS
+// logging.
+#include "glog/logging.h"
using ::aos::events::DataSocket;
using ::aos::events::RXUdpSocket;
@@ -29,7 +32,7 @@
: ImageStreamEvent(fname, params) {}
void ProcessImage(DataRef data, monotonic_clock::time_point monotonic_now) {
- LOG(INFO, "got frame: %d\n", (int)data.size());
+ LOG(INFO) << "got frame: " << data.size();
if (on_frame_) on_frame_(data, monotonic_now);
}
@@ -70,9 +73,8 @@
using namespace y2019::vision;
using frc971::jevois::CameraCommand;
// gflags::ParseCommandLineFlags(&argc, &argv, false);
- ::aos::logging::Init();
- ::aos::logging::AddImplementation(
- new ::aos::logging::StreamLogImplementation(stderr));
+ FLAGS_logtostderr = true;
+ google::InitGoogleLogging(argv[0]);
int itsDev = open_terminos("/dev/ttyS0");
frc971::jevois::CobsPacketizer<frc971::jevois::uart_to_camera_size()> cobs;
@@ -99,7 +101,7 @@
aos::vision::BlobList imgs =
aos::vision::FindBlobs(thresholder.Threshold(fmt, data.data(), 120));
finder_.PreFilter(&imgs);
- LOG(INFO, "Blobs: (%zu).\n", imgs.size());
+ LOG(INFO) << "Blobs: " << imgs.size();
constexpr bool verbose = false;
::std::vector<Polygon> raw_polys;
@@ -114,27 +116,27 @@
raw_polys.push_back(polygon);
}
}
- LOG(INFO, "Polygons: (%zu).\n", raw_polys.size());
+ LOG(INFO) << "Polygons: " << raw_polys.size();
// Calculate each component side of a possible target.
::std::vector<TargetComponent> target_component_list =
finder_.FillTargetComponentList(raw_polys, verbose);
- LOG(INFO, "Components: (%zu).\n", target_component_list.size());
+ LOG(INFO) << "Components: " << target_component_list.size();
// Put the compenents together into targets.
::std::vector<Target> target_list =
finder_.FindTargetsFromComponents(target_component_list, verbose);
- LOG(INFO, "Potential Target: (%zu).\n", target_list.size());
+ LOG(INFO) << "Potential Target: " << target_list.size();
// Use the solver to generate an intermediate version of our results.
::std::vector<IntermediateResult> results;
for (const Target &target : target_list) {
results.emplace_back(finder_.ProcessTargetToResult(target, verbose));
}
- LOG(INFO, "Raw Results: (%zu).\n", results.size());
+ LOG(INFO) << "Raw Results: " << results.size();
results = finder_.FilterResults(results, 30, verbose);
- LOG(INFO, "Results: (%zu).\n", results.size());
+ LOG(INFO) << "Results: " << results.size();
// TODO: Select top 3 (randomly?)
@@ -162,7 +164,7 @@
write(itsDev, serialized_frame.data(), serialized_frame.size());
if (n != (ssize_t)serialized_frame.size()) {
- LOG(INFO, "Some problem happened");
+ LOG(INFO) << "Some problem happened";
}
}
});
@@ -200,7 +202,7 @@
return system("touch /tmp/do_not_export_sd_card");
}
} else {
- printf("bad frame\n");
+ fprintf(stderr, "bad frame\n");
}
cobs.clear_received_packet();
}
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index fd9df73..e75b825 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -203,6 +203,11 @@
vacuum_sensor_ = make_unique<frc::AnalogInput>(port);
}
+ // Auto mode switches.
+ void set_autonomous_mode(int i, ::std::unique_ptr<frc::DigitalInput> sensor) {
+ autonomous_modes_.at(i) = ::std::move(sensor);
+ }
+
void RunIteration() override {
{
auto drivetrain_message = drivetrain_queue.position.MakeMessage();
@@ -254,6 +259,18 @@
superstructure_message.Send();
}
+
+ {
+ auto auto_mode_message = ::frc971::autonomous::auto_mode.MakeMessage();
+ auto_mode_message->mode = 0;
+ for (size_t i = 0; i < autonomous_modes_.size(); ++i) {
+ if (autonomous_modes_[i] && autonomous_modes_[i]->Get()) {
+ auto_mode_message->mode |= 1 << i;
+ }
+ }
+ LOG_STRUCT(DEBUG, "auto mode", *auto_mode_message);
+ auto_mode_message.Send();
+ }
}
private:
@@ -262,6 +279,8 @@
::std::unique_ptr<frc::AnalogInput> vacuum_sensor_;
+ ::std::array<::std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
+
::frc971::wpilib::AbsoluteEncoder intake_encoder_;
// TODO(sabina): Add wrist and elevator hall effects.
};
@@ -646,6 +665,9 @@
reader.set_pwm_trigger(true);
reader.set_vacuum_sensor(7);
+ reader.set_autonomous_mode(0, make_unique<frc::DigitalInput>(22));
+ reader.set_autonomous_mode(0, make_unique<frc::DigitalInput>(23));
+
::std::thread reader_thread(::std::ref(reader));
CameraReader camera_reader;