Merge "Bugfix: reference to home path was wrong."
diff --git a/WORKSPACE b/WORKSPACE
index ab7a70f..4bb6551 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -196,12 +196,14 @@
path = "third_party/gflags",
)
+# Downloaded from:
+# https://pypi.python.org/packages/source/g/glog/glog-0.1.tar.gz
http_archive(
name = "python_glog_repo",
build_file = "@//debian:glog.BUILD",
sha256 = "953fd80122c48023d1148e6d1bda2763fcab59c8a81682bb298238a5935547b0",
strip_prefix = "glog-0.1",
- url = "https://pypi.python.org/packages/source/g/glog/glog-0.1.tar.gz",
+ url = "http://frc971.org/Build-Dependencies/glog-0.1.tar.gz",
)
bind(
@@ -651,3 +653,13 @@
strip_prefix = "halide/",
url = "http://www.frc971.org/Build-Dependencies/halide-arm32-linux-32-trunk-65c26cba6a3eca2d08a0bccf113ca28746012cc3.tgz",
)
+
+# Downloaded from:
+# https://files.pythonhosted.org/packages/05/23/7f9a896da9e7ce4170377a7a14bb804b460761f9dd66734e6ad8f001a76c/opencv_contrib_python_nonfree-4.1.1.1-cp35-cp35m-manylinux1_x86_64.whl
+http_archive(
+ name = "opencv_contrib_nonfree_amd64",
+ build_file = "@//debian:opencv_python.BUILD",
+ sha256 = "c10e7712ee1f19bf382c64fc29b5d24fa0b5bfd901eab69cef83604713e6a89e",
+ type = "zip",
+ url = "http://www.frc971.org/Build-Dependencies/opencv_contrib_python_nonfree-4.1.1.1-cp35-cp35m-manylinux1_x86_64.whl",
+)
diff --git a/aos/aos_dump.cc b/aos/aos_dump.cc
index 8d6c2b1..2ecfe5e 100644
--- a/aos/aos_dump.cc
+++ b/aos/aos_dump.cc
@@ -27,6 +27,7 @@
const aos::Configuration *config_msg = &config.message();
::aos::ShmEventLoop event_loop(config_msg);
event_loop.SkipTimingReport();
+ event_loop.SkipAosLog();
if (argc == 1) {
std::cout << "Channels:\n";
diff --git a/aos/config_flattener.cc b/aos/config_flattener.cc
index 6fb4af3..71cda32 100644
--- a/aos/config_flattener.cc
+++ b/aos/config_flattener.cc
@@ -27,8 +27,7 @@
&configuration::MergeConfiguration(config, schemas).message(), true);
// TODO(austin): Figure out how to squash the schemas onto 1 line so it is
- // easier to read? Or figure out how to split them into a second file which
- // gets included.
+ // easier to read?
VLOG(1) << "Flattened config is " << merged_config;
util::WriteStringToFileOrDie(argv[1], merged_config);
return 0;
diff --git a/aos/configuration.cc b/aos/configuration.cc
index 095add5..c4f139c 100644
--- a/aos/configuration.cc
+++ b/aos/configuration.cc
@@ -470,6 +470,18 @@
}
}
+size_t ChannelIndex(const Configuration *configuration,
+ const Channel *channel) {
+ CHECK(configuration->channels() != nullptr) << ": No channels";
+
+ auto c = std::find(configuration->channels()->begin(),
+ configuration->channels()->end(), channel);
+ CHECK(c != configuration->channels()->end())
+ << ": Channel pointer not found in configuration()->channels()";
+
+ return std::distance(configuration->channels()->begin(), c);
+}
+
std::string CleanedChannelToString(const Channel *channel) {
FlatbufferDetachedBuffer<Channel> cleaned_channel = CopyFlatBuffer(channel);
cleaned_channel.mutable_message()->clear_schema();
@@ -605,6 +617,17 @@
return nullptr;
}
+const Node *GetNode(const Configuration *config, const Node *node) {
+ if (!MultiNode(config)) {
+ CHECK(node == nullptr) << ": Provided a node in a single node world.";
+ return nullptr;
+ } else {
+ CHECK(node != nullptr);
+ CHECK(node->has_name());
+ return GetNode(config, node->name()->string_view());
+ }
+}
+
const Node *GetNode(const Configuration *config, std::string_view name) {
CHECK(config->has_nodes())
<< ": Asking for a node from a single node configuration.";
@@ -617,6 +640,44 @@
return nullptr;
}
+const Node *GetNodeOrDie(const Configuration *config, const Node *node) {
+ if (!MultiNode(config)) {
+ CHECK(node == nullptr) << ": Provided a node in a single node world.";
+ return nullptr;
+ } else {
+ const Node *config_node = GetNode(config, node);
+ if (config_node == nullptr) {
+ LOG(FATAL) << "Couldn't find node matching " << FlatbufferToJson(node);
+ }
+ return config_node;
+ }
+}
+
+int GetNodeIndex(const Configuration *config, const Node *node) {
+ CHECK(config->has_nodes())
+ << ": Asking for a node from a single node configuration.";
+ int node_index = 0;
+ for (const Node *iterated_node : *config->nodes()) {
+ if (iterated_node == node) {
+ return node_index;
+ }
+ ++node_index;
+ }
+ LOG(FATAL) << "Node not found in the configuration.";
+}
+
+std::vector<const Node *> GetNodes(const Configuration *config) {
+ std::vector<const Node *> nodes;
+ if (configuration::MultiNode(config)) {
+ for (const Node *node : *config->nodes()) {
+ nodes.emplace_back(node);
+ }
+ } else {
+ nodes.emplace_back(nullptr);
+ }
+ return nodes;
+}
+
bool MultiNode(const Configuration *config) { return config->has_nodes(); }
bool ChannelIsSendableOnNode(const Channel *channel, const Node *node) {
diff --git a/aos/configuration.h b/aos/configuration.h
index 88687ad..a9fbfe3 100644
--- a/aos/configuration.h
+++ b/aos/configuration.h
@@ -60,15 +60,29 @@
channel->type()->string_view(), application_name, node);
}
+// Returns the channel index (or dies) of channel in the provided config.
+size_t ChannelIndex(const Configuration *config, const Channel *channel);
+
// Returns the Node out of the config with the matching name, or nullptr if it
// can't be found.
const Node *GetNode(const Configuration *config, std::string_view name);
+const Node *GetNode(const Configuration *config, const Node *node);
+// Returns a matching node, or nullptr if the provided node is nullptr and we
+// are in a single node world.
+const Node *GetNodeOrDie(const Configuration *config, const Node *node);
// Returns the Node out of the configuration which matches our hostname.
// CHECKs if it can't be found.
const Node *GetMyNode(const Configuration *config);
const Node *GetNodeFromHostname(const Configuration *config,
std::string_view name);
+// Returns a vector of the nodes in the config. (nullptr is considered the node
+// in a single node world.)
+std::vector<const Node *> GetNodes(const Configuration *config);
+
+// Returns the node index for a node. Note: node needs to exist inside config.
+int GetNodeIndex(const Configuration *config, const Node *node);
+
// Returns true if we are running in a multinode configuration.
bool MultiNode(const Configuration *config);
diff --git a/aos/configuration_test.cc b/aos/configuration_test.cc
index b8fc5b6..70515fd 100644
--- a/aos/configuration_test.cc
+++ b/aos/configuration_test.cc
@@ -39,6 +39,16 @@
FlatbufferToJson(config, true));
}
+// Tests that we can get back a ChannelIndex.
+TEST_F(ConfigurationTest, ChannelIndex) {
+ FlatbufferDetachedBuffer<Configuration> config =
+ ReadConfig("aos/testdata/config1.json");
+
+ EXPECT_EQ(
+ ChannelIndex(&config.message(), config.message().channels()->Get(1u)),
+ 1u);
+}
+
// Tests that we can read and merge a multinode configuration.
TEST_F(ConfigurationTest, ConfigMergeMultinode) {
FlatbufferDetachedBuffer<Configuration> config =
@@ -599,6 +609,63 @@
::testing::ElementsAreArray({"pi1"}));
}
+// Tests that we can pull out all the nodes.
+TEST_F(ConfigurationTest, GetNodes) {
+ {
+ FlatbufferDetachedBuffer<Configuration> config =
+ ReadConfig("aos/testdata/good_multinode.json");
+ const Node *pi1 = GetNode(&config.message(), "pi1");
+ const Node *pi2 = GetNode(&config.message(), "pi2");
+
+ EXPECT_THAT(GetNodes(&config.message()), ::testing::ElementsAre(pi1, pi2));
+ }
+
+ {
+ FlatbufferDetachedBuffer<Configuration> config =
+ ReadConfig("aos/testdata/config1.json");
+ EXPECT_THAT(GetNodes(&config.message()), ::testing::ElementsAre(nullptr));
+ }
+}
+
+// Tests that we can extract a node index from a config.
+TEST_F(ConfigurationTest, GetNodeIndex) {
+ FlatbufferDetachedBuffer<Configuration> config =
+ ReadConfig("aos/testdata/good_multinode.json");
+ const Node *pi1 = GetNode(&config.message(), "pi1");
+ const Node *pi2 = GetNode(&config.message(), "pi2");
+
+ EXPECT_EQ(GetNodeIndex(&config.message(), pi1), 0);
+ EXPECT_EQ(GetNodeIndex(&config.message(), pi2), 1);
+}
+
+// Tests that GetNodeOrDie handles both single and multi-node worlds and returns
+// valid nodes.
+TEST_F(ConfigurationDeathTest, GetNodeOrDie) {
+ FlatbufferDetachedBuffer<Configuration> config =
+ ReadConfig("aos/testdata/good_multinode.json");
+ FlatbufferDetachedBuffer<Configuration> config2 =
+ ReadConfig("aos/testdata/good_multinode.json");
+ {
+ // Simple case, nullptr -> nullptr
+ FlatbufferDetachedBuffer<Configuration> single_node_config =
+ ReadConfig("aos/testdata/config1.json");
+ EXPECT_EQ(nullptr, GetNodeOrDie(&single_node_config.message(), nullptr));
+
+ // Confirm that we die when a node is passed in.
+ EXPECT_DEATH(
+ {
+ GetNodeOrDie(&single_node_config.message(),
+ config.message().nodes()->Get(0));
+ },
+ "Provided a node in a single node world.");
+ }
+
+ const Node *pi1 = GetNode(&config.message(), "pi1");
+ // Now try a lookup using a node from a different instance of the config.
+ EXPECT_EQ(pi1,
+ GetNodeOrDie(&config.message(), config2.message().nodes()->Get(0)));
+}
+
} // namespace testing
} // namespace configuration
} // namespace aos
diff --git a/aos/controls/control_loop_test.h b/aos/controls/control_loop_test.h
index 25e1a45..76e82eb 100644
--- a/aos/controls/control_loop_test.h
+++ b/aos/controls/control_loop_test.h
@@ -4,8 +4,6 @@
#include <chrono>
#include <string_view>
-#include "gtest/gtest.h"
-
#include "aos/events/simulated_event_loop.h"
#include "aos/flatbuffers.h"
#include "aos/json_to_flatbuffer.h"
@@ -13,6 +11,7 @@
#include "aos/robot_state/robot_state_generated.h"
#include "aos/testing/test_logging.h"
#include "aos/time/time.h"
+#include "gtest/gtest.h"
namespace aos {
namespace testing {
@@ -80,9 +79,7 @@
// Simulate a reset of the process reading sensors, which tells loops that all
// index counts etc will be reset.
- void SimulateSensorReset() {
- ++reader_pid_;
- }
+ void SimulateSensorReset() { ++reader_pid_; }
// Sets the battery voltage in robot_state.
void set_battery_voltage(double battery_voltage) {
@@ -186,7 +183,8 @@
ControlLoopTestTemplated<TestBaseClass>::kTimeTick;
template <typename TestBaseClass>
-constexpr ::std::chrono::milliseconds ControlLoopTestTemplated<TestBaseClass>::kDSPacketTime;
+constexpr ::std::chrono::milliseconds
+ ControlLoopTestTemplated<TestBaseClass>::kDSPacketTime;
} // namespace testing
} // namespace aos
diff --git a/aos/events/BUILD b/aos/events/BUILD
index fb0f356..0470b6e 100644
--- a/aos/events/BUILD
+++ b/aos/events/BUILD
@@ -41,6 +41,16 @@
],
)
+cc_test(
+ name = "epoll_test",
+ srcs = ["epoll_test.cc"],
+ deps = [
+ ":epoll",
+ "//aos/testing:googletest",
+ "@com_github_google_glog//:glog",
+ ],
+)
+
cc_library(
name = "event_loop",
srcs = [
@@ -58,6 +68,8 @@
"//aos:configuration",
"//aos:configuration_fbs",
"//aos:flatbuffers",
+ "//aos/ipc_lib:data_alignment",
+ "//aos/logging:implementations",
"//aos/time",
"//aos/util:phased_loop",
"@com_github_google_flatbuffers//:flatbuffers",
@@ -103,6 +115,7 @@
src = "aos.json",
flatbuffers = [
":event_loop_fbs",
+ "//aos/logging:log_message_fbs",
],
)
@@ -204,6 +217,7 @@
hdrs = ["shm_event_loop.h"],
visibility = ["//visibility:public"],
deps = [
+ ":aos_logging",
":epoll",
":event_loop",
":event_loop_fbs",
@@ -214,6 +228,7 @@
"//aos/ipc_lib:signalfd",
"//aos/stl_mutex",
"//aos/util:phased_loop",
+ "@com_google_absl//absl/base",
],
)
@@ -279,6 +294,7 @@
],
visibility = ["//visibility:public"],
deps = [
+ ":aos_logging",
":event_loop",
":simple_channel",
"//aos/ipc_lib:index",
@@ -286,3 +302,19 @@
"@com_google_absl//absl/container:btree",
],
)
+
+cc_library(
+ name = "aos_logging",
+ srcs = [
+ "aos_logging.cc",
+ ],
+ hdrs = [
+ "aos_logging.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":event_loop",
+ "//aos/logging:implementations",
+ "//aos/logging:log_message_fbs",
+ ],
+)
diff --git a/aos/events/aos.json b/aos/events/aos.json
index 5d00baa..36b4610 100644
--- a/aos/events/aos.json
+++ b/aos/events/aos.json
@@ -7,6 +7,12 @@
"frequency": 50,
"num_senders": 20,
"max_size": 2048
+ },
+ {
+ "name": "/aos",
+ "type": "aos.logging.LogMessageFbs",
+ "frequency": 200,
+ "num_senders": 20
}
]
}
diff --git a/aos/events/aos_logging.cc b/aos/events/aos_logging.cc
new file mode 100644
index 0000000..f831071
--- /dev/null
+++ b/aos/events/aos_logging.cc
@@ -0,0 +1,31 @@
+#include "aos/events/aos_logging.h"
+
+namespace aos {
+
+void AosLogToFbs::Initialize(Sender<logging::LogMessageFbs> log_sender) {
+ log_sender_ = std::move(log_sender);
+ if (log_sender_) {
+ logging::RegisterCallbackImplementation(
+ [this](const logging::LogMessage &message_data) {
+ aos::Sender<logging::LogMessageFbs>::Builder message =
+ log_sender_.MakeBuilder();
+ auto message_str = message.fbb()->CreateString(
+ message_data.message, message_data.message_length);
+ auto name_str = message.fbb()->CreateString(message_data.name,
+ message_data.name_length);
+
+ ::aos::logging::LogMessageFbs::Builder builder =
+ message.MakeBuilder<::aos::logging::LogMessageFbs>();
+ builder.add_message(message_str);
+ builder.add_level(
+ static_cast<::aos::logging::Level>(message_data.level));
+ builder.add_source(message_data.source);
+ builder.add_name(name_str);
+
+ message.Send(builder.Finish());
+ },
+ false);
+ }
+}
+
+} // namespace aos
diff --git a/aos/events/aos_logging.h b/aos/events/aos_logging.h
new file mode 100644
index 0000000..b50569c
--- /dev/null
+++ b/aos/events/aos_logging.h
@@ -0,0 +1,25 @@
+#ifndef AOS_LOGGING_H_
+#define AOS_LOGGING_H_
+
+#include "aos/events/event_loop.h"
+#include "aos/logging/implementations.h"
+#include "aos/logging/log_message_generated.h"
+
+namespace aos {
+
+class AosLogToFbs {
+ public:
+ AosLogToFbs() {}
+
+ // TODO(Tyler): Deregister logger on destruction to avoid memory leaks
+
+ void Initialize(Sender<logging::LogMessageFbs> log_sender);
+
+ private:
+ Sender<logging::LogMessageFbs> log_sender_;
+ logging::ScopedLogRestorer prev_logger_;
+};
+
+} // namespace aos
+
+#endif // AOS_LOGGING_H_
diff --git a/aos/events/epoll.cc b/aos/events/epoll.cc
index e19cf59..b9e7edb 100644
--- a/aos/events/epoll.cc
+++ b/aos/events/epoll.cc
@@ -71,6 +71,7 @@
}
void EPoll::Run() {
+ run_ = true;
while (true) {
// Pull a single event out. Infinite timeout if we are supposed to be
// running, and 0 length timeout otherwise. This lets us flush the event
@@ -90,34 +91,53 @@
return;
}
}
- EventData *event_data = static_cast<struct EventData *>(event.data.ptr);
- if (event.events & (EPOLLIN | EPOLLPRI)) {
+ EventData *const event_data = static_cast<struct EventData *>(event.data.ptr);
+ if (event.events & kInEvents) {
+ CHECK(event_data->in_fn)
+ << ": No handler registered for input events on " << event_data->fd;
event_data->in_fn();
}
+ if (event.events & kOutEvents) {
+ CHECK(event_data->out_fn)
+ << ": No handler registered for output events on " << event_data->fd;
+ event_data->out_fn();
+ }
}
}
void EPoll::Quit() { PCHECK(write(quit_signal_fd_, "q", 1) == 1); }
void EPoll::OnReadable(int fd, ::std::function<void()> function) {
- ::std::unique_ptr<EventData> event_data(
- new EventData{fd, ::std::move(function)});
+ EventData *event_data = GetEventData(fd);
+ if (event_data == nullptr) {
+ fns_.emplace_back(std::make_unique<EventData>(fd));
+ event_data = fns_.back().get();
+ } else {
+ CHECK(!event_data->in_fn) << ": Duplicate in functions for " << fd;
+ }
+ event_data->in_fn = ::std::move(function);
+ DoEpollCtl(event_data, event_data->events | kInEvents);
+}
- struct epoll_event event;
- event.events = EPOLLIN | EPOLLPRI;
- event.data.ptr = event_data.get();
- PCHECK(epoll_ctl(epoll_fd_, EPOLL_CTL_ADD, fd, &event) == 0)
- << ": Failed to add fd " << fd;
- fns_.push_back(::std::move(event_data));
+void EPoll::OnWriteable(int fd, ::std::function<void()> function) {
+ EventData *event_data = GetEventData(fd);
+ if (event_data == nullptr) {
+ fns_.emplace_back(std::make_unique<EventData>(fd));
+ event_data = fns_.back().get();
+ } else {
+ CHECK(!event_data->out_fn) << ": Duplicate out functions for " << fd;
+ }
+ event_data->out_fn = ::std::move(function);
+ DoEpollCtl(event_data, event_data->events | kOutEvents);
}
// Removes fd from the event loop.
void EPoll::DeleteFd(int fd) {
auto element = fns_.begin();
- PCHECK(epoll_ctl(epoll_fd_, EPOLL_CTL_DEL, fd, nullptr) == 0);
while (fns_.end() != element) {
if (element->get()->fd == fd) {
fns_.erase(element);
+ PCHECK(epoll_ctl(epoll_fd_, EPOLL_CTL_DEL, fd, nullptr) == 0);
return;
}
++element;
@@ -125,5 +145,50 @@
LOG(FATAL) << "fd " << fd << " not found";
}
+void EPoll::EnableEvents(int fd, uint32_t events) {
+ EventData *const event_data = CHECK_NOTNULL(GetEventData(fd));
+ DoEpollCtl(event_data, event_data->events | events);
+}
+
+void EPoll::DisableEvents(int fd, uint32_t events) {
+ EventData *const event_data = CHECK_NOTNULL(GetEventData(fd));
+ DoEpollCtl(event_data, event_data->events & ~events);
+}
+
+EPoll::EventData *EPoll::GetEventData(int fd) {
+ const auto iterator = std::find_if(
+ fns_.begin(), fns_.end(),
+ [fd](const std::unique_ptr<EventData> &data) { return data->fd == fd; });
+ if (iterator == fns_.end()) {
+ return nullptr;
+ }
+ return iterator->get();
+}
+
+void EPoll::DoEpollCtl(EventData *event_data, const uint32_t new_events) {
+ const uint32_t old_events = event_data->events;
+ event_data->events = new_events;
+ if (new_events == 0) {
+ if (old_events == 0) {
+ // Not added, and doesn't need to be. Nothing to do here.
+ return;
+ }
+ // It was added, but should now be removed.
+ PCHECK(epoll_ctl(epoll_fd_, EPOLL_CTL_DEL, event_data->fd, nullptr) == 0);
+ return;
+ }
+
+ int operation = EPOLL_CTL_MOD;
+ if (old_events == 0) {
+ // If it wasn't added before, then this is the first time it's being added.
+ operation = EPOLL_CTL_ADD;
+ }
+ struct epoll_event event;
+ event.events = event_data->events;
+ event.data.ptr = event_data;
+ PCHECK(epoll_ctl(epoll_fd_, operation, event_data->fd, &event) == 0)
+ << ": Failed to " << operation << " epoll fd: " << event_data->fd;
+}
+
} // namespace internal
} // namespace aos
diff --git a/aos/events/epoll.h b/aos/events/epoll.h
index 7bc135c..51a20d0 100644
--- a/aos/events/epoll.h
+++ b/aos/events/epoll.h
@@ -63,25 +63,57 @@
void Quit();
// Registers a function to be called if the fd becomes readable.
- // There should only be 1 function registered for each fd.
+ // Only one function may be registered for readability on each fd.
void OnReadable(int fd, ::std::function<void()> function);
+ // Registers a function to be called if the fd becomes writeable.
+ // Only one function may be registered for writability on each fd.
+ void OnWriteable(int fd, ::std::function<void()> function);
+
// Removes fd from the event loop.
// All Fds must be cleaned up before this class is destroyed.
void DeleteFd(int fd);
+ // Enables calling the existing function registered for fd when it becomes
+ // writeable.
+ void EnableWriteable(int fd) { EnableEvents(fd, kOutEvents); }
+
+ // Disables calling the existing function registered for fd when it becomes
+ // writeable.
+ void DisableWriteable(int fd) { DisableEvents(fd, kOutEvents); }
+
private:
+ // Structure whose pointer should be returned by epoll. Makes looking up the
+ // function fast and easy.
+ struct EventData {
+ EventData(int fd_in) : fd(fd_in) {}
+ // We use pointers to these objects as persistent identifiers, so they can't
+ // be moved.
+ EventData(const EventData &) = delete;
+ EventData &operator=(const EventData &) = delete;
+
+ const int fd;
+ uint32_t events = 0;
+ ::std::function<void()> in_fn, out_fn;
+ };
+
+ void EnableEvents(int fd, uint32_t events);
+ void DisableEvents(int fd, uint32_t events);
+
+ EventData *GetEventData(int fd);
+
+ void DoEpollCtl(EventData *event_data, uint32_t new_events);
+
+ // TODO(Brian): Figure out a nicer way to handle EPOLLPRI than lumping it in
+ // with input.
+ static constexpr uint32_t kInEvents = EPOLLIN | EPOLLPRI;
+ static constexpr uint32_t kOutEvents = EPOLLOUT;
+
::std::atomic<bool> run_{true};
// Main epoll fd.
int epoll_fd_;
- // Structure whose pointer should be returned by epoll. Makes looking up the
- // function fast and easy.
- struct EventData {
- int fd;
- ::std::function<void()> in_fn;
- };
::std::vector<::std::unique_ptr<EventData>> fns_;
// Pipe pair for handling quit.
diff --git a/aos/events/epoll_test.cc b/aos/events/epoll_test.cc
new file mode 100644
index 0000000..4e6bbbc
--- /dev/null
+++ b/aos/events/epoll_test.cc
@@ -0,0 +1,172 @@
+#include "aos/events/epoll.h"
+
+#include <fcntl.h>
+#include <unistd.h>
+
+#include "gtest/gtest.h"
+#include "glog/logging.h"
+
+namespace aos {
+namespace internal {
+namespace testing {
+
+// A simple wrapper around both ends of a pipe along with some helpers to easily
+// read/write data through it.
+class Pipe {
+ public:
+ Pipe() { PCHECK(pipe2(fds_, O_NONBLOCK) == 0); }
+ ~Pipe() {
+ PCHECK(close(fds_[0]) == 0);
+ PCHECK(close(fds_[1]) == 0);
+ }
+
+ int read_fd() { return fds_[0]; }
+ int write_fd() { return fds_[1]; }
+
+ void Write(const std::string &data) {
+ CHECK_EQ(write(write_fd(), data.data(), data.size()),
+ static_cast<ssize_t>(data.size()));
+ }
+
+ std::string Read(size_t size) {
+ std::string result;
+ result.resize(size);
+ CHECK_EQ(read(read_fd(), result.data(), size), static_cast<ssize_t>(size));
+ return result;
+ }
+
+ private:
+ int fds_[2];
+};
+
+class EPollTest : public ::testing::Test {
+ public:
+ void RunFor(std::chrono::nanoseconds duration) {
+ TimerFd timerfd;
+ bool did_quit = false;
+ epoll_.OnReadable(timerfd.fd(), [this, &timerfd, &did_quit]() {
+ CHECK(!did_quit);
+ epoll_.Quit();
+ did_quit = true;
+ timerfd.Read();
+ });
+ timerfd.SetTime(monotonic_clock::now() + duration,
+ monotonic_clock::duration::zero());
+ epoll_.Run();
+ CHECK(did_quit);
+ epoll_.DeleteFd(timerfd.fd());
+ }
+
+ // Tests should avoid relying on ordering for events closer in time than this,
+ // or waiting for longer than this to ensure events happen in order.
+ static constexpr std::chrono::nanoseconds tick_duration() {
+ return std::chrono::milliseconds(50);
+ }
+
+ EPoll epoll_;
+};
+
+// Test that the basics of OnReadable work.
+TEST_F(EPollTest, BasicReadable) {
+ Pipe pipe;
+ bool got_data = false;
+ epoll_.OnReadable(pipe.read_fd(), [&]() {
+ ASSERT_FALSE(got_data);
+ ASSERT_EQ("some", pipe.Read(4));
+ got_data = true;
+ });
+ RunFor(tick_duration());
+ EXPECT_FALSE(got_data);
+
+ pipe.Write("some");
+ RunFor(tick_duration());
+ EXPECT_TRUE(got_data);
+
+ epoll_.DeleteFd(pipe.read_fd());
+}
+
+// Test that the basics of OnWriteable work.
+TEST_F(EPollTest, BasicWriteable) {
+ Pipe pipe;
+ int number_writes = 0;
+ epoll_.OnWriteable(pipe.write_fd(), [&]() {
+ pipe.Write(" ");
+ ++number_writes;
+ });
+
+ // First, fill up the pipe's write buffer.
+ RunFor(tick_duration());
+ EXPECT_GT(number_writes, 0);
+
+ // Now, if we try again, we shouldn't do anything.
+ const int bytes_in_pipe = number_writes;
+ number_writes = 0;
+ RunFor(tick_duration());
+ EXPECT_EQ(number_writes, 0);
+
+ // Empty the pipe, then fill it up again.
+ for (int i = 0; i < bytes_in_pipe; ++i) {
+ ASSERT_EQ(" ", pipe.Read(1));
+ }
+ number_writes = 0;
+ RunFor(tick_duration());
+ EXPECT_EQ(number_writes, bytes_in_pipe);
+
+ epoll_.DeleteFd(pipe.write_fd());
+}
+
+TEST(EPollDeathTest, InvalidFd) {
+ EPoll epoll;
+ Pipe pipe;
+ epoll.OnReadable(pipe.read_fd(), []() {});
+ EXPECT_DEATH(epoll.OnReadable(pipe.read_fd(), []() {}),
+ "Duplicate in functions");
+ epoll.OnWriteable(pipe.read_fd(), []() {});
+ EXPECT_DEATH(epoll.OnWriteable(pipe.read_fd(), []() {}),
+ "Duplicate out functions");
+
+ epoll.DeleteFd(pipe.read_fd());
+ EXPECT_DEATH(epoll.DeleteFd(pipe.read_fd()), "fd [0-9]+ not found");
+ EXPECT_DEATH(epoll.DeleteFd(pipe.write_fd()), "fd [0-9]+ not found");
+}
+
+// Tests that enabling/disabling a writeable FD works.
+TEST_F(EPollTest, WriteableEnableDisable) {
+ Pipe pipe;
+ int number_writes = 0;
+ epoll_.OnWriteable(pipe.write_fd(), [&]() {
+ pipe.Write(" ");
+ ++number_writes;
+ });
+
+ // First, fill up the pipe's write buffer.
+ RunFor(tick_duration());
+ EXPECT_GT(number_writes, 0);
+
+ // Empty the pipe.
+ const int bytes_in_pipe = number_writes;
+ for (int i = 0; i < bytes_in_pipe; ++i) {
+ ASSERT_EQ(" ", pipe.Read(1));
+ }
+
+ // If we disable writeable checking, then nothing should happen.
+ epoll_.DisableWriteable(pipe.write_fd());
+ number_writes = 0;
+ RunFor(tick_duration());
+ EXPECT_EQ(number_writes, 0);
+
+ // Disabling it again should be a NOP.
+ epoll_.DisableWriteable(pipe.write_fd());
+
+ // And then when we re-enable, it should fill the pipe up again.
+ epoll_.EnableWriteable(pipe.write_fd());
+ number_writes = 0;
+ RunFor(tick_duration());
+ EXPECT_EQ(number_writes, bytes_in_pipe);
+
+ epoll_.DeleteFd(pipe.write_fd());
+}
+
+} // namespace testing
+} // namespace internal
+} // namespace aos
diff --git a/aos/events/event_loop.cc b/aos/events/event_loop.cc
index e368df2..5b6f5c6 100644
--- a/aos/events/event_loop.cc
+++ b/aos/events/event_loop.cc
@@ -2,6 +2,7 @@
#include "aos/configuration.h"
#include "aos/configuration_generated.h"
+#include "aos/logging/implementations.h"
#include "glog/logging.h"
DEFINE_bool(timing_reports, true, "Publish timing reports.");
@@ -65,20 +66,29 @@
PhasedLoopHandler::~PhasedLoopHandler() {}
+EventLoop::EventLoop(const Configuration *configuration)
+ : timing_report_(flatbuffers::DetachedBuffer()),
+ configuration_(configuration) {
+ logging::Init();
+}
+
EventLoop::~EventLoop() {
CHECK_EQ(senders_.size(), 0u) << ": Not all senders destroyed";
CHECK_EQ(events_.size(), 0u) << ": Not all events unregistered";
}
int EventLoop::ChannelIndex(const Channel *channel) {
- CHECK(configuration_->channels() != nullptr) << ": No channels";
+ return configuration::ChannelIndex(configuration_, channel);
+}
- auto c = std::find(configuration_->channels()->begin(),
- configuration_->channels()->end(), channel);
- CHECK(c != configuration_->channels()->end())
- << ": Channel pointer not found in configuration()->channels()";
-
- return std::distance(configuration()->channels()->begin(), c);
+WatcherState *EventLoop::GetWatcherState(const Channel *channel) {
+ const int channel_index = ChannelIndex(channel);
+ for (const std::unique_ptr<WatcherState> &watcher : watchers_) {
+ if (watcher->channel_index() == channel_index) {
+ return watcher.get();
+ }
+ }
+ LOG(FATAL) << "No watcher found for channel";
}
void EventLoop::NewSender(RawSender *sender) {
diff --git a/aos/events/event_loop.h b/aos/events/event_loop.h
index 4a12096..80cc025 100644
--- a/aos/events/event_loop.h
+++ b/aos/events/event_loop.h
@@ -1,4 +1,5 @@
#ifndef AOS_EVENTS_EVENT_LOOP_H_
+
#define AOS_EVENTS_EVENT_LOOP_H_
#include <atomic>
@@ -11,6 +12,7 @@
#include "aos/events/event_loop_generated.h"
#include "aos/events/timing_statistics.h"
#include "aos/flatbuffers.h"
+#include "aos/ipc_lib/data_alignment.h"
#include "aos/json_to_flatbuffer.h"
#include "aos/time/time.h"
#include "aos/util/phased_loop.h"
@@ -141,6 +143,13 @@
// Returns the queue index that this was sent with.
uint32_t sent_queue_index() const { return sent_queue_index_; }
+ // Returns the associated flatbuffers-style allocator. This must be
+ // deallocated before the message is sent.
+ PreallocatedAllocator *fbb_allocator() {
+ fbb_allocator_ = PreallocatedAllocator(data(), size());
+ return &fbb_allocator_;
+ }
+
protected:
EventLoop *event_loop() { return event_loop_; }
@@ -166,6 +175,8 @@
const Channel *channel_;
internal::RawSenderTiming timing_;
+
+ PreallocatedAllocator fbb_allocator_{nullptr, 0};
};
// Fetches the newest message from a channel.
@@ -177,12 +188,26 @@
// Fetches the next message. Returns true if it fetched a new message. This
// method will only return messages sent after the Fetcher was created.
- bool FetchNext() { return fetcher_->FetchNext(); }
+ bool FetchNext() {
+ const bool result = fetcher_->FetchNext();
+ if (result) {
+ CheckChannelDataAlignment(fetcher_->context().data,
+ fetcher_->context().size);
+ }
+ return result;
+ }
// Fetches the most recent message. Returns true if it fetched a new message.
// This will return the latest message regardless of if it was sent before or
// after the fetcher was created.
- bool Fetch() { return fetcher_->Fetch(); }
+ bool Fetch() {
+ const bool result = fetcher_->Fetch();
+ if (result) {
+ CheckChannelDataAlignment(fetcher_->context().data,
+ fetcher_->context().size);
+ }
+ return result;
+ }
// Returns a pointer to the contained flatbuffer, or nullptr if there is no
// available message.
@@ -222,10 +247,19 @@
// builder.Send(t_builder.Finish());
class Builder {
public:
- Builder(RawSender *sender, void *data, size_t size)
- : alloc_(data, size), fbb_(size, &alloc_), sender_(sender) {
+ Builder(RawSender *sender, PreallocatedAllocator *allocator)
+ : fbb_(allocator->size(), allocator),
+ allocator_(allocator),
+ sender_(sender) {
+ CheckChannelDataAlignment(allocator->data(), allocator->size());
fbb_.ForceDefaults(1);
}
+ Builder() {}
+ Builder(const Builder &) = delete;
+ Builder(Builder &&) = default;
+
+ Builder &operator=(const Builder &) = delete;
+ Builder &operator=(Builder &&) = default;
flatbuffers::FlatBufferBuilder *fbb() { return &fbb_; }
@@ -236,19 +270,30 @@
bool Send(flatbuffers::Offset<T> offset) {
fbb_.Finish(offset);
- return sender_->Send(fbb_.GetSize());
+ const bool result = sender_->Send(fbb_.GetSize());
+ // Ensure fbb_ knows it shouldn't access the memory any more.
+ fbb_ = flatbuffers::FlatBufferBuilder();
+ return result;
}
// CHECKs that this message was sent.
- void CheckSent() { fbb_.Finished(); }
+ void CheckSent() {
+ CHECK(!allocator_->is_allocated()) << ": Message was not sent yet";
+ }
private:
- PreallocatedAllocator alloc_;
flatbuffers::FlatBufferBuilder fbb_;
+ PreallocatedAllocator *allocator_;
RawSender *sender_;
};
// Constructs an above builder.
+ //
+ // Only a single one of these may be "alive" for this object at any point in
+ // time. After calling Send on the result, it is no longer "alive". This means
+ // that you must manually reset a variable holding the return value (by
+ // assigning a default-constructed Builder to it) before calling this method
+ // again to overwrite the value in the variable.
Builder MakeBuilder();
// Sends a prebuilt flatbuffer.
@@ -257,6 +302,10 @@
// Returns the name of the underlying queue.
const Channel *channel() const { return sender_->channel(); }
+ operator bool() {
+ return sender_ ? true : false;
+ }
+
private:
friend class EventLoop;
Sender(std::unique_ptr<RawSender> sender) : sender_(std::move(sender)) {}
@@ -347,9 +396,7 @@
class EventLoop {
public:
- EventLoop(const Configuration *configuration)
- : timing_report_(flatbuffers::DetachedBuffer()),
- configuration_(configuration) {}
+ EventLoop(const Configuration *configuration);
virtual ~EventLoop();
@@ -473,6 +520,9 @@
// Prevents the event loop from sending a timing report.
void SkipTimingReport() { skip_timing_report_ = true; }
+ // Prevents AOS_LOG being sent to message on /aos
+ void SkipAosLog() { skip_logger_ = true; }
+
protected:
// Sets the name of the event loop. This is the application name.
virtual void set_name(const std::string_view name) = 0;
@@ -482,6 +532,16 @@
// Validates that channel exists inside configuration_ and finds its index.
int ChannelIndex(const Channel *channel);
+ // Returns the state for the watcher on the corresponding channel. This
+ // watcher must exist before calling this.
+ WatcherState *GetWatcherState(const Channel *channel);
+
+ // Returns a Sender's protected RawSender
+ template <typename T>
+ static RawSender *GetRawSender(aos::Sender<T> *sender) {
+ return sender->sender_.get();
+ }
+
// Context available for watchers, timers, and phased loops.
Context context_;
@@ -532,6 +592,9 @@
std::vector<EventLoopEvent *> events_;
+ // If true, don't send AOS_LOG to /aos
+ bool skip_logger_ = false;
+
private:
virtual pid_t GetTid() = 0;
diff --git a/aos/events/event_loop_param_test.cc b/aos/events/event_loop_param_test.cc
index b72a8e2..096d84a 100644
--- a/aos/events/event_loop_param_test.cc
+++ b/aos/events/event_loop_param_test.cc
@@ -2,13 +2,11 @@
#include <chrono>
-#include "glog/logging.h"
-#include "gmock/gmock.h"
-#include "gtest/gtest.h"
-
#include "aos/events/test_message_generated.h"
#include "aos/flatbuffer_merge.h"
#include "glog/logging.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
namespace aos {
namespace testing {
@@ -409,21 +407,19 @@
EXPECT_THAT(values, ::testing::ElementsAreArray({201, 202, 204}));
}
-
// Tests that FetchNext behaves correctly when we get two messages in the queue
// but don't consume the first until after the second has been sent.
TEST_P(AbstractEventLoopTest, FetchNextTest) {
-
auto send_loop = Make();
auto fetch_loop = Make();
auto sender = send_loop->MakeSender<TestMessage>("/test");
Fetcher<TestMessage> fetcher = fetch_loop->MakeFetcher<TestMessage>("/test");
{
- aos::Sender<TestMessage>::Builder msg = sender.MakeBuilder();
- TestMessage::Builder builder = msg.MakeBuilder<TestMessage>();
- builder.add_value(100);
- ASSERT_TRUE(msg.Send(builder.Finish()));
+ aos::Sender<TestMessage>::Builder msg = sender.MakeBuilder();
+ TestMessage::Builder builder = msg.MakeBuilder<TestMessage>();
+ builder.add_value(100);
+ ASSERT_TRUE(msg.Send(builder.Finish()));
}
{
@@ -642,7 +638,7 @@
EXPECT_EQ(report.message().name()->string_view(), "primary");
ASSERT_NE(report.message().senders(), nullptr);
- EXPECT_EQ(report.message().senders()->size(), 1);
+ EXPECT_EQ(report.message().senders()->size(), 2);
ASSERT_NE(report.message().timers(), nullptr);
EXPECT_EQ(report.message().timers()->size(), 2);
@@ -690,13 +686,10 @@
iteration_list.push_back(loop->monotonic_now());
});
- auto ender_timer = loop->AddTimer([&test_timer]() {
- test_timer->Disable();
- });
+ auto ender_timer = loop->AddTimer([&test_timer]() { test_timer->Disable(); });
test_timer->Setup(loop->monotonic_now(), ::std::chrono::milliseconds(20));
- ender_timer->Setup(loop->monotonic_now() +
- ::std::chrono::milliseconds(45));
+ ender_timer->Setup(loop->monotonic_now() + ::std::chrono::milliseconds(45));
EndEventLoop(loop.get(), ::std::chrono::milliseconds(150));
Run();
@@ -708,7 +701,8 @@
TEST_P(AbstractEventLoopDeathTest, InvalidChannel) {
auto loop = MakePrimary();
- const Channel *channel = loop->configuration()->channels()->Get(1);
+ const Channel *channel = configuration::GetChannel(
+ loop->configuration(), "/test", "aos.TestMessage", "", nullptr);
FlatbufferDetachedBuffer<Channel> channel_copy = CopyFlatBuffer(channel);
@@ -754,8 +748,7 @@
const aos::monotonic_clock::time_point monotonic_now =
loop1->monotonic_now();
- const aos::realtime_clock::time_point realtime_now =
- loop1->realtime_now();
+ const aos::realtime_clock::time_point realtime_now = loop1->realtime_now();
EXPECT_LE(loop1->context().monotonic_event_time, monotonic_now);
EXPECT_LE(loop1->context().realtime_event_time, realtime_now);
@@ -932,7 +925,7 @@
EXPECT_EQ(report.message().name()->string_view(), "primary");
ASSERT_NE(report.message().senders(), nullptr);
- EXPECT_EQ(report.message().senders()->size(), 1);
+ EXPECT_EQ(report.message().senders()->size(), 2);
ASSERT_NE(report.message().timers(), nullptr);
EXPECT_EQ(report.message().timers()->size(), 1);
@@ -996,7 +989,7 @@
EXPECT_EQ(primary_report.message().name()->string_view(), "primary");
ASSERT_NE(primary_report.message().senders(), nullptr);
- EXPECT_EQ(primary_report.message().senders()->size(), 2);
+ EXPECT_EQ(primary_report.message().senders()->size(), 3);
// Confirm that the sender looks sane.
EXPECT_EQ(
@@ -1148,7 +1141,7 @@
EXPECT_EQ(primary_report.message().name()->string_view(), "primary");
ASSERT_NE(primary_report.message().senders(), nullptr);
- EXPECT_EQ(primary_report.message().senders()->size(), 1);
+ EXPECT_EQ(primary_report.message().senders()->size(), 2);
ASSERT_NE(primary_report.message().timers(), nullptr);
EXPECT_EQ(primary_report.message().timers()->size(), 4);
@@ -1164,10 +1157,8 @@
EXPECT_EQ(primary_report.message().fetchers()->Get(0)->count(), 1);
EXPECT_GE(primary_report.message().fetchers()->Get(0)->latency()->average(),
0.1);
- EXPECT_GE(primary_report.message().fetchers()->Get(0)->latency()->min(),
- 0.1);
- EXPECT_GE(primary_report.message().fetchers()->Get(0)->latency()->max(),
- 0.1);
+ EXPECT_GE(primary_report.message().fetchers()->Get(0)->latency()->min(), 0.1);
+ EXPECT_GE(primary_report.message().fetchers()->Get(0)->latency()->max(), 0.1);
EXPECT_EQ(primary_report.message()
.fetchers()
->Get(0)
@@ -1188,17 +1179,20 @@
const std::string kData("971 is the best");
std::unique_ptr<aos::RawSender> sender =
- loop1->MakeRawSender(loop1->configuration()->channels()->Get(1));
+ loop1->MakeRawSender(configuration::GetChannel(
+ loop1->configuration(), "/test", "aos.TestMessage", "", nullptr));
std::unique_ptr<aos::RawFetcher> fetcher =
- loop3->MakeRawFetcher(loop3->configuration()->channels()->Get(1));
+ loop3->MakeRawFetcher(configuration::GetChannel(
+ loop3->configuration(), "/test", "aos.TestMessage", "", nullptr));
loop2->OnRun(
[&]() { EXPECT_TRUE(sender->Send(kData.data(), kData.size())); });
bool happened = false;
loop2->MakeRawWatcher(
- loop2->configuration()->channels()->Get(1),
+ configuration::GetChannel(loop2->configuration(), "/test",
+ "aos.TestMessage", "", nullptr),
[this, &kData, &fetcher, &happened](const Context &context,
const void *message) {
happened = true;
@@ -1239,10 +1233,12 @@
aos::realtime_clock::time_point(chrono::seconds(3132));
std::unique_ptr<aos::RawSender> sender =
- loop1->MakeRawSender(loop1->configuration()->channels()->Get(1));
+ loop1->MakeRawSender(configuration::GetChannel(
+ loop1->configuration(), "/test", "aos.TestMessage", "", nullptr));
std::unique_ptr<aos::RawFetcher> fetcher =
- loop3->MakeRawFetcher(loop3->configuration()->channels()->Get(1));
+ loop3->MakeRawFetcher(configuration::GetChannel(
+ loop3->configuration(), "/test", "aos.TestMessage", "", nullptr));
loop2->OnRun([&]() {
EXPECT_TRUE(sender->Send(kData.data(), kData.size(), monotonic_remote_time,
@@ -1251,7 +1247,8 @@
bool happened = false;
loop2->MakeRawWatcher(
- loop2->configuration()->channels()->Get(1),
+ configuration::GetChannel(loop2->configuration(), "/test",
+ "aos.TestMessage", "", nullptr),
[this, monotonic_remote_time, realtime_remote_time, &fetcher, &happened](
const Context &context, const void * /*message*/) {
happened = true;
@@ -1279,12 +1276,11 @@
const std::string kData("971 is the best");
std::unique_ptr<aos::RawSender> sender =
- loop1->MakeRawSender(loop1->configuration()->channels()->Get(1));
+ loop1->MakeRawSender(configuration::GetChannel(
+ loop1->configuration(), "/test", "aos.TestMessage", "", nullptr));
- const aos::monotonic_clock::time_point monotonic_now =
- loop1->monotonic_now();
- const aos::realtime_clock::time_point realtime_now =
- loop1->realtime_now();
+ const aos::monotonic_clock::time_point monotonic_now = loop1->monotonic_now();
+ const aos::realtime_clock::time_point realtime_now = loop1->realtime_now();
EXPECT_TRUE(sender->Send(kData.data(), kData.size()));
@@ -1334,8 +1330,10 @@
auto loop1 = Make();
auto loop2 = Make();
loop1->MakeWatcher("/test", [](const TestMessage &) {});
- loop2->MakeRawWatcher(configuration()->channels()->Get(1),
- [](const Context &, const void *) {});
+ loop2->MakeRawWatcher(
+ configuration::GetChannel(configuration(), "/test", "aos.TestMessage", "",
+ nullptr),
+ [](const Context &, const void *) {});
}
// Tests that fetcher work with a node setup.
@@ -1344,7 +1342,8 @@
auto loop1 = Make();
auto fetcher = loop1->MakeFetcher<TestMessage>("/test");
- auto raw_fetcher = loop1->MakeRawFetcher(configuration()->channels()->Get(1));
+ auto raw_fetcher = loop1->MakeRawFetcher(configuration::GetChannel(
+ configuration(), "/test", "aos.TestMessage", "", nullptr));
}
// Tests that sender work with a node setup.
@@ -1365,8 +1364,10 @@
"node");
EXPECT_DEATH(
{
- loop2->MakeRawWatcher(configuration()->channels()->Get(1),
- [](const Context &, const void *) {});
+ loop2->MakeRawWatcher(
+ configuration::GetChannel(configuration(), "/test",
+ "aos.TestMessage", "", nullptr),
+ [](const Context &, const void *) {});
},
"node");
}
@@ -1380,8 +1381,8 @@
"node");
EXPECT_DEATH(
{
- auto raw_fetcher =
- loop1->MakeRawFetcher(configuration()->channels()->Get(1));
+ auto raw_fetcher = loop1->MakeRawFetcher(configuration::GetChannel(
+ configuration(), "/test", "aos.TestMessage", "", nullptr));
},
"node");
}
diff --git a/aos/events/event_loop_param_test.h b/aos/events/event_loop_param_test.h
index 89e80ae..cbd5cd1 100644
--- a/aos/events/event_loop_param_test.h
+++ b/aos/events/event_loop_param_test.h
@@ -19,6 +19,10 @@
" \"channels\": [ \n"
" {\n"
" \"name\": \"/aos\",\n"
+ " \"type\": \"aos.logging.LogMessageFbs\"\n"
+ " },\n"
+ " {\n"
+ " \"name\": \"/aos\",\n"
" \"type\": \"aos.timing.Report\"\n"
" },\n"
" {\n"
@@ -58,6 +62,16 @@
std::string json = R"config({
"channels": [
{
+ "name": "/aos/me",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "me"
+ },
+ {
+ "name": "/aos/them",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "them"
+ },
+ {
"name": "/aos",
"type": "aos.timing.Report",
"source_node": "me"
@@ -87,6 +101,28 @@
"name": "them",
"hostname": "themhostname"
}
+ ],
+ "maps": [
+ {
+ "match": {
+ "name": "/aos",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "me"
+ },
+ "rename": {
+ "name": "/aos/me"
+ }
+ },
+ {
+ "match": {
+ "name": "/aos",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "them"
+ },
+ "rename": {
+ "name": "/aos/them"
+ }
+ }
]
})config";
diff --git a/aos/events/event_loop_tmpl.h b/aos/events/event_loop_tmpl.h
index c2c9884..dfb5a6d 100644
--- a/aos/events/event_loop_tmpl.h
+++ b/aos/events/event_loop_tmpl.h
@@ -23,7 +23,7 @@
template <typename T>
typename Sender<T>::Builder Sender<T>::MakeBuilder() {
- return Builder(sender_.get(), sender_->data(), sender_->size());
+ return Builder(sender_.get(), sender_->fbb_allocator());
}
template <typename Watch>
@@ -193,6 +193,7 @@
// context.
void DoCallCallback(std::function<monotonic_clock::time_point()> get_time,
Context context) {
+ CheckChannelDataAlignment(context.data, context.size);
const monotonic_clock::time_point monotonic_start_time = get_time();
{
const float start_latency =
diff --git a/aos/events/event_scheduler.cc b/aos/events/event_scheduler.cc
index 57f20ae..26752b1 100644
--- a/aos/events/event_scheduler.cc
+++ b/aos/events/event_scheduler.cc
@@ -4,6 +4,7 @@
#include <deque>
#include "aos/events/event_loop.h"
+#include "aos/logging/implementations.h"
namespace aos {
@@ -19,6 +20,7 @@
void EventScheduler::RunFor(distributed_clock::duration duration) {
const distributed_clock::time_point end_time =
distributed_now() + duration;
+ logging::ScopedLogRestorer prev_logger;
is_running_ = true;
for (std::function<void()> &on_run : on_run_) {
on_run();
@@ -39,6 +41,7 @@
}
void EventScheduler::Run() {
+ logging::ScopedLogRestorer prev_logger;
is_running_ = true;
for (std::function<void()> &on_run : on_run_) {
on_run();
diff --git a/aos/events/logging/BUILD b/aos/events/logging/BUILD
index b2be972..2b3df8f 100644
--- a/aos/events/logging/BUILD
+++ b/aos/events/logging/BUILD
@@ -53,6 +53,22 @@
)
cc_binary(
+ name = "log_edit",
+ srcs = [
+ "log_edit.cc",
+ ],
+ deps = [
+ ":logger",
+ "//aos:configuration",
+ "//aos:init",
+ "//aos:json_to_flatbuffer",
+ "//aos/util:file",
+ "@com_github_gflags_gflags//:gflags",
+ "@com_github_google_glog//:glog",
+ ],
+)
+
+cc_binary(
name = "log_stats",
srcs = [
"log_stats.cc",
@@ -68,6 +84,7 @@
"@com_github_google_glog//:glog",
],
)
+
cc_binary(
name = "logger_main",
srcs = [
diff --git a/aos/events/logging/log_cat.cc b/aos/events/logging/log_cat.cc
index 80ec8e7..f2427b8 100644
--- a/aos/events/logging/log_cat.cc
+++ b/aos/events/logging/log_cat.cc
@@ -33,6 +33,7 @@
std::unique_ptr<aos::EventLoop> printer_event_loop =
reader.event_loop_factory()->MakeEventLoop("printer", reader.node());
printer_event_loop->SkipTimingReport();
+ printer_event_loop->SkipAosLog();
bool found_channel = false;
const flatbuffers::Vector<flatbuffers::Offset<aos::Channel>> *channels =
diff --git a/aos/events/logging/log_edit.cc b/aos/events/logging/log_edit.cc
new file mode 100644
index 0000000..5db503a
--- /dev/null
+++ b/aos/events/logging/log_edit.cc
@@ -0,0 +1,62 @@
+#include <iostream>
+
+#include "aos/configuration.h"
+#include "aos/events/logging/logger.h"
+#include "aos/events/simulated_event_loop.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/util/file.h"
+#include "gflags/gflags.h"
+
+DEFINE_string(logfile, "/tmp/logfile.bfbs",
+ "Name of the logfile to read from.");
+DEFINE_bool(
+ replace, false,
+ "If true, replace the header on the log file with the JSON header.");
+DEFINE_string(
+ header, "",
+ "If provided, this is the path to the JSON with the log file header.");
+
+int main(int argc, char **argv) {
+ gflags::SetUsageMessage(
+ R"(This tool lets us manipulate log files.)");
+ aos::InitGoogle(&argc, &argv);
+
+ if (!FLAGS_header.empty()) {
+ if (FLAGS_replace) {
+ const ::std::string header_json =
+ aos::util::ReadFileToStringOrDie(FLAGS_header);
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.ForceDefaults(1);
+ flatbuffers::Offset<aos::logger::LogFileHeader> header =
+ aos::JsonToFlatbuffer<aos::logger::LogFileHeader>(header_json, &fbb);
+
+ fbb.FinishSizePrefixed(header);
+
+ const std::string orig_path = FLAGS_logfile + ".orig";
+ PCHECK(rename(FLAGS_logfile.c_str(), orig_path.c_str()) == 0);
+
+ aos::logger::SpanReader span_reader(orig_path);
+ CHECK(span_reader.ReadMessage().empty());
+
+ aos::logger::DetachedBufferWriter buffer_writer(FLAGS_logfile);
+ buffer_writer.QueueSizedFlatbuffer(&fbb);
+
+ while (true) {
+ absl::Span<const uint8_t> msg_data = span_reader.ReadMessage();
+ if (msg_data == absl::Span<const uint8_t>()) {
+ break;
+ }
+
+ buffer_writer.WriteSizedFlatbuffer(msg_data);
+ }
+ } else {
+ aos::logger::MessageReader reader(FLAGS_logfile);
+ aos::util::WriteStringToFileOrDie(
+ FLAGS_header, aos::FlatbufferToJson(reader.log_file_header(), true));
+ }
+ }
+
+ aos::Cleanup();
+ return 0;
+}
diff --git a/aos/events/logging/log_stats.cc b/aos/events/logging/log_stats.cc
index c766fe6..65da871 100644
--- a/aos/events/logging/log_stats.cc
+++ b/aos/events/logging/log_stats.cc
@@ -67,6 +67,7 @@
std::unique_ptr<aos::EventLoop> stats_event_loop =
log_reader_factory.MakeEventLoop("logstats", reader.node());
stats_event_loop->SkipTimingReport();
+ stats_event_loop->SkipAosLog();
// Read channel info and store in vector
bool found_channel = false;
diff --git a/aos/events/logging/logfile_utils.cc b/aos/events/logging/logfile_utils.cc
index e55b80d..a5ca084 100644
--- a/aos/events/logging/logfile_utils.cc
+++ b/aos/events/logging/logfile_utils.cc
@@ -39,6 +39,26 @@
QueueSizedFlatbuffer(fbb->Release());
}
+void DetachedBufferWriter::WriteSizedFlatbuffer(
+ absl::Span<const uint8_t> span) {
+ // Cheat aggressively... Write out the queued up data, and then write this
+ // data once without buffering. It is hard to make a DetachedBuffer out of
+ // this data, and we don't want to worry about lifetimes.
+ Flush();
+ iovec_.clear();
+ iovec_.reserve(1);
+
+ struct iovec n;
+ n.iov_base = const_cast<uint8_t *>(span.data());
+ n.iov_len = span.size();
+ iovec_.emplace_back(n);
+
+ const ssize_t written = writev(fd_, iovec_.data(), iovec_.size());
+
+ PCHECK(written == static_cast<ssize_t>(n.iov_len))
+ << ": Wrote " << written << " expected " << n.iov_len;
+}
+
void DetachedBufferWriter::QueueSizedFlatbuffer(
flatbuffers::DetachedBuffer &&buffer) {
queued_size_ += buffer.size();
diff --git a/aos/events/logging/logfile_utils.h b/aos/events/logging/logfile_utils.h
index 6b8e9aa..5b2bfa6 100644
--- a/aos/events/logging/logfile_utils.h
+++ b/aos/events/logging/logfile_utils.h
@@ -44,6 +44,8 @@
void QueueSizedFlatbuffer(flatbuffers::FlatBufferBuilder *fbb);
// Queues up a detached buffer directly.
void QueueSizedFlatbuffer(flatbuffers::DetachedBuffer &&buffer);
+ // Writes a Span. This is not terribly optimized right now.
+ void WriteSizedFlatbuffer(absl::Span<const uint8_t> span);
// Triggers data to be provided to the kernel and written.
void Flush();
diff --git a/aos/events/logging/logger.cc b/aos/events/logging/logger.cc
index 2de17d7..c501d7b 100644
--- a/aos/events/logging/logger.cc
+++ b/aos/events/logging/logger.cc
@@ -299,8 +299,12 @@
void LogReader::Register(EventLoop *event_loop) {
event_loop_ = event_loop;
+ // We don't run timing reports when trying to print out logged data, because
+ // otherwise we would end up printing out the timing reports themselves...
+ // This is only really relevant when we are replaying into a simulation.
// Otherwise we replay the timing report and try to resend it...
event_loop_->SkipTimingReport();
+ event_loop_->SkipAosLog();
for (size_t i = 0; i < channels_.size(); ++i) {
const Channel *const original_channel =
diff --git a/aos/events/logging/multinode_pingpong.json b/aos/events/logging/multinode_pingpong.json
index f85a4e1..be8a402 100644
--- a/aos/events/logging/multinode_pingpong.json
+++ b/aos/events/logging/multinode_pingpong.json
@@ -1,5 +1,21 @@
{
"channels": [
+ {
+ "name": "/aos/pi1",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "pi1",
+ "frequency": 200,
+ "num_senders": 20,
+ "max_size": 2048
+ },
+ {
+ "name": "/aos/pi2",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "pi2",
+ "frequency": 200,
+ "num_senders": 20,
+ "max_size": 2048
+ },
/* Logged on pi1 locally */
{
"name": "/aos/pi1",
@@ -57,6 +73,26 @@
}
],
"maps": [
+ {
+ "match": {
+ "name": "/aos",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "pi1"
+ },
+ "rename": {
+ "name": "/aos/pi1"
+ }
+ },
+ {
+ "match": {
+ "name": "/aos",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "pi2"
+ },
+ "rename": {
+ "name": "/aos/pi2"
+ }
+ },
{
"match": {
"name": "/aos",
diff --git a/aos/events/multinode_pingpong.json b/aos/events/multinode_pingpong.json
index c0c5087..80ffe76 100644
--- a/aos/events/multinode_pingpong.json
+++ b/aos/events/multinode_pingpong.json
@@ -2,6 +2,38 @@
"channels": [
{
"name": "/aos/pi1",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "pi1",
+ "frequency": 200,
+ "num_senders": 20,
+ "max_size": 2048
+ },
+ {
+ "name": "/aos/pi2",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "pi2",
+ "frequency": 200,
+ "num_senders": 20,
+ "max_size": 2048
+ },
+ {
+ "name": "/aos/pi3",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "pi3",
+ "frequency": 200,
+ "num_senders": 20,
+ "max_size": 2048
+ },
+ {
+ "name": "/aos/roborio",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "roborio",
+ "frequency": 200,
+ "num_senders": 20,
+ "max_size": 2048
+ },
+ {
+ "name": "/aos/pi1",
"type": "aos.message_bridge.ServerStatistics",
"source_node": "pi1",
"frequency": 2
@@ -137,6 +169,46 @@
{
"match": {
"name": "/aos",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "pi1"
+ },
+ "rename": {
+ "name": "/aos/pi1"
+ }
+ },
+ {
+ "match": {
+ "name": "/aos",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "pi2"
+ },
+ "rename": {
+ "name": "/aos/pi2"
+ }
+ },
+ {
+ "match": {
+ "name": "/aos",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "pi3"
+ },
+ "rename": {
+ "name": "/aos/pi3"
+ }
+ },
+ {
+ "match": {
+ "name": "/aos",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "roborio"
+ },
+ "rename": {
+ "name": "/aos/roborio"
+ }
+ },
+ {
+ "match": {
+ "name": "/aos",
"type": "aos.timing.Report",
"source_node": "pi1"
},
diff --git a/aos/events/shm_event_loop.cc b/aos/events/shm_event_loop.cc
index cc11520..b3026fa 100644
--- a/aos/events/shm_event_loop.cc
+++ b/aos/events/shm_event_loop.cc
@@ -5,12 +5,14 @@
#include <sys/syscall.h>
#include <sys/types.h>
#include <unistd.h>
+
#include <algorithm>
#include <atomic>
#include <chrono>
#include <iterator>
#include <stdexcept>
+#include "aos/events/aos_logging.h"
#include "aos/events/epoll.h"
#include "aos/events/event_loop_generated.h"
#include "aos/events/timing_statistics.h"
@@ -18,6 +20,7 @@
#include "aos/ipc_lib/signalfd.h"
#include "aos/realtime.h"
#include "aos/stl_mutex/stl_mutex.h"
+#include "aos/util/file.h"
#include "aos/util/phased_loop.h"
#include "glog/logging.h"
@@ -72,7 +75,7 @@
size_ = ipc_lib::LocklessQueueMemorySize(config_);
- MkdirP(path);
+ util::MkdirP(path, FLAGS_permissions);
// There are 2 cases. Either the file already exists, or it does not
// already exist and we need to create it. Start by trying to create it. If
@@ -123,24 +126,11 @@
const ipc_lib::LocklessQueueConfiguration &config() const { return config_; }
- private:
- void MkdirP(std::string_view path) {
- auto last_slash_pos = path.find_last_of("/");
-
- std::string folder(last_slash_pos == std::string_view::npos
- ? std::string_view("")
- : path.substr(0, last_slash_pos));
- if (folder.empty()) return;
- MkdirP(folder);
- VLOG(1) << "Creating " << folder;
- const int result = mkdir(folder.c_str(), FLAGS_permissions);
- if (result == -1 && errno == EEXIST) {
- VLOG(1) << "Already exists";
- return;
- }
- PCHECK(result == 0) << ": Error creating " << folder;
+ absl::Span<char> GetSharedMemory() const {
+ return absl::Span<char>(static_cast<char *>(data_), size_);
}
+ private:
ipc_lib::LocklessQueueConfiguration config_;
int fd_;
@@ -184,8 +174,8 @@
event_loop->configuration()->channel_storage_duration()))),
lockless_queue_(lockless_queue_memory_.memory(),
lockless_queue_memory_.config()),
- data_storage_(static_cast<AlignedChar *>(aligned_alloc(
- alignof(AlignedChar), channel->max_size())),
+ data_storage_(static_cast<char *>(malloc(channel->max_size() +
+ kChannelDataAlignment - 1)),
&free) {
context_.data = nullptr;
// Point the queue index at the next index to read starting now. This
@@ -217,7 +207,7 @@
actual_queue_index_.index(), &context_.monotonic_event_time,
&context_.realtime_event_time, &context_.monotonic_remote_time,
&context_.realtime_remote_time, &context_.remote_queue_index,
- &context_.size, reinterpret_cast<char *>(data_storage_.get()));
+ &context_.size, data_storage_start());
if (read_result == ipc_lib::LocklessQueue::ReadResult::GOOD) {
context_.queue_index = actual_queue_index_.index();
if (context_.remote_queue_index == 0xffffffffu) {
@@ -229,7 +219,7 @@
if (context_.realtime_remote_time == aos::realtime_clock::min_time) {
context_.realtime_remote_time = context_.realtime_event_time;
}
- context_.data = reinterpret_cast<char *>(data_storage_.get()) +
+ context_.data = data_storage_start() +
lockless_queue_.message_data_size() - context_.size;
actual_queue_index_ = actual_queue_index_.Increment();
}
@@ -267,7 +257,7 @@
queue_index.index(), &context_.monotonic_event_time,
&context_.realtime_event_time, &context_.monotonic_remote_time,
&context_.realtime_remote_time, &context_.remote_queue_index,
- &context_.size, reinterpret_cast<char *>(data_storage_.get()));
+ &context_.size, data_storage_start());
if (read_result == ipc_lib::LocklessQueue::ReadResult::GOOD) {
context_.queue_index = queue_index.index();
if (context_.remote_queue_index == 0xffffffffu) {
@@ -279,7 +269,7 @@
if (context_.realtime_remote_time == aos::realtime_clock::min_time) {
context_.realtime_remote_time = context_.realtime_event_time;
}
- context_.data = reinterpret_cast<char *>(data_storage_.get()) +
+ context_.data = data_storage_start() +
lockless_queue_.message_data_size() - context_.size;
actual_queue_index_ = queue_index.Increment();
}
@@ -314,7 +304,15 @@
void UnregisterWakeup() { lockless_queue_.UnregisterWakeup(); }
+ absl::Span<char> GetSharedMemory() const {
+ return lockless_queue_memory_.GetSharedMemory();
+ }
+
private:
+ char *data_storage_start() {
+ return RoundChannelData(data_storage_.get(), channel_->max_size());
+ }
+
const Channel *const channel_;
MMapedQueue lockless_queue_memory_;
ipc_lib::LocklessQueue lockless_queue_;
@@ -322,14 +320,7 @@
ipc_lib::QueueIndex actual_queue_index_ =
ipc_lib::LocklessQueue::empty_queue_index();
- struct AlignedChar {
- // Cortex-A72 (Raspberry Pi 4) and Cortex-A53 (Xavier AGX) both have 64 byte
- // cache lines.
- // V4L2 requires 64 byte alignment for USERPTR.
- alignas(64) char data;
- };
-
- std::unique_ptr<AlignedChar, decltype(&free)> data_storage_;
+ std::unique_ptr<char, decltype(&free)> data_storage_;
Context context_;
};
@@ -402,6 +393,10 @@
return true;
}
+ absl::Span<char> GetSharedMemory() const {
+ return lockless_queue_memory_.GetSharedMemory();
+ }
+
private:
MMapedQueue lockless_queue_memory_;
ipc_lib::LocklessQueue lockless_queue_;
@@ -456,6 +451,10 @@
void UnregisterWakeup() { return simple_shm_fetcher_.UnregisterWakeup(); }
+ absl::Span<char> GetSharedMemory() const {
+ return simple_shm_fetcher_.GetSharedMemory();
+ }
+
private:
bool has_new_data_ = false;
@@ -708,7 +707,8 @@
ScopedSignalMask mask({SIGINT, SIGHUP, SIGTERM});
std::unique_lock<stl_mutex> locker(mutex_);
- event_loops_.erase(std::find(event_loops_.begin(), event_loops_.end(), event_loop));
+ event_loops_.erase(
+ std::find(event_loops_.begin(), event_loops_.end(), event_loop));
if (event_loops_.size() == 0u) {
// The last caller restores the original signal handlers.
@@ -762,38 +762,45 @@
ReserveEvents();
- aos::SetCurrentThreadName(name_.substr(0, 16));
- // Now, all the callbacks are setup. Lock everything into memory and go RT.
- if (priority_ != 0) {
- ::aos::InitRT();
+ {
+ AosLogToFbs aos_logger;
+ if (!skip_logger_) {
+ aos_logger.Initialize(MakeSender<logging::LogMessageFbs>("/aos"));
+ }
- LOG(INFO) << "Setting priority to " << priority_;
- ::aos::SetCurrentThreadRealtimePriority(priority_);
+ aos::SetCurrentThreadName(name_.substr(0, 16));
+ // Now, all the callbacks are setup. Lock everything into memory and go RT.
+ if (priority_ != 0) {
+ ::aos::InitRT();
+
+ LOG(INFO) << "Setting priority to " << priority_;
+ ::aos::SetCurrentThreadRealtimePriority(priority_);
+ }
+
+ set_is_running(true);
+
+ // Now that we are realtime (but before the OnRun handlers run), snap the
+ // queue index.
+ for (::std::unique_ptr<WatcherState> &watcher : watchers_) {
+ watcher->Startup(this);
+ }
+
+ // Now that we are RT, run all the OnRun handlers.
+ for (const auto &run : on_run_) {
+ run();
+ }
+
+ // And start our main event loop which runs all the timers and handles Quit.
+ epoll_.Run();
+
+ // Once epoll exits, there is no useful nonrt work left to do.
+ set_is_running(false);
+
+ // Nothing time or synchronization critical needs to happen after this
+ // point. Drop RT priority.
+ ::aos::UnsetCurrentThreadRealtimePriority();
}
- set_is_running(true);
-
- // Now that we are realtime (but before the OnRun handlers run), snap the
- // queue index.
- for (::std::unique_ptr<WatcherState> &watcher : watchers_) {
- watcher->Startup(this);
- }
-
- // Now that we are RT, run all the OnRun handlers.
- for (const auto &run : on_run_) {
- run();
- }
-
- // And start our main event loop which runs all the timers and handles Quit.
- epoll_.Run();
-
- // Once epoll exits, there is no useful nonrt work left to do.
- set_is_running(false);
-
- // Nothing time or synchronization critical needs to happen after this point.
- // Drop RT priority.
- ::aos::UnsetCurrentThreadRealtimePriority();
-
for (::std::unique_ptr<WatcherState> &base_watcher : watchers_) {
internal::WatcherState *watcher =
reinterpret_cast<internal::WatcherState *>(base_watcher.get());
@@ -836,6 +843,17 @@
UpdateTimingReport();
}
+absl::Span<char> ShmEventLoop::GetWatcherSharedMemory(const Channel *channel) {
+ internal::WatcherState *const watcher_state =
+ static_cast<internal::WatcherState *>(GetWatcherState(channel));
+ return watcher_state->GetSharedMemory();
+}
+
+absl::Span<char> ShmEventLoop::GetShmSenderSharedMemory(
+ const aos::RawSender *sender) const {
+ return static_cast<const internal::ShmSender *>(sender)->GetSharedMemory();
+}
+
pid_t ShmEventLoop::GetTid() { return syscall(SYS_gettid); }
} // namespace aos
diff --git a/aos/events/shm_event_loop.h b/aos/events/shm_event_loop.h
index 52bb338..fa870b8 100644
--- a/aos/events/shm_event_loop.h
+++ b/aos/events/shm_event_loop.h
@@ -3,6 +3,8 @@
#include <vector>
+#include "absl/types/span.h"
+
#include "aos/events/epoll.h"
#include "aos/events/event_loop.h"
#include "aos/events/event_loop_generated.h"
@@ -68,8 +70,20 @@
int priority() const override { return priority_; }
+ // Returns the epoll loop used to run the event loop.
internal::EPoll *epoll() { return &epoll_; }
+ // Returns the local mapping of the shared memory used by the watcher on the
+ // specified channel. A watcher must be created on this channel before calling
+ // this.
+ absl::Span<char> GetWatcherSharedMemory(const Channel *channel);
+
+ // Returns the local mapping of the shared memory used by the provided Sender
+ template <typename T>
+ absl::Span<char> GetSenderSharedMemory(aos::Sender<T> *sender) const {
+ return GetShmSenderSharedMemory(GetRawSender(sender));
+ }
+
private:
friend class internal::WatcherState;
friend class internal::TimerHandlerState;
@@ -82,6 +96,9 @@
// Returns the TID of the event loop.
pid_t GetTid() override;
+ // Private method to access the shared memory mapping of a ShmSender
+ absl::Span<char> GetShmSenderSharedMemory(const aos::RawSender *sender) const;
+
std::vector<std::function<void()>> on_run_;
int priority_ = 0;
std::string name_;
@@ -90,7 +107,6 @@
internal::EPoll epoll_;
};
-
} // namespace aos
#endif // AOS_EVENTS_SHM_EVENT_LOOP_H_
diff --git a/aos/events/shm_event_loop_test.cc b/aos/events/shm_event_loop_test.cc
index 2edc3fc..87411d2 100644
--- a/aos/events/shm_event_loop_test.cc
+++ b/aos/events/shm_event_loop_test.cc
@@ -32,6 +32,7 @@
unlink((FLAGS_shm_base + "/test2/aos.TestMessage.v1").c_str());
unlink((FLAGS_shm_base + "/test2/aos.TestMessage.v1").c_str());
unlink((FLAGS_shm_base + "/aos/aos.timing.Report.v1").c_str());
+ unlink((FLAGS_shm_base + "/aos/aos.logging.LogMessageFbs.v1").c_str());
}
~ShmEventLoopTestFactory() { FLAGS_override_hostname = ""; }
@@ -183,6 +184,34 @@
EXPECT_EQ(times.size(), 2u);
}
+// Test GetWatcherSharedMemory in a few basic scenarios.
+TEST(ShmEventLoopDeathTest, GetWatcherSharedMemory) {
+ ShmEventLoopTestFactory factory;
+ auto generic_loop1 = factory.MakePrimary("primary");
+ ShmEventLoop *const loop1 = static_cast<ShmEventLoop *>(generic_loop1.get());
+ const auto channel = configuration::GetChannel(
+ loop1->configuration(), "/test", TestMessage::GetFullyQualifiedName(),
+ loop1->name(), loop1->node());
+
+ // First verify it handles an invalid channel reasonably.
+ EXPECT_DEATH(loop1->GetWatcherSharedMemory(channel),
+ "No watcher found for channel");
+
+ // Then, actually create a watcher, and verify it returns something sane.
+ loop1->MakeWatcher("/test", [](const TestMessage &) {});
+ EXPECT_FALSE(loop1->GetWatcherSharedMemory(channel).empty());
+}
+
+TEST(ShmEventLoopTest, GetSenderSharedMemory) {
+ ShmEventLoopTestFactory factory;
+ auto generic_loop1 = factory.MakePrimary("primary");
+ ShmEventLoop *const loop1 = static_cast<ShmEventLoop *>(generic_loop1.get());
+
+ // check that GetSenderSharedMemory returns non-null/non-empty memory span
+ auto sender = loop1->MakeSender<TestMessage>("/test");
+ EXPECT_FALSE(loop1->GetSenderSharedMemory(&sender).empty());
+}
+
// TODO(austin): Test that missing a deadline with a timer recovers as expected.
} // namespace testing
diff --git a/aos/events/simulated_event_loop.cc b/aos/events/simulated_event_loop.cc
index 108a485..01574fa 100644
--- a/aos/events/simulated_event_loop.cc
+++ b/aos/events/simulated_event_loop.cc
@@ -8,25 +8,24 @@
#include "aos/events/simulated_network_bridge.h"
#include "aos/json_to_flatbuffer.h"
#include "aos/util/phased_loop.h"
+#include "aos/events/aos_logging.h"
namespace aos {
// Container for both a message, and the context for it for simulation. This
// makes tracking the timestamps associated with the data easy.
struct SimulatedMessage {
- // Struct to let us force data to be well aligned.
- struct OveralignedChar {
- char data alignas(64);
- };
-
// Context for the data.
Context context;
// The data.
- char *data() { return reinterpret_cast<char *>(&actual_data[0]); }
+ char *data(size_t buffer_size) {
+ return RoundChannelData(&actual_data[0], buffer_size);
+ }
- // Then the data.
- OveralignedChar actual_data[];
+ // Then the data, including padding on the end so we can align the buffer we
+ // actually return from data().
+ char actual_data[];
};
class SimulatedEventLoop;
@@ -125,9 +124,9 @@
// This is a shared_ptr so we don't have to implement refcounting or copying.
std::shared_ptr<SimulatedMessage> MakeSimulatedMessage(size_t size) {
SimulatedMessage *message = reinterpret_cast<SimulatedMessage *>(
- malloc(sizeof(SimulatedMessage) + size));
+ malloc(sizeof(SimulatedMessage) + size + kChannelDataAlignment - 1));
message->context.size = size;
- message->context.data = message->data();
+ message->context.data = message->data(size);
return std::shared_ptr<SimulatedMessage>(message, free);
}
@@ -144,7 +143,7 @@
if (!message_) {
message_ = MakeSimulatedMessage(simulated_channel_->max_size());
}
- return message_->data();
+ return message_->data(simulated_channel_->max_size());
}
size_t size() override { return simulated_channel_->max_size(); }
@@ -187,7 +186,9 @@
// Now fill in the message. size is already populated above, and
// queue_index will be populated in simulated_channel_. Put this at the
// back of the data segment.
- memcpy(message_->data() + simulated_channel_->max_size() - size, msg, size);
+ memcpy(message_->data(simulated_channel_->max_size()) +
+ simulated_channel_->max_size() - size,
+ msg, size);
return DoSend(size, monotonic_remote_time, realtime_remote_time,
remote_queue_index);
@@ -419,7 +420,14 @@
int priority() const override { return priority_; }
- void Setup() { MaybeScheduleTimingReports(); }
+ void Setup() {
+ MaybeScheduleTimingReports();
+ if (!skip_logger_) {
+ logging::ScopedLogRestorer prev_logger;
+ log_sender_.Initialize(MakeSender<logging::LogMessageFbs>("/aos"));
+ log_impl_ = logging::GetImplementation();
+ }
+ }
private:
friend class SimulatedTimerHandler;
@@ -455,6 +463,9 @@
const Node *const node_;
const pid_t tid_;
+
+ AosLogToFbs log_sender_;
+ logging::LogImplementation *log_impl_ = nullptr;
};
void SimulatedEventLoopFactory::set_send_delay(
@@ -553,6 +564,10 @@
const monotonic_clock::time_point monotonic_now =
simulated_event_loop_->monotonic_now();
+ logging::ScopedLogRestorer prev_logger;
+ if (simulated_event_loop_->log_impl_ != nullptr) {
+ logging::SetImplementation(simulated_event_loop_->log_impl_);
+ }
Context context = msgs_.front()->context;
if (context.remote_queue_index == 0xffffffffu) {
@@ -606,8 +621,8 @@
uint32_t SimulatedChannel::Send(std::shared_ptr<SimulatedMessage> message) {
const uint32_t queue_index = next_queue_index_.index();
message->context.queue_index = queue_index;
- message->context.data =
- message->data() + channel()->max_size() - message->context.size;
+ message->context.data = message->data(channel()->max_size()) +
+ channel()->max_size() - message->context.size;
next_queue_index_ = next_queue_index_.Increment();
latest_message_ = message;
@@ -660,6 +675,10 @@
void SimulatedTimerHandler::HandleEvent() {
const ::aos::monotonic_clock::time_point monotonic_now =
simulated_event_loop_->monotonic_now();
+ logging::ScopedLogRestorer prev_logger;
+ if (simulated_event_loop_->log_impl_ != nullptr) {
+ logging::SetImplementation(simulated_event_loop_->log_impl_);
+ }
if (repeat_offset_ != ::aos::monotonic_clock::zero()) {
// Reschedule.
while (base_ <= monotonic_now) base_ += repeat_offset_;
@@ -706,6 +725,10 @@
void SimulatedPhasedLoopHandler::HandleEvent() {
monotonic_clock::time_point monotonic_now =
simulated_event_loop_->monotonic_now();
+ logging::ScopedLogRestorer prev_logger;
+ if (simulated_event_loop_->log_impl_ != nullptr) {
+ logging::SetImplementation(simulated_event_loop_->log_impl_);
+ }
Call(
[monotonic_now]() { return monotonic_now; },
[this](monotonic_clock::time_point sleep_time) { Schedule(sleep_time); });
diff --git a/aos/flatbuffer_merge.cc b/aos/flatbuffer_merge.cc
index c8cc742..1f5c8a0 100644
--- a/aos/flatbuffer_merge.cc
+++ b/aos/flatbuffer_merge.cc
@@ -526,4 +526,27 @@
return fbb.Release();
}
+bool CompareFlatBuffer(const flatbuffers::TypeTable *typetable,
+ const flatbuffers::Table *t1,
+ const flatbuffers::Table *t2) {
+ // Copying flatbuffers is deterministic for the same typetable. So, copy both
+ // to guarantee that they are sorted the same, then check that the memory
+ // matches.
+ //
+ // There has to be a better way to do this, but the efficiency hit of this
+ // implementation is fine for the usages that we have now. We are better off
+ // abstracting this into a library call where we can fix it later easily.
+ flatbuffers::FlatBufferBuilder fbb1;
+ fbb1.ForceDefaults(1);
+ fbb1.Finish(MergeFlatBuffers(typetable, t1, nullptr, &fbb1));
+ flatbuffers::FlatBufferBuilder fbb2;
+ fbb2.ForceDefaults(1);
+ fbb2.Finish(MergeFlatBuffers(typetable, t2, nullptr, &fbb2));
+
+ if (fbb1.GetSize() != fbb2.GetSize()) return false;
+
+ return memcmp(fbb1.GetBufferPointer(), fbb2.GetBufferPointer(),
+ fbb1.GetSize()) == 0;
+}
+
} // namespace aos
diff --git a/aos/flatbuffer_merge.h b/aos/flatbuffer_merge.h
index dafba34..5e84160 100644
--- a/aos/flatbuffer_merge.h
+++ b/aos/flatbuffer_merge.h
@@ -77,6 +77,18 @@
return FlatbufferDetachedBuffer<T>(fbb.Release());
}
+// Compares 2 flatbuffers. Returns true if they match, false otherwise.
+bool CompareFlatBuffer(const flatbuffers::TypeTable *typetable,
+ const flatbuffers::Table *t1,
+ const flatbuffers::Table *t2);
+
+template <class T>
+inline bool CompareFlatBuffer(const T *t1, const T *t2) {
+ return CompareFlatBuffer(T::MiniReflectTypeTable(),
+ reinterpret_cast<const flatbuffers::Table *>(t1),
+ reinterpret_cast<const flatbuffers::Table *>(t2));
+}
+
} // namespace aos
#endif // AOS_FLATBUFFER_MERGE_H_
diff --git a/aos/flatbuffer_merge_test.cc b/aos/flatbuffer_merge_test.cc
index 079eb11..371639a 100644
--- a/aos/flatbuffer_merge_test.cc
+++ b/aos/flatbuffer_merge_test.cc
@@ -20,6 +20,13 @@
const ::std::string merged_output =
FlatbufferToJson(fb_merged, ConfigurationTypeTable());
EXPECT_EQ(expected_output, merged_output);
+
+ aos::FlatbufferDetachedBuffer<Configuration> expected_message(
+ JsonToFlatbuffer(std::string(expected_output).c_str(),
+ ConfigurationTypeTable()));
+ EXPECT_TRUE(
+ CompareFlatBuffer(flatbuffers::GetRoot<Configuration>(fb_merged.data()),
+ &expected_message.message()));
}
void JsonMerge(const ::std::string in1, const ::std::string in2,
@@ -335,6 +342,18 @@
"\"name\": \"woo2\" }, { \"name\": \"wo3\" } ] }");
}
+// Tests a compare of 2 basic (different) messages.
+TEST_F(FlatbufferMerge, CompareDifferent) {
+ aos::FlatbufferDetachedBuffer<Configuration> message1(JsonToFlatbuffer(
+ "{ \"single_application\": { \"name\": \"wow\", \"priority\": 7 } }",
+ ConfigurationTypeTable()));
+ aos::FlatbufferDetachedBuffer<Configuration> message2(JsonToFlatbuffer(
+ "{ \"single_application\": { \"name\": \"wow\", \"priority\": 8 } }",
+ ConfigurationTypeTable()));
+
+ EXPECT_FALSE(CompareFlatBuffer(&message1.message(), &message2.message()));
+}
+
// TODO(austin): enums
// TODO(austin): unions
// TODO(austin): struct
diff --git a/aos/flatbuffers.h b/aos/flatbuffers.h
index c1c0db0..d9fcab6 100644
--- a/aos/flatbuffers.h
+++ b/aos/flatbuffers.h
@@ -12,6 +12,8 @@
// This class is a base class for all sizes of array backed allocators.
class FixedAllocatorBase : public flatbuffers::Allocator {
public:
+ ~FixedAllocatorBase() override { CHECK(!is_allocated_); }
+
// TODO(austin): Read the contract for these.
uint8_t *allocate(size_t) override;
@@ -25,6 +27,7 @@
virtual size_t size() const = 0;
void Reset() { is_allocated_ = false; }
+ bool is_allocated() const { return is_allocated_; }
private:
bool is_allocated_ = false;
@@ -51,14 +54,32 @@
class PreallocatedAllocator : public FixedAllocatorBase {
public:
PreallocatedAllocator(void *data, size_t size) : data_(data), size_(size) {}
- uint8_t *data() override { return reinterpret_cast<uint8_t *>(data_); }
- const uint8_t *data() const override {
- return reinterpret_cast<const uint8_t *>(data_);
+ PreallocatedAllocator(const PreallocatedAllocator&) = delete;
+ PreallocatedAllocator(PreallocatedAllocator &&other)
+ : data_(other.data_), size_(other.size_) {
+ CHECK(!is_allocated());
+ CHECK(!other.is_allocated());
}
- size_t size() const override { return size_; }
+
+ PreallocatedAllocator &operator=(const PreallocatedAllocator &) = delete;
+ PreallocatedAllocator &operator=(PreallocatedAllocator &&other) {
+ CHECK(!is_allocated());
+ CHECK(!other.is_allocated());
+ data_ = other.data_;
+ size_ = other.size_;
+ return *this;
+ }
+
+ uint8_t *data() final {
+ return reinterpret_cast<uint8_t *>(CHECK_NOTNULL(data_));
+ }
+ const uint8_t *data() const final {
+ return reinterpret_cast<const uint8_t *>(CHECK_NOTNULL(data_));
+ }
+ size_t size() const final { return size_; }
private:
- void* data_;
+ void *data_;
size_t size_;
};
diff --git a/aos/ipc_lib/BUILD b/aos/ipc_lib/BUILD
index 57adc6d..91b050c 100644
--- a/aos/ipc_lib/BUILD
+++ b/aos/ipc_lib/BUILD
@@ -204,6 +204,7 @@
visibility = ["//visibility:public"],
deps = [
":aos_sync",
+ ":data_alignment",
":index",
"//aos:realtime",
"//aos/time",
@@ -259,3 +260,14 @@
"//aos/testing:test_logging",
],
)
+
+cc_library(
+ name = "data_alignment",
+ hdrs = [
+ "data_alignment.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "@com_github_google_glog//:glog",
+ ],
+)
diff --git a/aos/ipc_lib/data_alignment.h b/aos/ipc_lib/data_alignment.h
new file mode 100644
index 0000000..2f59b78
--- /dev/null
+++ b/aos/ipc_lib/data_alignment.h
@@ -0,0 +1,41 @@
+#ifndef AOS_IPC_LIB_DATA_ALIGNMENT_H_
+#define AOS_IPC_LIB_DATA_ALIGNMENT_H_
+
+#include "glog/logging.h"
+
+namespace aos {
+
+// All data buffers sent over or received from a channel will guarantee this
+// alignment for their end. Flatbuffers aligns from the end, so this is what
+// matters.
+//
+// 64 is a reasonable choice for now:
+// Cortex-A72 (Raspberry Pi 4) and Cortex-A53 (Xavier AGX) both have 64 byte
+// cache lines.
+// V4L2 requires 64 byte alignment for USERPTR buffers.
+static constexpr size_t kChannelDataAlignment = 64;
+
+template <typename T>
+inline void CheckChannelDataAlignment(T *data, size_t size) {
+ CHECK_EQ((reinterpret_cast<uintptr_t>(data) + size) % kChannelDataAlignment,
+ 0u)
+ << ": data pointer is not end aligned as it should be: " << data << " + "
+ << size;
+}
+
+// Aligns the beginning of a channel data buffer. There must be
+// kChannelDataAlignment-1 extra bytes beyond the end to potentially use after
+// aligning it.
+inline char *RoundChannelData(char *data, size_t size) {
+ const uintptr_t data_value = reinterpret_cast<uintptr_t>(data);
+ const uintptr_t data_end = data_value + size;
+ const uintptr_t data_end_max = data_end + (kChannelDataAlignment - 1);
+ const uintptr_t rounded_data_end =
+ data_end_max - (data_end_max % kChannelDataAlignment);
+ const uintptr_t rounded_data = rounded_data_end - size;
+ return reinterpret_cast<char *>(rounded_data);
+}
+
+} // namespace aos
+
+#endif // AOS_IPC_LIB_DATA_ALIGNMENT_H_
diff --git a/aos/ipc_lib/lockless_queue.cc b/aos/ipc_lib/lockless_queue.cc
index 903150b..c323b8b 100644
--- a/aos/ipc_lib/lockless_queue.cc
+++ b/aos/ipc_lib/lockless_queue.cc
@@ -241,7 +241,8 @@
size_t LocklessQueueConfiguration::message_size() const {
// Round up the message size so following data is aligned appropriately.
- return LocklessQueueMemory::AlignmentRoundUp(message_data_size) +
+ return LocklessQueueMemory::AlignmentRoundUp(message_data_size +
+ (kChannelDataAlignment - 1)) +
sizeof(Message);
}
@@ -549,7 +550,7 @@
Message *message = memory_->GetMessage(scratch_index);
message->header.queue_index.Invalidate();
- return &message->data[0];
+ return message->data(memory_->message_data_size());
}
void LocklessQueue::Sender::Send(
@@ -788,7 +789,7 @@
}
*monotonic_remote_time = m->header.monotonic_remote_time;
*realtime_remote_time = m->header.realtime_remote_time;
- memcpy(data, &m->data[0], message_data_size());
+ memcpy(data, m->data(memory_->message_data_size()), message_data_size());
*length = m->header.length;
// And finally, confirm that the message *still* points to the queue index we
@@ -891,8 +892,9 @@
::std::cout << " }" << ::std::endl;
::std::cout << " data: {";
+ const char *const m_data = m->data(memory->message_data_size());
for (size_t j = 0; j < m->header.length; ++j) {
- char data = m->data[j];
+ char data = m_data[j];
if (j != 0) {
::std::cout << " ";
}
diff --git a/aos/ipc_lib/lockless_queue.h b/aos/ipc_lib/lockless_queue.h
index 976f758..0384aa8 100644
--- a/aos/ipc_lib/lockless_queue.h
+++ b/aos/ipc_lib/lockless_queue.h
@@ -7,6 +7,7 @@
#include <vector>
#include "aos/ipc_lib/aos_sync.h"
+#include "aos/ipc_lib/data_alignment.h"
#include "aos/ipc_lib/index.h"
#include "aos/time/time.h"
@@ -76,7 +77,19 @@
size_t length;
} header;
- char data[];
+ char *data(size_t message_size) { return RoundedData(message_size); }
+ const char *data(size_t message_size) const {
+ return RoundedData(message_size);
+ }
+
+ private:
+ // This returns a non-const pointer into a const object. Be very careful about
+ // const correctness in publicly accessible APIs using it.
+ char *RoundedData(size_t message_size) const {
+ return RoundChannelData(const_cast<char *>(&data_pointer[0]), message_size);
+ }
+
+ char data_pointer[];
};
struct LocklessQueueConfiguration {
diff --git a/aos/ipc_lib/lockless_queue_memory.h b/aos/ipc_lib/lockless_queue_memory.h
index 0c0973c..cbe76a7 100644
--- a/aos/ipc_lib/lockless_queue_memory.h
+++ b/aos/ipc_lib/lockless_queue_memory.h
@@ -89,18 +89,24 @@
// Getters for each of the 4 lists.
Sender *GetSender(size_t sender_index) {
+ static_assert(alignof(Sender) <= kDataAlignment,
+ "kDataAlignment is too small");
return reinterpret_cast<Sender *>(&data[0] + SizeOfQueue() +
SizeOfMessages() + SizeOfWatchers() +
sender_index * sizeof(Sender));
}
Watcher *GetWatcher(size_t watcher_index) {
+ static_assert(alignof(Watcher) <= kDataAlignment,
+ "kDataAlignment is too small");
return reinterpret_cast<Watcher *>(&data[0] + SizeOfQueue() +
SizeOfMessages() +
watcher_index * sizeof(Watcher));
}
AtomicIndex *GetQueue(uint32_t index) {
+ static_assert(alignof(AtomicIndex) <= kDataAlignment,
+ "kDataAlignment is too small");
return reinterpret_cast<AtomicIndex *>(&data[0] +
sizeof(AtomicIndex) * index);
}
@@ -109,6 +115,8 @@
// sender list, since those are messages available to be filled in and sent.
// This removes the need to find lost messages when a sender dies.
Message *GetMessage(Index index) {
+ static_assert(alignof(Message) <= kDataAlignment,
+ "kDataAlignment is too small");
return reinterpret_cast<Message *>(&data[0] + SizeOfQueue() +
index.message_index() * message_size());
}
diff --git a/aos/logging/BUILD b/aos/logging/BUILD
index 4931696..214df63 100644
--- a/aos/logging/BUILD
+++ b/aos/logging/BUILD
@@ -1,3 +1,5 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+
# The primary client logging interface.
cc_library(
name = "logging",
@@ -150,3 +152,10 @@
"@com_google_absl//absl/base",
],
)
+
+flatbuffer_cc_library(
+ name = "log_message_fbs",
+ srcs = ["log_message.fbs"],
+ visibility = ["//visibility:public"],
+ gen_reflections = 1,
+)
diff --git a/aos/logging/implementations.cc b/aos/logging/implementations.cc
index cfc14c0..04831a2 100644
--- a/aos/logging/implementations.cc
+++ b/aos/logging/implementations.cc
@@ -131,10 +131,6 @@
void SetImplementation(LogImplementation *implementation, bool update_global) {
internal::Context *context = internal::Context::Get();
- if (implementation == nullptr) {
- AOS_LOG(FATAL, "SetImplementation got invalid implementation");
- }
-
context->implementation = implementation;
if (update_global) {
SetGlobalImplementation(implementation);
@@ -150,6 +146,10 @@
return old;
}
+LogImplementation *GetImplementation() {
+ return internal::Context::Get()->implementation;
+}
+
void Init() {
static absl::once_flag once;
absl::call_once(once, DoInit);
@@ -264,7 +264,7 @@
void RegisterCallbackImplementation(
const ::std::function<void(const LogMessage &)> &callback,
- bool update_global = true) {
+ bool update_global) {
Init();
SetImplementation(new CallbackLogImplementation(callback), update_global);
}
diff --git a/aos/logging/implementations.h b/aos/logging/implementations.h
index 78980ff..6cf8376 100644
--- a/aos/logging/implementations.h
+++ b/aos/logging/implementations.h
@@ -132,6 +132,7 @@
// when needed or by calling Load()).
// The logging system takes ownership of implementation. It will delete it if
// necessary, so it must be created with new.
+// TODO: Log implementations are never deleted. Need means to safely deregister.
void SetImplementation(LogImplementation *implementation,
bool update_global = true);
@@ -139,6 +140,8 @@
// implementation.
LogImplementation *SwapImplementation(LogImplementation *implementation);
+LogImplementation *GetImplementation();
+
// Must be called at least once per process/load before anything else is
// called. This function is safe to call multiple times from multiple
// tasks/threads.
@@ -164,7 +167,18 @@
void RegisterQueueImplementation();
void RegisterCallbackImplementation(
- const ::std::function<void(const LogMessage &)> &callback);
+ const ::std::function<void(const LogMessage &)> &callback,
+ bool update_global = true);
+
+class ScopedLogRestorer {
+ public:
+ ScopedLogRestorer() { prev_impl_ = GetImplementation(); }
+
+ ~ScopedLogRestorer() { SetImplementation(prev_impl_); }
+
+ private:
+ LogImplementation *prev_impl_;
+};
// This is where all of the code that is only used by actual LogImplementations
// goes.
diff --git a/aos/logging/implementations_test.cc b/aos/logging/implementations_test.cc
index a7b8b7d..272214a 100644
--- a/aos/logging/implementations_test.cc
+++ b/aos/logging/implementations_test.cc
@@ -244,6 +244,19 @@
EXPECT_EQ(kExpected1, ::std::string(buffer));
}
+TEST(ScopedLogRestorerTest, RestoreTest) {
+ LogImplementation *curr_impl = GetImplementation();
+
+ {
+ ScopedLogRestorer log_restorer;
+
+ logging::RegisterCallbackImplementation([] (const LogMessage&) {});
+ ASSERT_NE(curr_impl, GetImplementation());
+ }
+
+ ASSERT_EQ(curr_impl, GetImplementation());
+}
+
} // namespace testing
} // namespace logging
} // namespace aos
diff --git a/aos/logging/log_message.fbs b/aos/logging/log_message.fbs
new file mode 100644
index 0000000..789724f
--- /dev/null
+++ b/aos/logging/log_message.fbs
@@ -0,0 +1,27 @@
+namespace aos.logging;
+
+// Log level, corresponding to levels in logging.h
+enum Level : byte {
+ ERROR = -1,
+ DEBUG = 0,
+ INFO = 1,
+ WARNING = 2,
+ FATAL = 4,
+ LOG_UNKNOWN = 5
+}
+
+table LogMessageFbs {
+ // Text of the log message, includng file name and line
+ message:string (id: 0);
+
+ // Severity of the log message
+ level:Level (id: 1);
+
+ // Pid of the process creating the log message
+ source:int (id:2);
+
+ // Application name
+ name:string (id:3);
+}
+
+root_type LogMessageFbs;
diff --git a/aos/network/message_bridge_test_common.json b/aos/network/message_bridge_test_common.json
index d0085c2..a073869 100644
--- a/aos/network/message_bridge_test_common.json
+++ b/aos/network/message_bridge_test_common.json
@@ -2,6 +2,22 @@
"channels": [
{
"name": "/aos/pi1",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "pi1",
+ "frequency": 200,
+ "num_senders": 20,
+ "max_size": 2048
+ },
+ {
+ "name": "/aos/pi2",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "pi2",
+ "frequency": 200,
+ "num_senders": 20,
+ "max_size": 2048
+ },
+ {
+ "name": "/aos/pi1",
"type": "aos.message_bridge.ServerStatistics",
"source_node": "pi1",
"frequency": 2
diff --git a/aos/util/file.cc b/aos/util/file.cc
index af1f85b..b334ded 100644
--- a/aos/util/file.cc
+++ b/aos/util/file.cc
@@ -1,6 +1,8 @@
#include "aos/util/file.h"
#include <fcntl.h>
+#include <sys/stat.h>
+#include <sys/types.h>
#include <unistd.h>
#include <string_view>
@@ -46,5 +48,23 @@
}
}
+void MkdirP(std::string_view path, mode_t mode) {
+ auto last_slash_pos = path.find_last_of("/");
+
+ std::string folder(last_slash_pos == std::string_view::npos
+ ? std::string_view("")
+ : path.substr(0, last_slash_pos));
+ if (folder.empty()) return;
+ MkdirP(folder, mode);
+ const int result = mkdir(folder.c_str(), mode);
+ if (result == -1 && errno == EEXIST) {
+ VLOG(2) << folder << " already exists";
+ return;
+ } else {
+ VLOG(1) << "Created " << folder;
+ }
+ PCHECK(result == 0) << ": Error creating " << folder;
+}
+
} // namespace util
} // namespace aos
diff --git a/aos/util/file.h b/aos/util/file.h
index 3aebd87..d6724af 100644
--- a/aos/util/file.h
+++ b/aos/util/file.h
@@ -15,6 +15,8 @@
void WriteStringToFileOrDie(const std::string_view filename,
const std::string_view contents);
+void MkdirP(std::string_view path, mode_t mode);
+
} // namespace util
} // namespace aos
diff --git a/build_tests/BUILD b/build_tests/BUILD
index a4c5829..0e3b8f7 100644
--- a/build_tests/BUILD
+++ b/build_tests/BUILD
@@ -115,3 +115,12 @@
srcs = ["python_fbs.py"],
deps = [":test_python_fbs"],
)
+
+py_test(
+ name = "python3_opencv",
+ srcs = ["python_opencv.py"],
+ default_python_version = "PY3",
+ main = "python_opencv.py",
+ srcs_version = "PY2AND3",
+ deps = ["@opencv_contrib_nonfree_amd64//:python_opencv"],
+)
diff --git a/build_tests/python_opencv.py b/build_tests/python_opencv.py
new file mode 100644
index 0000000..c353c79
--- /dev/null
+++ b/build_tests/python_opencv.py
@@ -0,0 +1,6 @@
+#!/usr/bin/python3
+
+import cv2
+
+if __name__ == '__main__':
+ cv2.xfeatures2d.SIFT_create()
diff --git a/debian/matplotlib.bzl b/debian/matplotlib.bzl
index be520f3..7032745 100644
--- a/debian/matplotlib.bzl
+++ b/debian/matplotlib.bzl
@@ -100,146 +100,146 @@
}
def build_matplotlib(version, tkinter_py_version = None, copy_shared_files = True):
- """Creates a py_library rule for matplotlib for the given python version.
+ """Creates a py_library rule for matplotlib for the given python version.
- See debian/matplotlib.BUILD for the usage.
+ See debian/matplotlib.BUILD for the usage.
- All the rules generated by this will be suffixed by version. Only one
- instance of this macro should set copy_shared_files, which generate the
- files that are shared between python versions.
+ All the rules generated by this will be suffixed by version. Only one
+ instance of this macro should set copy_shared_files, which generate the
+ files that are shared between python versions.
- tkinter_py_version is used because for the Python3 instance, some files
- are in folders named python3 and some are in folders named python3.5...
+ tkinter_py_version is used because for the Python3 instance, some files
+ are in folders named python3 and some are in folders named python3.5...
- version numbers should both be strings.
- """
- if tkinter_py_version == None:
- tkinter_py_version = version
+ version numbers should both be strings.
+ """
+ if tkinter_py_version == None:
+ tkinter_py_version = version
- native.genrule(
- name = "patch_init" + version,
- srcs = [
- "usr/lib/python" + version + "/dist-packages/matplotlib/__init__.py",
- "@//debian:matplotlib_patches",
- ],
- outs = [version + "/matplotlib/__init__.py"],
- cmd = " && ".join([
- "cp $(location usr/lib/python" + version + "/dist-packages/matplotlib/__init__.py) $@",
- "readonly PATCH=\"$$(readlink -f $(location @patch))\"",
- "readonly FILE=\"$$(readlink -f $(location @//debian:matplotlib_patches))\"",
- "(cd $(@D) && \"$${PATCH}\" -p1 < \"$${FILE}\") > /dev/null",
- ]),
- tools = [
- "@patch",
- ],
- )
-
- _src_files = native.glob(
- include = ["usr/lib/python" + version + "/dist-packages/**/*.py"],
- exclude = [
- "usr/lib/python" + version + "/dist-packages/matplotlib/__init__.py",
- ],
- )
-
- _data_files = native.glob([
- "usr/share/matplotlib/mpl-data/**",
- "usr/share/tcltk/**",
- ])
-
- _src_copied = ["/".join([version] + f.split("/")[4:]) for f in _src_files]
-
- _builtin_so_files = native.glob([
- "usr/lib/python" + version + "/dist-packages/**/*x86_64-linux-gnu.so",
- "usr/lib/python" + tkinter_py_version + "/lib-dynload/*.so",
- ])
-
- _system_so_files = native.glob([
- "usr/lib/**/*.so*",
- "lib/x86_64-linux-gnu/**/*.so*",
- ])
-
- _builtin_so_copied = ["/".join([version] + f.split("/")[4:]) for f in _builtin_so_files]
-
- rpath_prefix = "rpathed" + version + "/"
-
- _system_so_copied = [rpath_prefix + f for f in _system_so_files]
-
- _builtin_rpaths = [":".join([
- "\\$$ORIGIN/%s" % rel,
- "\\$$ORIGIN/%s/%s/usr/lib/x86_64-linux-gnu" % (rel, rpath_prefix),
- "\\$$ORIGIN/%s/%s/usr/lib" % (rel, rpath_prefix),
- "\\$$ORIGIN/%s/%s/lib/x86_64-linux-gnu" % (rel, rpath_prefix),
- ]) for rel in ["/".join([".." for _ in so.split("/")[1:]]) for so in _builtin_so_copied]]
-
- _system_rpaths = [":".join([
- "\\$$ORIGIN/%s/%s/usr/lib/x86_64-linux-gnu" % (rel, rpath_prefix),
- "\\$$ORIGIN/%s/%s/lib/x86_64-linux-gnu" % (rel, rpath_prefix),
- ]) for rel in ["/".join([".." for _ in so.split("/")[1:]]) for so in _system_so_copied]]
-
- native.genrule(
- name = "run_patchelf_builtin" + version,
- srcs = _builtin_so_files,
- outs = _builtin_so_copied,
- cmd = "\n".join(
- [
- "cp $(location %s) $(location %s)" % (src, dest)
- for src, dest in zip(_builtin_so_files, _builtin_so_copied)
- ] +
- ["$(location @patchelf) --set-rpath %s $(location %s)" % (rpath, so) for rpath, so in zip(_builtin_rpaths, _builtin_so_copied)],
- ),
- tools = [
- "@patchelf",
- ],
- )
-
- native.genrule(
- name = "run_patchelf_system" + version,
- srcs = _system_so_files,
- outs = _system_so_copied,
- cmd = "\n".join(
- [
- "cp $(location %s) $(location %s)" % (src, dest)
- for src, dest in zip(_system_so_files, _system_so_copied)
- ] +
- ["$(location @patchelf) --set-rpath %s $(location %s)" % (rpath, so) for rpath, so in zip(_system_rpaths, _system_so_copied)],
- ),
- tools = [
- "@patchelf",
- ],
- )
-
- native.genrule(
- name = "copy_files" + version,
- srcs = _src_files,
- outs = _src_copied,
- cmd = " && ".join(["cp $(location %s) $(location %s)" % (src, dest) for src, dest in zip(
- _src_files,
- _src_copied,
- )]),
- )
-
- if copy_shared_files:
native.genrule(
- name = "create_rc" + version,
- outs = ["usr/share/matplotlib/mpl-data/matplotlibrc"],
- cmd = "\n".join([
- "cat > $@ << END",
- # This is necessary to make matplotlib actually plot things to the
- # screen by default.
- "backend : TkAgg",
- "END",
+ name = "patch_init" + version,
+ srcs = [
+ "usr/lib/python" + version + "/dist-packages/matplotlib/__init__.py",
+ "@//debian:matplotlib_patches",
+ ],
+ outs = [version + "/matplotlib/__init__.py"],
+ cmd = " && ".join([
+ "cp $(location usr/lib/python" + version + "/dist-packages/matplotlib/__init__.py) $@",
+ "readonly PATCH=\"$$(readlink -f $(location @patch))\"",
+ "readonly FILE=\"$$(readlink -f $(location @//debian:matplotlib_patches))\"",
+ "(cd $(@D) && \"$${PATCH}\" -p1 < \"$${FILE}\") > /dev/null",
]),
+ tools = [
+ "@patch",
+ ],
)
- native.py_library(
- name = "matplotlib" + version,
- srcs = _src_copied + [
- version + "/matplotlib/__init__.py",
- ],
- data = _data_files + _builtin_so_copied + _system_so_copied + [
- ":usr/share/matplotlib/mpl-data/matplotlibrc",
- ] + native.glob(["etc/**"]),
- imports = ["usr/lib/python" + version + "/dist-packages", version, "."],
- restricted_to = ["@//tools:k8"],
- visibility = ["//visibility:public"],
- )
+ _src_files = native.glob(
+ include = ["usr/lib/python" + version + "/dist-packages/**/*.py"],
+ exclude = [
+ "usr/lib/python" + version + "/dist-packages/matplotlib/__init__.py",
+ ],
+ )
+
+ _data_files = native.glob([
+ "usr/share/matplotlib/mpl-data/**",
+ "usr/share/tcltk/**",
+ ])
+
+ _src_copied = ["/".join([version] + f.split("/")[4:]) for f in _src_files]
+
+ _builtin_so_files = native.glob([
+ "usr/lib/python" + version + "/dist-packages/**/*x86_64-linux-gnu.so",
+ "usr/lib/python" + tkinter_py_version + "/lib-dynload/*.so",
+ ])
+
+ _system_so_files = native.glob([
+ "usr/lib/**/*.so*",
+ "lib/x86_64-linux-gnu/**/*.so*",
+ ])
+
+ _builtin_so_copied = ["/".join([version] + f.split("/")[4:]) for f in _builtin_so_files]
+
+ rpath_prefix = "rpathed" + version + "/"
+
+ _system_so_copied = [rpath_prefix + f for f in _system_so_files]
+
+ _builtin_rpaths = [":".join([
+ "\\$$ORIGIN/%s" % rel,
+ "\\$$ORIGIN/%s/%s/usr/lib/x86_64-linux-gnu" % (rel, rpath_prefix),
+ "\\$$ORIGIN/%s/%s/usr/lib" % (rel, rpath_prefix),
+ "\\$$ORIGIN/%s/%s/lib/x86_64-linux-gnu" % (rel, rpath_prefix),
+ ]) for rel in ["/".join([".." for _ in so.split("/")[1:]]) for so in _builtin_so_copied]]
+
+ _system_rpaths = [":".join([
+ "\\$$ORIGIN/%s/%s/usr/lib/x86_64-linux-gnu" % (rel, rpath_prefix),
+ "\\$$ORIGIN/%s/%s/lib/x86_64-linux-gnu" % (rel, rpath_prefix),
+ ]) for rel in ["/".join([".." for _ in so.split("/")[1:]]) for so in _system_so_copied]]
+
+ native.genrule(
+ name = "run_patchelf_builtin" + version,
+ srcs = _builtin_so_files,
+ outs = _builtin_so_copied,
+ cmd = "\n".join(
+ [
+ "cp $(location %s) $(location %s)" % (src, dest)
+ for src, dest in zip(_builtin_so_files, _builtin_so_copied)
+ ] +
+ ["$(location @patchelf) --set-rpath %s $(location %s)" % (rpath, so) for rpath, so in zip(_builtin_rpaths, _builtin_so_copied)],
+ ),
+ tools = [
+ "@patchelf",
+ ],
+ )
+
+ native.genrule(
+ name = "run_patchelf_system" + version,
+ srcs = _system_so_files,
+ outs = _system_so_copied,
+ cmd = "\n".join(
+ [
+ "cp $(location %s) $(location %s)" % (src, dest)
+ for src, dest in zip(_system_so_files, _system_so_copied)
+ ] +
+ ["$(location @patchelf) --set-rpath %s $(location %s)" % (rpath, so) for rpath, so in zip(_system_rpaths, _system_so_copied)],
+ ),
+ tools = [
+ "@patchelf",
+ ],
+ )
+
+ native.genrule(
+ name = "copy_files" + version,
+ srcs = _src_files,
+ outs = _src_copied,
+ cmd = " && ".join(["cp $(location %s) $(location %s)" % (src, dest) for src, dest in zip(
+ _src_files,
+ _src_copied,
+ )]),
+ )
+
+ if copy_shared_files:
+ native.genrule(
+ name = "create_rc" + version,
+ outs = ["usr/share/matplotlib/mpl-data/matplotlibrc"],
+ cmd = "\n".join([
+ "cat > $@ << END",
+ # This is necessary to make matplotlib actually plot things to the
+ # screen by default.
+ "backend : TkAgg",
+ "END",
+ ]),
+ )
+
+ native.py_library(
+ name = "matplotlib" + version,
+ srcs = _src_copied + [
+ version + "/matplotlib/__init__.py",
+ ],
+ data = _data_files + _builtin_so_copied + _system_so_copied + [
+ ":usr/share/matplotlib/mpl-data/matplotlibrc",
+ ] + native.glob(["etc/**"]),
+ imports = ["usr/lib/python" + version + "/dist-packages", version, "."],
+ restricted_to = ["@//tools:k8"],
+ visibility = ["//visibility:public"],
+ )
diff --git a/debian/opencv_python.BUILD b/debian/opencv_python.BUILD
new file mode 100644
index 0000000..5afa180
--- /dev/null
+++ b/debian/opencv_python.BUILD
@@ -0,0 +1,10 @@
+py_library(
+ name = "python_opencv",
+ srcs = glob(["**/*.py"]),
+ data = glob(
+ include = ["**/*"],
+ exclude = ["**/*.py"],
+ ),
+ imports = ["."],
+ visibility = ["//visibility:public"],
+)
diff --git a/debian/packages.bzl b/debian/packages.bzl
index ba1f840..3043452 100644
--- a/debian/packages.bzl
+++ b/debian/packages.bzl
@@ -55,14 +55,14 @@
)
def _convert_deb_to_target(deb):
- """Converts a debian package filename to a valid bazel target name."""
- target = deb
- target = target.replace('-', '_')
- target = target.replace('.', '_')
- target = target.replace(':', '_')
- target = target.replace('+', 'x')
- target = target.replace('~', '_')
- return "deb_%s_repo" % target
+ """Converts a debian package filename to a valid bazel target name."""
+ target = deb
+ target = target.replace("-", "_")
+ target = target.replace(".", "_")
+ target = target.replace(":", "_")
+ target = target.replace("+", "x")
+ target = target.replace("~", "_")
+ return "deb_%s_repo" % target
def generate_repositories_for_debs(files, base_url = "http://www.frc971.org/Build-Dependencies"):
"""A WORKSPACE helper to add all the deb packages in the dictionary as a repo.
diff --git a/frc971/analysis/py_log_reader.cc b/frc971/analysis/py_log_reader.cc
index 6a002df..a9a1db1 100644
--- a/frc971/analysis/py_log_reader.cc
+++ b/frc971/analysis/py_log_reader.cc
@@ -90,6 +90,7 @@
tools->event_loop =
tools->reader->event_loop_factory()->MakeEventLoop("data_fetcher");
tools->event_loop->SkipTimingReport();
+ tools->event_loop->SkipAosLog();
return 0;
}
diff --git a/frc971/codelab/basic_test.cc b/frc971/codelab/basic_test.cc
index ad526b7..fcb0f15 100644
--- a/frc971/codelab/basic_test.cc
+++ b/frc971/codelab/basic_test.cc
@@ -90,6 +90,10 @@
" },\n"
" {\n"
" \"name\": \"/aos\",\n"
+ " \"type\": \"aos.LogMessageFbs\"\n"
+ " },\n"
+ " {\n"
+ " \"name\": \"/aos\",\n"
" \"type\": \"aos.RobotState\"\n"
" },\n"
" {\n"
diff --git a/frc971/control_loops/control_loops.fbs b/frc971/control_loops/control_loops.fbs
index 4f1e888..a95d407 100644
--- a/frc971/control_loops/control_loops.fbs
+++ b/frc971/control_loops/control_loops.fbs
@@ -61,6 +61,14 @@
absolute_encoder:double;
}
+// Represents all of the data for a single encoder.
+// The relative encoder values are relative to where the encoder was at some
+// arbitrary point in time.
+table RelativePosition {
+ // Current position read from the encoder.
+ encoder:double;
+}
+
// The internal state of a zeroing estimator.
table EstimatorState {
// If true, there has been a fatal error for the estimator.
@@ -105,6 +113,14 @@
absolute_position:double;
}
+table RelativeEncoderEstimatorState {
+ // If true, there has been a fatal error for the estimator.
+ error:bool;
+
+ // The estimated position of the joint.
+ position:double;
+}
+
// The internal state of a zeroing estimator.
table IndexEstimatorState {
// If true, there has been a fatal error for the estimator.
diff --git a/frc971/control_loops/position_sensor_sim.cc b/frc971/control_loops/position_sensor_sim.cc
index 612801e..b874b1d 100644
--- a/frc971/control_loops/position_sensor_sim.cc
+++ b/frc971/control_loops/position_sensor_sim.cc
@@ -91,6 +91,10 @@
negedge_value_ = start_position;
}
+void PositionSensorSimulator::InitializeRelativeEncoder() {
+ current_position_ = 0.0;
+}
+
void PositionSensorSimulator::MoveTo(double new_position) {
{
const int lower_start_segment = lower_index_edge_.current_index_segment();
@@ -218,5 +222,14 @@
return builder->Finish();
}
+template <>
+flatbuffers::Offset<RelativePosition>
+PositionSensorSimulator::GetSensorValues<RelativePositionBuilder>(
+ RelativePositionBuilder *builder) {
+ builder->add_encoder(current_position_);
+ return builder->Finish();
+}
+
+
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/position_sensor_sim.h b/frc971/control_loops/position_sensor_sim.h
index 6be6bbc..faa5dd5 100644
--- a/frc971/control_loops/position_sensor_sim.h
+++ b/frc971/control_loops/position_sensor_sim.h
@@ -1,10 +1,8 @@
#ifndef FRC971_CONTROL_LOOPS_POSITION_SENSOR_SIM_H_
#define FRC971_CONTROL_LOOPS_POSITION_SENSOR_SIM_H_
-#include "flatbuffers/flatbuffers.h"
-
#include "aos/testing/random_seed.h"
-
+#include "flatbuffers/flatbuffers.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/gaussian_noise.h"
@@ -50,6 +48,11 @@
double known_index_lower,
double known_index_upper);
+ // Initializes a sensor simulation with a single relative enoder, providing
+ // position information relative to a starting position of 0. For use with
+ // RelativePosition sensor values.
+ void InitializeRelativeEncoder();
+
// Simulate the structure moving to a new position. The new value is measured
// relative to absolute zero. This will update the simulated sensors with new
// readings.
diff --git a/frc971/control_loops/profiled_subsystem.fbs b/frc971/control_loops/profiled_subsystem.fbs
index 46bc9fc..e4b2f25 100644
--- a/frc971/control_loops/profiled_subsystem.fbs
+++ b/frc971/control_loops/profiled_subsystem.fbs
@@ -197,6 +197,42 @@
estimator_state:frc971.AbsoluteEncoderEstimatorState;
}
+table RelativeEncoderProfiledJointStatus {
+ // The state of the subsystem, if applicable. -1 otherwise.
+ // TODO(alex): replace with enum.
+ state:int;
+
+ // If true, we have aborted.
+ estopped:bool;
+
+ // Position of the joint.
+ position:float;
+ // Velocity of the joint in units/second.
+ velocity:float;
+ // Profiled goal position of the joint.
+ goal_position:float;
+ // Profiled goal velocity of the joint in units/second.
+ goal_velocity:float;
+ // Unprofiled goal position from absoulte zero of the joint.
+ unprofiled_goal_position:float;
+ // Unprofiled goal velocity of the joint in units/second.
+ unprofiled_goal_velocity:float;
+
+ // The estimated voltage error.
+ voltage_error:float;
+
+ // The calculated velocity with delta x/delta t
+ calculated_velocity:float;
+
+ // Components of the control loop output
+ position_power:float;
+ velocity_power:float;
+ feedforwards_power:float;
+
+ // State of the estimator.
+ estimator_state:frc971.RelativeEncoderEstimatorState;
+}
+
table StaticZeroingSingleDOFProfiledSubsystemGoal {
unsafe_goal:double;
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
index 1c04c25..d515a07 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
@@ -345,6 +345,10 @@
" },\n"
" {\n"
" \"name\": \"/aos\",\n"
+ " \"type\": \"aos.logging.LogMessageFbs\"\n"
+ " },\n"
+ " {\n"
+ " \"name\": \"/aos\",\n"
" \"type\": \"aos.RobotState\"\n"
" },\n"
" {\n"
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index 1c6bb77..fe1b461 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -6,10 +6,9 @@
#include <cstdint>
#include <vector>
-#include "frc971/control_loops/control_loops_generated.h"
-#include "frc971/constants.h"
-
#include "flatbuffers/flatbuffers.h"
+#include "frc971/constants.h"
+#include "frc971/control_loops/control_loops_generated.h"
// TODO(pschrader): Flag an error if encoder index pulse is not n revolutions
// away from the last one (i.e. got extra counts from noise, etc..)
@@ -29,7 +28,7 @@
using Position = TPosition;
using ZeroingConstants = TZeroingConstants;
using State = TState;
- virtual ~ZeroingEstimator(){}
+ virtual ~ZeroingEstimator() {}
// Returns true if the logic considers the corresponding mechanism to be
// zeroed.
@@ -61,9 +60,10 @@
// Estimates the position with an incremental encoder with an index pulse and a
// potentiometer.
-class PotAndIndexPulseZeroingEstimator : public ZeroingEstimator<PotAndIndexPosition,
- constants::PotAndIndexPulseZeroingConstants,
- EstimatorState> {
+class PotAndIndexPulseZeroingEstimator
+ : public ZeroingEstimator<PotAndIndexPosition,
+ constants::PotAndIndexPulseZeroingConstants,
+ EstimatorState> {
public:
explicit PotAndIndexPulseZeroingEstimator(
const constants::PotAndIndexPulseZeroingConstants &constants);
@@ -148,10 +148,11 @@
// Estimates the position with an incremental encoder and a hall effect sensor.
class HallEffectAndPositionZeroingEstimator
: public ZeroingEstimator<HallEffectAndPosition,
- constants::HallEffectZeroingConstants,
- HallEffectAndPositionEstimatorState> {
+ constants::HallEffectZeroingConstants,
+ HallEffectAndPositionEstimatorState> {
public:
- explicit HallEffectAndPositionZeroingEstimator(const ZeroingConstants &constants);
+ explicit HallEffectAndPositionZeroingEstimator(
+ const ZeroingConstants &constants);
// Update the internal logic with the next sensor values.
void UpdateEstimate(const Position &info) override;
@@ -372,9 +373,10 @@
// Zeros by seeing all the index pulses in the range of motion of the mechanism
// and using that to figure out which index pulse is which.
-class PulseIndexZeroingEstimator : public ZeroingEstimator<IndexPosition,
- constants::EncoderPlusIndexZeroingConstants,
- IndexEstimatorState> {
+class PulseIndexZeroingEstimator
+ : public ZeroingEstimator<IndexPosition,
+ constants::EncoderPlusIndexZeroingConstants,
+ IndexEstimatorState> {
public:
explicit PulseIndexZeroingEstimator(const ZeroingConstants &constants)
: constants_(constants) {
@@ -516,6 +518,47 @@
double position_ = 0.0;
};
+class RelativeEncoderZeroingEstimator
+ : public ZeroingEstimator<RelativePosition, void,
+ RelativeEncoderEstimatorState> {
+ public:
+ explicit RelativeEncoderZeroingEstimator() {}
+
+ // Update position with new position from encoder
+ void UpdateEstimate(const RelativePosition &position) override {
+ position_ = position.encoder();
+ }
+
+ // We alre always zeroed
+ bool zeroed() const override { return true; }
+
+ // Starting position of the joint
+ double offset() const override { return 0; }
+
+ // Has an error occured? Note: Only triggered by TriggerError()
+ bool error() const override { return error_; }
+
+ // Offset is always ready, since we are always zeroed.
+ bool offset_ready() const override { return true; }
+
+ void TriggerError() override { error_ = true; }
+
+ void Reset() override { error_ = false; }
+
+ flatbuffers::Offset<State> GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const override {
+ State::Builder builder(*fbb);
+ builder.add_error(error_);
+ builder.add_position(position_);
+ return builder.Finish();
+ }
+
+ private:
+ // Position from encoder relative to start
+ double position_ = 0;
+ bool error_ = false;
+};
+
} // namespace zeroing
} // namespace frc971
diff --git a/frc971/zeroing/zeroing_test.cc b/frc971/zeroing/zeroing_test.cc
index 61d2d0c..2b715ea 100644
--- a/frc971/zeroing/zeroing_test.cc
+++ b/frc971/zeroing/zeroing_test.cc
@@ -1,14 +1,14 @@
+#include "frc971/zeroing/zeroing.h"
+
#include <unistd.h>
#include <memory>
-
#include <random>
-#include "gtest/gtest.h"
-#include "frc971/control_loops/control_loops_generated.h"
-#include "frc971/zeroing/zeroing.h"
#include "aos/die.h"
+#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/position_sensor_sim.h"
+#include "gtest/gtest.h"
namespace frc971 {
namespace zeroing {
@@ -72,6 +72,14 @@
*simulator->FillSensorValues<HallEffectAndPosition>(&fbb));
}
+ void MoveTo(PositionSensorSimulator *simulator,
+ RelativeEncoderZeroingEstimator *estimator, double new_position) {
+ simulator->MoveTo(new_position);
+ FBB fbb;
+ estimator->UpdateEstimate(
+ *simulator->FillSensorValues<RelativePosition>(&fbb));
+ }
+
template <typename T>
double GetEstimatorPosition(T *estimator) {
FBB fbb;
@@ -789,5 +797,21 @@
ASSERT_TRUE(estimator.error());
}
+TEST_F(ZeroingTest, TestRelativeEncoderZeroingWithoutMovement) {
+ PositionSensorSimulator sim(1.0);
+ RelativeEncoderZeroingEstimator estimator;
+
+ sim.InitializeRelativeEncoder();
+
+ ASSERT_TRUE(estimator.zeroed());
+ ASSERT_TRUE(estimator.offset_ready());
+ EXPECT_DOUBLE_EQ(estimator.offset(), 0.0);
+ EXPECT_DOUBLE_EQ(GetEstimatorPosition(&estimator), 0.0);
+
+ MoveTo(&sim, &estimator, 0.1);
+
+ EXPECT_DOUBLE_EQ(GetEstimatorPosition(&estimator), 0.1);
+}
+
} // namespace zeroing
} // namespace frc971
diff --git a/third_party/flatbuffers/include/flatbuffers/flatbuffers.h b/third_party/flatbuffers/include/flatbuffers/flatbuffers.h
index 3d83cc4..5fd610b 100644
--- a/third_party/flatbuffers/include/flatbuffers/flatbuffers.h
+++ b/third_party/flatbuffers/include/flatbuffers/flatbuffers.h
@@ -1839,6 +1839,33 @@
return CreateVectorOfSortedTables(data(*v), v->size());
}
+ /// @brief Specialized version of `StartVector` for non-copying use cases
+ /// where the size isn't known beforehand.
+ /// Write the data any time later to the returned buffer pointer `buf`.
+ /// Make sure to call `EndIndeterminateVector` later with the final size.
+ /// @param[in] max_len The maximum number of elements to store in the
+ /// `vector`.
+ /// @param[in] elemsize The size of each element in the `vector`.
+ // @param[in] alignment The minimal alignment for the start of the data.
+ /// @param[out] buf A pointer to a `uint8_t` pointer that can be
+ /// written to at a later time to serialize the data into a `vector`
+ /// in the buffer.
+ void StartIndeterminateVector(size_t max_len, size_t elemsize,
+ size_t alignment, uint8_t **buf) {
+ NotNested();
+ nested = true;
+ PreAlign(max_len * elemsize, alignment);
+ buf_.ensure_space(max_len * elemsize);
+ auto vec_start = GetSize() + max_len * elemsize;
+ *buf = buf_.data_at(vec_start);
+ }
+
+ uoffset_t EndIndeterminateVector(size_t len, size_t elemsize) {
+ buf_.make_space(len * elemsize);
+ auto vec_end = EndVector(len);
+ return vec_end;
+ }
+
/// @brief Specialized version of `CreateVector` for non-copying use cases.
/// Write the data any time later to the returned buffer pointer `buf`.
/// @param[in] len The number of elements to store in the `vector`.
diff --git a/y2017/control_loops/superstructure/vision_time_adjuster_test.cc b/y2017/control_loops/superstructure/vision_time_adjuster_test.cc
index 701f8ed..d7a5fb4 100644
--- a/y2017/control_loops/superstructure/vision_time_adjuster_test.cc
+++ b/y2017/control_loops/superstructure/vision_time_adjuster_test.cc
@@ -1,12 +1,11 @@
#include "y2017/control_loops/superstructure/vision_time_adjuster.h"
-#include "gtest/gtest.h"
-
#include "aos/configuration.h"
#include "aos/events/simulated_event_loop.h"
#include "aos/testing/test_logging.h"
#include "aos/time/time.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "gtest/gtest.h"
#include "y2017/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2017/vision/vision_generated.h"
@@ -20,7 +19,8 @@
: ::testing::Test(),
configuration_(aos::configuration::ReadConfig("y2017/config.json")),
event_loop_factory_(&configuration_.message()),
- simulation_event_loop_(event_loop_factory_.MakeEventLoop("drivetrain")),
+ simulation_event_loop_(
+ event_loop_factory_.MakeEventLoop("drivetrain")),
drivetrain_status_sender_(
simulation_event_loop_
->MakeSender<::frc971::control_loops::drivetrain::Status>(
diff --git a/y2019/control_loops/drivetrain/drivetrain_replay.cc b/y2019/control_loops/drivetrain/drivetrain_replay.cc
index cb07b68..4f40d75 100644
--- a/y2019/control_loops/drivetrain/drivetrain_replay.cc
+++ b/y2019/control_loops/drivetrain/drivetrain_replay.cc
@@ -41,6 +41,7 @@
std::unique_ptr<aos::EventLoop> log_writer_event_loop =
reader.event_loop_factory()->MakeEventLoop("log_writer");
log_writer_event_loop->SkipTimingReport();
+ log_writer_event_loop->SkipAosLog();
CHECK(nullptr == log_writer_event_loop->node());
aos::logger::Logger writer(&file_writer, log_writer_event_loop.get());
diff --git a/y2019/control_loops/drivetrain/target_selector_test.cc b/y2019/control_loops/drivetrain/target_selector_test.cc
index 43a0456..9bb7ef8 100644
--- a/y2019/control_loops/drivetrain/target_selector_test.cc
+++ b/y2019/control_loops/drivetrain/target_selector_test.cc
@@ -39,7 +39,8 @@
TargetSelectorParamTest()
: configuration_(aos::configuration::ReadConfig("y2019/config.json")),
event_loop_factory_(&configuration_.message()),
- event_loop_(this->event_loop_factory_.MakeEventLoop("drivetrain")),
+ event_loop_(
+ this->event_loop_factory_.MakeEventLoop("drivetrain")),
test_event_loop_(this->event_loop_factory_.MakeEventLoop("test")),
target_selector_hint_sender_(
test_event_loop_->MakeSender<
diff --git a/y2020/BUILD b/y2020/BUILD
index 53ccb8e..1d281bb 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -33,6 +33,7 @@
"//frc971/control_loops:pose",
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//y2020/control_loops/drivetrain:polydrivetrain_plants",
+ "//y2020/control_loops/superstructure/hood:hood_plants",
"@com_google_absl//absl/base",
],
)
@@ -108,6 +109,7 @@
"//y2020/control_loops/superstructure:superstructure_position_fbs",
"//y2019/control_loops/drivetrain:target_selector_fbs",
"//y2020/control_loops/superstructure:superstructure_status_fbs",
+ "//y2020/vision:vision_fbs",
],
visibility = ["//visibility:public"],
deps = [
diff --git a/y2020/constants.cc b/y2020/constants.cc
index 70876c3..a16b2f1 100644
--- a/y2020/constants.cc
+++ b/y2020/constants.cc
@@ -13,6 +13,8 @@
#include "aos/mutex/mutex.h"
#include "aos/network/team_number.h"
+#include "y2020/control_loops/superstructure/hood/integral_hood_plant.h"
+
namespace y2020 {
namespace constants {
@@ -26,6 +28,25 @@
const Values *DoGetValuesForTeam(uint16_t team) {
Values *const r = new Values();
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::AbsoluteEncoderZeroingEstimator> *const hood =
+ &r->hood;
+
+ // Hood constants.
+ hood->zeroing_voltage = 3.0;
+ hood->operating_voltage = 12.0;
+ hood->zeroing_profile_params = {0.5, 3.0};
+ hood->default_profile_params = {6.0, 30.0};
+ hood->range = Values::kHoodRange();
+ hood->make_integral_loop =
+ control_loops::superstructure::hood::MakeIntegralHoodLoop;
+ hood->zeroing_constants.average_filter_size = Values::kZeroingSampleSize;
+ hood->zeroing_constants.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kHoodEncoderRatio();
+ hood->zeroing_constants.zeroing_threshold = 0.0005;
+ hood->zeroing_constants.moving_buffer_size = 20;
+ hood->zeroing_constants.allowable_encoder_error = 0.9;
+ hood->zeroing_constants.middle_position = Values::kHoodRange().middle();
switch (team) {
// A set of constants for tests.
@@ -33,12 +54,15 @@
break;
case kCompTeamNumber:
+ hood->zeroing_constants.measured_absolute_position = 0.0;
break;
case kPracticeTeamNumber:
+ hood->zeroing_constants.measured_absolute_position = 0.0;
break;
case kCodingRobotTeamNumber:
+ hood->zeroing_constants.measured_absolute_position = 0.0;
break;
default:
diff --git a/y2020/constants.h b/y2020/constants.h
index 2e915f9..47b8543 100644
--- a/y2020/constants.h
+++ b/y2020/constants.h
@@ -10,6 +10,7 @@
#include "frc971/control_loops/pose.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "y2020/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2020/control_loops/superstructure/hood/hood_plant.h"
namespace y2020 {
namespace constants {
@@ -28,6 +29,32 @@
constants::Values::kDrivetrainEncoderRatio() *
kDrivetrainEncoderCountsPerRevolution();
}
+
+ // Hood
+ static constexpr double kHoodEncoderCountsPerRevolution() { return 4096.0; }
+
+ //TODO(sabina): Update constants
+ static constexpr double kHoodEncoderRatio() { return 1.0; }
+
+ static constexpr double kMaxHoodEncoderPulsesPerSecond() {
+ return control_loops::superstructure::hood::kFreeSpeed *
+ control_loops::superstructure::hood::kOutputRatio /
+ kHoodEncoderRatio() / (2.0 * M_PI) *
+ kHoodEncoderCountsPerRevolution();
+ }
+
+ static constexpr ::frc971::constants::Range kHoodRange() {
+ return ::frc971::constants::Range{
+ 0.00, // Back Hard
+ 0.79, // Front Hard
+ 0.14, // Back Soft
+ 0.78 // Front Soft
+ };
+ }
+
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::AbsoluteEncoderZeroingEstimator>
+ hood;
};
// Creates (once) a Values instance for ::aos::network::GetTeamNumber() and
diff --git a/y2020/control_loops/python/BUILD b/y2020/control_loops/python/BUILD
index edc72f5..b7f393b 100644
--- a/y2020/control_loops/python/BUILD
+++ b/y2020/control_loops/python/BUILD
@@ -49,6 +49,22 @@
],
)
+py_binary(
+ name = "hood",
+ srcs = [
+ "hood.py",
+ ],
+ legacy_create_init = False,
+ restricted_to = ["//tools:k8"],
+ deps = [
+ ":python_init",
+ "//external:python-gflags",
+ "//external:python-glog",
+ "//frc971/control_loops/python:angular_system",
+ "//frc971/control_loops/python:controls",
+ ],
+)
+
py_library(
name = "python_init",
srcs = ["__init__.py"],
diff --git a/y2020/control_loops/python/hood.py b/y2020/control_loops/python/hood.py
new file mode 100644
index 0000000..8c99940
--- /dev/null
+++ b/y2020/control_loops/python/hood.py
@@ -0,0 +1,60 @@
+#!/usr/bin/python
+
+from aos.util.trapezoid_profile import TrapezoidProfile
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import angular_system
+from frc971.control_loops.python import controls
+import numpy
+import sys
+from matplotlib import pylab
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+try:
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+ pass
+
+'''
+Hood is an angular subsystem due to the mounting of the encoder on the hood joint.
+We are currently electing to ignore potential non-linearity.
+'''
+
+#TODO: update constants
+kHood = angular_system.AngularSystemParams(
+ name='Hood',
+ motor=control_loop.BAG(),
+ # meters / rad (used the displacement of the lead screw and the angle)
+ G=(0.1601 / 0.6457),
+ J=0.3,
+ q_pos=0.20,
+ q_vel=5.0,
+ kalman_q_pos=0.12,
+ kalman_q_vel=2.0,
+ kalman_q_voltage=4.0,
+ kalman_r_position=0.05)
+
+
+def main(argv):
+ if FLAGS.plot:
+ R = numpy.matrix([[numpy.pi / 2.0], [0.0]])
+ angular_system.PlotKick(kHood, R)
+ angular_system.PlotMotion(kHood, R)
+
+ # Write the generated constants out to a file.
+ if len(argv) != 5:
+ glog.fatal(
+ 'Expected .h file name and .cc file name for the hood and integral hood.'
+ )
+ else:
+ namespaces = ['y2020', 'control_loops', 'superstructure', 'hood']
+ angular_system.WriteAngularSystem(kHood, argv[1:3], argv[3:5],
+ namespaces)
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2020/control_loops/superstructure/hood/BUILD b/y2020/control_loops/superstructure/hood/BUILD
new file mode 100644
index 0000000..6c1a60b
--- /dev/null
+++ b/y2020/control_loops/superstructure/hood/BUILD
@@ -0,0 +1,32 @@
+package(default_visibility = ["//y2020:__subpackages__"])
+
+genrule(
+ name = "genrule_hood",
+ outs = [
+ "hood_plant.h",
+ "hood_plant.cc",
+ "integral_hood_plant.h",
+ "integral_hood_plant.cc",
+ ],
+ cmd = "$(location //y2020/control_loops/python:hood) $(OUTS)",
+ tools = [
+ "//y2020/control_loops/python:hood",
+ ],
+)
+
+cc_library(
+ name = "hood_plants",
+ srcs = [
+ "hood_plant.cc",
+ "integral_hood_plant.cc",
+ ],
+ hdrs = [
+ "hood_plant.h",
+ "integral_hood_plant.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:hybrid_state_feedback_loop",
+ "//frc971/control_loops:state_feedback_loop",
+ ],
+)
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index a3978a4..91a7586 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -12,32 +12,50 @@
Superstructure::Superstructure(::aos::EventLoop *event_loop,
const ::std::string &name)
: aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
- name) {
- event_loop->SetRuntimeRealtimePriority(30);
+ name),
+ hood_(constants::GetValues().hood) {
+ event_loop->SetRuntimeRealtimePriority(30);
}
-void Superstructure::RunIteration(const Goal * /*unsafe_goal*/,
- const Position * /*position*/,
+void Superstructure::RunIteration(const Goal *unsafe_goal,
+ const Position *position,
aos::Sender<Output>::Builder *output,
aos::Sender<Status>::Builder *status) {
if (WasReset()) {
AOS_LOG(ERROR, "WPILib reset, restarting\n");
+ hood_.Reset();
}
+ OutputT output_struct;
+
+ flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus> hood_status_offset =
+ hood_.Iterate(unsafe_goal != nullptr ? unsafe_goal->hood() : nullptr,
+ position->hood(),
+ output != nullptr ? &(output_struct.hood_voltage) : nullptr,
+ status->fbb());
+
+ bool zeroed;
+ bool estopped;
+
+ const AbsoluteEncoderProfiledJointStatus *hood_status =
+ GetMutableTemporaryPointer(*status->fbb(), hood_status_offset);
+ zeroed = hood_status->zeroed();
+ estopped = hood_status->estopped();
if (output != nullptr) {
- OutputT output_struct;
output->Send(Output::Pack(*output->fbb(), &output_struct));
}
Status::Builder status_builder = status->MakeBuilder<Status>();
- status_builder.add_zeroed(true);
- status_builder.add_estopped(false);
+ status_builder.add_zeroed(zeroed);
+ status_builder.add_estopped(estopped);
+
+ status_builder.add_hood(hood_status_offset);
status->Send(status_builder.Finish());
}
+} // namespace superstructure
} // namespace control_loops
} // namespace y2020
-} // namespace y2020
diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
index 9aaca8e..dbc1ccd 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -19,12 +19,21 @@
explicit Superstructure(::aos::EventLoop *event_loop,
const ::std::string &name = "/superstructure");
+ using AbsoluteEncoderSubsystem =
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+ ::frc971::zeroing::AbsoluteEncoderZeroingEstimator,
+ ::frc971::control_loops::AbsoluteEncoderProfiledJointStatus>;
+
+ const AbsoluteEncoderSubsystem &hood() const { return hood_; }
+
protected:
virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
aos::Sender<Output>::Builder *output,
aos::Sender<Status>::Builder *status) override;
private:
+ AbsoluteEncoderSubsystem hood_;
+
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2020/control_loops/superstructure/superstructure_goal.fbs b/y2020/control_loops/superstructure/superstructure_goal.fbs
index 8a38b95..5953590 100644
--- a/y2020/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2020/control_loops/superstructure/superstructure_goal.fbs
@@ -3,7 +3,8 @@
namespace y2020.control_loops.superstructure;
table Goal {
-
+ // Zero is at the horizontal, positive towards the front (meters on the lead screw).
+ hood:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal;
}
root_type Goal;
diff --git a/y2020/control_loops/superstructure/superstructure_output.fbs b/y2020/control_loops/superstructure/superstructure_output.fbs
index 3682db6..e245712 100644
--- a/y2020/control_loops/superstructure/superstructure_output.fbs
+++ b/y2020/control_loops/superstructure/superstructure_output.fbs
@@ -1,7 +1,8 @@
namespace y2020.control_loops.superstructure;
table Output {
-
+ // Votage sent to the hood. Positive moves up.
+ hood_voltage:double;
}
root_type Output;
diff --git a/y2020/control_loops/superstructure/superstructure_position.fbs b/y2020/control_loops/superstructure/superstructure_position.fbs
index c4b0a9a..50b9c36 100644
--- a/y2020/control_loops/superstructure/superstructure_position.fbs
+++ b/y2020/control_loops/superstructure/superstructure_position.fbs
@@ -4,6 +4,8 @@
table Position {
+ // Zero is at the horizontal, positive towards the front (meters on the lead screw).
+ hood:frc971.AbsolutePosition;
}
root_type Position;
diff --git a/y2020/control_loops/superstructure/superstructure_status.fbs b/y2020/control_loops/superstructure/superstructure_status.fbs
index d0e9266..8c29e23 100644
--- a/y2020/control_loops/superstructure/superstructure_status.fbs
+++ b/y2020/control_loops/superstructure/superstructure_status.fbs
@@ -10,6 +10,8 @@
// If true, we have aborted. This is the or of all subsystem estops.
estopped:bool;
+ //Subsystem status.
+ hood:frc971.control_loops.AbsoluteEncoderProfiledJointStatus;
}
root_type Status;
diff --git a/y2020/vision/BUILD b/y2020/vision/BUILD
index 021a466..2e3723d 100644
--- a/y2020/vision/BUILD
+++ b/y2020/vision/BUILD
@@ -1,7 +1,38 @@
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos:config.bzl", "aos_config")
flatbuffer_cc_library(
name = "vision_fbs",
srcs = ["vision.fbs"],
gen_reflections = 1,
+ visibility = ["//y2020:__subpackages__"],
+)
+
+cc_library(
+ name = "v4l2_reader",
+ srcs = [
+ "v4l2_reader.cc",
+ ],
+ hdrs = [
+ "v4l2_reader.h",
+ ],
+ deps = [
+ ":vision_fbs",
+ "//aos/events:event_loop",
+ "//aos/scoped:scoped_fd",
+ "@com_github_google_glog//:glog",
+ "@com_google_absl//absl/base",
+ ],
+)
+
+cc_binary(
+ name = "camera_reader",
+ srcs = [
+ "camera_reader.cc",
+ ],
+ deps = [
+ ":v4l2_reader",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ ],
)
diff --git a/y2020/vision/camera_reader.cc b/y2020/vision/camera_reader.cc
new file mode 100644
index 0000000..e5bcb64
--- /dev/null
+++ b/y2020/vision/camera_reader.cc
@@ -0,0 +1,42 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+
+#include "y2020/vision/v4l2_reader.h"
+
+namespace frc971 {
+namespace vision {
+namespace {
+
+void CameraReaderMain() {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ aos::ShmEventLoop event_loop(&config.message());
+ V4L2Reader v4l2_reader(&event_loop, "/dev/video0");
+
+ while (true) {
+ const auto image = v4l2_reader.ReadLatestImage();
+ if (image.empty()) {
+ LOG(INFO) << "No image, sleeping";
+ std::this_thread::sleep_for(std::chrono::milliseconds(10));
+ continue;
+ }
+
+ // Now, process image.
+ // TODO(Brian): Actually process it, rather than just logging its size...
+ LOG(INFO) << image.size();
+ std::this_thread::sleep_for(std::chrono::milliseconds(70));
+
+ v4l2_reader.SendLatestImage();
+ }
+}
+
+} // namespace
+} // namespace vision
+} // namespace frc971
+
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ frc971::vision::CameraReaderMain();
+}
diff --git a/y2020/vision/v4l2_reader.cc b/y2020/vision/v4l2_reader.cc
new file mode 100644
index 0000000..727f8ba
--- /dev/null
+++ b/y2020/vision/v4l2_reader.cc
@@ -0,0 +1,134 @@
+#include "y2020/vision/v4l2_reader.h"
+
+#include <fcntl.h>
+#include <linux/videodev2.h>
+#include <sys/ioctl.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+
+namespace frc971 {
+namespace vision {
+
+V4L2Reader::V4L2Reader(aos::EventLoop *event_loop,
+ const std::string &device_name)
+ : fd_(open(device_name.c_str(), O_RDWR | O_NONBLOCK)) {
+ PCHECK(fd_.get() != -1);
+
+ // First, clean up after anybody else who left the device streaming.
+ {
+ int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ PCHECK(Ioctl(VIDIOC_STREAMOFF, &type) == 0);
+ }
+
+ {
+ struct v4l2_format format;
+ memset(&format, 0, sizeof(format));
+ format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ format.fmt.pix.width = cols_;
+ format.fmt.pix.height = rows_;
+ format.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
+ // This means we want to capture from a progressive (non-interlaced) source.
+ format.fmt.pix.field = V4L2_FIELD_NONE;
+ PCHECK(Ioctl(VIDIOC_S_FMT, &format) == 0);
+ CHECK_EQ(static_cast<int>(format.fmt.pix.width), cols_);
+ CHECK_EQ(static_cast<int>(format.fmt.pix.height), rows_);
+ CHECK_EQ(static_cast<int>(format.fmt.pix.bytesperline),
+ cols_ * 2 /* bytes per pixel */);
+ CHECK_EQ(format.fmt.pix.sizeimage, ImageSize());
+ }
+
+ {
+ struct v4l2_requestbuffers request;
+ memset(&request, 0, sizeof(request));
+ request.count = buffers_.size();
+ request.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ request.memory = V4L2_MEMORY_USERPTR;
+ PCHECK(Ioctl(VIDIOC_REQBUFS, &request) == 0);
+ CHECK_EQ(request.count, buffers_.size())
+ << ": Kernel refused to give us the number of buffers we asked for";
+ }
+
+ for (size_t i = 0; i < buffers_.size(); ++i) {
+ buffers_[i].sender = event_loop->MakeSender<CameraImage>("/camera");
+ EnqueueBuffer(i);
+ }
+
+ {
+ int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ PCHECK(Ioctl(VIDIOC_STREAMON, &type) == 0);
+ }
+}
+
+absl::Span<const char> V4L2Reader::ReadLatestImage() {
+ // First, enqueue any old buffer we already have. This is the one which may
+ // have been sent.
+ if (saved_buffer_ != -1) {
+ EnqueueBuffer(saved_buffer_);
+ saved_buffer_ = -1;
+ }
+ while (true) {
+ const int previous_buffer = saved_buffer_;
+ saved_buffer_ = DequeueBuffer();
+ if (saved_buffer_ != -1) {
+ // We got a new buffer. Return the previous one (if relevant) and keep
+ // going.
+ if (previous_buffer != -1) {
+ EnqueueBuffer(previous_buffer);
+ }
+ continue;
+ }
+ if (previous_buffer == -1) {
+ // There were no images to read. Return an indication of that.
+ return absl::Span<const char>();
+ }
+ // We didn't get a new one, but we already got one in a previous
+ // iteration, which means we found an image so return it.
+ saved_buffer_ = previous_buffer;
+ return buffers_[saved_buffer_].DataSpan(ImageSize());
+ }
+}
+
+void V4L2Reader::SendLatestImage() {
+ buffers_[saved_buffer_].Send(rows_, cols_, ImageSize());
+}
+
+int V4L2Reader::Ioctl(unsigned long number, void *arg) {
+ return ioctl(fd_.get(), number, arg);
+}
+
+int V4L2Reader::DequeueBuffer() {
+ struct v4l2_buffer buffer;
+ memset(&buffer, 0, sizeof(buffer));
+ buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buffer.memory = V4L2_MEMORY_USERPTR;
+ const int result = Ioctl(VIDIOC_DQBUF, &buffer);
+ if (result == -1 && errno == EAGAIN) {
+ return -1;
+ }
+ PCHECK(result == 0) << ": VIDIOC_DQBUF failed";
+ CHECK_LT(buffer.index, buffers_.size());
+ LOG(INFO) << "dequeued " << buffer.index;
+ CHECK_EQ(reinterpret_cast<uintptr_t>(buffers_[buffer.index].data_pointer),
+ buffer.m.userptr);
+ CHECK_EQ(ImageSize(), buffer.length);
+ return buffer.index;
+}
+
+void V4L2Reader::EnqueueBuffer(int buffer_number) {
+ LOG(INFO) << "enqueueing " << buffer_number;
+ CHECK_GE(buffer_number, 0);
+ CHECK_LT(buffer_number, static_cast<int>(buffers_.size()));
+ buffers_[buffer_number].InitializeMessage(ImageSize());
+ struct v4l2_buffer buffer;
+ memset(&buffer, 0, sizeof(buffer));
+ buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buffer.memory = V4L2_MEMORY_USERPTR;
+ buffer.index = buffer_number;
+ buffer.m.userptr =
+ reinterpret_cast<uintptr_t>(buffers_[buffer_number].data_pointer);
+ buffer.length = ImageSize();
+ PCHECK(Ioctl(VIDIOC_QBUF, &buffer) == 0);
+}
+
+} // namespace vision
+} // namespace frc971
diff --git a/y2020/vision/v4l2_reader.h b/y2020/vision/v4l2_reader.h
new file mode 100644
index 0000000..969f4a8
--- /dev/null
+++ b/y2020/vision/v4l2_reader.h
@@ -0,0 +1,107 @@
+#ifndef Y2020_VISION_V4L2_READER_H_
+#define Y2020_VISION_V4L2_READER_H_
+
+#include <array>
+#include <string>
+
+#include "absl/types/span.h"
+#include "glog/logging.h"
+
+#include "aos/events/event_loop.h"
+#include "aos/scoped/scoped_fd.h"
+#include "y2020/vision/vision_generated.h"
+
+namespace frc971 {
+namespace vision {
+
+// Reads images from a V4L2 capture device (aka camera).
+class V4L2Reader {
+ public:
+ // device_name is the name of the device file (like "/dev/video0").
+ V4L2Reader(aos::EventLoop *event_loop, const std::string &device_name);
+
+ V4L2Reader(const V4L2Reader &) = delete;
+ V4L2Reader &operator=(const V4L2Reader &) = delete;
+
+ // Reads the latest image.
+ //
+ // Returns an empty span if no image was available since this object was
+ // created. The data referenced in the return value is valid until this method
+ // is called again.
+ absl::Span<const char> ReadLatestImage();
+
+ // Sends the latest image.
+ //
+ // ReadLatestImage() must have returned a non-empty span the last time it was
+ // called. After calling this, the data which was returned from
+ // ReadLatestImage() will no longer be valid.
+ void SendLatestImage();
+
+ private:
+ static constexpr int kNumberBuffers = 16;
+
+ struct Buffer {
+ void InitializeMessage(size_t max_image_size) {
+ builder = aos::Sender<CameraImage>::Builder();
+ builder = sender.MakeBuilder();
+ // The kernel has an undocumented requirement that the buffer is aligned
+ // to 64 bytes. If you give it a nonaligned pointer, it will return EINVAL
+ // and only print something in dmesg with the relevant dynamic debug
+ // prints turned on.
+ builder.fbb()->StartIndeterminateVector(max_image_size, 1, 64,
+ &data_pointer);
+ CHECK_EQ(reinterpret_cast<uintptr_t>(data_pointer) % 64, 0u)
+ << ": Flatbuffers failed to align things as requested";
+ }
+
+ void Send(int rows, int cols, size_t image_size) {
+ const auto data_offset =
+ builder.fbb()->EndIndeterminateVector(image_size, 1);
+ auto image_builder = builder.MakeBuilder<CameraImage>();
+ image_builder.add_data(data_offset);
+ image_builder.add_rows(rows);
+ image_builder.add_cols(cols);
+ builder.Send(image_builder.Finish());
+ data_pointer = nullptr;
+ }
+
+ absl::Span<const char> DataSpan(size_t image_size) {
+ return absl::Span<const char>(reinterpret_cast<char *>(data_pointer),
+ image_size);
+ }
+
+ aos::Sender<CameraImage> sender;
+ aos::Sender<CameraImage>::Builder builder;
+
+ uint8_t *data_pointer = nullptr;
+ };
+
+ // TODO(Brian): This concept won't exist once we start using variable-size
+ // H.264 frames.
+ size_t ImageSize() const { return rows_ * cols_ * 2 /* bytes per pixel */; }
+
+ // Attempts to dequeue a buffer (nonblocking). Returns the index of the new
+ // buffer, or -1 if there wasn't a frame to dequeue.
+ int DequeueBuffer();
+
+ void EnqueueBuffer(int buffer);
+
+ int Ioctl(unsigned long number, void *arg);
+
+ // The mmaped V4L2 buffers.
+ std::array<Buffer, kNumberBuffers> buffers_;
+
+ // If this is non-negative, it's the buffer number we're currently holding
+ // onto.
+ int saved_buffer_ = -1;
+
+ const int rows_ = 480;
+ const int cols_ = 640;
+
+ aos::ScopedFD fd_;
+};
+
+} // namespace vision
+} // namespace frc971
+
+#endif // Y2020_VISION_V4L2_READER_H_
diff --git a/y2020/y2020.json b/y2020/y2020.json
index 7680ef5..238d73f 100644
--- a/y2020/y2020.json
+++ b/y2020/y2020.json
@@ -24,11 +24,21 @@
"name": "/superstructure",
"type": "y2020.control_loops.superstructure.Position",
"frequency": 200
+ },
+ {
+ "name": "/camera",
+ "type": "frc971.vision.CameraImage",
+ "frequency": 25,
+ "max_size": 620000,
+ "num_senders": 18
}
],
"applications": [
{
"name": "drivetrain"
+ },
+ {
+ "name": "camera_reader"
}
],
"imports": [