Removed Common
Change-Id: I01ea8f07220375c2ad9bc0092281d4f27c642303
diff --git a/aos/util/BUILD b/aos/util/BUILD
new file mode 100644
index 0000000..70f23c4
--- /dev/null
+++ b/aos/util/BUILD
@@ -0,0 +1,287 @@
+package(default_visibility = ["//visibility:public"])
+
+py_library(
+ name = "py_trapezoid_profile",
+ srcs = [
+ "trapezoid_profile.py",
+ ],
+ deps = [
+ ":python_init",
+ ],
+)
+
+cc_library(
+ name = "run_command",
+ srcs = [
+ "run_command.cc",
+ ],
+ hdrs = [
+ "run_command.h",
+ ],
+ deps = [
+ "//aos/logging",
+ ],
+)
+
+cc_test(
+ name = "run_command_test",
+ srcs = [
+ "run_command_test.cc",
+ ],
+ deps = [
+ ":run_command",
+ ":thread",
+ "//aos/logging",
+ "//aos/testing:googletest",
+ ],
+)
+
+cc_library(
+ name = "death_test_log_implementation",
+ hdrs = [
+ "death_test_log_implementation.h",
+ ],
+ deps = [
+ "//aos/logging:implementations",
+ ],
+)
+
+cc_library(
+ name = "inet_addr",
+ srcs = [
+ "inet_addr.cc",
+ ],
+ hdrs = [
+ "inet_addr.h",
+ ],
+ deps = [
+ "//aos:byteorder",
+ "//aos:network_port",
+ ],
+)
+
+cc_library(
+ name = "phased_loop",
+ srcs = [
+ "phased_loop.cc",
+ ],
+ hdrs = [
+ "phased_loop.h",
+ ],
+ deps = [
+ "//aos/time:time",
+ "//aos/logging",
+ ],
+)
+
+cc_library(
+ name = "log_interval",
+ hdrs = [
+ "log_interval.h",
+ ],
+ deps = [
+ "//aos/time:time",
+ "//aos/logging",
+ ],
+)
+
+cc_library(
+ name = "string_to_num",
+ hdrs = [
+ "string_to_num.h",
+ ],
+ compatible_with = [
+ "//tools:armhf-debian",
+ ],
+)
+
+cc_test(
+ name = "string_to_num_test",
+ srcs = [
+ "string_to_num_test.cc",
+ ],
+ deps = [
+ ":string_to_num",
+ "//aos/testing:googletest",
+ ],
+)
+
+cc_library(
+ name = "thread",
+ srcs = [
+ "thread.cc",
+ ],
+ hdrs = [
+ "thread.h",
+ ],
+ deps = [
+ "//aos:macros",
+ "//aos/logging",
+ ],
+)
+
+cc_library(
+ name = "trapezoid_profile",
+ srcs = [
+ "trapezoid_profile.cc",
+ ],
+ hdrs = [
+ "trapezoid_profile.h",
+ ],
+ linkopts = [
+ "-lm",
+ ],
+ deps = [
+ "//aos/time:time",
+ "//aos/logging",
+ "//third_party/eigen",
+ ],
+)
+
+cc_test(
+ name = "trapezoid_profile_test",
+ srcs = [
+ "trapezoid_profile_test.cc",
+ ],
+ deps = [
+ ":trapezoid_profile",
+ "//aos/testing:googletest",
+ ],
+)
+
+cc_library(
+ name = "wrapping_counter",
+ srcs = [
+ "wrapping_counter.cc",
+ ],
+ hdrs = [
+ "wrapping_counter.h",
+ ],
+)
+
+cc_test(
+ name = "wrapping_counter_test",
+ srcs = [
+ "wrapping_counter_test.cc",
+ ],
+ deps = [
+ ":wrapping_counter",
+ "//aos/testing:googletest",
+ ],
+)
+
+cc_library(
+ name = "options",
+ hdrs = [
+ "options.h",
+ ],
+ compatible_with = [
+ "//tools:armhf-debian",
+ ],
+)
+
+cc_test(
+ name = "options_test",
+ srcs = [
+ "options_test.cc",
+ ],
+ deps = [
+ ":options",
+ "//aos/testing:googletest",
+ ],
+)
+
+cc_library(
+ name = "compiler_memory_barrier",
+ hdrs = [
+ "compiler_memory_barrier.h",
+ ],
+ compatible_with = [
+ "//tools:armhf-debian",
+ ],
+)
+
+cc_library(
+ name = "global_factory",
+ hdrs = [
+ "global_factory.h",
+ ],
+)
+
+cc_test(
+ name = "global_factory_test",
+ srcs = [
+ "global_factory_test.cc",
+ ],
+ deps = [
+ ":global_factory",
+ "//aos/testing:googletest",
+ ],
+)
+
+cc_library(
+ name = "linked_list",
+ hdrs = [
+ "linked_list.h",
+ ],
+ deps = [
+ "//aos/transaction:transaction",
+ ],
+)
+
+cc_test(
+ name = "linked_list_test",
+ srcs = [
+ "linked_list_test.cc",
+ ],
+ deps = [
+ ":linked_list",
+ "//aos/logging",
+ "//aos/testing:googletest",
+ ],
+)
+
+cc_test(
+ name = "phased_loop_test",
+ srcs = [
+ "phased_loop_test.cc",
+ ],
+ deps = [
+ ":phased_loop",
+ "//aos/testing:googletest",
+ "//aos/testing:test_logging",
+ ],
+)
+
+cc_library(
+ name = "file",
+ srcs = [
+ "file.cc",
+ ],
+ hdrs = [
+ "file.h",
+ ],
+ deps = [
+ "//aos/scoped:scoped_fd",
+ ],
+)
+
+cc_test(
+ name = "file_test",
+ size = "small",
+ srcs = [
+ "file_test.cc",
+ ],
+ deps = [
+ ":file",
+ "//aos/testing:googletest",
+ "//aos/testing:test_logging",
+ ],
+)
+
+py_library(
+ name = "python_init",
+ srcs = ["__init__.py"],
+ visibility = ["//visibility:public"],
+ deps = ["//aos:python_init"],
+)
diff --git a/aos/util/__init__.py b/aos/util/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/aos/util/__init__.py
diff --git a/aos/util/compiler_memory_barrier.h b/aos/util/compiler_memory_barrier.h
new file mode 100644
index 0000000..33da511
--- /dev/null
+++ b/aos/util/compiler_memory_barrier.h
@@ -0,0 +1,11 @@
+#ifndef AOS_UTIL_COMPILER_MEMORY_BARRIER_H_
+#define AOS_UTIL_COMPILER_MEMORY_BARRIER_H_
+
+// Prevents the compiler from reordering memory operations around this.
+// Using this function makes it clearer what you're doing and easier to be
+// portable.
+static inline void aos_compiler_memory_barrier(void) {
+ __asm__ __volatile__("" ::: "memory");
+}
+
+#endif // AOS_UTIL_COMPILER_MEMORY_BARRIER_H_
diff --git a/aos/util/death_test_log_implementation.h b/aos/util/death_test_log_implementation.h
new file mode 100644
index 0000000..f061c61
--- /dev/null
+++ b/aos/util/death_test_log_implementation.h
@@ -0,0 +1,27 @@
+#ifndef AOS_UTIL_DEATH_TEST_LOG_IMPLEMENTATION_H_
+#define AOS_UTIL_DEATH_TEST_LOG_IMPLEMENTATION_H_
+
+#include <stdlib.h>
+
+#include "aos/logging/implementations.h"
+
+namespace aos {
+namespace util {
+
+// Prints all FATAL messages to stderr and then abort(3)s before the regular
+// stuff can print out anything else. Ignores all other messages.
+// This is useful in death tests that expect a LOG(FATAL) to cause the death.
+class DeathTestLogImplementation : public logging::HandleMessageLogImplementation {
+ public:
+ virtual void HandleMessage(const logging::LogMessage &message) override {
+ if (message.level == FATAL) {
+ logging::internal::PrintMessage(stderr, message);
+ abort();
+ }
+ }
+};
+
+} // namespace util
+} // namespace aos
+
+#endif // AOS_UTIL_DEATH_TEST_LOG_IMPLEMENTATION_H_
diff --git a/aos/util/file.cc b/aos/util/file.cc
new file mode 100644
index 0000000..dc31ddd
--- /dev/null
+++ b/aos/util/file.cc
@@ -0,0 +1,28 @@
+#include "aos/util/file.h"
+
+#include <fcntl.h>
+#include <unistd.h>
+
+#include "aos/scoped/scoped_fd.h"
+
+namespace aos {
+namespace util {
+
+::std::string ReadFileToStringOrDie(const ::std::string &filename) {
+ ::std::string r;
+ ScopedFD fd(PCHECK(open(filename.c_str(), O_RDONLY)));
+ while (true) {
+ char buffer[1024];
+ const ssize_t result = read(fd.get(), buffer, sizeof(buffer));
+ if (result < 0) {
+ PLOG(FATAL, "reading from %s", filename.c_str());
+ } else if (result == 0) {
+ break;
+ }
+ r.append(buffer, result);
+ }
+ return r;
+}
+
+} // namespace util
+} // namespace aos
diff --git a/aos/util/file.h b/aos/util/file.h
new file mode 100644
index 0000000..0c1f11f
--- /dev/null
+++ b/aos/util/file.h
@@ -0,0 +1,16 @@
+#ifndef AOS_UTIL_FILE_H_
+#define AOS_UTIL_FILE_H_
+
+#include <string>
+
+namespace aos {
+namespace util {
+
+// Returns the complete contents of filename. LOG(FATAL)s if any errors are
+// encountered.
+::std::string ReadFileToStringOrDie(const ::std::string &filename);
+
+} // namespace util
+} // namespace aos
+
+#endif // AOS_UTIL_FILE_H_
diff --git a/aos/util/file_test.cc b/aos/util/file_test.cc
new file mode 100644
index 0000000..f904259
--- /dev/null
+++ b/aos/util/file_test.cc
@@ -0,0 +1,40 @@
+#include "aos/util/file.h"
+
+#include <stdlib.h>
+
+#include <string>
+
+#include "gtest/gtest.h"
+
+#include "aos/testing/test_logging.h"
+
+namespace aos {
+namespace util {
+namespace testing {
+
+class FileTest : public ::testing::Test {
+ protected:
+ FileTest() {
+ ::aos::testing::EnableTestLogging();
+ }
+};
+
+// Basic test of reading a normal file.
+TEST_F(FileTest, ReadNormalFile) {
+ const ::std::string tmpdir(getenv("TEST_TMPDIR"));
+ const ::std::string test_file = tmpdir + "/test_file";
+ ASSERT_EQ(0, system(("echo contents > " + test_file).c_str()));
+ EXPECT_EQ("contents\n", ReadFileToStringOrDie(test_file));
+}
+
+// Tests reading a file with 0 size, among other weird things.
+TEST_F(FileTest, ReadSpecialFile) {
+ const ::std::string stat = ReadFileToStringOrDie("/proc/self/stat");
+ EXPECT_EQ('\n', stat[stat.size() - 1]);
+ const ::std::string my_pid = ::std::to_string(getpid());
+ EXPECT_EQ(my_pid, stat.substr(0, my_pid.size()));
+}
+
+} // namespace testing
+} // namespace util
+} // namespace aos
diff --git a/aos/util/global_factory.h b/aos/util/global_factory.h
new file mode 100644
index 0000000..5e47972
--- /dev/null
+++ b/aos/util/global_factory.h
@@ -0,0 +1,93 @@
+#ifndef AOS_UTIL_GLOBAL_FACTORY_H_
+#define AOS_UTIL_GLOBAL_FACTORY_H_
+
+#include <functional>
+#include <memory>
+#include <string>
+#include <unordered_map>
+
+// // File Usage Description:
+// class ExampleBaseClass { virtual ~ExampleBaseClass(); }
+// class ExampleSubClass : public ExampleBaseClass {}
+//
+// // At namespace scope in header file:
+// SETUP_FACTORY(ExampleBaseClass);
+// // At namespace scope in cc file:
+// REGISTER_SUBCLASS("ExampleSubClass", ExampleBaseClass, ExampleSubClass);
+//
+// // When you want an object of type "ExampleSubClass".
+// std::unique_ptr<ExampleBaseClass> constructed_item =
+// ExampleBaseClassGlobalFactory::Get("ExampleSubClass")();
+
+// Helper macro to set up a Factory Family for a particular type.
+// Put this is the header file along-side the base class.
+#define SETUP_FACTORY(BaseClass, ...) \
+ using BaseClass##GlobalFactory = \
+ ::aos::GlobalFactory<BaseClass, ##__VA_ARGS__>
+
+// Helper macro to set up a Factory for a subtype. For BaseClass
+// This should happen in a .cc file not a header file to avoid multiple
+// linkage.
+#define REGISTER_SUBCLASS_BY_KEY(key, BaseClass, SubClass) \
+ BaseClass##GlobalFactory::SubClassRegisterer<SubClass> \
+ register_for_##SubClass(key)
+
+// Proxy to above but where SubClass name is the key.
+#define REGISTER_SUBCLASS(BaseClass, SubClass) \
+ REGISTER_SUBCLASS_BY_KEY(#SubClass, BaseClass, SubClass)
+
+namespace aos {
+
+// Maintains a static std::unordered_map<std::string, FactoryFunction> for
+// BaseClass. Best to use with macros above.
+template <typename BaseClass, typename... FactoryArgs>
+class GlobalFactory {
+ public:
+ using FactoryFunction =
+ std::function<std::unique_ptr<BaseClass>(FactoryArgs &&...)>;
+
+ // Gets the factory function by named. This will return a null factory
+ // std::function if the factory is not available, so one would be wise
+ // to check this function before use.
+ // It is an error to call this during static initialization.
+ static const FactoryFunction &Get(const std::string &name) {
+ const auto &map = *GetMap();
+ auto item = map.find(name);
+ if (item == map.end()) {
+ static FactoryFunction null_create_fn;
+ return null_create_fn;
+ }
+ return item->second;
+ }
+
+ // Installs a factory function for constructing SubClass
+ // using name "name". It is an error not call this at namespace scope
+ // through the REGISTER_SUBCLASS macro above.
+ template <typename SubClass>
+ class SubClassRegisterer {
+ public:
+ explicit SubClassRegisterer(const char *name) {
+ (*GetMap())[name] = [](FactoryArgs &&... args) {
+ return std::unique_ptr<BaseClass>(
+ new SubClass(std::forward<FactoryArgs>(args)...));
+ };
+ }
+ };
+
+ // Fetch all factory functions.
+ static const std::unordered_map<std::string, FactoryFunction> &GetAll() {
+ return *GetMap();
+ }
+
+ private:
+ // Actual map. (Protected by static from concurrent construction
+ // if there is nothing registered at static linkage time).
+ static std::unordered_map<std::string, FactoryFunction> *GetMap() {
+ static std::unordered_map<std::string, FactoryFunction> map;
+ return ↦
+ }
+};
+
+} // namespace aos
+
+#endif // AOS_UTIL_GLOBAL_FACTORY_H_
diff --git a/aos/util/global_factory_test.cc b/aos/util/global_factory_test.cc
new file mode 100644
index 0000000..d50e931
--- /dev/null
+++ b/aos/util/global_factory_test.cc
@@ -0,0 +1,67 @@
+#include "aos/util/global_factory.h"
+#include "gtest/gtest.h"
+
+namespace aos {
+
+namespace test_a {
+class BaseType {
+ public:
+ virtual ~BaseType() {}
+
+ virtual std::pair<int, int> Get() = 0;
+};
+
+SETUP_FACTORY(BaseType, int, int);
+
+class BaseTypeNoArgs {
+ public:
+ virtual ~BaseTypeNoArgs() {}
+
+ virtual int Get() = 0;
+};
+
+SETUP_FACTORY(BaseTypeNoArgs);
+
+} // namespace test_a
+
+namespace test_b {
+
+class SubType : public test_a::BaseType {
+ public:
+ SubType(int t1, int t2) : value_(t1, t2) {}
+ std::pair<int, int> Get() override { return value_; }
+
+ private:
+ std::pair<int, int> value_;
+};
+
+REGISTER_SUBCLASS(test_a::BaseType, SubType);
+
+} // namespace test_b
+
+namespace {
+
+class SubType1 : public test_a::BaseTypeNoArgs {
+ public:
+ int Get() override { return 1; }
+};
+
+class SubType2 : public test_a::BaseTypeNoArgs {
+ public:
+ int Get() override { return 2; }
+};
+REGISTER_SUBCLASS(test_a::BaseTypeNoArgs, SubType1);
+REGISTER_SUBCLASS(test_a::BaseTypeNoArgs, SubType2);
+
+TEST(GlobalFactoryTest, CheckFactory) {
+ auto val = test_a::BaseTypeGlobalFactory::Get("SubType")(2, 7)->Get();
+ EXPECT_EQ(val.first, 2);
+ EXPECT_EQ(val.second, 7);
+}
+TEST(GlobalFactoryTest, CheckFactoryNoArgs) {
+ EXPECT_EQ(1, test_a::BaseTypeNoArgsGlobalFactory::Get("SubType1")()->Get());
+ EXPECT_EQ(2, test_a::BaseTypeNoArgsGlobalFactory::Get("SubType2")()->Get());
+}
+
+} // namespace
+} // namespace aos
diff --git a/aos/util/inet_addr.cc b/aos/util/inet_addr.cc
new file mode 100644
index 0000000..edb7dbb
--- /dev/null
+++ b/aos/util/inet_addr.cc
@@ -0,0 +1,41 @@
+#include "aos/util/inet_addr.h"
+
+#include <stdlib.h>
+#ifndef __VXWORKS__
+#include <string.h>
+
+#include "aos/byteorder.h"
+#else
+
+template<typename T>
+T hton(T);
+
+template<uint32_t>
+uint32_t hton(uint32_t v) { return v; }
+
+#endif
+
+namespace aos {
+namespace util {
+
+const char *MakeIPAddress(const in_addr &base_address,
+ ::aos::NetworkAddress last_segment) {
+ in_addr address = base_address;
+ SetLastSegment(&address, last_segment);
+
+#ifdef __VXWORKS__
+ char *r = static_cast<char *>(malloc(INET_ADDR_LEN));
+ inet_ntoa_b(address, r);
+ return r;
+#else
+ return strdup(inet_ntoa(address));
+#endif
+}
+
+void SetLastSegment(in_addr *address, ::aos::NetworkAddress last_segment) {
+ address->s_addr &= ~(hton<uint32_t>(0xFF));
+ address->s_addr |= hton<uint32_t>(static_cast<uint8_t>(last_segment));
+}
+
+} // namespace util
+} // namespace aos
diff --git a/aos/util/inet_addr.h b/aos/util/inet_addr.h
new file mode 100644
index 0000000..dc2d613
--- /dev/null
+++ b/aos/util/inet_addr.h
@@ -0,0 +1,29 @@
+#ifndef AOS_UTIL_INET_ADDR_H_
+#define AOS_UTIL_INET_ADDR_H_
+
+#ifdef __VXWORKS__
+#include <inetLib.h>
+#else
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#endif
+
+#include "aos/network_port.h"
+
+namespace aos {
+namespace util {
+
+// Makes an IP address string from base_address with the last byte set to
+// last_segment.
+// Returns a malloc(3)ed string.
+const char *MakeIPAddress(const in_addr &base_address,
+ ::aos::NetworkAddress last_segment);
+
+// Sets the last byte of *address to last_segment.
+void SetLastSegment(in_addr *address, ::aos::NetworkAddress last_segment);
+
+} // namespace util
+} // namespace aos
+
+#endif // AOS_UTIL_INET_ADDR_H_
diff --git a/aos/util/linked_list.h b/aos/util/linked_list.h
new file mode 100644
index 0000000..5a5824e
--- /dev/null
+++ b/aos/util/linked_list.h
@@ -0,0 +1,113 @@
+#ifndef AOS_UTIL_LINKED_LIST_H_
+#define AOS_UTIL_LINKED_LIST_H_
+
+#include <functional>
+
+#include "aos/transaction/transaction.h"
+
+namespace aos {
+namespace util {
+
+// Handles manipulating an intrusive linked list. T must look like the
+// following:
+// struct T {
+// ...
+// T *next;
+// ...
+// };
+// This class doesn't deal with creating or destroying them, so
+// constructors/destructors/other members variables/member functions are all
+// fine, but the next pointer must be there for this class to work.
+// This class will handle all manipulations of next. It does not need to be
+// initialized before calling Add and should not be changed afterwards.
+// next can (and probably should) be private if the appropriate instantiation of
+// this class is friended.
+template <class T>
+class LinkedList {
+ public:
+ T *head() const { return head_; }
+
+ bool Empty() const { return head() == nullptr; }
+
+ void Add(T *t) {
+ Add<0>(t, nullptr);
+ }
+
+ // restore_points (if non-null) will be used so the operation can be safely
+ // reverted at any point.
+ template <int number_works>
+ void Add(T *t, transaction::WorkStack<transaction::RestorePointerWork,
+ number_works> *restore_pointers) {
+ if (restore_pointers != nullptr) restore_pointers->AddWork(&t->next);
+ t->next = head();
+ if (restore_pointers != nullptr) restore_pointers->AddWork(&head_);
+ head_ = t;
+ }
+
+ void Remove(T *t) {
+ Remove<0>(t, nullptr);
+ }
+
+ // restore_points (if non-null) will be used so the operation can be safely
+ // reverted at any point.
+ template <int number_works>
+ void Remove(T *t, transaction::WorkStack<transaction::RestorePointerWork,
+ number_works> *restore_pointers) {
+ T **pointer = &head_;
+ while (*pointer != nullptr) {
+ if (*pointer == t) {
+ if (restore_pointers != nullptr) {
+ restore_pointers->AddWork(pointer);
+ }
+ *pointer = t->next;
+ return;
+ }
+ pointer = &(*pointer)->next;
+ }
+ LOG(FATAL, "%p is not in the list\n", t);
+ }
+
+ // Calls function for each element of the list.
+ // function can modify these elements in any way except touching the next
+ // pointer (including by calling other methods of this object).
+ void Each(::std::function<void(T *)> function) const {
+ T *c = head();
+ while (c != nullptr) {
+ T *const next = c->next;
+ function(c);
+ c = next;
+ }
+ }
+
+ // Returns the first element of the list where function returns true or
+ // nullptr if it returns false for all.
+ T *Find(::std::function<bool(const T *)> function) const {
+ T *c = head();
+ while (c != nullptr) {
+ if (function(c)) return c;
+ c = c->next;
+ }
+ return nullptr;
+ }
+
+ private:
+ T *head_ = nullptr;
+};
+
+// Keeps track of something along with a next pointer. Useful for things that
+// either have types without next pointers or for storing pointers to things
+// that belong in multiple lists.
+template <class V>
+struct LinkedListReference {
+ V item;
+
+ private:
+ friend class LinkedList<LinkedListReference>;
+
+ LinkedListReference *next;
+};
+
+} // namespace util
+} // namespace aos
+
+#endif // AOS_UTIL_LINKED_LIST_H_
diff --git a/aos/util/linked_list_test.cc b/aos/util/linked_list_test.cc
new file mode 100644
index 0000000..6959628
--- /dev/null
+++ b/aos/util/linked_list_test.cc
@@ -0,0 +1,119 @@
+#include "aos/util/linked_list.h"
+
+#include <vector>
+
+#include "gtest/gtest.h"
+
+namespace aos {
+namespace util {
+namespace testing {
+
+class LinkedListTest : public ::testing::Test {
+ public:
+ virtual ~LinkedListTest() {
+ while (list.head() != nullptr) {
+ RemoveElement(list.head());
+ }
+ }
+
+ struct Member {
+ Member(int i) : i(i) {}
+
+ int i;
+ Member *next = nullptr;
+ };
+ LinkedList<Member> list;
+
+ Member *AddElement(int i) {
+ Member *member = new Member(i);
+ list.Add(member);
+ return member;
+ }
+
+ void RemoveElement(Member *member) {
+ list.Remove(member);
+ delete member;
+ }
+
+ Member *GetMember(int i) const {
+ return list.Find([i](const Member *member) { return member->i == i; });
+ }
+
+ bool HasMember(int i) const { return GetMember(i) != nullptr; }
+
+ ::std::vector<int> GetMembers() {
+ ::std::vector<int> r;
+ list.Each([&r](Member *member) { r.push_back(member->i); });
+ return r;
+ }
+};
+
+// Tests that adding and removing elements works correctly.
+TEST_F(LinkedListTest, Basic) {
+ EXPECT_TRUE(list.Empty());
+ AddElement(971);
+ EXPECT_FALSE(list.Empty());
+ AddElement(254);
+ EXPECT_FALSE(list.Empty());
+ AddElement(1678);
+ EXPECT_FALSE(list.Empty());
+
+ EXPECT_EQ((::std::vector<int>{1678, 254, 971}), GetMembers());
+
+ EXPECT_EQ(1678, list.head()->i);
+ RemoveElement(list.head());
+ EXPECT_EQ(254, list.head()->i);
+ EXPECT_FALSE(list.Empty());
+ RemoveElement(list.head());
+ EXPECT_EQ(971, list.head()->i);
+ EXPECT_FALSE(list.Empty());
+ RemoveElement(list.head());
+ EXPECT_TRUE(list.Empty());
+}
+
+TEST_F(LinkedListTest, Each) {
+ ::std::vector<int> found;
+ auto add_to_found = [&found](Member *member) {
+ found.push_back(member->i);
+ };
+
+ AddElement(971);
+ found.clear();
+ list.Each(add_to_found);
+ EXPECT_EQ((::std::vector<int>{971}), found);
+
+ AddElement(254);
+ found.clear();
+ list.Each(add_to_found);
+ EXPECT_EQ((::std::vector<int>{254, 971}), found);
+
+ AddElement(1678);
+ found.clear();
+ list.Each(add_to_found);
+ EXPECT_EQ((::std::vector<int>{1678, 254, 971}), found);
+}
+
+TEST_F(LinkedListTest, Find) {
+ auto find_254 = [](const Member *member) { return member->i == 254; };
+
+ AddElement(971);
+ EXPECT_EQ(nullptr, list.Find(find_254));
+ Member *member = AddElement(254);
+ EXPECT_EQ(member, list.Find(find_254));
+ AddElement(1678);
+ EXPECT_EQ(member, list.Find(find_254));
+}
+
+// Removing an element from the middle of the list used to break it.
+TEST_F(LinkedListTest, RemoveFromMiddle) {
+ AddElement(971);
+ auto in_middle = AddElement(254);
+ AddElement(1678);
+ RemoveElement(in_middle);
+
+ EXPECT_EQ((::std::vector<int>{1678, 971}), GetMembers());
+}
+
+} // namespace testing
+} // namespace util
+} // namespace aos
diff --git a/aos/util/log_interval.h b/aos/util/log_interval.h
new file mode 100644
index 0000000..5e76166
--- /dev/null
+++ b/aos/util/log_interval.h
@@ -0,0 +1,97 @@
+#ifndef AOS_UTIL_LOG_INTERVAL_H_
+#define AOS_UTIL_LOG_INTERVAL_H_
+
+#include "aos/time/time.h"
+#include "aos/logging/logging.h"
+
+#include <string>
+
+namespace aos {
+namespace util {
+
+// A class to help with logging things that happen a lot only occasionally.
+//
+// Intended use {
+// static LogInterval interval(::std::chrono::millseconds(200));
+//
+// if (WantToLog()) {
+// interval.WantToLog();
+// }
+// if (interval.ShouldLog()) {
+// LOG(DEBUG, "thingie happened! (%d times)\n", interval.Count());
+// }
+// }
+class LogInterval {
+ public:
+ constexpr LogInterval(::std::chrono::nanoseconds interval)
+ : interval_(interval) {}
+
+ void WantToLog() {
+ if (count_ == 0) {
+ last_done_ = ::aos::monotonic_clock::now();
+ }
+ ++count_;
+ }
+ bool ShouldLog() {
+ const ::aos::monotonic_clock::time_point now =
+ ::aos::monotonic_clock::now();
+ const bool r = now >= interval_ + last_done_ && count_ > 0;
+ if (r) {
+ last_done_ = now;
+ }
+ return r;
+ }
+ int Count() {
+ const int r = count_;
+ count_ = 0;
+ return r;
+ }
+
+ ::std::chrono::nanoseconds interval() const { return interval_; }
+
+ private:
+ int count_ = 0;
+ const ::std::chrono::nanoseconds interval_;
+ ::aos::monotonic_clock::time_point last_done_ =
+ ::aos::monotonic_clock::min_time;
+};
+
+// This one is even easier to use. It always logs with a message "%s %d
+// times\n". Call LOG_INTERVAL wherever it should log and make sure Print gets
+// called often (ie not after a conditional return)
+class SimpleLogInterval {
+ public:
+ SimpleLogInterval(::std::chrono::nanoseconds interval, log_level level,
+ const ::std::string &message)
+ : interval_(interval), level_(level), message_(message) {}
+
+#define LOG_INTERVAL(simple_log) \
+ simple_log.WantToLog(LOG_SOURCENAME ": " STRINGIFY(__LINE__))
+ void WantToLog(const char *context) {
+ context_ = context;
+ interval_.WantToLog();
+ }
+
+ void Print() {
+ if (interval_.ShouldLog()) {
+ CHECK_NOTNULL(context_);
+ log_do(level_, "%s: %.*s %d times over %f sec\n", context_,
+ static_cast<int>(message_.size()), message_.data(),
+ interval_.Count(),
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+ interval_.interval()).count());
+ context_ = NULL;
+ }
+ }
+
+ private:
+ LogInterval interval_;
+ const log_level level_;
+ const ::std::string message_;
+ const char *context_ = NULL;
+};
+
+} // namespace util
+} // namespace aos
+
+#endif // AOS_UTIL_LOG_INTERVAL_H_
diff --git a/aos/util/options.h b/aos/util/options.h
new file mode 100644
index 0000000..39f7e8f
--- /dev/null
+++ b/aos/util/options.h
@@ -0,0 +1,92 @@
+#ifndef AOS_UTIL_OPTIONS_H_
+#define AOS_UTIL_OPTIONS_H_
+
+#include <sys/types.h>
+
+namespace aos {
+
+template <class Owner>
+class Options;
+
+// An "option" that can be combined with other options and passed as one
+// argument. This class is designed to emulate integral constants (except be
+// type-safe), so its instances can be combined with operator| (creating an
+// Options), and an Options can be tested for membership with operator&.
+// Owner is the only class that can construct Option instances and is used to
+// differentiate between options for different classes. It is safe to only have
+// a forward declaration for Owner in scope when instantiating either of these
+// template classes.
+template <class Owner>
+class Options {
+ public:
+ // Represents a single options. Instances of this should be created as
+ // constants by Owner.
+ class Option {
+ public:
+ constexpr Options operator|(Option option) const {
+ return Options(bit_ | option.bit_);
+ }
+
+ constexpr bool operator==(Option other) const { return bit_ == other.bit_; }
+
+ constexpr unsigned int printable() const { return bit_; }
+
+ private:
+ // Bit must have exactly 1 bit set and that must be a different one for each
+ // instance.
+ explicit constexpr Option(unsigned int bit) : bit_(bit) {}
+
+ unsigned int bit_;
+
+ friend class Options;
+ friend Owner;
+ };
+
+ constexpr Options(Option option) : bits_(option.bit_) {}
+
+ constexpr bool operator&(Option option) const {
+ return (bits_ & option.bit_) != 0;
+ }
+
+ constexpr Options operator|(Option option) const {
+ return Options(bits_ | option.bit_);
+ }
+ constexpr Options operator|(Options options) const {
+ return Options(bits_ | options.bits_);
+ }
+
+ constexpr bool operator==(Options other) const {
+ return bits_ == other.bits_;
+ }
+
+ constexpr unsigned int printable() const { return bits_; }
+
+ // Returns true if no Options other than the ones in options are set.
+ // Useful for validating that no illegal options are passed.
+ constexpr bool NoOthersSet(Options options) const {
+ return (bits_ & ~options.bits_) == 0;
+ }
+
+ // Returns true if exactly 1 of the Options in options is set.
+ // Useful for validating that one of a group of mutually exclusive options has
+ // been passed.
+ constexpr bool ExactlyOneSet(Options options) const {
+ return __builtin_popcount(bits_ & options.bits_) == 1;
+ }
+
+ // Returns true if all of the Options in options are set.
+ constexpr bool AllSet(Options options) const {
+ return (bits_ & options.bits_) == options.bits_;
+ }
+
+ private:
+ explicit constexpr Options(unsigned int bits) : bits_(bits) {}
+
+ unsigned int bits_;
+
+ friend class Option;
+};
+
+} // namespace options
+
+#endif // AOS_UTIL_OPTIONS_H_
diff --git a/aos/util/options_test.cc b/aos/util/options_test.cc
new file mode 100644
index 0000000..ee228cb
--- /dev/null
+++ b/aos/util/options_test.cc
@@ -0,0 +1,56 @@
+#include "aos/util/options.h"
+
+#include "gtest/gtest.h"
+
+namespace aos {
+namespace testing {
+
+class OptionsTest : public ::testing::Test {
+ public:
+ static constexpr Options<OptionsTest>::Option kOne{1}, kTwo{2}, kThree{4},
+ kFour{8};
+};
+
+constexpr Options<OptionsTest>::Option OptionsTest::kOne, OptionsTest::kTwo,
+ OptionsTest::kThree, OptionsTest::kFour;
+
+TEST_F(OptionsTest, Basic) {
+ const Options<OptionsTest> one_three = kOne | kThree;
+ EXPECT_TRUE(one_three & kOne);
+ EXPECT_FALSE(one_three & kTwo);
+ EXPECT_TRUE(one_three & kThree);
+}
+
+TEST_F(OptionsTest, NoOthersSet) {
+ const Options<OptionsTest> one_three = kOne | kThree;
+ EXPECT_TRUE(one_three.NoOthersSet(one_three));
+ EXPECT_TRUE(one_three.NoOthersSet(kOne | kTwo | kThree));
+ EXPECT_TRUE(one_three.NoOthersSet(kOne | kThree | kFour));
+ EXPECT_TRUE(one_three.NoOthersSet(kOne | kTwo | kThree | kFour));
+ EXPECT_FALSE(one_three.NoOthersSet(kOne));
+ EXPECT_FALSE(one_three.NoOthersSet(kThree));
+ EXPECT_FALSE(one_three.NoOthersSet(kTwo | kFour));
+}
+
+TEST_F(OptionsTest, ExactlyOneSet) {
+ const Options<OptionsTest> one_three = kOne | kThree;
+ EXPECT_TRUE(one_three.ExactlyOneSet(kOne | kTwo));
+ EXPECT_FALSE(one_three.ExactlyOneSet(one_three));
+ EXPECT_TRUE(one_three.ExactlyOneSet(kTwo | kThree | kFour));
+ EXPECT_FALSE(one_three.ExactlyOneSet(kOne | kTwo | kThree | kFour));
+}
+
+TEST_F(OptionsTest, AllSet) {
+ const Options<OptionsTest> one_three = kOne | kThree;
+ EXPECT_TRUE(one_three.AllSet(one_three));
+ EXPECT_TRUE(one_three.AllSet(kOne));
+ EXPECT_FALSE(one_three.AllSet(kTwo));
+ EXPECT_TRUE(one_three.AllSet(kThree));
+ EXPECT_FALSE(one_three.AllSet(kFour));
+ EXPECT_FALSE(one_three.AllSet(kOne | kTwo | kFour));
+ EXPECT_FALSE(one_three.AllSet(kTwo | kThree | kFour));
+ EXPECT_FALSE(one_three.AllSet(kOne | kTwo | kThree | kFour));
+}
+
+} // namespace testing
+} // namespace aos
diff --git a/aos/util/phased_loop.cc b/aos/util/phased_loop.cc
new file mode 100644
index 0000000..a349718
--- /dev/null
+++ b/aos/util/phased_loop.cc
@@ -0,0 +1,27 @@
+#include "aos/util/phased_loop.h"
+
+namespace aos {
+namespace time {
+
+int PhasedLoop::Iterate(const monotonic_clock::time_point now) {
+ const monotonic_clock::time_point next_time =
+ monotonic_clock::time_point(
+ (((now - offset_).time_since_epoch() + monotonic_clock::duration(1)) /
+ interval_) *
+ interval_) +
+ ((now.time_since_epoch() < offset_) ? monotonic_clock::zero()
+ : interval_) +
+ offset_;
+
+ const monotonic_clock::duration difference = next_time - last_time_;
+ const int result = difference / interval_;
+ CHECK_EQ(
+ 0, (next_time - offset_).time_since_epoch().count() % interval_.count());
+ CHECK_GE(next_time, now);
+ CHECK_LE(next_time - now, interval_);
+ last_time_ = next_time;
+ return result;
+}
+
+} // namespace timing
+} // namespace aos
diff --git a/aos/util/phased_loop.h b/aos/util/phased_loop.h
new file mode 100644
index 0000000..1c1aef1
--- /dev/null
+++ b/aos/util/phased_loop.h
@@ -0,0 +1,88 @@
+#ifndef AOS_UTIL_PHASED_LOOP_H_
+#define AOS_UTIL_PHASED_LOOP_H_
+
+#include "aos/time/time.h"
+
+#include "aos/logging/logging.h"
+
+namespace aos {
+namespace time {
+
+// Handles sleeping until a fixed offset from some time interval.
+class PhasedLoop {
+ public:
+ // For example, with interval = 1s and offset = 0.1s this will fire at:
+ // 0.1s
+ // 1.1s
+ // ...
+ // 10000.1s
+ // offset must be >= chrono::seconds(0) and < interval.
+ PhasedLoop(
+ const monotonic_clock::duration interval,
+ const monotonic_clock::duration offset = monotonic_clock::duration(0))
+ : interval_(interval), offset_(offset), last_time_(offset) {
+ CHECK_GE(offset, monotonic_clock::duration(0));
+ CHECK_GT(interval, monotonic_clock::duration(0));
+ CHECK_LT(offset, interval);
+ Reset();
+ }
+
+ // Updates the offset and interval.
+ void set_interval_and_offset(const monotonic_clock::duration interval,
+ const monotonic_clock::duration offset) {
+ interval_ = interval;
+ offset_ = offset;
+ CHECK_GE(offset_, monotonic_clock::duration(0));
+ CHECK_GT(interval_, monotonic_clock::duration(0));
+ CHECK_LT(offset_, interval_);
+ }
+
+ // Computes the offset given an interval and a time that we should trigger.
+ static monotonic_clock::duration OffsetFromIntervalAndTime(
+ const monotonic_clock::duration interval,
+ const monotonic_clock::time_point monotonic_trigger) {
+ CHECK_GT(interval, monotonic_clock::duration(0));
+ return monotonic_trigger.time_since_epoch() -
+ (monotonic_trigger.time_since_epoch() / interval) * interval +
+ ((monotonic_trigger.time_since_epoch() >= monotonic_clock::zero())
+ ? monotonic_clock::zero()
+ : interval);
+ }
+
+ // Resets the count of skipped iterations.
+ // Iterate(monotonic_now) will return 1 and set sleep_time() to something
+ // within interval of monotonic_now.
+ void Reset(const monotonic_clock::time_point monotonic_now =
+ monotonic_clock::now()) {
+ Iterate(monotonic_now - interval_);
+ }
+
+ // Calculates the next time to run after monotonic_now.
+ // The result can be retrieved with sleep_time().
+ // Returns the number of iterations which have passed (1 if this is called
+ // often enough). This can be < 1 iff monotonic_now goes backwards between
+ // calls.
+ int Iterate(const monotonic_clock::time_point monotonic_now =
+ monotonic_clock::now());
+
+ // Sleeps until the next time and returns the number of iterations which have
+ // passed.
+ int SleepUntilNext() {
+ const int r = Iterate(monotonic_clock::now());
+ ::std::this_thread::sleep_until(sleep_time());
+ return r;
+ }
+
+ monotonic_clock::time_point sleep_time() const { return last_time_; }
+
+ private:
+ monotonic_clock::duration interval_, offset_;
+
+ // The time we most recently slept until.
+ monotonic_clock::time_point last_time_ = monotonic_clock::epoch();
+};
+
+} // namespace time
+} // namespace aos
+
+#endif // AOS_UTIL_PHASED_LOOP_H_
diff --git a/aos/util/phased_loop_test.cc b/aos/util/phased_loop_test.cc
new file mode 100644
index 0000000..2a52b90
--- /dev/null
+++ b/aos/util/phased_loop_test.cc
@@ -0,0 +1,190 @@
+#include "aos/util/phased_loop.h"
+
+#include "gtest/gtest.h"
+
+#include "aos/testing/test_logging.h"
+
+namespace aos {
+namespace time {
+namespace testing {
+
+using ::std::chrono::milliseconds;
+
+class PhasedLoopTest : public ::testing::Test {
+ protected:
+ PhasedLoopTest() { ::aos::testing::EnableTestLogging(); }
+};
+
+typedef PhasedLoopTest PhasedLoopDeathTest;
+
+monotonic_clock::time_point InMs(int ms) {
+ return monotonic_clock::time_point(::std::chrono::milliseconds(ms));
+}
+
+TEST_F(PhasedLoopTest, Reset) {
+ {
+ PhasedLoop loop(milliseconds(100), milliseconds(0));
+
+ loop.Reset(monotonic_clock::epoch());
+ EXPECT_EQ(InMs(0), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(monotonic_clock::epoch()));
+ EXPECT_EQ(InMs(100), loop.sleep_time());
+
+ loop.Reset(InMs(99));
+ EXPECT_EQ(InMs(0), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(InMs(99)));
+ EXPECT_EQ(InMs(100), loop.sleep_time());
+
+ loop.Reset(InMs(100));
+ EXPECT_EQ(InMs(100), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(InMs(199)));
+ EXPECT_EQ(InMs(200), loop.sleep_time());
+
+ loop.Reset(InMs(101));
+ EXPECT_EQ(InMs(100), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(InMs(101)));
+ EXPECT_EQ(InMs(200), loop.sleep_time());
+ }
+ {
+ PhasedLoop loop(milliseconds(100), milliseconds(1));
+ loop.Reset(monotonic_clock::epoch());
+ EXPECT_EQ(InMs(-99), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(monotonic_clock::epoch()));
+ EXPECT_EQ(InMs(1), loop.sleep_time());
+ }
+ {
+ PhasedLoop loop(milliseconds(100), milliseconds(99));
+
+ loop.Reset(monotonic_clock::epoch());
+ EXPECT_EQ(InMs(-1), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(monotonic_clock::epoch()));
+ EXPECT_EQ(InMs(99), loop.sleep_time());
+
+ loop.Reset(InMs(98));
+ EXPECT_EQ(InMs(-1), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(InMs(98)));
+ EXPECT_EQ(InMs(99), loop.sleep_time());
+
+ loop.Reset(InMs(99));
+ EXPECT_EQ(InMs(99), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(InMs(99)));
+ EXPECT_EQ(InMs(199), loop.sleep_time());
+
+ loop.Reset(InMs(100));
+ EXPECT_EQ(InMs(99), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(InMs(100)));
+ EXPECT_EQ(InMs(199), loop.sleep_time());
+ }
+}
+
+TEST_F(PhasedLoopTest, Iterate) {
+ {
+ PhasedLoop loop(milliseconds(100), milliseconds(99));
+ loop.Reset(monotonic_clock::epoch());
+ EXPECT_EQ(1, loop.Iterate(monotonic_clock::epoch()));
+ EXPECT_EQ(InMs(99), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(InMs(100)));
+ EXPECT_EQ(InMs(199), loop.sleep_time());
+ EXPECT_EQ(0, loop.Iterate(InMs(100)));
+ EXPECT_EQ(InMs(199), loop.sleep_time());
+ EXPECT_EQ(0, loop.Iterate(InMs(101)));
+ EXPECT_EQ(InMs(199), loop.sleep_time());
+ EXPECT_EQ(0, loop.Iterate(InMs(198)));
+ EXPECT_EQ(InMs(199), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(InMs(199)));
+ EXPECT_EQ(InMs(299), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(InMs(300)));
+ EXPECT_EQ(InMs(399), loop.sleep_time());
+ EXPECT_EQ(3, loop.Iterate(InMs(600)));
+ EXPECT_EQ(InMs(699), loop.sleep_time());
+ }
+ {
+ PhasedLoop loop(milliseconds(100), milliseconds(1));
+ loop.Reset(monotonic_clock::epoch());
+ EXPECT_EQ(1, loop.Iterate(monotonic_clock::epoch()));
+ EXPECT_EQ(InMs(1), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(InMs(100)));
+ EXPECT_EQ(InMs(101), loop.sleep_time());
+ EXPECT_EQ(0, loop.Iterate(InMs(100)));
+ EXPECT_EQ(InMs(101), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(InMs(103)));
+ EXPECT_EQ(InMs(201), loop.sleep_time());
+ EXPECT_EQ(0, loop.Iterate(InMs(198)));
+ EXPECT_EQ(InMs(201), loop.sleep_time());
+ EXPECT_EQ(0, loop.Iterate(InMs(200)));
+ EXPECT_EQ(InMs(201), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(InMs(201)));
+ EXPECT_EQ(InMs(301), loop.sleep_time());
+ EXPECT_EQ(3, loop.Iterate(InMs(600)));
+ EXPECT_EQ(InMs(601), loop.sleep_time());
+ }
+}
+
+// Makes sure that everything works correctly when crossing zero.
+// This seems like a rare case at first, but starting from zero needs to
+// work, which means negatives should too.
+TEST_F(PhasedLoopTest, CrossingZero) {
+ PhasedLoop loop(milliseconds(100), milliseconds(1));
+ loop.Reset(InMs(-1000));
+ EXPECT_EQ(InMs(-1099), loop.sleep_time());
+ EXPECT_EQ(9, loop.Iterate(InMs(-250)));
+ EXPECT_EQ(InMs(-199), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(InMs(-199)));
+ EXPECT_EQ(InMs(-99), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(InMs(-90)));
+ EXPECT_EQ(InMs(1), loop.sleep_time());
+ EXPECT_EQ(0, loop.Iterate(InMs(0)));
+ EXPECT_EQ(InMs(1), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(InMs(1)));
+ EXPECT_EQ(InMs(101), loop.sleep_time());
+
+ EXPECT_EQ(0, loop.Iterate(InMs(2)));
+ EXPECT_EQ(InMs(101), loop.sleep_time());
+
+ EXPECT_EQ(-2, loop.Iterate(InMs(-101)));
+ EXPECT_EQ(InMs(-99), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(InMs(-99)));
+ EXPECT_EQ(InMs(1), loop.sleep_time());
+
+ EXPECT_EQ(0, loop.Iterate(InMs(-99)));
+ EXPECT_EQ(InMs(1), loop.sleep_time());
+}
+
+// Tests OffsetFromIntervalAndTime for various edge conditions.
+TEST_F(PhasedLoopTest, OffsetFromIntervalAndTimeTest) {
+ PhasedLoop loop(milliseconds(1000), milliseconds(300));
+
+ EXPECT_EQ(milliseconds(1),
+ loop.OffsetFromIntervalAndTime(milliseconds(1000), InMs(1001)));
+
+ EXPECT_EQ(milliseconds(0),
+ loop.OffsetFromIntervalAndTime(milliseconds(1000), InMs(1000)));
+
+ EXPECT_EQ(milliseconds(0),
+ loop.OffsetFromIntervalAndTime(milliseconds(1000), InMs(0)));
+
+ EXPECT_EQ(milliseconds(999),
+ loop.OffsetFromIntervalAndTime(milliseconds(1000), InMs(-1)));
+
+ EXPECT_EQ(milliseconds(7),
+ loop.OffsetFromIntervalAndTime(milliseconds(1000), InMs(19115007)));
+
+ EXPECT_EQ(milliseconds(7), loop.OffsetFromIntervalAndTime(milliseconds(1000),
+ InMs(-19115993)));
+}
+
+// Tests that passing invalid values to the constructor dies correctly.
+TEST_F(PhasedLoopDeathTest, InvalidValues) {
+ EXPECT_DEATH(PhasedLoop(milliseconds(1), milliseconds(2)),
+ ".*offset<interval.*");
+ EXPECT_DEATH(PhasedLoop(milliseconds(1), milliseconds(1)),
+ ".*offset<interval.*");
+ EXPECT_DEATH(PhasedLoop(milliseconds(1), milliseconds(-1)),
+ ".*offset>=monotonic_clock::duration\\(0\\).*");
+ EXPECT_DEATH(PhasedLoop(milliseconds(0), milliseconds(0)),
+ ".*interval>monotonic_clock::duration\\(0\\).*");
+}
+
+} // namespace testing
+} // namespace time
+} // namespace aos
diff --git a/aos/util/run_command.cc b/aos/util/run_command.cc
new file mode 100644
index 0000000..2b08cb2
--- /dev/null
+++ b/aos/util/run_command.cc
@@ -0,0 +1,73 @@
+#include "aos/util/run_command.h"
+
+#include <signal.h>
+#include <sys/types.h>
+#include <sys/wait.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/stat.h>
+
+#include "aos/logging/logging.h"
+
+namespace aos {
+namespace util {
+namespace {
+
+// RAII class to block SIGCHLD and then restore it on destruction.
+class BlockSIGCHLD {
+ public:
+ BlockSIGCHLD() {
+ sigset_t to_block;
+ sigemptyset(&to_block);
+ sigaddset(&to_block, SIGCHLD);
+ if (sigprocmask(SIG_BLOCK, &to_block, &original_blocked_) == -1) {
+ PLOG(FATAL, "sigprocmask(SIG_BLOCK, %p, %p) failed",
+ &to_block, &original_blocked_);
+ }
+ }
+ ~BlockSIGCHLD() {
+ if (sigprocmask(SIG_SETMASK, &original_blocked_, nullptr) == -1) {
+ PLOG(FATAL, "sigprocmask(SIG_SETMASK, %p, nullptr) failed",
+ &original_blocked_);
+ }
+ }
+
+ private:
+ sigset_t original_blocked_;
+};
+
+} // namespace
+
+int RunCommand(const char *command) {
+ BlockSIGCHLD blocker;
+ const pid_t pid = fork();
+ switch (pid) {
+ case 0: // in child
+ {
+ int new_stdin = open("/dev/null", O_RDONLY);
+ if (new_stdin == -1) _exit(127);
+ int new_stdout = open("/dev/null", O_WRONLY);
+ if (new_stdout == -1) _exit(127);
+ int new_stderr = open("/dev/null", O_WRONLY);
+ if (new_stderr == -1) _exit(127);
+ if (dup2(new_stdin, 0) != 0) _exit(127);
+ if (dup2(new_stdout, 1) != 1) _exit(127);
+ if (dup2(new_stderr, 2) != 2) _exit(127);
+ execl("/bin/sh", "sh", "-c", command, nullptr);
+ _exit(127);
+ }
+ case -1:
+ return -1;
+ default:
+ int stat;
+ while (waitpid(pid, &stat, 0) == -1) {
+ if (errno != EINTR) {
+ return -1;
+ }
+ }
+ return stat;
+ }
+}
+
+} // namespace util
+} // namespace aos
diff --git a/aos/util/run_command.h b/aos/util/run_command.h
new file mode 100644
index 0000000..02c6785
--- /dev/null
+++ b/aos/util/run_command.h
@@ -0,0 +1,17 @@
+#ifndef AOS_UTIL_RUN_COMMAND_H_
+#define AOS_UTIL_RUN_COMMAND_H_
+
+namespace aos {
+namespace util {
+
+// Improved replacement for system(3). Doesn't block signals like system(3) and
+// is thread-safe. Also makes sure all 3 standard streams are /dev/null.
+//
+// This means that it passes command to `/bin/sh -c` and returns -1 or a status
+// like from wait(2).
+int RunCommand(const char *command);
+
+} // namespace util
+} // namespace aos
+
+#endif // AOS_UTIL_RUN_COMMAND_H_
diff --git a/aos/util/run_command_test.cc b/aos/util/run_command_test.cc
new file mode 100644
index 0000000..775b716
--- /dev/null
+++ b/aos/util/run_command_test.cc
@@ -0,0 +1,61 @@
+#include "aos/util/run_command.h"
+
+#include "gtest/gtest.h"
+
+#include "aos/util/thread.h"
+
+namespace aos {
+namespace util {
+namespace testing {
+
+TEST(RunCommandTest, True) {
+ int result = RunCommand("true");
+ ASSERT_NE(-1, result);
+ ASSERT_TRUE(WIFEXITED(result));
+ EXPECT_EQ(0, WEXITSTATUS(result));
+}
+
+TEST(RunCommandTest, False) {
+ int result = RunCommand("false");
+ ASSERT_NE(-1, result);
+ ASSERT_TRUE(WIFEXITED(result));
+ EXPECT_EQ(1, WEXITSTATUS(result));
+}
+
+TEST(RunCommandTest, CommandNotFound) {
+ int result = RunCommand("ajflkjasdlfa");
+ ASSERT_NE(-1, result);
+ ASSERT_TRUE(WIFEXITED(result));
+ EXPECT_EQ(127, WEXITSTATUS(result));
+}
+
+TEST(RunCommandTest, KilledBySignal) {
+ int result = RunCommand("kill -QUIT $$");
+ ASSERT_NE(-1, result);
+ ASSERT_TRUE(WIFSIGNALED(result));
+ EXPECT_EQ(SIGQUIT, WTERMSIG(result));
+}
+
+TEST(RunCommandTest, MultipleThreads) {
+ int result1, result2;
+ util::FunctionThread t1([&result1](util::Thread *) {
+ result1 = RunCommand("true");
+ });
+ util::FunctionThread t2([&result2](util::Thread *) {
+ result2 = RunCommand("true");
+ });
+ t1.Start();
+ t2.Start();
+ t1.WaitUntilDone();
+ t2.WaitUntilDone();
+ ASSERT_NE(-1, result1);
+ ASSERT_NE(-1, result2);
+ ASSERT_TRUE(WIFEXITED(result1));
+ ASSERT_TRUE(WIFEXITED(result2));
+ EXPECT_EQ(0, WEXITSTATUS(result1));
+ EXPECT_EQ(0, WEXITSTATUS(result2));
+}
+
+} // namespace testing
+} // namespace util
+} // namespace aos
diff --git a/aos/util/string_to_num.h b/aos/util/string_to_num.h
new file mode 100644
index 0000000..86210ca
--- /dev/null
+++ b/aos/util/string_to_num.h
@@ -0,0 +1,29 @@
+#ifndef AOS_UTIL_STRING_TO_NUM_H_
+#define AOS_UTIL_STRING_TO_NUM_H_
+
+#include <sstream>
+#include <string>
+
+namespace aos {
+namespace util {
+
+// Converts a string into a specified numeric type. If it can't be converted
+// completely or at all, or if the converted number would overflow the
+// specified type, it returns false.
+template<typename T>
+inline bool StringToNumber(const ::std::string &input, T *out_num) {
+ ::std::istringstream stream(input);
+ stream >> *out_num;
+
+ if (stream.fail() || !stream.eof()) {
+ return false;
+ }
+
+ return true;
+}
+
+
+} // util
+} // aos
+
+#endif
diff --git a/aos/util/string_to_num_test.cc b/aos/util/string_to_num_test.cc
new file mode 100644
index 0000000..3f467cf
--- /dev/null
+++ b/aos/util/string_to_num_test.cc
@@ -0,0 +1,49 @@
+#include <stdint.h>
+
+#include <string>
+
+#include "gtest/gtest.h"
+
+#include "aos/util/string_to_num.h"
+
+namespace aos {
+namespace util {
+namespace testing {
+
+TEST(StringToNumTest, CorrectNumber) {
+ int result;
+ ASSERT_TRUE(StringToNumber<int>(::std::string("42"), &result));
+ EXPECT_EQ(result, 42);
+}
+
+TEST(StringToNumTest, NegativeTest) {
+ int result;
+ ASSERT_TRUE(StringToNumber<int>(::std::string("-42"), &result));
+ EXPECT_EQ(result, -42);
+}
+
+TEST(StringToNumTest, NonNumber) {
+ int result;
+ ASSERT_FALSE(StringToNumber<int>(::std::string("Daniel"), &result));
+}
+
+TEST(StringToNumTest, NumberWithText) {
+ int result;
+ ASSERT_FALSE(StringToNumber<int>(::std::string("42Daniel"), &result));
+}
+
+TEST(StringToNumTest, OverflowTest) {
+ uint32_t result;
+ // 2 << 32 should overflow.
+ ASSERT_FALSE(StringToNumber<uint32_t>(::std::string("4294967296"), &result));
+}
+
+TEST(StringToNumTest, FloatingPointTest) {
+ double result;
+ ASSERT_TRUE(StringToNumber<double>(::std::string("3.1415927"), &result));
+ EXPECT_EQ(result, 3.1415927);
+}
+
+} // testing
+} // util
+} // aos
diff --git a/aos/util/thread.cc b/aos/util/thread.cc
new file mode 100644
index 0000000..6b3ef76
--- /dev/null
+++ b/aos/util/thread.cc
@@ -0,0 +1,76 @@
+#include "aos/util/thread.h"
+
+#include <pthread.h>
+#include <signal.h>
+
+#include "aos/logging/logging.h"
+
+namespace aos {
+namespace util {
+
+Thread::Thread() : started_(false), joined_(false), should_terminate_(false) {}
+
+Thread::~Thread() {
+ CHECK(!(started_ && !joined_));
+}
+
+void Thread::Start() {
+ CHECK(!started_);
+ started_ = true;
+ CHECK(pthread_create(&thread_, NULL, &Thread::StaticRun, this) == 0);
+}
+
+void Thread::Join() {
+ CHECK(!joined_ && started_);
+ joined_ = true;
+ should_terminate_.store(true);
+ CHECK(pthread_join(thread_, NULL) == 0);
+}
+
+bool Thread::TryJoin() {
+ CHECK(!joined_ && started_);
+#ifdef AOS_SANITIZER_thread
+ // ThreadSanitizer misses the tryjoin and then complains about leaking the
+ // thread. Instead, we'll just check if the thread is still around and then
+ // do a regular Join() iff it isn't.
+ // TODO(brians): Remove this once tsan learns about pthread_tryjoin_np.
+ const int kill_ret = pthread_kill(thread_, 0);
+ // If it's still around.
+ if (kill_ret == 0) return false;
+ // If it died, we'll get ESRCH. Otherwise, something went wrong.
+ if (kill_ret != ESRCH) {
+ PELOG(FATAL, kill_ret, "pthread_kill(thread_, 0) failed");
+ }
+ Join();
+ return true;
+#else
+ const int ret = pthread_tryjoin_np(thread_, nullptr);
+ if (ret == 0) {
+ joined_ = true;
+ return true;
+ } else if (ret == EBUSY) {
+ return false;
+ } else {
+ PELOG(FATAL, ret, "pthread_tryjoin_np(thread_, nullptr) failed");
+ }
+#endif
+}
+
+void Thread::RequestStop() {
+ CHECK(!joined_ && started_);
+ should_terminate_.store(true);
+}
+
+void Thread::WaitUntilDone() {
+ CHECK(!joined_ && started_);
+ joined_ = true;
+ CHECK(pthread_join(thread_, NULL) == 0);
+}
+
+void *Thread::StaticRun(void *self) {
+ static_cast<Thread *>(self)->Run();
+ return NULL;
+}
+
+} // namespace util
+} // namespace aos
diff --git a/aos/util/thread.h b/aos/util/thread.h
new file mode 100644
index 0000000..5992810
--- /dev/null
+++ b/aos/util/thread.h
@@ -0,0 +1,90 @@
+#ifndef AOS_UTIL_THREAD_H_
+#define AOS_UTIL_THREAD_H_
+
+#include <functional>
+#include <atomic>
+
+#include <pthread.h>
+
+#include "aos/macros.h"
+
+namespace aos {
+namespace util {
+
+// A nice wrapper around a pthreads thread.
+//
+// TODO(aschuh): Test this.
+class Thread {
+ public:
+ Thread();
+ virtual ~Thread();
+
+ // Actually creates the thread.
+ void Start();
+
+ // Asks the code to stop and then waits until it has done so.
+ // This or TryJoin() (returning true) must be called exactly once for every
+ // instance.
+ void Join();
+
+ // If the code has already finished, returns true. Does not block waiting if
+ // it isn't.
+ // Join() must not be called on this instance if this returns true.
+ // This must return true or Join() must be called exactly once for every
+ // instance.
+ bool TryJoin();
+
+ // Asks the code to stop (in preparation for a Join()).
+ void RequestStop();
+
+ // Waits until the code has stopped. Does not ask it to do so.
+ void WaitUntilDone();
+
+ protected:
+ // Subclasses need to call this periodically if they are going to loop to
+ // check whether they have been asked to stop.
+ bool should_continue() {
+ return !should_terminate_.load();
+ }
+
+ private:
+ // Where subclasses actually do something.
+ //
+ // They should not block for long periods of time without checking
+ // should_continue().
+ virtual void Run() = 0;
+
+ static void *StaticRun(void *self);
+
+ pthread_t thread_;
+ bool started_;
+ bool joined_;
+ ::std::atomic_bool should_terminate_;
+
+ DISALLOW_COPY_AND_ASSIGN(Thread);
+};
+
+class FunctionThread : public Thread {
+ public:
+ FunctionThread(::std::function<void(FunctionThread *)> function)
+ : function_(function) {}
+
+ // Runs function in a new thread and waits for it to return.
+ static void RunInOtherThread(::std::function<void()> function) {
+ FunctionThread t([&function](FunctionThread *) { function(); });
+ t.Start();
+ t.Join();
+ }
+
+ private:
+ virtual void Run() override {
+ function_(this);
+ }
+
+ const ::std::function<void(FunctionThread *)> function_;
+};
+
+} // namespace util
+} // namespace aos
+
+#endif // AOS_UTIL_THREAD_H_
diff --git a/aos/util/trapezoid_profile.cc b/aos/util/trapezoid_profile.cc
new file mode 100644
index 0000000..e58324f
--- /dev/null
+++ b/aos/util/trapezoid_profile.cc
@@ -0,0 +1,130 @@
+#include "aos/util/trapezoid_profile.h"
+
+#include "aos/logging/logging.h"
+
+using ::Eigen::Matrix;
+
+namespace aos {
+namespace util {
+
+TrapezoidProfile::TrapezoidProfile(::std::chrono::nanoseconds delta_time)
+ : maximum_acceleration_(0), maximum_velocity_(0), timestep_(delta_time) {
+ output_.setZero();
+}
+
+void TrapezoidProfile::UpdateVals(double acceleration,
+ double delta_time) {
+ output_(0) += output_(1) * delta_time +
+ 0.5 * acceleration * delta_time * delta_time;
+ output_(1) += acceleration * delta_time;
+}
+
+const Matrix<double, 2, 1> &TrapezoidProfile::Update(
+ double goal_position,
+ double goal_velocity) {
+ CalculateTimes(goal_position - output_(0), goal_velocity);
+
+ double next_timestep =
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(timestep_)
+ .count();
+
+ if (acceleration_time_ > next_timestep) {
+ UpdateVals(acceleration_, next_timestep);
+ } else {
+ UpdateVals(acceleration_, acceleration_time_);
+
+ next_timestep -= acceleration_time_;
+ if (constant_time_ > next_timestep) {
+ UpdateVals(0, next_timestep);
+ } else {
+ UpdateVals(0, constant_time_);
+ next_timestep -= constant_time_;
+ if (deceleration_time_ > next_timestep) {
+ UpdateVals(deceleration_, next_timestep);
+ } else {
+ UpdateVals(deceleration_, deceleration_time_);
+ next_timestep -= deceleration_time_;
+ UpdateVals(0, next_timestep);
+ }
+ }
+ }
+
+ return output_;
+}
+
+void TrapezoidProfile::CalculateTimes(double distance_to_target,
+ double goal_velocity) {
+ if (distance_to_target == 0) {
+ // We're there. Stop everything.
+ // TODO(aschuh): Deal with velocity not right.
+ acceleration_time_ = 0;
+ acceleration_ = 0;
+ constant_time_ = 0;
+ deceleration_time_ = 0;
+ deceleration_ = 0;
+ return;
+ } else if (distance_to_target < 0) {
+ // Recurse with everything inverted.
+ output_(1) *= -1;
+ CalculateTimes(-distance_to_target, -goal_velocity);
+ output_(1) *= -1;
+ acceleration_ *= -1;
+ deceleration_ *= -1;
+ return;
+ }
+
+ constant_time_ = 0;
+ acceleration_ = maximum_acceleration_;
+ double maximum_acceleration_velocity =
+ distance_to_target * 2 * std::abs(acceleration_) +
+ output_(1) * output_(1);
+ if (maximum_acceleration_velocity > 0) {
+ maximum_acceleration_velocity = sqrt(maximum_acceleration_velocity);
+ } else {
+ maximum_acceleration_velocity = -sqrt(-maximum_acceleration_velocity);
+ }
+
+ // Since we know what we'd have to do if we kept after it to decelerate, we
+ // know the sign of the acceleration.
+ if (maximum_acceleration_velocity > goal_velocity) {
+ deceleration_ = -maximum_acceleration_;
+ } else {
+ deceleration_ = maximum_acceleration_;
+ }
+
+ // We now know the top velocity we can get to.
+ double top_velocity = sqrt((distance_to_target +
+ (output_(1) * output_(1)) /
+ (2.0 * acceleration_) +
+ (goal_velocity * goal_velocity) /
+ (2.0 * deceleration_)) /
+ (-1.0 / (2.0 * deceleration_) +
+ 1.0 / (2.0 * acceleration_)));
+
+ // If it can go too fast, we now know how long we get to accelerate for and
+ // how long to go at constant velocity.
+ if (top_velocity > maximum_velocity_) {
+ acceleration_time_ = (maximum_velocity_ - output_(1)) /
+ maximum_acceleration_;
+ constant_time_ = (distance_to_target +
+ (goal_velocity * goal_velocity -
+ maximum_velocity_ * maximum_velocity_) /
+ (2.0 * maximum_acceleration_)) / maximum_velocity_;
+ } else {
+ acceleration_time_ = (top_velocity - output_(1)) /
+ acceleration_;
+ }
+
+ CHECK_GT(top_velocity, -maximum_velocity_);
+
+ if (output_(1) > maximum_velocity_) {
+ constant_time_ = 0;
+ acceleration_time_ = 0;
+ }
+
+ deceleration_time_ = (goal_velocity - top_velocity) /
+ deceleration_;
+}
+
+} // namespace util
+} // namespace aos
diff --git a/aos/util/trapezoid_profile.h b/aos/util/trapezoid_profile.h
new file mode 100644
index 0000000..944c423
--- /dev/null
+++ b/aos/util/trapezoid_profile.h
@@ -0,0 +1,69 @@
+#ifndef AOS_UTIL_TRAPEZOID_PROFILE_H_
+#define AOS_UTIL_TRAPEZOID_PROFILE_H_
+
+#include "Eigen/Dense"
+
+#include "aos/macros.h"
+#include "aos/time/time.h"
+
+namespace aos {
+namespace util {
+
+// Calculates a trapezoidal motion profile (like for a control loop's goals).
+// Supports having the end speed and position changed in the middle.
+//
+// The only units assumption that this class makes is that the unit of time is
+// seconds.
+class TrapezoidProfile {
+ public:
+ // delta_time is how long between each call to Update.
+ TrapezoidProfile(::std::chrono::nanoseconds delta_time);
+
+ // Updates the state.
+ const Eigen::Matrix<double, 2, 1> &Update(double goal_position,
+ double goal_velocity);
+ // Useful for preventing windup etc.
+ void MoveCurrentState(const Eigen::Matrix<double, 2, 1> ¤t) {
+ output_ = current;
+ }
+
+ // Useful for preventing windup etc.
+ void MoveGoal(double dx) { output_(0, 0) += dx; }
+
+ void SetGoal(double x) { output_(0, 0) = x; }
+
+ void set_maximum_acceleration(double maximum_acceleration) {
+ maximum_acceleration_ = maximum_acceleration;
+ }
+ void set_maximum_velocity(double maximum_velocity) {
+ maximum_velocity_ = maximum_velocity;
+ }
+
+ private:
+ // Basic kinematics to update output_, given that we are going to accelerate
+ // by acceleration over delta_time.
+ void UpdateVals(double acceleration, double delta_time);
+ // Calculates how long to go for each segment.
+ void CalculateTimes(double distance_to_target, double goal_velocity);
+ // output_ is where it should go at time_.
+ Eigen::Matrix<double, 2, 1> output_;
+
+ double acceleration_time_;
+ double acceleration_;
+ double constant_time_;
+ double deceleration_time_;
+ double deceleration_;
+
+ double maximum_acceleration_;
+ double maximum_velocity_;
+
+ // How long between calls to Update.
+ ::std::chrono::nanoseconds timestep_;
+
+ DISALLOW_COPY_AND_ASSIGN(TrapezoidProfile);
+};
+
+} // namespace util
+} // namespace aos
+
+#endif // AOS_UTIL_TRAPEZOID_PROFILE_H_
diff --git a/aos/util/trapezoid_profile.py b/aos/util/trapezoid_profile.py
new file mode 100644
index 0000000..f956bf0
--- /dev/null
+++ b/aos/util/trapezoid_profile.py
@@ -0,0 +1,156 @@
+#!/usr/bin/python
+
+import numpy
+
+class TrapezoidProfile(object):
+ """Computes a trapezoidal motion profile
+
+ Attributes:
+ _acceleration_time: the amount of time the robot will travel at the
+ specified acceleration (s)
+ _acceleration: the acceleration the robot will use to get to the target
+ (unit/s^2)
+ _constant_time: amount of time to travel at a constant velocity to reach
+ target (s)
+ _deceleration_time: amount of time to decelerate (at specified
+ deceleration) to target (s)
+ _deceleration: decceleration the robot needs to get to goal velocity
+ (units/s^2)
+ _maximum_acceleration: the maximum acceleration (units/s^2)
+ _maximum_velocity: the maximum velocity (unit/s)
+ _timestep: time between calls to Update (delta_time)
+ _output: output array containing distance to goal and velocity
+ """
+ def __init__(self, delta_time):
+ """Constructs a TrapezoidProfile.
+
+ Args:
+ delta_time: time between calls to Update (seconds)
+ """
+ self._acceleration_time = 0
+ self._acceleration = 0
+ self._constant_time = 0
+ self._deceleration_time = 0
+ self._deceleration = 0
+
+ self._maximum_acceleration = 0
+ self._maximum_velocity = 0
+ self._timestep = delta_time
+
+ self._output = numpy.array(numpy.zeros((2,1)))
+
+ # Updates the state
+ def Update(self, goal_position, goal_velocity):
+ self._CalculateTimes(goal_position - self._output[0], goal_velocity)
+
+ next_timestep = self._timestep
+
+ # We now have the amount of time we need to accelerate to follow the
+ # profile, the amount of time we need to move at constant velocity
+ # to follow the profile, and the amount of time we need to decelerate to
+ # follow the profile. Do as much of that as we have time left in dt.
+ if self._acceleration_time > next_timestep:
+ self._UpdateVals(self._acceleration, next_timestep)
+ else:
+ self._UpdateVals(self._acceleration, self._acceleration_time)
+
+ next_timestep -= self._acceleration_time
+ if self._constant_time > next_timestep:
+ self._UpdateVals(0, next_timestep)
+ else:
+ self._UpdateVals(0, self._constant_time)
+ next_timestep -= self._constant_time;
+ if self._deceleration_time > next_timestep:
+ self._UpdateVals(self._deceleration, next_timestep)
+ else:
+ self._UpdateVals(self._deceleration, self._deceleration_time)
+ next_timestep -= self._deceleration_time
+ self._UpdateVals(0, next_timestep)
+
+ return self._output
+
+ # Useful for preventing windup etc.
+ def MoveCurrentState(self, current):
+ self._output = current
+
+ # Useful for preventing windup etc.
+ def MoveGoal(self, dx):
+ self._output[0] += dx
+
+ def SetGoal(self, x):
+ self._output[0] = x
+
+ def set_maximum_acceleration(self, maximum_acceleration):
+ self._maximum_acceleration = maximum_acceleration
+
+ def set_maximum_velocity(self, maximum_velocity):
+ self._maximum_velocity = maximum_velocity
+
+ def _UpdateVals(self, acceleration, delta_time):
+ self._output[0, 0] += (self._output[1, 0] * delta_time
+ + 0.5 * acceleration * delta_time * delta_time)
+ self._output[1, 0] += acceleration * delta_time
+
+ def _CalculateTimes(self, distance_to_target, goal_velocity):
+ if distance_to_target == 0:
+ self._acceleration_time = 0
+ self._acceleration = 0
+ self._constant_time = 0
+ self._deceleration_time = 0
+ self._deceleration = 0
+ return
+ elif distance_to_target < 0:
+ # Recurse with everything inverted.
+ self._output[1] *= -1
+ self._CalculateTimes(-distance_to_target, -goal_velocity)
+ self._output[1] *= -1
+ self._acceleration *= -1
+ self._deceleration *= -1
+ return
+
+ self._constant_time = 0
+ self._acceleration = self._maximum_acceleration
+ maximum_acceleration_velocity = (
+ distance_to_target * 2 * numpy.abs(self._acceleration)
+ + self._output[1] * self._output[1])
+ if maximum_acceleration_velocity > 0:
+ maximum_acceleration_velocity = numpy.sqrt(maximum_acceleration_velocity)
+ else:
+ maximum_acceleration_velocity = -numpy.sqrt(-maximum_acceleration_velocity)
+
+ # Since we know what we'd have to do if we kept after it to decelerate, we
+ # know the sign of the acceleration.
+ if maximum_acceleration_velocity > goal_velocity:
+ self._deceleration = -self._maximum_acceleration
+ else:
+ self._deceleration = self._maximum_acceleration
+
+ # We now know the top velocity we can get to.
+ top_velocity = numpy.sqrt((distance_to_target +
+ (self._output[1] * self._output[1]) /
+ (2.0 * self._acceleration) +
+ (goal_velocity * goal_velocity) /
+ (2.0 * self._deceleration)) /
+ (-1.0 / (2.0 * self._deceleration) +
+ 1.0 / (2.0 * self._acceleration)))
+
+ # If it can go too fast, we now know how long we get to accelerate for and
+ # how long to go at constant velocity.
+ if top_velocity > self._maximum_velocity:
+ self._acceleration_time = ((self._maximum_velocity - self._output[1]) /
+ self._maximum_acceleration)
+ self._constant_time = (distance_to_target +
+ (goal_velocity * goal_velocity -
+ self._maximum_velocity * self._maximum_velocity) /
+ (2.0 * self._maximum_acceleration)) / self._maximum_velocity
+ else:
+ self._acceleration_time = (
+ (top_velocity - self._output[1]) / self._acceleration)
+
+ if self._output[1] > self._maximum_velocity:
+ self._constant_time = 0
+ self._acceleration_time = 0
+
+ self._deceleration_time = (
+ (goal_velocity - top_velocity) / self._deceleration)
+
diff --git a/aos/util/trapezoid_profile_test.cc b/aos/util/trapezoid_profile_test.cc
new file mode 100644
index 0000000..79795e1
--- /dev/null
+++ b/aos/util/trapezoid_profile_test.cc
@@ -0,0 +1,128 @@
+#include "gtest/gtest.h"
+
+#include "Eigen/Dense"
+
+#include "aos/util/trapezoid_profile.h"
+
+namespace aos {
+namespace util {
+namespace testing {
+
+class TrapezoidProfileTest : public ::testing::Test {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+ protected:
+ TrapezoidProfileTest() : profile_(delta_time) {
+ position_.setZero();
+ profile_.set_maximum_acceleration(0.75);
+ profile_.set_maximum_velocity(1.75);
+ }
+
+ // Runs an iteration.
+ void RunIteration(double goal_position,
+ double goal_velocity) {
+ position_ = profile_.Update(goal_position,
+ goal_velocity);
+ }
+
+ const Eigen::Matrix<double, 2, 1> &position() { return position_; }
+
+ TrapezoidProfile profile_;
+
+ ::testing::AssertionResult At(double position, double velocity) {
+ static const double kDoubleNear = 0.00001;
+ if (::std::abs(velocity - position_(1)) > kDoubleNear) {
+ return ::testing::AssertionFailure() << "velocity is " << position_(1) <<
+ " not " << velocity;
+ }
+ if (::std::abs(position - position_(0)) > kDoubleNear) {
+ return ::testing::AssertionFailure() << "position is " << position_(0) <<
+ " not " << position;
+ }
+ return ::testing::AssertionSuccess() << "at " << position <<
+ " moving at " << velocity;
+ }
+
+ private:
+ static constexpr ::std::chrono::nanoseconds delta_time =
+ ::std::chrono::milliseconds(10);
+
+ Eigen::Matrix<double, 2, 1> position_;
+};
+
+constexpr ::std::chrono::nanoseconds TrapezoidProfileTest::delta_time;
+
+TEST_F(TrapezoidProfileTest, ReachesGoal) {
+ for (int i = 0; i < 450; ++i) {
+ RunIteration(3, 0);
+ }
+ EXPECT_TRUE(At(3, 0));
+}
+
+// Tests that decresing the maximum velocity in the middle when it is already
+// moving faster than the new max is handled correctly.
+TEST_F(TrapezoidProfileTest, ContinousUnderVelChange) {
+ profile_.set_maximum_velocity(1.75);
+ RunIteration(12.0, 0);
+ double last_pos = position()(0);
+ double last_vel = 1.75;
+ for (int i = 0; i < 1600; ++i) {
+ if (i == 400) {
+ profile_.set_maximum_velocity(0.75);
+ }
+ RunIteration(12.0, 0);
+ if (i >= 400) {
+ EXPECT_TRUE(::std::abs(last_pos - position()(0)) <= 1.75 * 0.01);
+ EXPECT_NEAR(last_vel, ::std::abs(last_pos - position()(0)), 0.0001);
+ }
+ last_vel = ::std::abs(last_pos - position()(0));
+ last_pos = position()(0);
+ }
+ EXPECT_TRUE(At(12.0, 0));
+}
+
+// There is some somewhat tricky code for dealing with going backwards.
+TEST_F(TrapezoidProfileTest, Backwards) {
+ for (int i = 0; i < 400; ++i) {
+ RunIteration(-2, 0);
+ }
+ EXPECT_TRUE(At(-2, 0));
+}
+
+TEST_F(TrapezoidProfileTest, SwitchGoalInMiddle) {
+ for (int i = 0; i < 200; ++i) {
+ RunIteration(-2, 0);
+ }
+ EXPECT_FALSE(At(-2, 0));
+ for (int i = 0; i < 550; ++i) {
+ RunIteration(0, 0);
+ }
+ EXPECT_TRUE(At(0, 0));
+}
+
+// Checks to make sure that it hits top speed.
+TEST_F(TrapezoidProfileTest, TopSpeed) {
+ for (int i = 0; i < 200; ++i) {
+ RunIteration(4, 0);
+ }
+ EXPECT_NEAR(1.5, position()(1), 10e-5);
+ for (int i = 0; i < 2000; ++i) {
+ RunIteration(4, 0);
+ }
+ EXPECT_TRUE(At(4, 0));
+}
+
+// Tests that the position and velocity exactly match at the end. Some code we
+// have assumes this to be true as a simplification.
+TEST_F(TrapezoidProfileTest, ExactlyReachesGoal) {
+ for (int i = 0; i < 450; ++i) {
+ RunIteration(1, 0);
+ }
+ EXPECT_EQ(position()(1), 0.0);
+ EXPECT_EQ(position()(0), 1.0);
+}
+
+} // namespace testing
+} // namespace util
+} // namespace aos
diff --git a/aos/util/wrapping_counter.cc b/aos/util/wrapping_counter.cc
new file mode 100644
index 0000000..f98d7e3
--- /dev/null
+++ b/aos/util/wrapping_counter.cc
@@ -0,0 +1,19 @@
+#include "aos/util/wrapping_counter.h"
+
+namespace aos {
+namespace util {
+
+WrappingCounter::WrappingCounter(int32_t initial_count)
+ : count_(initial_count), last_count_(0) {}
+
+int32_t WrappingCounter::Update(uint8_t current) {
+ if (last_count_ > current) {
+ count_ += 0x100;
+ }
+ count_ = (count_ & 0xffffff00) | current;
+ last_count_ = current;
+ return count_;
+}
+
+} // namespace util
+} // namespace aos
diff --git a/aos/util/wrapping_counter.h b/aos/util/wrapping_counter.h
new file mode 100644
index 0000000..a327dd8
--- /dev/null
+++ b/aos/util/wrapping_counter.h
@@ -0,0 +1,34 @@
+#ifndef AOS_UTIL_WRAPPING_COUNTER_H_
+#define AOS_UTIL_WRAPPING_COUNTER_H_
+
+#include <stdint.h>
+
+namespace aos {
+namespace util {
+
+// Deals correctly with 1-byte counters which wrap.
+// This is only possible if the counter never wraps twice between Update calls.
+// It will also fail if the counter ever goes down (that will be interpreted as
+// +255 instead of -1, for example).
+class WrappingCounter {
+ public:
+ WrappingCounter(int32_t initial_count = 0);
+
+ // Updates the internal counter with a new raw value.
+ // Returns count() for convenience.
+ int32_t Update(uint8_t current);
+
+ // Resets the actual count to value.
+ void Reset(int32_t value = 0) { count_ = value; }
+
+ int32_t count() const { return count_; }
+
+ private:
+ int32_t count_;
+ uint8_t last_count_;
+};
+
+} // namespace util
+} // namespace aos
+
+#endif // AOS_UTIL_WRAPPING_COUNTER_H_
diff --git a/aos/util/wrapping_counter_test.cc b/aos/util/wrapping_counter_test.cc
new file mode 100644
index 0000000..e5c58f3
--- /dev/null
+++ b/aos/util/wrapping_counter_test.cc
@@ -0,0 +1,58 @@
+#include "aos/util/wrapping_counter.h"
+
+#include <limits.h>
+
+#include "gtest/gtest.h"
+
+namespace aos {
+namespace util {
+namespace testing {
+
+TEST(WrappingCounterTest, Basic) {
+ WrappingCounter test_counter;
+ EXPECT_EQ(0, test_counter.count());
+ EXPECT_EQ(1, test_counter.Update(1));
+ EXPECT_EQ(1, test_counter.Update(1));
+ EXPECT_EQ(2, test_counter.Update(2));
+ EXPECT_EQ(7, test_counter.Update(7));
+ EXPECT_EQ(7, test_counter.count());
+ EXPECT_EQ(123, test_counter.Update(123));
+ EXPECT_EQ(123, test_counter.count());
+}
+
+TEST(WrappingCounterTest, Reset) {
+ WrappingCounter test_counter;
+ test_counter.Update(5);
+ test_counter.Reset();
+ EXPECT_EQ(0, test_counter.count());
+ test_counter.Reset(56);
+ EXPECT_EQ(56, test_counter.count());
+}
+
+namespace {
+void test_wrapping(int16_t start, int16_t step) {
+ WrappingCounter test_counter;
+ for (int16_t i = start; i < INT16_MAX - step; i += step) {
+ EXPECT_EQ(i, test_counter.Update(i & 0xFF));
+ }
+}
+}
+
+// This tests the basic wrapping functionality.
+TEST(WrappingCounterTest, ReasonableWrapping) {
+ test_wrapping(0, 13);
+ test_wrapping(0, 53);
+ test_wrapping(0, 64);
+ test_wrapping(0, 73);
+}
+
+// It would be reasonable for these to fail if the implementation changes.
+TEST(WrappingCounterTest, UnreasonableWrapping) {
+ test_wrapping(0, 128);
+ test_wrapping(0, 213);
+ test_wrapping(0, 255);
+}
+
+} // namespace testing
+} // namespace util
+} // namespace aos