copied everything over from 2012 and removed all of the actual robot code except the drivetrain stuff
git-svn-id: https://robotics.mvla.net/svn/frc971/2013/trunk/src@4078 f308d9b7-e957-4cde-b6ac-9a88185e7312
diff --git a/aos/common/Configuration.cpp b/aos/common/Configuration.cpp
new file mode 100644
index 0000000..e0b56ea
--- /dev/null
+++ b/aos/common/Configuration.cpp
@@ -0,0 +1,150 @@
+#include "Configuration.h"
+
+#include <stdio.h>
+#include <string.h>
+#include <errno.h>
+#include <stdlib.h>
+#include <stdarg.h>
+#include <sys/types.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#ifdef __VXWORKS__
+#include <ifLib.h>
+#include <inetLib.h>
+#else
+#include <ifaddrs.h>
+#endif
+
+#include "aos/aos_core.h"
+#ifndef __VXWORKS__
+#include "aos/common/unique_malloc_ptr.h"
+#endif
+
+namespace aos {
+namespace configuration {
+
+namespace {
+
+#ifdef __VXWORKS__
+const size_t kMaxAddrLength = INET_ADDR_LEN;
+#else
+// Including the terminating '\0'.
+const size_t kMaxAddrLength = 18;
+#endif
+// Returns whether or not the current IP address is in ownIPAddress.
+bool GetOwnIPAddress();
+
+#ifdef __VXWORKS__
+// vxworks doesn't have real asprintf.......
+static inline int aos_asprintf(size_t max_len, char **strp, const char *fmt, ...) {
+ *strp = static_cast<char *>(malloc(max_len));
+ if (*strp == NULL) {
+ return -1;
+ }
+ va_list argp;
+ va_start(argp, fmt);
+ const int r = vsnprintf(*strp, max_len, fmt, argp);
+ va_end(argp);
+ if (static_cast<uintmax_t>(r) >= max_len) {
+ errno = EOVERFLOW;
+ free(*strp);
+ return -1;
+ }
+ return r;
+}
+
+// 4-slot cRIO: motfec0
+// 8-slot cRIO port 1: fec0
+// ifShow will show you all of the ones on a cRIO
+const char *const kCrioNetInterfaces[] = {"fec0", "motfec0"};
+bool GetOwnIPAddress(char *buffer, size_t bufferSize) {
+ if (bufferSize < INET_ADDR_LEN) {
+ LOG(ERROR, "bufferSize(%jd) isn't >= INET_ADDR_LEN(%jd)\n",
+ static_cast<intmax_t>(bufferSize),
+ static_cast<intmax_t>(INET_ADDR_LEN));
+ return false;
+ }
+ for (size_t i = 0;
+ i < sizeof(kCrioNetInterfaces) / sizeof(kCrioNetInterfaces[0]); ++i) {
+ if (ifAddrGet(const_cast<char *>(kCrioNetInterfaces[i]), buffer) != OK) {
+ LOG(DEBUG, "ifAddrGet(\"%s\", %p) failed with %d: %s\n",
+ kCrioNetInterfaces[i], buffer, errno, strerror(errno));
+ } else {
+ return true;
+ }
+ }
+ LOG(ERROR, "couldn't get the cRIO's IP address\n");
+ return false;
+}
+#else
+static inline int aos_asprintf(size_t, char **strp, const char *fmt, ...) {
+ va_list argp;
+ va_start(argp, fmt);
+ const int r = vasprintf(strp, fmt, argp);
+ va_end(argp);
+ return r;
+}
+
+const char *const kAtomNetInterface = "eth0";
+bool GetOwnIPAddress(char *buffer, size_t bufferSize) {
+ ifaddrs *addrs;
+ if (getifaddrs(&addrs) != 0) {
+ LOG(ERROR, "getifaddrs(%p) failed with %d: %s\n", &addrs,
+ errno, strerror(errno));
+ return false;
+ }
+ // Smart pointers don't work very well for iterating through a linked list,
+ // but it does do a very nice job of making sure that addrs gets freed.
+ unique_c_ptr<ifaddrs, freeifaddrs> addrs_deleter(addrs);
+
+ for (; addrs != NULL; addrs = addrs->ifa_next) {
+ if (addrs->ifa_addr->sa_family == AF_INET) {
+ if (strcmp(kAtomNetInterface, addrs->ifa_name) == 0) {
+ const char *ipAddress = inet_ntoa(
+ reinterpret_cast<sockaddr_in *>(addrs->ifa_addr)->sin_addr);
+ strncpy(buffer, ipAddress, bufferSize);
+ return true;
+ }
+ }
+ }
+ LOG(ERROR, "couldn't find an AF_INET interface named \"%s\"\n",
+ kAtomNetInterface);
+ return false;
+}
+#endif
+
+const char *RawIPAddress(uint8_t last_part) {
+ char ownIPAddress[kMaxAddrLength];
+ if (!GetOwnIPAddress(ownIPAddress, sizeof(ownIPAddress))) {
+ return NULL;
+ }
+ char *last_dot = strrchr(ownIPAddress, '.');
+ if (last_dot == NULL) {
+ LOG(ERROR, "can't find any '.'s in \"%s\"\n", ownIPAddress);
+ return NULL;
+ }
+ *last_dot = '\0';
+
+ char *r;
+ if (aos_asprintf(kMaxAddrLength, &r, "%s.%hhu",
+ ownIPAddress, last_part) == -1) {
+ return NULL;
+ }
+ return r;
+}
+
+} // namespace
+
+const char *GetIPAddress(NetworkDevice device) {
+ switch (device) {
+ case NetworkDevice::kAtom:
+ return RawIPAddress(179);
+ case NetworkDevice::kCRIO:
+ return RawIPAddress(2);
+ }
+ LOG(FATAL, "Unknown network device.");
+ return NULL;
+}
+
+} // namespace configuration
+} // namespace aos
diff --git a/aos/common/Configuration.h b/aos/common/Configuration.h
new file mode 100644
index 0000000..bf2dc80
--- /dev/null
+++ b/aos/common/Configuration.h
@@ -0,0 +1,42 @@
+#ifndef AOS_COMMON_CONFIGURATION_H_
+#define AOS_COMMON_CONFIGURATION_H_
+
+#include "aos/aos_stdint.h"
+
+namespace aos {
+
+// Constants representing the various ports used for communications and some
+// documentation about what each is used for.
+enum class NetworkPort : uint16_t {
+ // UDP socket sending motor values from the atom to the crio.
+ kMotors = 9710,
+ // UDP socket forwarding drivers station packets from the crio to the atom.
+ kDS = 9711,
+ // UDP socket sending sensor values from the crio to the atom.
+ kSensors = 9712,
+ // TCP socket(s) (automatically reconnects) sending logs from the crio to the
+ // atom.
+ kLogs = 9713,
+ // HTTP server that sends out camera feeds in mjpg format.
+ // Should not be changed because this number shows up elsewhere too.
+ kCameraStreamer = 9714,
+};
+
+// Holds global configuration data. All of the public static functions are safe
+// to call concurrently (the ones that need to create locks on the cRIO).
+namespace configuration {
+
+// Constants indentifying various devices on the network.
+enum class NetworkDevice {
+ // The computer that the cRIO talks to.
+ kAtom,
+ kCRIO,
+};
+// Returns the IP address to get to the specified machine.
+// The return value should be passed to free(3) if it is no longer needed.
+const char *GetIPAddress(NetworkDevice device);
+
+} // namespace configuration
+} // namespace aos
+
+#endif
diff --git a/aos/common/byteorder.h b/aos/common/byteorder.h
new file mode 100644
index 0000000..3444f98
--- /dev/null
+++ b/aos/common/byteorder.h
@@ -0,0 +1,132 @@
+#ifndef AOS_COMMON_BYTEORDER_H_
+#define AOS_COMMON_BYTEORDER_H_
+
+#ifndef __VXWORKS__
+#include <endian.h> // endian(3)
+#endif
+
+// Contains functions for converting between host and network byte order for
+// things other than 16/32 bit integers (those are provided by byteorder(3)).
+// Also gives a nice templated interface to these functions.
+
+namespace aos {
+
+#ifndef __VXWORKS__
+namespace {
+
+template<typename int_type, typename T, int_type (*function)(int_type)> static inline void copier(const void *in, void *out) {
+ static_assert(sizeof(T) == sizeof(int_type), "bad template args");
+ // Have confirmed by looking at the assembly code that this generates the same
+ // code as the (undefined by the c++ standard) way of using a union to do it.
+ // (which is pretty efficient)
+ int_type temp;
+ memcpy(&temp, in, sizeof(T));
+ temp = function(temp);
+ memcpy(out, &temp, sizeof(T));
+}
+template<typename int_type, typename T, int_type (*function)(int_type)> static inline T memcpier(T in) {
+ copier<int_type, T, function>(&in, &in);
+ return in;
+}
+
+// Can't make them static because they have to have external linkage to be used as a
+// template parameter.
+// Needed because be64toh etc are macros. (not that the manpage says anything...)
+// These are used instead of ntohs etc because gcc didn't inline those.
+inline uint64_t _be64toh(uint64_t in) { return be64toh(in); }
+inline uint64_t _htobe64(uint64_t in) { return htobe64(in); }
+inline uint32_t _be32toh(uint32_t in) { return be32toh(in); }
+inline uint32_t _htobe32(uint32_t in) { return htobe32(in); }
+inline uint16_t _be16toh(uint16_t in) { return be16toh(in); }
+inline uint16_t _htobe16(uint16_t in) { return htobe16(in); }
+
+template<int bytes, typename T> class do_ntoh {
+ public:
+ static inline T apply(T net);
+ static inline void copy(const char *in, T &net);
+};
+template<typename T> class do_ntoh<1, T> {
+ public:
+ static inline T apply(T net) { return net; }
+ static inline void copy(const char *in, T *host) { host[0] = in[0]; }
+};
+template<typename T> class do_ntoh<2, T> {
+ public:
+ static inline T apply(T net) { return memcpier<uint16_t, T, _be16toh>(net); }
+ static inline void copy(const char *in, T *host) { copier<uint16_t, T, _be16toh>(in, host); }
+};
+template<typename T> class do_ntoh<4, T> {
+ public:
+ static inline T apply(T net) { return memcpier<uint32_t, T, _be32toh>(net); }
+ static inline void copy(const char *in, T *host) { copier<uint32_t, T, _be32toh>(in, host); }
+};
+template<typename T> class do_ntoh<8, T> {
+ public:
+ static inline T apply(T net) { return memcpier<uint64_t, T, _be64toh>(net); }
+ static inline void copy(const char *in, T *host) { copier<uint64_t, T, _be64toh>(in, host); }
+};
+template<int bytes, typename T> class do_hton {
+ public:
+ static inline T apply(T host);
+ static inline void copy(const T *host, char *out);
+};
+template<typename T> class do_hton<1, T> {
+ public:
+ static inline T apply(T host) { return host; }
+ static inline void copy(const T *host, char *out) { out[0] = host[0]; }
+};
+template<typename T> class do_hton<2, T> {
+ public:
+ static inline T apply(T host) { return memcpier<uint16_t, T, _htobe16>(host); }
+ static inline void copy(const T *host, char *out) { copier<uint16_t, T, _htobe16>(host, out); }
+};
+template<typename T> class do_hton<4, T> {
+ public:
+ static inline T apply(T host) { return memcpier<uint32_t, T, _htobe32>(host); }
+ static inline void copy(const T *host, char *out) { copier<uint32_t, T, _htobe32>(host, out); }
+};
+template<typename T> class do_hton<8, T> {
+ public:
+ static inline T apply(T host) { return memcpier<uint64_t, T, _htobe64>(host); }
+ static inline void copy(const T *host, char *out) { copier<uint64_t, T, _htobe64>(host, out); }
+};
+
+} // namespace
+#endif // ifndef __VXWORKS__
+
+// Converts T from network to host byte order.
+template<typename T> static inline T ntoh(T net) {
+#ifndef __VXWORKS__
+ return do_ntoh<sizeof(net), T>::apply(net);
+#else
+ return net;
+#endif
+}
+// Converts T from host to network byte order.
+template<typename T> static inline T hton(T host) {
+#ifndef __VXWORKS__
+ return do_hton<sizeof(host), T>::apply(host);
+#else
+ return host;
+#endif
+}
+
+template<typename T> static inline void to_host(const char *input, T *host) {
+#ifndef __VXWORKS__
+ do_ntoh<sizeof(*host), T>::copy(input, host);
+#else
+ memcpy(host, input, sizeof(*host));
+#endif
+}
+template<typename T> static inline void to_network(const T *host, char *output) {
+#ifndef __VXWORKS__
+ do_hton<sizeof(*host), T>::copy(host, output);
+#else
+ memcpy(output, host, sizeof(*host));
+#endif
+}
+
+} // namespace aos
+
+#endif
+
diff --git a/aos/common/common.gyp b/aos/common/common.gyp
new file mode 100644
index 0000000..6dca969
--- /dev/null
+++ b/aos/common/common.gyp
@@ -0,0 +1,197 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'queue_test_queue',
+ 'type': 'static_library',
+ 'sources': [
+ '<(AOS)/common/test_queue.q',
+ ],
+ 'variables': {
+ 'header_path': 'aos/common',
+ },
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'includes': ['../build/queues.gypi'],
+ },
+ {
+ 'target_name': 'queue_testutils',
+ 'type': 'static_library',
+ 'sources': [
+ 'queue_testutils.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:ipc_lib'
+ ],
+ },
+ {
+ 'target_name': 'time',
+ 'type': 'static_library',
+ 'sources': [
+ 'time.cc'
+ ],
+ 'dependencies': [
+ # TODO(aschuh): Fix this dependency loop by
+ # providing a logging interface.
+ # '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/build/aos.gyp:aos/ResourceList.h',
+ ],
+ },
+ {
+ 'target_name': 'common',
+ 'type': 'static_library',
+ 'sources': [
+ 'Configuration.cpp',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:logging',
+ ],
+ 'conditions': [
+ ['OS=="crio"', {
+ 'dependencies': [
+ '<(EXTERNALS):WPILib',
+ ]}],
+ ],
+ },
+ {
+ 'target_name': 'queues',
+ 'type': 'static_library',
+ 'sources': [
+ 'queue.cc',
+ ],
+ 'conditions': [
+ ['OS=="crio"', {
+ 'dependencies': [
+ '<(EXTERNALS):WPILib',
+ ],
+ },
+ {
+ 'dependencies': [
+ '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:ipc_lib',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:ipc_lib',
+ ],
+ }]
+ ],
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:common',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/common.gyp:common',
+ ],
+ },
+ {
+ 'target_name': 'control_loop_queues',
+ 'type': 'static_library',
+ 'sources': [ '<(AOS)/common/control_loop/control_loops.q' ],
+ 'variables': {
+ 'header_path': 'aos/common/control_loop',
+ },
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'includes': ['../build/queues.gypi'],
+ },
+ {
+ 'target_name': 'timing_so',
+ 'type': 'shared_library',
+ 'sources': [
+ 'control_loop/Timing.cpp'
+ ],
+ 'variables': {'no_rsync': 1},
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:aos_shared_lib',
+ ],
+ 'direct_dependent_settings': {
+ 'variables': {
+ 'jni_libs': [
+ 'timing_so',
+ ],
+ },
+ },
+ 'export_dependent_settings': [
+ '<(AOS)/build/aos.gyp:aos_shared_lib',
+ ],
+ },
+ {
+ 'target_name': 'timing',
+ 'type': 'static_library',
+ 'sources': [
+ 'control_loop/Timing.cpp'
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/build/aos.gyp:logging',
+ ],
+ },
+ {
+ 'target_name': 'controls',
+ 'type': 'static_library',
+ 'sources': [],
+ 'dependencies': [
+ '<(AOS)/common/messages/messages.gyp:aos_queues',
+ '<(AOS)/build/aos.gyp:logging',
+ 'timing',
+ ],
+ },
+ {
+ 'target_name': 'queue_test',
+ 'type': 'executable',
+ 'sources': [
+ '<(AOS)/common/queue_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ '<(AOS)/build/aos.gyp:libaos',
+ 'queue_testutils',
+ 'common',
+ 'queue_test_queue',
+ ],
+ },
+ {
+ 'target_name': 'type_traits_test',
+ 'type': '<(aos_target)',
+ 'sources': [
+ 'type_traits_test.cpp',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ '<(AOS)/build/aos.gyp:libaos',
+ ],
+ },
+ {
+ 'target_name': 'time_test',
+ 'type': '<(aos_target)',
+ 'sources': [
+ 'time_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ '<(AOS)/build/aos.gyp:libaos',
+ ],
+ },
+ {
+ 'target_name': 'mutex_test',
+ 'type': '<(aos_target)',
+ 'sources': [
+ 'mutex_test.cpp',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ '<(AOS)/build/aos.gyp:libaos',
+ ],
+ },
+ {
+ 'target_name': 'die_test',
+ 'type': 'executable',
+ 'sources': [
+ 'die_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ '<(AOS)/build/aos.gyp:libaos',
+ ],
+ },
+ ],
+}
diff --git a/aos/common/commonmath.h b/aos/common/commonmath.h
new file mode 100644
index 0000000..a77210f
--- /dev/null
+++ b/aos/common/commonmath.h
@@ -0,0 +1,18 @@
+#ifndef AOS_COMMON_MATH_H_
+#define AOS_COMMON_MATH_H_
+
+namespace aos {
+
+// Clips a value so that it is in [min, max]
+inline double Clip(double value, double min, double max) {
+ if (value > max) {
+ value = max;
+ } else if (value < min) {
+ value = min;
+ }
+ return value;
+}
+
+} // namespace aos
+
+#endif // AOS_COMMON_MATH_H_
diff --git a/aos/common/control_loop/ControlLoop-tmpl.h b/aos/common/control_loop/ControlLoop-tmpl.h
new file mode 100644
index 0000000..80d592a
--- /dev/null
+++ b/aos/common/control_loop/ControlLoop-tmpl.h
@@ -0,0 +1,122 @@
+#include <stddef.h>
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/messages/RobotState.q.h"
+
+namespace aos {
+namespace control_loops {
+
+// TODO(aschuh): Tests.
+
+template <class T, bool has_position>
+void ControlLoop<T, has_position>::ZeroOutputs() {
+ aos::ScopedMessagePtr<OutputType> output =
+ control_loop_->output.MakeMessage();
+ Zero(output.get());
+ output.Send();
+}
+
+template <class T, bool has_position>
+void ControlLoop<T, has_position>::Iterate() {
+ // Temporary storage for printing out inputs and outputs.
+ char state[1024];
+
+ // Fetch the latest control loop goal and position. If there is no new
+ // goal, we will just reuse the old one.
+ // If there is no goal, we haven't started up fully. It isn't worth
+ // the added complexity for each loop implementation to handle that case.
+ control_loop_->goal.FetchLatest();
+ // TODO(aschuh): Check the age here if we want the loop to stop on old
+ // goals.
+ const GoalType *goal = control_loop_->goal.get();
+ if (goal == NULL) {
+ LOG(ERROR, "No prior control loop goal.\n");
+ ZeroOutputs();
+ return;
+ }
+ goal->Print(state, sizeof(state));
+ LOG(DEBUG, "goal={%s}\n", state);
+
+ // Only pass in a position if we got one this cycle.
+ const PositionType *position = NULL;
+
+ // Only fetch the latest position if we have one.
+ if (has_position) {
+ // If the position is stale, this is really bad. Try fetching a position
+ // and check how fresh it is, and then take the appropriate action.
+ if (control_loop_->position.FetchLatest()) {
+ position = control_loop_->position.get();
+ } else {
+ if (control_loop_->position.get()) {
+ int msec_age = control_loop_->position.Age().ToMSec();
+ if (!control_loop_->position.IsNewerThanMS(kPositionTimeoutMs)) {
+ LOG(ERROR, "Stale position. %d ms > %d ms. Outputs disabled.\n",
+ msec_age, kPositionTimeoutMs);
+ ZeroOutputs();
+ return;
+ } else {
+ LOG(ERROR, "Stale position. %d ms\n", msec_age);
+ }
+ } else {
+ LOG(ERROR, "Never had a position.\n");
+ ZeroOutputs();
+ return;
+ }
+ }
+ position->Print(state, sizeof(state));
+ LOG(DEBUG, "position={%s}\n", state);
+ }
+
+ bool outputs_enabled = false;
+
+ // Check to see if we got a driver station packet recently.
+ if (aos::robot_state.FetchLatest()) {
+ outputs_enabled = true;
+ } else if (aos::robot_state.IsNewerThanMS(kDSPacketTimeoutMs)) {
+ outputs_enabled = true;
+ } else {
+ if (aos::robot_state.get()) {
+ int msec_age = aos::robot_state.Age().ToMSec();
+ LOG(ERROR, "Driver Station packet is too old (%d ms).\n", msec_age);
+ } else {
+ LOG(ERROR, "No Driver Station packet.\n");
+ }
+ }
+
+ // Run the iteration.
+ aos::ScopedMessagePtr<StatusType> status =
+ control_loop_->status.MakeMessage();
+ if (status.get() == NULL) {
+ return;
+ }
+
+ if (outputs_enabled) {
+ aos::ScopedMessagePtr<OutputType> output =
+ control_loop_->output.MakeMessage();
+ RunIteration(goal, position, output.get(), status.get());
+
+ output->Print(state, sizeof(state));
+ LOG(DEBUG, "output={%s}\n", state);
+ output.Send();
+ } else {
+ // The outputs are disabled, so pass NULL in for the output.
+ RunIteration(goal, position, NULL, status.get());
+ ZeroOutputs();
+ }
+
+ status->Print(state, sizeof(state));
+ LOG(DEBUG, "status={%s}\n", state);
+ status.Send();
+}
+
+template <class T, bool has_position>
+void ControlLoop<T, has_position>::Run() {
+ while (true) {
+ ::aos::time::PhasedLoop10MS(0);
+ Iterate();
+ }
+}
+
+} // namespace control_loops
+} // namespace aos
diff --git a/aos/common/control_loop/ControlLoop.h b/aos/common/control_loop/ControlLoop.h
new file mode 100644
index 0000000..b69cf01
--- /dev/null
+++ b/aos/common/control_loop/ControlLoop.h
@@ -0,0 +1,133 @@
+#ifndef AOS_CONTROL_LOOP_CONTROL_LOOP_H_
+#define AOS_CONTROL_LOOP_CONTROL_LOOP_H_
+
+#include <cstring>
+
+#include "aos/aos_core.h"
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/messages/RobotState.q.h"
+#include "aos/common/type_traits.h"
+
+namespace aos {
+namespace control_loops {
+
+// Interface to describe runnable jobs.
+class Runnable {
+ public:
+ virtual ~Runnable() {}
+ // Runs forever.
+ virtual void Run() = 0;
+ // Does one quick piece of work and return. Does _not_ block.
+ virtual void Iterate() = 0;
+};
+
+class SerializableControlLoop : public Runnable {
+ public:
+ // Returns the size of all the data to be sent when serialized.
+ virtual size_t SeralizedSize() = 0;
+ // Serialize the current data.
+ virtual void Serialize(char *buffer) const = 0;
+ // Serialize zeroed data in case the data is out of date.
+ virtual void SerializeZeroMessage(char *buffer) const = 0;
+ // Deserialize data into the control loop.
+ virtual void Deserialize(const char *buffer) = 0;
+ // Unique identifier for the control loop.
+ // Most likely the hash of the queue group.
+ virtual uint32_t UniqueID() = 0;
+};
+
+// Provides helper methods to assist in writing control loops.
+// This template expects to be constructed with a queue group as an argument
+// that has a goal, position, status, and output queue.
+// It will then call the RunIteration method every cycle that it has enough
+// valid data for the control loop to run.
+// If has_position is false, the control loop will always use NULL as the
+// position and not check the queue. This is used for "loops" that control
+// motors open loop.
+template <class T, bool has_position = true>
+class ControlLoop : public SerializableControlLoop {
+ public:
+ // Maximum age of position packets before the loop will be disabled due to
+ // invalid position data.
+ static const int kPositionTimeoutMs = 100;
+ // Maximum age of driver station packets before the loop will be disabled.
+ static const int kDSPacketTimeoutMs = 100;
+
+ ControlLoop(T *control_loop) : control_loop_(control_loop) {}
+
+ // Create some convenient typedefs to reference the Goal, Position, Status,
+ // and Output structures.
+ typedef typename std::remove_reference<
+ decltype(*(static_cast<T *>(NULL)->goal.MakeMessage().get()))>::type
+ GoalType;
+ typedef typename std::remove_reference<
+ decltype(*(static_cast<T *>(NULL)->position.MakeMessage().get()))>::type
+ PositionType;
+ typedef typename std::remove_reference<
+ decltype(*(static_cast<T *>(NULL)->status.MakeMessage().get()))>::type
+ StatusType;
+ typedef typename std::remove_reference<
+ decltype(*(static_cast<T *>(NULL)->output.MakeMessage().get()))>::type
+ OutputType;
+
+ // Constructs and sends a message on the output queue which will stop all the
+ // motors. Calls Zero to clear all the state.
+ void ZeroOutputs();
+
+ // Sets the output to zero.
+ // Over-ride if a value of zero is not "off" for this subsystem.
+ virtual void Zero(OutputType *output) { output->Zero(); }
+
+ // Runs the loop forever.
+ virtual void Run();
+
+ // Runs one cycle of the loop.
+ virtual void Iterate();
+
+ // Returns the name of the queue group.
+ const char *name() { return control_loop_->name(); }
+
+ // Methods to serialize all the data that should be sent over the network.
+ virtual size_t SeralizedSize() { return control_loop_->goal->Size(); }
+ virtual void Serialize(char *buffer) const {
+ control_loop_->goal->Serialize(buffer);
+ }
+ virtual void SerializeZeroMessage(char *buffer) const {
+ GoalType zero_goal;
+ zero_goal.Zero();
+ zero_goal.Serialize(buffer);
+ }
+
+ virtual void Deserialize(const char *buffer) {
+ ScopedMessagePtr<GoalType> new_msg = control_loop_->goal.MakeMessage();
+ new_msg->Deserialize(buffer);
+ new_msg.Send();
+ }
+
+ virtual uint32_t UniqueID() { return control_loop_->hash(); }
+
+ protected:
+ // Runs an iteration of the control loop.
+ // goal is the last goal that was sent. It might be any number of cycles old.
+ // position is the current position, or NULL if we didn't get a position this
+ // cycle.
+ // output is the values to be sent to the motors. This is NULL if the output
+ // is going to be ignored and set to 0.
+ // status is the status of the control loop.
+ // Both output and status should be filled in by the implementation.
+ virtual void RunIteration(const GoalType *goal,
+ const PositionType *position,
+ OutputType *output,
+ StatusType *status) = 0;
+
+ private:
+ // Pointer to the queue group
+ T *control_loop_;
+};
+
+} // namespace control_loops
+} // namespace aos
+
+#include "aos/common/control_loop/ControlLoop-tmpl.h" // IWYU pragma: export
+
+#endif
diff --git a/aos/common/control_loop/Timing.cpp b/aos/common/control_loop/Timing.cpp
new file mode 100644
index 0000000..63fda44
--- /dev/null
+++ b/aos/common/control_loop/Timing.cpp
@@ -0,0 +1,19 @@
+#include "aos/common/logging/logging.h"
+#include "aos/common/control_loop/Timing.h"
+
+#include "aos/common/time.h"
+
+namespace aos {
+namespace time {
+
+void PhasedLoopXMS(int ms, int offset) {
+ // TODO(brians): Rewrite this cleaner.
+ // TODO(brians): Tests!
+ int64_t period_nsec = Time::InMS(ms).nsec();
+ SleepUntil(Time::InNS((Time::Now().ToNSec() / period_nsec +
+ static_cast<int64_t>(1)) * period_nsec +
+ Time::InUS(offset).ToNSec()));
+}
+
+} // namespace timing
+} // namespace aos
diff --git a/aos/common/control_loop/Timing.h b/aos/common/control_loop/Timing.h
new file mode 100644
index 0000000..139376d
--- /dev/null
+++ b/aos/common/control_loop/Timing.h
@@ -0,0 +1,19 @@
+#ifndef __AOS_TIMEOUT_H_
+#define __AOS_TIMEOUT_H_
+
+#include <time.h>
+#include <string>
+
+namespace aos {
+namespace time {
+
+// Will not be accurate if ms isn't a factor of 1000.
+// offset is in us.
+void PhasedLoopXMS(int ms, int offset);
+// offset is in us.
+inline void PhasedLoop10MS(int offset) { PhasedLoopXMS(10, offset); }
+
+} // namespace time
+} // namespace aos
+
+#endif
diff --git a/aos/common/control_loop/control_loops.q b/aos/common/control_loop/control_loops.q
new file mode 100644
index 0000000..5ba30ec
--- /dev/null
+++ b/aos/common/control_loop/control_loops.q
@@ -0,0 +1,38 @@
+package aos.control_loops;
+
+interface IsDone {
+ bool done;
+};
+
+interface ControlLoop {
+ queue goal;
+ queue position;
+ queue output;
+ queue IsDone status;
+};
+
+message Goal {
+ double goal;
+};
+
+message Position {
+ double position;
+};
+
+message Output {
+ double pwm;
+};
+
+message Status {
+ bool done;
+};
+
+// Single Input Single Output control loop.
+queue_group SISO {
+ implements ControlLoop;
+
+ queue Goal goal;
+ queue Position position;
+ queue Output output;
+ queue Status status;
+};
diff --git a/aos/common/debugging-tips.txt b/aos/common/debugging-tips.txt
new file mode 100644
index 0000000..5d52af2
--- /dev/null
+++ b/aos/common/debugging-tips.txt
@@ -0,0 +1,48 @@
+[General]
+Check the logs! Very few things will fail without putting something in the logs.
+ If they do, that is a bug unless our code is never getting run (there are
+ innumerable ways that could happen, but it generally doesn't).
+ To check the logs, run `LogDisplayer` if `BinaryLogReader` has been started or
+ just run `LogStreamer` if you want to do simple testing without writing logs
+ to a file. See `LogDisplayer --help` for options.
+All of the binaries get put in the same place. That is
+ src/out_atom/Default/outputs on the build machine and
+ /home/driver/robot_code/bin on the fitpc.
+
+[Startup]
+Low level startup errors often end up in /aos_fatal_error.* on the cRIO and
+ /tmp/aos_fatal_error.* under linux. Also helpful are the /tmp/starter*_std*
+ files (if the standard start scripts are being used) and
+ aos/crio/bin/netconsole.sh for reading cRIO stdout and stderr.
+ If lots of the /tmp/starter_*std* files (with different numbers) are being
+ created, that means that starter_exe is dying constantly.
+
+[Anything Not Running]
+Check starter_exe's log messages. They might mention that it is constantly dying
+ on startup and being restarted.
+
+[Control Loop(s) Not Working]
+Are robot_state messages going out? An aos::JoystickInput (often JoystickReader)
+ should be sending them.
+Is it being fed goal messages?
+Is it getting position messages?
+Is something looking at the output and doing something useful with it?
+
+[No Driver Station Packets]
+Check to make sure that JSR (a cRIO-side task) is saying that it's sending
+ messages. Also check JoystickReader (or whatever inherits from
+ aos::JoystickInput) is running and see if it's receiving anything.
+
+[Attaching a Debugger]
+In order to make attaching a debugger very useful, you have to compile the code
+ with debugging symbols. The way to do that is to modify the appropriate
+ build.sh to pass "yes" instead of "no" as an argument to the aos build.sh. You
+ have to modify a .gyp file (touching it is sufficient) to get it to use the
+ new build flag.
+Attaching GDB to an existing robot code process works like normal (you have to
+ root first because that's what all of the code currently runs as).
+If a process is dying repeatedly, the easiest way to debug it is to kill
+ starter_loop.sh and (it has to be after that) starter_exe. After doing that,
+ NOTHING will get restarted (including the normal restart after changing a
+ binary) so that you can start a process under GDB like usual (shouldn't need
+ anything special if done as root).
diff --git a/aos/common/die.cc b/aos/common/die.cc
new file mode 100644
index 0000000..195df45
--- /dev/null
+++ b/aos/common/die.cc
@@ -0,0 +1,77 @@
+#include "aos/common/die.h"
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <errno.h>
+#include <sys/types.h>
+#include <unistd.h>
+#include <string.h>
+#ifdef __VXWORKS__
+#include <taskLib.h>
+// Have to re-declare it with __attribute__((noreturn)).
+extern "C" void abort() __attribute__((noreturn));
+#endif
+
+#include <string>
+
+#include "aos/aos_stdint.h"
+
+namespace aos {
+
+void Die(const char *format, ...) {
+ va_list args;
+ va_start(args, format);
+ VDie(format, args);
+}
+
+namespace {
+// Calculates the filename to dump the message into.
+const std::string GetFilename() {
+#ifdef __VXWORKS__
+ const char *name = taskName(0); // get the name of this task
+ if (name == NULL) name = "<unknown>";
+ const std::string first_part = "/aos_fatal_error.";
+ return first_part + std::string(name);
+#else
+ char *filename;
+ if (asprintf(&filename, "/tmp/aos_fatal_error.%jd",
+ static_cast<intmax_t>(getpid())) > 0) {
+ std::string r(filename);
+ free(filename);
+ return r;
+ } else {
+ fprintf(stderr, "aos fatal: asprintf(%p, \"thingie with %%jd\", %jd)"
+ " failed with %d (%s)\n", &filename,
+ static_cast<intmax_t>(getpid()), errno, strerror(errno));
+ return std::string();
+ }
+#endif
+}
+} // namespace
+void VDie(const char *format, va_list args_in) {
+ va_list args;
+
+ fputs("aos fatal: ERROR!! details following\n", stderr);
+ va_copy(args, args_in);
+ vfprintf(stderr, format, args);
+ va_end(args);
+ fputs("aos fatal: ERROR!! see stderr for details\n", stdout);
+
+ const std::string filename = GetFilename();
+ if (!filename.empty()) {
+ FILE *error_file = fopen(filename.c_str(), "w");
+ if (error_file != NULL) {
+ va_copy(args, args_in);
+ vfprintf(error_file, format, args);
+ va_end(args);
+ fclose(error_file);
+ } else {
+ fprintf(stderr, "aos fatal: fopen('%s', \"w\") failed with %d (%s)\n",
+ filename.c_str(), errno, strerror(errno));
+ }
+ }
+
+ abort();
+}
+
+} // namespace aos
diff --git a/aos/common/die.h b/aos/common/die.h
new file mode 100644
index 0000000..28519a8
--- /dev/null
+++ b/aos/common/die.h
@@ -0,0 +1,20 @@
+#ifndef AOS_COMMON_DIE_H_
+#define AOS_COMMON_DIE_H_
+
+#include <stdarg.h>
+
+namespace aos {
+
+// Terminates the task/process and logs a message (without using the logging
+// framework). Designed for use in code that can't use the logging framework
+// (code that can should LOG(FATAL), which calls this).
+void Die(const char *format, ...)
+ __attribute__((noreturn))
+ __attribute__((format(gnu_printf, 1, 2)));
+void VDie(const char *format, va_list args)
+ __attribute__((noreturn))
+ __attribute__((format(gnu_printf, 1, 0)));
+
+} // namespace aos
+
+#endif // AOS_COMMON_DIE_H_
diff --git a/aos/common/die_test.cc b/aos/common/die_test.cc
new file mode 100644
index 0000000..687543c
--- /dev/null
+++ b/aos/common/die_test.cc
@@ -0,0 +1,14 @@
+#include "aos/common/die.h"
+
+#include "gtest/gtest.h"
+
+namespace aos {
+namespace testing {
+
+TEST(DieDeathTest, Works) {
+ EXPECT_EXIT(Die("str=%s num=%d\n", "hi", 5),
+ ::testing::KilledBySignal(SIGABRT), ".*str=hi num=5\n");
+}
+
+} // namespace testing
+} // namespace aos
diff --git a/aos/common/input/SensorInput-tmpl.h b/aos/common/input/SensorInput-tmpl.h
new file mode 100644
index 0000000..e92a923
--- /dev/null
+++ b/aos/common/input/SensorInput-tmpl.h
@@ -0,0 +1,41 @@
+#include "aos/common/input/SensorInput.h"
+#ifndef __VXWORKS__
+#include "aos/common/network/ReceiveSocket.h"
+#include "aos/common/Configuration.h"
+#endif
+
+namespace aos {
+
+#ifdef __VXWORKS__
+template<class Values> SEM_ID SensorInput<Values>::lock_ = semBCreate(SEM_Q_PRIORITY, SEM_FULL);
+template<class Values> std::vector<SensorInput<Values> *> SensorInput<Values>::running_;
+#endif
+template<class Values> void SensorInput<Values>::Run() {
+#ifndef __VXWORKS__
+ ReceiveSocket sock(NetworkPort::kSensors);
+ Values values;
+ while (true) {
+ if (sock.Recv(&values, sizeof(values)) == -1) {
+ LOG(WARNING, "socket receive failed\n");
+ continue;
+ }
+ RunIteration(values);
+ }
+#else
+ semTake(lock_, WAIT_FOREVER);
+ running_.push_back(this);
+ semGive(lock_);
+#endif
+}
+
+#ifdef __VXWORKS__
+template<class Values> void SensorInput<Values>::RunIterationAll(Values &vals) {
+ semTake(lock_, WAIT_FOREVER);
+ for (auto it = running_.begin(); it != running_.end(); ++it) {
+ (*it)->RunIteration(vals);
+ }
+ semGive(lock_);
+}
+#endif
+
+} // namespace aos
diff --git a/aos/common/input/SensorInput.h b/aos/common/input/SensorInput.h
new file mode 100644
index 0000000..7f8eaa7
--- /dev/null
+++ b/aos/common/input/SensorInput.h
@@ -0,0 +1,36 @@
+#ifndef AOS_INPUT_SENSOR_INPUT_H_
+#define AOS_INPUT_SENSOR_INPUT_H_
+
+#include "aos/aos_core.h"
+#ifdef __VXWORKS__
+#include <vector>
+#include <semLib.h>
+#endif
+
+namespace aos {
+
+// Class for implementing code that takes information from a sensor struct and
+// places it into queues. Subclasses should be compiled for both the atom and
+// the crio to support crio control loops.
+template<class Values> class SensorInput {
+ protected:
+ virtual void RunIteration(Values &values) = 0;
+ public:
+ // Enters an infinite loop that reads values and calls RunIteration.
+ void Run();
+
+#ifdef __VXWORKS__
+ // Calls RunIteration on all instances with values.
+ static void RunIterationAll(Values &values);
+ private:
+ static SEM_ID lock_;
+ static std::vector<SensorInput *> running_;
+#endif
+};
+
+} // namespace aos
+
+#include "SensorInput-tmpl.h"
+
+#endif
+
diff --git a/aos/common/inttypes.h b/aos/common/inttypes.h
new file mode 100644
index 0000000..e10a33b
--- /dev/null
+++ b/aos/common/inttypes.h
@@ -0,0 +1,19 @@
+#ifndef AOS_COMMON_INTTYPES_H_
+#define AOS_COMMON_INTTYPES_H_
+
+// This file is here because the vxworks headers do not have an inttypes.h file
+// and being able to print out fixed size types is very useful. Any fixed size
+// formats that we need on the cRIO should get added here.
+
+#ifndef __VXWORKS__
+#include <inttypes.h>
+#else
+// It warns about just "d", but not this, which is kind of weird because
+// sizeof(int) == sizeof(long) == sizeof(int32_t) == 4, but oh well.
+#define PRId32 "ld"
+#define PRIx32 "lx"
+#define PRId64 "lld"
+#define PRIu16 "u"
+#endif
+
+#endif // AOS_COMMON_INTTYPES_H_
diff --git a/aos/common/logging/logging.h b/aos/common/logging/logging.h
new file mode 100644
index 0000000..e3ff839
--- /dev/null
+++ b/aos/common/logging/logging.h
@@ -0,0 +1,102 @@
+#ifndef AOS_COMMON_LOGGING_LOGGING_H_
+// must be kept in sync with crio/logging/crio_logging.h and atom_code/logging/atom_logging.h
+#define AOS_COMMON_LOGGING_LOGGING_H_
+
+#include <stdint.h>
+#include <sys/types.h>
+#include <unistd.h>
+#include <string.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef uint8_t log_level;
+#define DECL_LEVELS \
+DECL_LEVEL(DEBUG, 0); /* stuff that gets printed out every cycle */ \
+DECL_LEVEL(INFO, 1); /* things like PosEdge/NegEdge */ \
+/* things that might still work if they happen occasionally but should be watched */ \
+DECL_LEVEL(WARNING, 2); \
+/*-1 so that vxworks macro of same name will have same effect if used*/ \
+DECL_LEVEL(ERROR, -1); /* errors */ \
+DECL_LEVEL(FATAL, 4); /* serious errors. the logging code will terminate the process/task */ \
+DECL_LEVEL(LOG_UNKNOWN, 5); /* unknown logging level */
+#define DECL_LEVEL(name, value) extern const log_level name;
+#undef ERROR
+DECL_LEVELS
+#undef DECL_LEVEL
+
+#define STRINGIFY(x) TO_STRING(x)
+#define TO_STRING(x) #x
+
+//not static const size_t for c code
+#define LOG_MESSAGE_LEN 300
+
+// Allows format to not be a string constant.
+#define LOG_DYNAMIC(level, format, args...) do{ \
+ static char log_buf[LOG_MESSAGE_LEN]; \
+ int ret = snprintf(log_buf, sizeof(log_buf), format, ##args); \
+ if(ret < 0 || (uintmax_t)ret >= LOG_MESSAGE_LEN){ \
+ LOG(WARNING, "next message was too long so not subbing in args\n"); \
+ LOG(level, "%s", format); \
+ }else{ \
+ LOG(level, "%s", log_buf); \
+ } \
+}while(0)
+
+// The struct that the crio-side code uses for making logging calls.
+// Packed so it's the same on the atom and the crio.
+typedef struct {
+ // pid_t here at the front like in log_message
+ pid_t identifier; // must ALWAYS be -1 to identify that this is a crio log message
+ log_level level;
+ // still has to fit in LOG_MESSAGE_LEN on the atom side
+ char message[LOG_MESSAGE_LEN - 50];
+ double time;
+ uint8_t sequence;
+} __attribute__((packed)) log_crio_message;
+
+#ifdef __cplusplus
+// Just sticks the message into the shared memory queue.
+int log_crio_message_send(log_crio_message &to_send);
+// Returns left > right. LOG_UNKNOWN is most important.
+static inline bool log_gt_important(log_level left, log_level right) {
+ log_level l = left, r = right;
+ if (l == ERROR) l = 3;
+ if (r == ERROR) r = 3;
+ return left > right;
+}
+#endif
+
+// Returns a string representing level or "unknown".
+static inline const char *log_str(log_level level) {
+ // c doesn't really have const variables so they don't work in case statements
+ if (level == DEBUG) return "DEBUG";
+ if (level == INFO) return "INFO";
+ if (level == WARNING) return "WARNING";
+ if (level == ERROR) return "ERROR";
+ if (level == FATAL) return "FATAL";
+ return "unknown";
+}
+// Returns the log level represented by str or LOG_UNKNOWN.
+static inline log_level str_log(const char *str) {
+ if (!strcmp(str, "DEBUG")) return DEBUG;
+ if (!strcmp(str, "INFO")) return INFO;
+ if (!strcmp(str, "WARNING")) return WARNING;
+ if (!strcmp(str, "ERROR")) return ERROR;
+ if (!strcmp(str, "FATAL")) return FATAL;
+ return LOG_UNKNOWN;
+}
+
+#ifdef __cplusplus
+}
+#endif
+
+#ifdef __unix
+#include "aos/atom_code/logging/atom_logging.h" // IWYU pragma: export
+#else
+#include "aos/crio/logging/crio_logging.h" // IWYU pragma: export
+#endif
+
+#endif
+
diff --git a/aos/common/macros.h b/aos/common/macros.h
new file mode 100644
index 0000000..88fc52e
--- /dev/null
+++ b/aos/common/macros.h
@@ -0,0 +1,25 @@
+#ifndef _AOS_COMMON_MACROS_H_
+#define _AOS_COMMON_MACROS_H_
+
+// This file has some common macros etc.
+
+// A macro to disallow the copy constructor and operator= functions
+// This should be used in the private: declarations for a class
+#define DISALLOW_COPY_AND_ASSIGN(TypeName) \
+ TypeName(const TypeName&); \
+ void operator=(const TypeName&)
+// A macro to wrap arguments to macros that contain commas.
+// Useful for DISALLOW_COPY_AND_ASSIGNing templated types with multiple template
+// arguments.
+#define MACRO_ARG(...) __VA_ARGS__
+// Double-wraps macro arguments.
+// Necessary to use commas in gtest predicate arguments.
+#define MACRO_DARG(...) (__VA_ARGS__)
+
+#ifdef __GNUC__
+#define UNUSED_VARIABLE __attribute__ ((unused))
+#else
+#define UNUSED_VARIABLE
+#endif
+
+#endif // _AOS_COMMON_MACROS_H_
diff --git a/aos/common/messages/QueueHolder.h b/aos/common/messages/QueueHolder.h
new file mode 100644
index 0000000..6cf2835
--- /dev/null
+++ b/aos/common/messages/QueueHolder.h
@@ -0,0 +1,173 @@
+#ifndef __AOS_MESSAGES_QUEUE_HOLDER_H_
+#define __AOS_MESSAGES_QUEUE_HOLDER_H_
+
+#include <stddef.h>
+
+#include <algorithm>
+
+#include "aos/aos_core.h"
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/byteorder.h"
+#include "aos/common/time.h"
+#include "aos/common/type_traits.h"
+
+namespace aos {
+
+// Specializations of TypeOperator and QueueBuilder that are actually
+// implemented are created in the generated code for all message value types.
+// These specializations have the same functions declared in the actual types.
+
+// Defines a way for types to be manipulated.
+template<typename T> class TypeOperator {
+ public:
+ // Sets all fields to their default constructor.
+ static void Zero(T &t_);
+ // Returns the size of buffer NToH and HToN use.
+ static size_t Size();
+ // Converts everything from network to host byte order.
+ // input must have Size() bytes available in it.
+ static void NToH(char *input, T &t_);
+ // Converts everything from host to network byte order and puts it into output.
+ // output must have Size() bytes available in it.
+ static void HToN(const T &t_, char *output);
+ // Creates a string with the names and values of all the fields.
+ // The return value might will be to a static buffer.
+ static const char *Print(const T &t_);
+};
+
+template<typename T> class QueueHolder;
+
+// An easy way to set values for queue messages.
+// Each specialization has chainable setter functions for building a message
+// of type T to put into a queue (like QueueBuilder<T> &field(int value);).
+template<class T> class QueueBuilder {
+ public:
+ QueueBuilder(QueueHolder<T> &holder);
+ bool Send();
+};
+
+// Internal class to make implementing identical behavior with structs that go
+// into queues easier. Also a central location for the documentation.
+//
+// When compiled for the cRIO, everything except Clear does nothing (and Get
+// turns into just a Clear) which means that the internal T instance is the only one.
+// Also, the internal instance becomes static.
+//
+// To look at the current message, you Get a copy and then View the result.
+// To make changes, you modify the message that you can View (after possibly
+// Clearing it) and then you Send it. You can also just Clear the message, put
+// data into it, and then Send it. A way to modify the local message is using
+// the Builder function.
+// Note that there is no way to avoid potentially overwriting other changes between
+// when you Get one and when you Send it (mainly applicable to 1-length queues).
+//
+// T must be POD and have a "timespec set_time;" field.
+//
+// This first class doesn't have the builder; QueueHolder does.
+#define aos_check_rv __attribute__((warn_unused_result))
+template<typename T> class QueueHolderNoBuilder {
+#ifndef __VXWORKS__
+ aos_queue *const queue_;
+ static_assert(shm_ok<T>::value, "T must be able to"
+ " go through shared memory and memcpy");
+ T t_;
+#else
+ static T t_;
+#endif
+ public:
+#ifndef __VXWORKS__
+ explicit QueueHolderNoBuilder(aos_queue *queue) : queue_(queue) {}
+#else
+ QueueHolderNoBuilder() {}
+#endif
+ // Gets the current value and stores it in View().
+ // check_time is whether or not to check to see if the last time a value was Sent
+ // was too long ago (returns false if it was)
+ // Returns true if View() is now the current value.
+ // IMPORTANT: If this function returns false, the contents of View() are
+ // either the same as they were before or (if check_time is true) the current
+ // message. That is why it creates compile-time warnings if the return value
+ // is not checked.
+ bool Get(bool check_time) aos_check_rv;
+ // Looks at the current value. Starts out undefined.
+ // If the last Get call returned false, then this the contents of the
+ // return value are undefined.
+#ifdef __VXWORKS__
+ static
+#endif
+ inline T &View() { return t_; }
+ // Clears (calls the default constructor of) all the fields of the current
+ // Goal.
+ void Clear() { TypeOperator<T>::Zero(t_); }
+ // Sends the current value. Does not affect the current value.
+ // Returns whether or not the Send succeeded.
+ bool Send();
+ // Returns a string containing the values of all the fields in the current
+ // value.
+ // The return value is valid until Print is called again. The class owns the
+ // memory.
+ const char *Print() const { return TypeOperator<T>::Print(t_); }
+};
+
+template<typename T>
+bool QueueHolderNoBuilder<T>::Get(bool check_time) {
+#ifdef __VXWORKS__
+ (void)check_time;
+ return true;
+#else
+ const T *msg = static_cast<const T *>(aos_queue_read_msg(queue_,
+ PEEK | NON_BLOCK));
+ if (msg == NULL) {
+ return false;
+ }
+ static_assert(sizeof(t_) == sizeof(*msg), "something is wrong here");
+ memcpy(&t_, msg, sizeof(t_));
+ aos_queue_free_msg(queue_, msg);
+ if (check_time && !((time::Time::Now() - time::Time(t_.set_time)) > time::Time::InMS(45))) {
+ LOG(WARNING, "too long since last Set msg={%s}\n", Print());
+ return false;
+ } else {
+ return true;
+ }
+#endif
+}
+template<typename T>
+bool QueueHolderNoBuilder<T>::Send() {
+#ifndef __VXWORKS__
+ T *msg = static_cast<T *>(aos_queue_get_msg(queue_));
+ if (msg == NULL) {
+ return false;
+ }
+ static_assert(sizeof(*msg) == sizeof(t_), "something is wrong here");
+ memcpy(msg, &t_, sizeof(t_));
+ msg->set_time = ::aos::time::Time::Now().ToTimespec();
+
+ return aos_queue_write_msg_free(queue_, msg, OVERRIDE) == 0;
+#else
+ return true;
+#endif
+}
+#ifdef __VXWORKS__
+template<typename T> T QueueHolderNoBuilder<T>::t_;
+#endif
+template<typename T> class QueueHolder : public QueueHolderNoBuilder<T> {
+ QueueBuilder<T> builder_;
+ public:
+#ifndef __VXWORKS__
+ explicit QueueHolder(aos_queue *queue) : QueueHolderNoBuilder<T>(queue),
+ builder_(*this) {}
+#else
+ QueueHolder() : builder_(*this) {}
+#endif
+ // Clears the current Goal and returns an object that allows setting various
+ // fields with chained method calls and then calling Send() on it.
+ QueueBuilder<T> &Builder() {
+ QueueHolderNoBuilder<T>::Clear();
+ return builder_;
+ }
+};
+
+} // namespace aos
+
+#endif
+
diff --git a/aos/common/messages/QueueHolder.swg b/aos/common/messages/QueueHolder.swg
new file mode 100644
index 0000000..816e297
--- /dev/null
+++ b/aos/common/messages/QueueHolder.swg
@@ -0,0 +1,9 @@
+namespace aos {
+
+template<class T> class QueueBuilder {
+ public:
+ bool Send();
+};
+
+} // namespace aos
+
diff --git a/aos/common/messages/RobotState.q b/aos/common/messages/RobotState.q
new file mode 100644
index 0000000..32baa05
--- /dev/null
+++ b/aos/common/messages/RobotState.q
@@ -0,0 +1,14 @@
+package aos;
+
+message RobotState {
+ bool enabled;
+ bool autonomous;
+ uint16_t team_id;
+};
+
+// The robot_state Queue is checked by all control loops to make sure that the
+// joystick code hasn't died.
+// It also provides information about whether or not the robot is in autonomous
+// mode and what the team_id is.
+
+queue RobotState robot_state;
diff --git a/aos/common/messages/messages.gyp b/aos/common/messages/messages.gyp
new file mode 100644
index 0000000..94508c4
--- /dev/null
+++ b/aos/common/messages/messages.gyp
@@ -0,0 +1,45 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'aos_queues',
+ 'type': 'static_library',
+ 'sources': [
+ 'RobotState.q',
+ ],
+ 'variables': {
+ 'header_path': 'aos/common/messages',
+ },
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:aos_internal_nolib',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/build/aos.gyp:aos_internal_nolib',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'includes': ['../../build/queues.gypi'],
+ },
+ {
+ 'target_name': 'queues_so',
+ 'type': 'shared_library',
+ 'sources': [
+ 'RobotState.q',
+ ],
+ 'variables': {
+ 'header_path': 'aos/common/messages',
+ },
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:aos_internal_nolib',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/build/aos.gyp:aos_internal_nolib',
+ ],
+ 'direct_dependent_settings': {
+ 'variables': {
+ 'jni_libs': ['queues_so'],
+ },
+ },
+ 'includes': ['../../build/queues.gypi'],
+ },
+ ],
+}
diff --git a/aos/common/mutex.h b/aos/common/mutex.h
new file mode 100644
index 0000000..c1d6f95
--- /dev/null
+++ b/aos/common/mutex.h
@@ -0,0 +1,70 @@
+#ifndef AOS_COMMON_MUTEX_H_
+#define AOS_COMMON_MUTEX_H_
+
+#ifdef __VXWORKS__
+#include <semLib.h>
+#endif
+
+#include "aos/aos_core.h"
+#include "aos/common/macros.h"
+#include "aos/common/type_traits.h"
+
+namespace aos {
+
+// An abstraction of a mutex that has implementations both for the
+// atom and for the cRIO.
+// If there are multiple tasks or processes contending for the mutex,
+// higher priority ones will succeed in locking first,
+// and tasks of equal priorities have the same chance of getting the lock.
+// There is no priority inversion protection.
+class Mutex {
+ public:
+ // Creates an unlocked mutex.
+ Mutex();
+#ifdef __VXWORKS__
+ // Will not make sure that it is either locked or unlocked.
+ ~Mutex();
+#endif
+ // Locks the mutex. If it fails, it calls LOG(FATAL).
+ void Lock();
+ // Unlocks the mutex. Fails like Lock.
+ // Multiple unlocking might be considered failure.
+ void Unlock();
+ // Locks the mutex unless it is already locked.
+ // Returns whether it succeeded or not.
+ // Doesn't wait for the mutex to be unlocked if it is locked.
+ bool TryLock();
+
+ private:
+#ifdef __VXWORKS__
+ typedef SEM_ID ImplementationType;
+#else
+ typedef mutex ImplementationType;
+#endif
+ ImplementationType impl_;
+#ifdef __VXWORKS__
+ DISALLOW_COPY_AND_ASSIGN(Mutex);
+#endif
+};
+
+// A class that locks a Mutex when constructed and unlocks it when destructed.
+// Designed to be used as a local variable so that
+// the mutex will be unlocked when the scope is exited.
+// Should it fail for some reason, it dies with LOG(FATAL).
+class MutexLocker {
+ public:
+ explicit MutexLocker(Mutex *mutex) : mutex_(mutex) {
+ mutex_->Lock();
+ }
+ ~MutexLocker() {
+ mutex_->Unlock();
+ }
+
+ private:
+ Mutex *mutex_;
+ DISALLOW_COPY_AND_ASSIGN(MutexLocker);
+};
+
+} // namespace aos
+
+#endif // AOS_COMMON_MUTEX_H_
diff --git a/aos/common/mutex_test.cpp b/aos/common/mutex_test.cpp
new file mode 100644
index 0000000..a5d5e86
--- /dev/null
+++ b/aos/common/mutex_test.cpp
@@ -0,0 +1,55 @@
+#include "aos/common/mutex.h"
+
+#include "gtest/gtest.h"
+
+namespace aos {
+namespace testing {
+
+class MutexTest : public ::testing::Test {
+ public:
+ Mutex test_mutex;
+};
+
+typedef MutexTest MutexDeathTest;
+
+TEST_F(MutexTest, TryLock) {
+ EXPECT_TRUE(test_mutex.TryLock());
+ EXPECT_FALSE(test_mutex.TryLock());
+}
+
+TEST_F(MutexTest, Lock) {
+ test_mutex.Lock();
+ EXPECT_FALSE(test_mutex.TryLock());
+}
+
+TEST_F(MutexTest, Unlock) {
+ test_mutex.Lock();
+ EXPECT_FALSE(test_mutex.TryLock());
+ test_mutex.Unlock();
+ EXPECT_TRUE(test_mutex.TryLock());
+}
+
+#ifndef __VXWORKS__
+// Sees what happens with multiple unlocks.
+TEST_F(MutexDeathTest, RepeatUnlock) {
+ test_mutex.Lock();
+ test_mutex.Unlock();
+ EXPECT_DEATH(test_mutex.Unlock(), ".*multiple unlock.*");
+}
+
+// Sees what happens if you unlock without ever locking (or unlocking) it.
+TEST_F(MutexDeathTest, NeverLock) {
+ EXPECT_DEATH(test_mutex.Unlock(), ".*multiple unlock.*");
+}
+#endif
+
+TEST_F(MutexTest, MutexLocker) {
+ {
+ aos::MutexLocker locker(&test_mutex);
+ EXPECT_FALSE(test_mutex.TryLock());
+ }
+ EXPECT_TRUE(test_mutex.TryLock());
+}
+
+} // namespace testing
+} // namespace aos
diff --git a/aos/common/network/ReceiveSocket.cpp b/aos/common/network/ReceiveSocket.cpp
new file mode 100644
index 0000000..671f966
--- /dev/null
+++ b/aos/common/network/ReceiveSocket.cpp
@@ -0,0 +1,32 @@
+#include "ReceiveSocket.h"
+#include <string.h>
+#include <math.h>
+#include <errno.h>
+#include <stdint.h>
+#include <stddef.h>
+
+#include "aos/common/network/SocketLibraries.h"
+#include "aos/aos_core.h"
+
+namespace aos {
+
+static const char *localhost = "0.0.0.0";
+
+int ReceiveSocket::Connect(NetworkPort port) {
+ Reset();
+ const int ret = Socket::Connect(port, localhost);
+ if (ret != 0) {
+ return ret;
+ }
+
+ if (bind(socket_, &addr_.addr,
+ sizeof(addr_)) == -1) {
+ LOG(ERROR, "failed to bind to address '%s' because of %d: %s\n", localhost,
+ errno, strerror(errno));
+ return last_ret_ = -1;
+ }
+ return last_ret_ = 0;
+}
+
+} // namespace aos
+
diff --git a/aos/common/network/ReceiveSocket.h b/aos/common/network/ReceiveSocket.h
new file mode 100644
index 0000000..bd834c9
--- /dev/null
+++ b/aos/common/network/ReceiveSocket.h
@@ -0,0 +1,19 @@
+#ifndef AOS_NETWORK_RECEIVE_SOCKET_H_
+#define AOS_NETWORK_RECEIVE_SOCKET_H_
+
+#include "Socket.h"
+
+namespace aos {
+
+class ReceiveSocket : public Socket {
+ public:
+ inline ReceiveSocket(NetworkPort port) { Connect(port); }
+ int Connect(NetworkPort port);
+
+ inline int Recv(void *buf, int length) { return Socket::Recv(buf, length); }
+ inline int Recv(void *buf, int length, long usec) { return Socket::Recv(buf, length, usec); }
+};
+
+} // namespace aos
+
+#endif
diff --git a/aos/common/network/SendSocket.cpp b/aos/common/network/SendSocket.cpp
new file mode 100644
index 0000000..89665dc
--- /dev/null
+++ b/aos/common/network/SendSocket.cpp
@@ -0,0 +1,42 @@
+#include "aos/common/network/SendSocket.h"
+
+#include <string.h>
+#include <errno.h>
+#include <stdint.h>
+#include <stddef.h>
+#include <math.h>
+
+#include "aos/aos_core.h"
+#include "aos/common/network/SocketLibraries.h"
+
+namespace aos {
+
+int SendSocket::Connect(NetworkPort port, const char *robot_ip, int type) {
+ Reset();
+ const int ret = Socket::Connect(port, robot_ip, type);
+ if (ret != 0) {
+ return ret;
+ }
+
+ if (connect(socket_, &addr_.addr,
+ sizeof(addr_)) < 0) {
+ LOG(ERROR, "couldn't connect to ip '%s' because of %d: %s\n", robot_ip,
+ errno, strerror(errno));
+ last_ret_ = 1;
+ }
+
+ hold_msg_len_ = 0;
+ return last_ret_ = 0;
+}
+
+int SendSocket::SendHoldMsg() {
+ if (hold_msg_len_ > 0) {
+ int val = Send(hold_msg_, hold_msg_len_);
+ hold_msg_len_ = 0;
+ return val;
+ }
+ return -1;
+}
+
+} // namespace aos
+
diff --git a/aos/common/network/SendSocket.h b/aos/common/network/SendSocket.h
new file mode 100644
index 0000000..6d3feb1
--- /dev/null
+++ b/aos/common/network/SendSocket.h
@@ -0,0 +1,28 @@
+#ifndef AOS_NETWORK_SEND_SOCKET_H_
+#define AOS_NETWORK_SEND_SOCKET_H_
+
+#include "Socket.h"
+
+namespace aos {
+
+class SendSocket : public Socket {
+ public:
+ //inline int Send(const void *buf, int length) { return Socket::Send(buf, length); }
+ // Connect must be called before use.
+ SendSocket() {}
+ // Calls Connect automatically.
+ SendSocket(NetworkPort port, const char *robot_ip) {
+ Connect(port, robot_ip);
+ }
+ int Connect(NetworkPort port, const char *robot_ip, int type = SOCK_DGRAM);
+
+ static const size_t MAX_MSG = 4096;
+ char hold_msg_[MAX_MSG];
+ size_t hold_msg_len_;
+ int SendHoldMsg();
+};
+
+} // namespace aos
+
+#endif
+
diff --git a/aos/common/network/Socket.cpp b/aos/common/network/Socket.cpp
new file mode 100644
index 0000000..b01bcb8
--- /dev/null
+++ b/aos/common/network/Socket.cpp
@@ -0,0 +1,77 @@
+#include "aos/common/network/Socket.h"
+
+#include <string.h>
+#include <errno.h>
+
+#include "aos/aos_core.h"
+#include "aos/common/network/SocketLibraries.h"
+
+namespace aos {
+
+int Socket::Connect(NetworkPort port, const char *address, int type) {
+ last_ret_ = 0;
+ if ((socket_ = socket(AF_INET, type, 0)) < 0) {
+ LOG(ERROR, "failed to create socket because of %d: %s\n", errno, strerror(errno));
+ return last_ret_ = 1;
+ }
+
+ memset(&addr_, 0, sizeof(addr_));
+ addr_.in.sin_family = AF_INET;
+ addr_.in.sin_port = htons(static_cast<uint16_t>(port));
+#ifndef __VXWORKS__
+ const int failure_return = 0;
+#else
+ const int failure_return = -1;
+#endif
+ if (inet_aton(lame_unconst(address), &addr_.in.sin_addr) == failure_return) {
+ LOG(ERROR, "Invalid IP address '%s' because of %d: %s\n", address,
+ errno, strerror(errno));
+ return last_ret_ = -1;
+ }
+
+ return last_ret_ = 0;
+}
+Socket::Socket() : socket_(-1), last_ret_(2) {}
+Socket::~Socket() {
+ close(socket_);
+}
+
+void Socket::Reset() {
+ if (socket_ != -1) {
+ close(socket_);
+ socket_ = -1;
+ }
+ last_ret_ = 0;
+}
+
+int Socket::Recv(void *buf, int length) {
+ const int ret = recv(socket_, static_cast<char *>(buf), length, 0);
+ last_ret_ = (ret == -1) ? -1 : 0;
+ return ret;
+}
+int Socket::Recv(void *buf, int length, long usec) {
+ timeval tv;
+ tv.tv_sec = 0;
+ tv.tv_usec = usec;
+ fd_set fds;
+ FD_ZERO(&fds);
+ FD_SET(socket_, &fds);
+ switch (select(FD_SETSIZE, &fds, NULL, NULL, &tv)) {
+ case 1:
+ return Recv(buf, length);
+ case 0:
+ return last_ret_ = 0;
+ default:
+ perror("select on socket to receive from failed");
+ return last_ret_ = -1;
+ }
+}
+int Socket::Send(const void *buf, int length) {
+ const int ret = write(socket_,
+ lame_unconst(static_cast<const char *>(buf)), length);
+ //const int ret = length;
+ last_ret_ = (ret == -1) ? -1 : 0;
+ return ret;
+}
+
+} // namespace aos
diff --git a/aos/common/network/Socket.h b/aos/common/network/Socket.h
new file mode 100644
index 0000000..ceee754
--- /dev/null
+++ b/aos/common/network/Socket.h
@@ -0,0 +1,42 @@
+#ifndef AOS_NETWORK_SOCKET_H_
+#define AOS_NETWORK_SOCKET_H_
+
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <stdio.h>
+#include "aos/common/Configuration.h"
+
+namespace aos {
+
+class Socket {
+ public:
+ int LastStatus() const { return last_ret_; }
+
+ int Send(const void *buf, int length);
+ int Recv(void *buf, int length);
+ int Recv(void *buf, int length, long usec); // returns 0 if timed out
+ protected:
+ int Connect(NetworkPort port, const char *address, int type = SOCK_DGRAM);
+ Socket();
+ ~Socket();
+
+ // Resets socket_ and last_ret_.
+ void Reset();
+
+ union {
+ sockaddr_in in;
+ sockaddr addr;
+ } addr_; // filled in by Connect
+
+ int socket_;
+ int last_ret_;
+};
+
+} // namespace aos
+
+#endif
+
diff --git a/aos/common/network/SocketLibraries.h b/aos/common/network/SocketLibraries.h
new file mode 100644
index 0000000..7e8825a
--- /dev/null
+++ b/aos/common/network/SocketLibraries.h
@@ -0,0 +1,16 @@
+// Includes the socket libraries under vxworks and linux.
+// Defines a lame_unconst macro for the vxworks functions that need it.
+
+#ifndef __VXWORKS__
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/select.h>
+#define lame_unconst(a) a
+#else
+#include <inetLib.h>
+#include <sockLib.h>
+#include <selectLib.h>
+// Vxworks is missing the const in a couple of its function signatures, so...
+#define lame_unconst(a) const_cast<char *>(a)
+#endif
+
diff --git a/aos/common/network/network.gyp b/aos/common/network/network.gyp
new file mode 100644
index 0000000..ca3e653
--- /dev/null
+++ b/aos/common/network/network.gyp
@@ -0,0 +1,56 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'socket_so',
+ 'type': 'shared_library',
+ 'variables': {'no_rsync': 1},
+ 'sources': [
+ 'ReceiveSocket.cpp',
+ 'SendSocket.cpp',
+ 'Socket.cpp',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:aos_shared_lib',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/build/aos.gyp:aos_shared_lib',
+ ],
+ 'conditions': [
+ ['OS=="crio"', {
+ 'dependencies': [
+ '<(EXTERNALS):WPILib',
+ ]}
+ ],
+ ],
+ 'direct_dependent_settings': {
+ 'variables': {
+ 'jni_libs': [
+ 'socket_so',
+ ],
+ },
+ },
+ },
+ {
+ 'target_name': 'socket',
+ 'type': 'static_library',
+ 'sources': [
+ 'ReceiveSocket.cpp',
+ 'SendSocket.cpp',
+ 'Socket.cpp',
+ ],
+ 'conditions': [
+ ['OS=="crio"', {
+ 'dependencies': [
+ '<(EXTERNALS):WPILib',
+ ]}
+ ],
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:logging',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/build/aos.gyp:logging',
+ ],
+ },
+ ],
+}
diff --git a/aos/common/queue.cc b/aos/common/queue.cc
new file mode 100644
index 0000000..7e7f2a1
--- /dev/null
+++ b/aos/common/queue.cc
@@ -0,0 +1,37 @@
+#include "aos/common/queue.h"
+
+#include "aos/common/byteorder.h"
+#include "aos/common/inttypes.h"
+
+namespace aos {
+
+void Message::Zero() {
+ sent_time.set_sec(0);
+ sent_time.set_nsec(0);
+}
+
+size_t Message::Deserialize(const char *buffer) {
+ int32_t sec;
+ int32_t nsec;
+ to_host(&buffer[0], &sec);
+ to_host(&buffer[4], &nsec);
+ sent_time.set_sec(sec);
+ sent_time.set_nsec(nsec);
+ return Size();
+}
+// Serializes the common fields into the buffer.
+size_t Message::Serialize(char *buffer) const {
+ // TODO(aschuh): to_network shouldn't need a pointer.
+ int32_t sec = sent_time.sec();
+ int32_t nsec = sent_time.nsec();
+ to_network(&sec, &buffer[0]);
+ to_network(&nsec, &buffer[4]);
+ return Size();
+}
+
+size_t Message::Print(char *buffer, int length) const {
+ return snprintf(buffer, length, "%"PRId32".%09"PRId32"s",
+ sent_time.sec(), sent_time.nsec());
+}
+
+} // namespace aos
diff --git a/aos/common/queue.h b/aos/common/queue.h
new file mode 100644
index 0000000..7b38e67
--- /dev/null
+++ b/aos/common/queue.h
@@ -0,0 +1,313 @@
+#ifndef AOS_COMMON_QUEUE_H_
+#define AOS_COMMON_QUEUE_H_
+
+#include <assert.h>
+
+#if defined(__VXWORKS__) || defined(__TEST_VXWORKS__)
+#define USE_UNSAFE
+#else
+#undef USE_UNSAFE
+#endif
+
+#include "aos/aos_core.h"
+#include "aos/common/macros.h"
+#ifndef USE_UNSAFE
+#include "aos/atom_code/ipc_lib/queue.h"
+#endif // USE_UNSAFE
+#include "aos/common/time.h"
+
+
+namespace aos {
+
+// This class is a base class for all messages sent over queues.
+class Message {
+ public:
+ typedef ::aos::time::Time Time;
+ // The time that the message was sent at.
+ Time sent_time;
+
+ Message() : sent_time(0, 0) {}
+
+ // Zeros out the time.
+ void Zero();
+ // Returns the size of the message in bytes.
+ static size_t Size() { return sizeof(Time); }
+
+ // Deserializes the common fields from the buffer.
+ size_t Deserialize(const char *buffer);
+ // Serializes the common fields into the buffer.
+ size_t Serialize(char *buffer) const;
+
+ // Populates sent_time with the current time.
+ void SetTimeToNow() { sent_time = Time::Now(); }
+
+ // Writes the contents of the message to the provided buffer.
+ size_t Print(char *buffer, int length) const;
+};
+
+template <class T> class Queue;
+template <class T> class MessageBuilder;
+template <class T> class ScopedMessagePtr;
+#ifndef USE_UNSAFE
+template <class T> class SafeMessageBuilder;
+template <class T> class SafeScopedMessagePtr;
+#endif // USE_UNSAFE
+
+// A ScopedMessagePtr<> manages a queue message pointer.
+// It frees it properly when the ScopedMessagePtr<> goes out of scope or gets
+// sent. By design, there is no way to get the ScopedMessagePtr to release the
+// message pointer.
+template <class T>
+class ScopedMessagePtr {
+ public:
+ // Returns a pointer to the message.
+ // This stays valid until Send or the destructor have been called.
+ const T *get() const { return msg_; }
+ T *get() { return msg_; }
+
+ const T &operator*() const {
+ const T *msg = get();
+ assert(msg != NULL);
+ return *msg;
+ }
+
+ T &operator*() {
+ T *msg = get();
+ assert(msg != NULL);
+ return *msg;
+ }
+
+ const T *operator->() const {
+ const T *msg = get();
+ assert(msg != NULL);
+ return msg;
+ }
+
+ T *operator->() {
+ T *msg = get();
+ assert(msg != NULL);
+ return msg;
+ }
+
+ operator bool() {
+ return msg_ != NULL;
+ }
+
+ // Sends the message and removes our reference to it.
+ // If the queue is full, over-ride the oldest message in it with our new
+ // message.
+ // Returns true on success, and false otherwise.
+ // The message will be freed.
+ bool Send();
+
+ // Sends the message and removes our reference to it.
+ // If the queue is full, block until it is no longer full.
+ // Returns true on success, and false otherwise.
+ // The message will be freed.
+ bool SendBlocking();
+
+ // Frees the contained message.
+ ~ScopedMessagePtr() {
+ reset();
+ }
+
+#ifndef SWIG
+ // Implements a move constructor. This only takes rvalue references
+ // because we want to allow someone to say
+ // ScopedMessagePtr<X> ptr = queue.MakeMessage();
+ // but we don't want to allow them to then say
+ // ScopedMessagePtr<X> new_ptr = ptr;
+ // And, if they do actually want to copy the pointer, then it will correctly
+ // clear out the source so there aren't 2 pointers to the message lying
+ // around.
+ ScopedMessagePtr(ScopedMessagePtr<T> &&ptr)
+ :
+#ifndef USE_UNSAFE
+ queue_(ptr.queue_),
+#endif // USE_UNSAFE
+ msg_(ptr.msg_) {
+ ptr.msg_ = NULL;
+ }
+#endif // SWIG
+
+ private:
+ // Provide access to set_queue and the constructor for init.
+ friend class aos::Queue<typename std::remove_const<T>::type>;
+ // Provide access to the copy constructor for MakeWithBuilder.
+ friend class aos::MessageBuilder<T>;
+
+#ifndef USE_UNSAFE
+ // Only Queue should be able to build a queue.
+ ScopedMessagePtr(aos_queue *queue, T *msg)
+ : queue_(queue), msg_(msg) {}
+#else
+ ScopedMessagePtr(T *msg)
+ : msg_(msg) {}
+#endif // USE_UNSAFE
+
+ // Sets the pointer to msg, freeing the old value if it was there.
+ // This is private because nobody should be able to get a pointer to a message
+ // that needs to be scoped without using the queue.
+ void reset(T *msg = NULL);
+
+#ifndef USE_UNSAFE
+ // Sets the queue that owns this message.
+ void set_queue(aos_queue *queue) { queue_ = queue; }
+
+ // The queue that the message is a part of.
+ aos_queue *queue_;
+#endif // USE_UNSAFE
+ // The message or NULL.
+ T *msg_;
+
+ // Protect evil constructors.
+ DISALLOW_COPY_AND_ASSIGN(ScopedMessagePtr<T>);
+};
+
+// Specializations for the Builders will be automatically generated in the .q.h
+// header files with all of the handy builder methods.
+// This builder uses an actual shm message pointer, which is more efficient and
+// more dangerous than the linux only SafeMessageBuilder.
+template <class T>
+class MessageBuilder {
+ public:
+ typedef T Message;
+ bool Send();
+};
+
+// TODO(aschuh): Base class
+// T must be a Message with the same format as the messages generated by
+// the q files.
+template <class T>
+class Queue {
+ public:
+ typedef T Message;
+
+ Queue(const char *queue_name)
+ : queue_name_(queue_name),
+#ifdef USE_UNSAFE
+ queue_msg_(&msg_)
+#else
+ queue_(NULL),
+ queue_msg_(NULL, NULL)
+#endif // USE_UNSAFE
+ {
+ static_assert(shm_ok<T>::value,
+ "The provided message type can't be put in shmem.");
+ }
+
+ // Initializes the queue. This may optionally be called to do any one time
+ // work before sending information, and may be be called mulitple times.
+ // Init will be called when a message is sent, but this will cause sending to
+ // take a different amount of time the first cycle.
+ void Init();
+
+ // Fetches the next message from the queue.
+ // Returns true if there was a new message available and we successfully
+ // fetched it. This removes the message from the queue for all readers.
+ // TODO(aschuh): Fix this to use a different way of fetching messages so other
+ // readers can also FetchNext.
+ bool FetchNext();
+ bool FetchNextBlocking();
+
+ // Fetches the last message from the queue.
+ // Returns true if there was a new message available and we successfully
+ // fetched it.
+ bool FetchLatest();
+
+ // Returns the age of the message.
+ const time::Time Age() { return time::Time::Now() - queue_msg_->sent_time; }
+
+ // Returns true if the latest value in the queue is newer than age mseconds.
+ bool IsNewerThanMS(int age) {
+ // TODO(aschuh): Log very verbosely if something is _ever_ stale;
+ if (get() != NULL) {
+ return Age() < time::Time::InMS(age);
+ } else {
+ return false;
+ }
+ }
+
+ // Returns a pointer to the current message.
+ // This pointer will be valid until a new message is fetched.
+ const T *get() const { return queue_msg_.get(); }
+
+ // Returns a reference to the message.
+ // The message will be valid until a new message is fetched.
+ const T &operator*() const {
+ const T *msg = get();
+ assert(msg != NULL);
+ return *msg;
+ }
+
+ // Returns a pointer to the current message.
+ // This pointer will be valid until a new message is fetched.
+ const T *operator->() const {
+ const T *msg = get();
+ assert(msg != NULL);
+ return msg;
+ }
+
+#ifndef USE_UNSAFE
+ // Returns a scoped_ptr containing a message.
+ // GCC will optimize away the copy constructor, so this is safe.
+ SafeScopedMessagePtr<T> SafeMakeMessage();
+
+ // Returns a message builder that contains a pre-allocated message.
+ aos::SafeMessageBuilder<T> SafeMakeWithBuilder();
+#endif // USE_UNSAFE
+
+#ifndef SWIG
+ // Returns a scoped_ptr containing a message.
+ // GCC will optimize away the copy constructor, so this is safe.
+ ScopedMessagePtr<T> MakeMessage();
+
+ // Returns a message builder that contains a pre-allocated message.
+ aos::MessageBuilder<T> MakeWithBuilder();
+#endif // SWIG
+
+ const char *name() const { return queue_name_; }
+
+ private:
+ const char *queue_name_;
+
+#ifdef USE_UNSAFE
+ // The unsafe queue only has 1 entry and no safety, so 1 message is fine.
+ T msg_;
+#else
+ T *MakeRawMessage();
+ // Pointer to the queue that this object fetches from.
+ aos_queue *queue_;
+#endif
+ // Scoped pointer holding the latest message or NULL.
+ ScopedMessagePtr<const T> queue_msg_;
+ DISALLOW_COPY_AND_ASSIGN(Queue<T>);
+};
+
+// Base class for all queue groups.
+class QueueGroup {
+ public:
+ // Constructs a queue group given its name and a unique hash of the name and
+ // type.
+ QueueGroup(const char *name, uint32_t hash) : name_(name), hash_(hash) {}
+
+ // Returns the name of the queue group.
+ const char *name() const { return name_.c_str(); }
+ // Returns a unique hash representing this instance of the queue group.
+ uint32_t hash() const { return hash_; }
+
+ private:
+ std::string name_;
+ uint32_t hash_;
+};
+
+} // namespace aos
+
+#ifdef USE_UNSAFE
+#include "aos/crio/queue-tmpl.h"
+#else
+#include "aos/atom_code/queue-tmpl.h"
+#endif
+#undef USE_UNSAFE
+
+#endif // AOS_COMMON_QUEUE_H_
diff --git a/aos/common/queue_test.cc b/aos/common/queue_test.cc
new file mode 100644
index 0000000..b24fcd5
--- /dev/null
+++ b/aos/common/queue_test.cc
@@ -0,0 +1,327 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/test_queue.q.h"
+#include "aos/common/queue_testutils.h"
+
+
+using ::aos::time::Time;
+
+namespace aos {
+namespace common {
+
+// TODO(aschuh): Pull this out somewhere and test it.
+class Thread {
+ public:
+ Thread () {
+ started_ = false;
+ joined_ = false;
+ }
+
+ ~Thread() {
+ if (!joined_ && started_) {
+ assert(false);
+ }
+ }
+
+ void Start() {
+ assert(!started_);
+ pthread_create(&thread_, NULL, &Thread::StaticRun, this);
+ started_ = true;
+ }
+
+ virtual void Run() = 0;
+
+ void Join() {
+ assert(!joined_);
+ pthread_join(thread_, NULL);
+ joined_ = true;
+ }
+ private:
+ static void *StaticRun(void *thread_class) {
+ static_cast<Thread *>(thread_class)->Run();
+ return NULL;
+ }
+
+ pthread_t thread_;
+ bool started_;
+ bool joined_;
+
+ DISALLOW_COPY_AND_ASSIGN(Thread);
+};
+
+namespace testing {
+
+class QueueTest : public ::testing::Test {
+ protected:
+ GlobalCoreInstance my_core;
+ // Create a new instance of the test queue so that it invalidates the queue
+ // that it points to. Otherwise, we will have a pointer to shared memory that
+ // is no longer valid.
+ ::aos::Queue<TestingMessage> my_test_queue;
+
+ QueueTest() : my_test_queue(".aos.common.testing.test_queue") {}
+};
+
+class MyThread : public Thread {
+ public:
+ MyThread() : threaded_test_queue(".aos.common.testing.test_queue") {}
+
+ virtual void Run() {
+ ASSERT_TRUE(threaded_test_queue.FetchNextBlocking());
+ EXPECT_TRUE(threaded_test_queue->test_bool);
+ EXPECT_EQ(0x971, threaded_test_queue->test_int);
+ }
+
+ ::aos::Queue<TestingMessage> threaded_test_queue;
+ private:
+ DISALLOW_COPY_AND_ASSIGN(MyThread);
+};
+
+
+// Tests that we can send a message to another thread and it blocking receives
+// it at the correct time.
+TEST_F(QueueTest, FetchBlocking) {
+ MyThread t;
+ t.Start();
+ usleep(50000);
+ my_test_queue.MakeWithBuilder().test_bool(true).test_int(0x971).Send();
+ t.Join();
+ EXPECT_EQ(true, t.threaded_test_queue.IsNewerThanMS(20));
+}
+
+// Tests that we can send a message with the message pointer and get it back.
+TEST_F(QueueTest, SendMessage) {
+ ScopedMessagePtr<TestingMessage> msg = my_test_queue.MakeMessage();
+ msg->test_bool = true;
+ msg->test_int = 0x971;
+ msg.Send();
+
+ ASSERT_TRUE(my_test_queue.FetchLatest());
+ EXPECT_TRUE(my_test_queue->test_bool);
+ EXPECT_EQ(0x971, my_test_queue->test_int);
+}
+
+// Tests that we can send a message with the message pointer and get it back.
+TEST_F(QueueTest, SendMessageBlockingSafe) {
+ SafeScopedMessagePtr<TestingMessage> msg = my_test_queue.SafeMakeMessage();
+ msg->test_bool = true;
+ msg->test_int = 0x971;
+ ASSERT_TRUE(msg.SendBlocking());
+
+ ASSERT_TRUE(my_test_queue.FetchLatest());
+ EXPECT_TRUE(my_test_queue->test_bool);
+ EXPECT_EQ(0x971, my_test_queue->test_int);
+}
+
+// Tests that we can send a message with the message pointer and get it back.
+TEST_F(QueueTest, SendMessageSafe) {
+ SafeScopedMessagePtr<TestingMessage> msg = my_test_queue.SafeMakeMessage();
+ msg->test_bool = true;
+ msg->test_int = 0x971;
+ msg.Send();
+
+ ASSERT_TRUE(my_test_queue.FetchLatest());
+ EXPECT_TRUE(my_test_queue->test_bool);
+ EXPECT_EQ(0x971, my_test_queue->test_int);
+}
+
+// Tests that we can send a message with the builder and get it back.
+TEST_F(QueueTest, SendWithBuilder) {
+ my_test_queue.MakeWithBuilder().test_bool(true).test_int(0x971).Send();
+
+ ASSERT_TRUE(my_test_queue.FetchLatest());
+ EXPECT_EQ(true, my_test_queue->test_bool);
+ EXPECT_EQ(0x971, my_test_queue->test_int);
+ EXPECT_EQ(true, my_test_queue.IsNewerThanMS(10000));
+}
+
+// Tests that various pointer deref functions at least seem to work.
+TEST_F(QueueTest, PointerDeref) {
+ my_test_queue.MakeWithBuilder().test_bool(true).test_int(0x971).Send();
+
+ ASSERT_TRUE(my_test_queue.FetchLatest());
+ const TestingMessage *msg_ptr = my_test_queue.get();
+ ASSERT_NE(static_cast<TestingMessage*>(NULL), msg_ptr);
+ EXPECT_EQ(0x971, msg_ptr->test_int);
+ EXPECT_EQ(msg_ptr, &(*my_test_queue));
+}
+
+// Tests that FetchNext doesn't miss any messages.
+TEST_F(QueueTest, FetchNext) {
+ for (int i = 0; i < 10; ++i) {
+ my_test_queue.MakeWithBuilder().test_bool(true).test_int(i).Send();
+ }
+
+ for (int i = 0; i < 10; ++i) {
+ ASSERT_TRUE(my_test_queue.FetchNext());
+ EXPECT_EQ(i, my_test_queue->test_int);
+ }
+}
+
+// Tests that FetchLatest skips a missing message.
+TEST_F(QueueTest, FetchLatest) {
+ my_test_queue.MakeWithBuilder().test_bool(true).test_int(0x254).Send();
+ my_test_queue.MakeWithBuilder().test_bool(true).test_int(0x971).Send();
+
+ ASSERT_TRUE(my_test_queue.FetchLatest());
+ EXPECT_EQ(0x971, my_test_queue->test_int);
+}
+
+// Tests that FetchLatest works with multiple readers.
+TEST_F(QueueTest, FetchLatestMultiple) {
+ ::aos::Queue<TestingMessage> my_second_test_queue(
+ ".aos.common.testing.test_queue");
+ my_test_queue.MakeWithBuilder().test_bool(true).test_int(0x254).Send();
+ my_test_queue.MakeWithBuilder().test_bool(true).test_int(0x971).Send();
+
+ ASSERT_TRUE(my_test_queue.FetchLatest());
+ EXPECT_EQ(0x971, my_test_queue->test_int);
+ ASSERT_TRUE(my_second_test_queue.FetchLatest());
+ ASSERT_TRUE(my_second_test_queue.get() != NULL);
+ EXPECT_EQ(0x971, my_second_test_queue->test_int);
+}
+
+
+// Tests that fetching without a new message returns false.
+TEST_F(QueueTest, FetchLatestWithoutMessage) {
+ my_test_queue.MakeWithBuilder().test_bool(true).test_int(0x254).Send();
+ EXPECT_TRUE(my_test_queue.FetchLatest());
+ EXPECT_FALSE(my_test_queue.FetchLatest());
+ EXPECT_FALSE(my_test_queue.FetchLatest());
+ EXPECT_EQ(0x254, my_test_queue->test_int);
+}
+
+// Tests that fetching without a message returns false.
+TEST_F(QueueTest, FetchOnFreshQueue) {
+ EXPECT_FALSE(my_test_queue.FetchLatest());
+ EXPECT_EQ(static_cast<TestingMessage*>(NULL), my_test_queue.get());
+}
+
+// Tests that fetch next without a message returns false.
+TEST_F(QueueTest, FetchNextOnFreshQueue) {
+ EXPECT_FALSE(my_test_queue.FetchNext());
+ EXPECT_EQ(static_cast<TestingMessage*>(NULL), my_test_queue.get());
+}
+
+// Tests that fetch next without a new message returns false.
+TEST_F(QueueTest, FetchNextWithoutMessage) {
+ my_test_queue.MakeWithBuilder().test_bool(true).test_int(0x254).Send();
+ EXPECT_TRUE(my_test_queue.FetchNext());
+ EXPECT_FALSE(my_test_queue.FetchNext());
+ EXPECT_NE(static_cast<TestingMessage*>(NULL), my_test_queue.get());
+}
+
+// Tests that age makes some sense.
+TEST_F(QueueTest, Age) {
+ my_test_queue.MakeWithBuilder().test_bool(true).test_int(0x971).Send();
+
+ ASSERT_TRUE(my_test_queue.FetchLatest());
+ EXPECT_TRUE(my_test_queue.IsNewerThanMS(100));
+ const Time age = my_test_queue.Age();
+ EXPECT_EQ(0, age.sec());
+ EXPECT_GE(100000000, age.nsec());
+}
+
+
+class GroupTest : public ::testing::Test {
+ protected:
+ GlobalCoreInstance my_core;
+ // Create a new instance of the test group so that it invalidates the queue
+ // that it points to. Otherwise, we will have a pointer to shared memory that
+ // is no longer valid.
+ TwoQueues my_test_queuegroup;
+
+ GroupTest()
+ : my_test_queuegroup(".aos.common.testing.test_queuegroup",
+ 0x20561114,
+ ".aos.common.testing.test_queuegroup.first",
+ ".aos.common.testing.test_queuegroup.second") {}
+};
+
+// Tests that the hash gets preserved.
+TEST_F(GroupTest, Hash) {
+ EXPECT_EQ(static_cast<uint32_t>(0x20561114), my_test_queuegroup.hash());
+}
+
+// Tests that the hash works.
+TEST_F(GroupTest, RealHash) {
+ EXPECT_EQ(static_cast<uint32_t>(0x5b25097f), test_queuegroup.hash());
+}
+
+// Tests that name works.
+TEST_F(GroupTest, Name) {
+ EXPECT_EQ(std::string(".aos.common.testing.test_queuegroup"),
+ std::string(my_test_queuegroup.name()));
+}
+
+
+class MessageTest : public ::testing::Test {
+ public:
+ TestingMessage msg;
+};
+
+TEST_F(MessageTest, Zeroing) {
+ msg.test_bool = true;
+ msg.test_int = 0x254;
+ msg.SetTimeToNow();
+
+ msg.Zero();
+
+ EXPECT_FALSE(msg.test_bool);
+ EXPECT_EQ(0, msg.test_int);
+ EXPECT_EQ(0, msg.sent_time.sec());
+ EXPECT_EQ(0, msg.sent_time.nsec());
+}
+
+TEST_F(MessageTest, Size) {
+ EXPECT_EQ(static_cast<size_t>(13), msg.Size());
+}
+
+TEST_F(MessageTest, Serialize) {
+ char serialized_data[msg.Size()];
+ msg.test_bool = true;
+ msg.test_int = 0x254;
+ msg.SetTimeToNow();
+
+ msg.Serialize(serialized_data);
+
+ TestingMessage new_msg;
+ new_msg.Deserialize(serialized_data);
+
+ EXPECT_EQ(msg.test_bool, new_msg.test_bool);
+ EXPECT_EQ(msg.test_int, new_msg.test_int);
+ EXPECT_EQ(msg.sent_time, new_msg.sent_time);
+}
+
+// Tests that Print prints out a message nicely.
+TEST_F(MessageTest, Print) {
+ char printdata[1024];
+ msg.test_bool = true;
+ msg.test_int = 2056;
+ msg.sent_time = Time(971, 254);
+
+ std::string golden("971.000000254s, t, 2056");
+ EXPECT_EQ(golden.size(), msg.Print(printdata, sizeof(printdata)));
+
+ EXPECT_EQ(golden, std::string(printdata));
+}
+
+// Tests that the hash never changes. If it changes, then someone broke the
+// hash routine or changed the message declaration. Both changes need to be
+// validated by hand.
+TEST_F(MessageTest, Hash) {
+ EXPECT_EQ(static_cast<uint32_t>(0xcf740cc1),
+ static_cast<uint32_t>(TestingMessage::kHash));
+}
+
+TEST_F(MessageTest, SetNow) {
+ msg.SetTimeToNow();
+ EXPECT_LE(msg.sent_time - Time::Now(), Time::InMS(20));
+}
+
+} // namespace testing
+} // namespace common
+} // namespace aos
diff --git a/aos/common/queue_testutils.cc b/aos/common/queue_testutils.cc
new file mode 100644
index 0000000..6cce012
--- /dev/null
+++ b/aos/common/queue_testutils.cc
@@ -0,0 +1,26 @@
+#include "aos/common/queue_testutils.h"
+#include "aos/common/queue.h"
+
+namespace aos {
+namespace common {
+namespace testing {
+
+GlobalCoreInstance::GlobalCoreInstance() {
+ const size_t kCoreSize = 0x100000;
+ global_core = &global_core_data_;
+ global_core->owner = 1;
+ void *memory = malloc(kCoreSize);
+ assert(memory != NULL);
+ memset(memory, 0, kCoreSize);
+
+ assert(aos_core_use_address_as_shared_mem(memory, kCoreSize) == 0);
+}
+
+GlobalCoreInstance::~GlobalCoreInstance() {
+ free(global_core->mem_struct);
+ global_core = NULL;
+}
+
+} // namespace testing
+} // namespace common
+} // namespace aos
diff --git a/aos/common/queue_testutils.h b/aos/common/queue_testutils.h
new file mode 100644
index 0000000..81f1efb
--- /dev/null
+++ b/aos/common/queue_testutils.h
@@ -0,0 +1,18 @@
+#include "aos/common/queue.h"
+
+namespace aos {
+namespace common {
+namespace testing {
+
+class GlobalCoreInstance {
+ public:
+ GlobalCoreInstance();
+ ~GlobalCoreInstance();
+
+ private:
+ struct aos_core global_core_data_;
+};
+
+} // namespace testing
+} // namespace common
+} // namespace aos
diff --git a/aos/common/scoped_fd.h b/aos/common/scoped_fd.h
new file mode 100644
index 0000000..e654d3d
--- /dev/null
+++ b/aos/common/scoped_fd.h
@@ -0,0 +1,38 @@
+#include "aos/common/macros.h"
+
+namespace aos {
+
+// Smart "pointer" (container) for a file descriptor.
+class ScopedFD {
+ public:
+ explicit ScopedFD(int fd = -1) : fd_(fd) {}
+ ~ScopedFD() {
+ Close();
+ }
+ int get() const { return fd_; }
+ int release() {
+ const int r = fd_;
+ fd_ = -1;
+ return r;
+ }
+ void reset(int new_fd = -1) {
+ if (fd_ != new_fd) {
+ Close();
+ fd_ = new_fd;
+ }
+ }
+ operator bool() const { return fd_ != -1; }
+ private:
+ int fd_;
+ void Close() {
+ if (fd_ != -1) {
+ if (close(fd_) == -1) {
+ LOG(WARNING, "close(%d) failed with %d: %s\n", fd_,
+ errno, strerror(errno));
+ }
+ }
+ }
+ DISALLOW_COPY_AND_ASSIGN(ScopedFD);
+};
+
+} // namespace aos
diff --git a/aos/common/scoped_ptr.h b/aos/common/scoped_ptr.h
new file mode 100644
index 0000000..e4df78e
--- /dev/null
+++ b/aos/common/scoped_ptr.h
@@ -0,0 +1,43 @@
+#ifndef AOS_COMMON_SCOPED_PTR_H_
+#define AOS_COMMON_SCOPED_PTR_H_
+
+#include "aos/common/macros.h"
+
+namespace aos {
+
+// A simple scoped_ptr implementation that works under both linux and vxworks.
+template<typename T>
+class scoped_ptr {
+ public:
+ typedef T element_type;
+
+ explicit scoped_ptr(T *p = NULL) : p_(p) {}
+ ~scoped_ptr() {
+ delete p_;
+ }
+
+ T &operator*() const { return *p_; }
+ T *operator->() const { return p_; }
+ T *get() const { return p_; }
+
+ operator bool() const { return p_ != NULL; }
+
+ void swap(scoped_ptr<T> &other) {
+ T *temp = other.p_;
+ other.p_ = p_;
+ p_ = other.p_;
+ }
+ void reset(T *p = NULL) {
+ if (p_ != NULL) delete p_;
+ p_ = p;
+ }
+
+ private:
+ T *p_;
+
+ DISALLOW_COPY_AND_ASSIGN(scoped_ptr<T>);
+};
+
+} // namespace aos
+
+#endif // AOS_COMMON_SCOPED_PTR_H_
diff --git a/aos/common/test_queue.q b/aos/common/test_queue.q
new file mode 100644
index 0000000..a7b441f
--- /dev/null
+++ b/aos/common/test_queue.q
@@ -0,0 +1,21 @@
+package aos.common.testing;
+
+message TestingMessage {
+ bool test_bool;
+ int32_t test_int;
+};
+
+message OtherTestingMessage {
+ bool test_bool;
+ int32_t test_int;
+ double test_double;
+};
+
+queue TestingMessage test_queue;
+
+queue_group TwoQueues {
+ queue TestingMessage first;
+ queue OtherTestingMessage second;
+};
+
+queue_group TwoQueues test_queuegroup;
diff --git a/aos/common/time.cc b/aos/common/time.cc
new file mode 100644
index 0000000..1d97b80
--- /dev/null
+++ b/aos/common/time.cc
@@ -0,0 +1,166 @@
+#include "aos/common/time.h"
+
+#ifdef __VXWORKS__
+#include <taskLib.h>
+#endif
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/inttypes.h"
+
+namespace aos {
+namespace time {
+
+Time Time::Now(clockid_t clock) {
+ timespec temp;
+ if (clock_gettime(clock, &temp) != 0) {
+ // TODO(aschuh): There needs to be a pluggable low level logging interface
+ // so we can break this dependency loop. This also would help during
+ // startup.
+ LOG(FATAL, "clock_gettime(%jd, %p) failed with %d: %s\n",
+ static_cast<uintmax_t>(clock), &temp, errno, strerror(errno));
+ }
+ return Time(temp);
+}
+void Time::Check() {
+ if (nsec_ >= kNSecInSec || nsec_ < 0) {
+ LOG(FATAL, "0 <= nsec_(%"PRId32") < %"PRId32" isn't true.\n",
+ nsec_, kNSecInSec);
+ }
+ static_assert(aos::shm_ok<Time>::value,
+ "it should be able to go through shared memory");
+}
+
+Time &Time::operator+=(const Time &rhs) {
+ sec_ += rhs.sec_;
+ nsec_ += rhs.nsec_;
+ if (nsec_ >= kNSecInSec) {
+ nsec_ -= kNSecInSec;
+ sec_ += 1;
+ }
+ return *this;
+}
+const Time Time::operator+(const Time &rhs) const {
+ return Time(*this) += rhs;
+}
+Time &Time::operator-=(const Time &rhs) {
+ sec_ -= rhs.sec_;
+ nsec_ -= rhs.nsec_;
+ if (nsec_ < 0) {
+ nsec_ += kNSecInSec;
+ sec_ -= 1;
+ }
+ return *this;
+}
+const Time Time::operator-(const Time &rhs) const {
+ return Time(*this) -= rhs;
+}
+Time &Time::operator*=(int32_t rhs) {
+ const int64_t temp = static_cast<int64_t>(nsec_) *
+ static_cast<int64_t>(rhs);
+ sec_ *= rhs; // better not overflow, or the result is just too big
+ nsec_ = temp % kNSecInSec;
+ sec_ += (temp - nsec_) / kNSecInSec;
+ return *this;
+}
+const Time Time::operator*(int32_t rhs) const {
+ return Time(*this) *= rhs;
+}
+Time &Time::operator/=(int32_t rhs) {
+ nsec_ = (sec_ % rhs) * (kNSecInSec / rhs) + nsec_ / rhs;
+ sec_ /= rhs;
+ return *this;
+}
+const Time Time::operator/(int32_t rhs) const {
+ return Time(*this) /= rhs;
+}
+Time &Time::operator%=(int32_t rhs) {
+ nsec_ = ToNSec() % rhs;
+ const int wraps = nsec_ / kNSecInSec;
+ sec_ = wraps;
+ nsec_ -= kNSecInSec * wraps;
+ return *this;
+}
+const Time Time::operator%(int32_t rhs) const {
+ return Time(*this) %= rhs;
+}
+
+bool Time::operator==(const Time &rhs) const {
+ return sec_ == rhs.sec_ && nsec_ == rhs.nsec_;
+}
+bool Time::operator!=(const Time &rhs) const {
+ return !(*this == rhs);
+}
+bool Time::operator<(const Time &rhs) const {
+ return sec_ < rhs.sec_ || (sec_ == rhs.sec_ && nsec_ < rhs.nsec_);
+}
+bool Time::operator>(const Time &rhs) const {
+ return sec_ > rhs.sec_ || (sec_ == rhs.sec_ && nsec_ > rhs.nsec_);
+}
+bool Time::operator<=(const Time &rhs) const {
+ return sec_ < rhs.sec_ || (sec_ == rhs.sec_ && nsec_ <= rhs.nsec_);
+}
+bool Time::operator>=(const Time &rhs) const {
+ return sec_ > rhs.sec_ || (sec_ == rhs.sec_ && nsec_ >= rhs.nsec_);
+}
+
+bool Time::IsWithin(const Time &other, int64_t amount) const {
+ const int64_t temp = ToNSec() - other.ToNSec();
+ return temp <= amount && temp >= -amount;
+}
+
+std::ostream &operator<<(std::ostream &os, const Time& time) {
+ return os << "Time{" << time.sec_ << "s, " << time.nsec_ << "ns}";
+}
+
+void SleepFor(const Time &time, clockid_t clock) {
+#ifdef __VXWORKS__
+ SleepUntil(Time::Now(clock) + time, clock);
+#else
+ timespec converted(time.ToTimespec()), remaining;
+ int failure = EINTR;
+ do {
+ // This checks whether the last time through the loop actually failed or got
+ // interrupted.
+ if (failure != EINTR) {
+ LOG(FATAL, "clock_nanosleep(%jd, 0, %p, %p) returned %d: %s\n",
+ static_cast<intmax_t>(clock), &converted, &remaining,
+ failure, strerror(failure));
+ }
+ failure = clock_nanosleep(clock, 0, &converted, &remaining);
+ memcpy(&converted, &remaining, sizeof(converted));
+ } while (failure != 0);
+#endif
+}
+
+void SleepUntil(const Time &time, clockid_t clock) {
+#ifdef __VXWORKS__
+ if (clock != CLOCK_REALTIME) {
+ LOG(FATAL, "vxworks only supports CLOCK_REALTIME\n");
+ }
+ // Vxworks nanosleep is definitely broken (fails horribly at doing remaining
+ // right), and I don't really want to know how else it's broken, so I'm using
+ // taskDelay instead because that's simpler.
+ // The +1 is because sleep functions are supposed to sleep for at least the
+ // requested amount, so we have to round up to the next clock tick.
+ while (taskDelay((time - Time::Now(clock)).ToTicks() + 1) != 0) {
+ if (errno != EINTR) {
+ LOG(FATAL, "taskDelay(some ticks) failed with %d: %s\n",
+ errno, strerror(errno));
+ }
+ }
+#else
+ timespec converted(time.ToTimespec());
+ int failure;
+ while ((failure = clock_nanosleep(clock, TIMER_ABSTIME,
+ &converted, NULL)) != 0) {
+ if (failure != EINTR) {
+ LOG(FATAL, "clock_nanosleep(%jd, TIMER_ABSTIME, %p, NULL)"
+ " returned %d: %s\n", static_cast<intmax_t>(clock), &converted,
+ failure, strerror(failure));
+ }
+ }
+#endif
+}
+
+} // namespace time
+} // namespace aos
diff --git a/aos/common/time.h b/aos/common/time.h
new file mode 100644
index 0000000..57c5f26
--- /dev/null
+++ b/aos/common/time.h
@@ -0,0 +1,159 @@
+#ifndef AOS_COMMON_TIME_H_
+#define AOS_COMMON_TIME_H_
+
+#include <stdint.h>
+#include <time.h>
+
+#ifndef __VXWORKS__
+#include <type_traits>
+#else
+#include <sysLib.h>
+#endif
+#include <ostream>
+
+#include "aos/aos_stdint.h"
+#include "aos/common/type_traits.h"
+
+namespace aos {
+namespace time {
+
+// A nice structure for representing times.
+// 0 <= nsec_ < kNSecInSec should always be true. All functions here will make
+// sure that that is true if it was on all inputs (including *this).
+//
+// The arithmetic and comparison operators are overloaded because they make
+// complete sense and are very useful. The default copy and assignment stuff is
+// left because it works fine. Multiplication and division of Times by Times are
+// not implemented because I can't think of any uses for them and there are
+// multiple ways to do it.
+struct Time {
+ public:
+ static const int32_t kNSecInSec = 1000000000;
+ static const int32_t kNSecInMSec = 1000000;
+ static const int32_t kNSecInUSec = 1000;
+ static const int32_t kMSecInSec = 1000;
+ static const int32_t kUSecInSec = 1000000;
+ Time(int32_t sec, int32_t nsec) : sec_(sec), nsec_(nsec) {
+ Check();
+ }
+ #ifndef SWIG
+ explicit Time(const struct timespec &value)
+ : sec_(value.tv_sec), nsec_(value.tv_nsec) {
+ Check();
+ }
+ struct timespec ToTimespec() const {
+ struct timespec ans;
+ ans.tv_sec = sec_;
+ ans.tv_nsec = nsec_;
+ return ans;
+ }
+ #endif // SWIG
+ // CLOCK_MONOTONIC on the fitpc and CLOCK_REALTIME on the cRIO because the
+ // cRIO doesn't have any others.
+ // CLOCK_REALTIME is the default realtime clock and CLOCK_MONOTONIC doesn't
+ // change when somebody changes the wall clock (like the ntp deamon or
+ // whatever). See clock_gettime(2) for details.
+ //
+ // This is the clock that code that just wants to sleep for a certain amount
+ // of time or measure how long something takes should use.
+ #ifndef __VXWORKS__
+ static const clockid_t kDefaultClock = CLOCK_MONOTONIC;
+ #else
+ static const clockid_t kDefaultClock = CLOCK_REALTIME;
+ #endif
+ // Creates a Time representing the current value of the specified clock or
+ // dies.
+ static Time Now(clockid_t clock = kDefaultClock);
+
+ // Constructs a Time representing seconds.
+ static Time InSeconds(double seconds) {
+ return Time(static_cast<int32_t>(seconds),
+ (seconds - static_cast<int32_t>(seconds)) * kNSecInSec);
+ }
+
+ // Constructs a time representing microseconds.
+ static Time InNS(int64_t nseconds) {
+ return Time(nseconds / static_cast<int64_t>(kNSecInSec),
+ nseconds % kNSecInSec);
+ }
+
+ // Constructs a time representing microseconds.
+ static Time InUS(int useconds) {
+ return Time(useconds / kUSecInSec, (useconds % kUSecInSec) * kNSecInUSec);
+ }
+
+ // Constructs a time representing mseconds.
+ static Time InMS(int mseconds) {
+ return Time(mseconds / kMSecInSec, (mseconds % kMSecInSec) * kNSecInMSec);
+ }
+
+ // Checks whether or not this time is within amount nanoseconds of other.
+ bool IsWithin(const Time &other, int64_t amount) const;
+
+ // Returns the time represented all in nanoseconds.
+ int64_t ToNSec() const {
+ return static_cast<int64_t>(sec_) * static_cast<int64_t>(kNSecInSec) +
+ static_cast<int64_t>(nsec_);
+ }
+#ifdef __VXWORKS__
+ // Returns the time represented all in system clock ticks. The system clock
+ // rate is retrieved using sysClkRateGet().
+ int ToTicks() const {
+ return ToNSec() / static_cast<int64_t>(kNSecInSec / sysClkRateGet());
+ }
+#endif
+
+ // Returns the time represented in milliseconds.
+ int64_t ToMSec() const {
+ return static_cast<int64_t>(sec_) * static_cast<int64_t>(kMSecInSec) +
+ (static_cast<int64_t>(nsec_) / static_cast<int64_t>(kNSecInMSec));
+ }
+
+ #ifndef SWIG
+ Time &operator+=(const Time &rhs);
+ Time &operator-=(const Time &rhs);
+ Time &operator*=(int32_t rhs);
+ Time &operator/=(int32_t rhs);
+ Time &operator%=(int32_t rhs);
+ #endif
+ const Time operator+(const Time &rhs) const;
+ const Time operator-(const Time &rhs) const;
+ const Time operator*(int32_t rhs) const;
+ const Time operator/(int32_t rhs) const;
+ const Time operator%(int32_t rhs) const;
+
+ bool operator==(const Time &rhs) const;
+ bool operator!=(const Time &rhs) const;
+ bool operator<(const Time &rhs) const;
+ bool operator>(const Time &rhs) const;
+ bool operator<=(const Time &rhs) const;
+ bool operator>=(const Time &rhs) const;
+
+ #ifndef SWIG
+ // For gtest etc.
+ friend std::ostream &operator<<(std::ostream &os, const Time &time);
+ #endif // SWIG
+
+ int32_t sec() const { return sec_; }
+ void set_sec(int32_t sec) { sec_ = sec; }
+ int32_t nsec() const { return nsec_; }
+ void set_nsec(int32_t nsec) {
+ nsec_ = nsec;
+ Check();
+ }
+
+ private:
+ int32_t sec_, nsec_;
+ // LOG(FATAL)s if nsec_ is >= kNSecInSec.
+ void Check();
+};
+
+// Sleeps for the amount of time represented by time counted by clock.
+void SleepFor(const Time &time, clockid_t clock = Time::kDefaultClock);
+// Sleeps until clock is at the time represented by time.
+void SleepUntil(const Time &time, clockid_t clock = Time::kDefaultClock);
+
+} // namespace time
+} // namespace aos
+
+#endif // AOS_COMMON_TIME_H_
diff --git a/aos/common/time_test.cc b/aos/common/time_test.cc
new file mode 100644
index 0000000..6587759
--- /dev/null
+++ b/aos/common/time_test.cc
@@ -0,0 +1,132 @@
+#include "aos/common/time.h"
+
+#include "gtest/gtest.h"
+
+#include "aos/common/macros.h"
+
+namespace aos {
+namespace time {
+namespace testing {
+
+TEST(TimeTest, timespecConversions) {
+ timespec start{1234, 5678}; // NOLINT
+ Time time(start);
+ EXPECT_EQ(start.tv_sec, static_cast<signed time_t>(time.sec()));
+ EXPECT_EQ(start.tv_nsec, time.nsec());
+ timespec end = time.ToTimespec();
+ EXPECT_EQ(start.tv_sec, end.tv_sec);
+ EXPECT_EQ(start.tv_nsec, end.tv_nsec);
+}
+
+// It's kind of hard not to test Now and SleepFor at the same time.
+TEST(TimeTest, NowAndSleepFor) {
+ // without this, it tends to fail the first time (ends up sleeping for way
+ // longer than it should the second time, where it actually matters)
+ SleepFor(Time(0, Time::kNSecInSec / 10));
+ Time start = Time::Now();
+ SleepFor(Time(0, Time::kNSecInSec * 2 / 10));
+ EXPECT_TRUE(MACRO_DARG((Time::Now() - start)
+ .IsWithin(Time(0, Time::kNSecInSec * 2 / 10),
+ Time::kNSecInSec / 1000)));
+}
+
+TEST(TimeTest, AbsoluteSleep) {
+ Time start = Time::Now();
+ SleepFor(Time(0, Time::kNSecInSec / 10));
+ SleepUntil((start + Time(0, Time::kNSecInSec * 2 / 10)));
+ EXPECT_TRUE(MACRO_DARG((Time::Now() - start)
+ .IsWithin(Time(0, Time::kNSecInSec * 2 / 10),
+ Time::kNSecInSec / 1000)));
+}
+
+TEST(TimeTest, Addition) {
+ Time t(54, 500);
+ EXPECT_EQ(MACRO_DARG(Time(54, 5500)), t + MACRO_DARG(Time(0, 5000)));
+ EXPECT_EQ(MACRO_DARG(Time(56, 500)), t + MACRO_DARG(Time(2, 0)));
+ EXPECT_EQ(MACRO_DARG(Time(57, 6500)), t + MACRO_DARG(Time(3, 6000)));
+ EXPECT_EQ(MACRO_DARG(Time(50, 300)),
+ t + MACRO_DARG(Time(-5, Time::kNSecInSec - 200)));
+}
+TEST(TimeTest, Subtraction) {
+ Time t(54, 500);
+ EXPECT_EQ(MACRO_DARG(Time(54, 300)), t - MACRO_DARG(Time(0, 200)));
+ EXPECT_EQ(MACRO_DARG(Time(42, 500)), t - MACRO_DARG(Time(12, 0)));
+ EXPECT_EQ(MACRO_DARG(Time(50, 100)), t - MACRO_DARG(Time(4, 400)));
+ EXPECT_EQ(MACRO_DARG(Time(53, 600)),
+ t - MACRO_DARG(Time(0, Time::kNSecInSec - 100)));
+ EXPECT_EQ(MACRO_DARG(Time(55, 800)),
+ t - MACRO_DARG(Time(-2, Time::kNSecInSec - 300)));
+}
+
+TEST(TimeTest, Multiplication) {
+ Time t(54, Time::kNSecInSec / 3);
+ EXPECT_EQ(MACRO_DARG(Time(108, Time::kNSecInSec / 3 * 2)), t * 2);
+ EXPECT_EQ(MACRO_DARG(Time(271, Time::kNSecInSec / 3 * 2 - 1)), t * 5);
+}
+TEST(TimeTest, Division) {
+ EXPECT_EQ(MACRO_DARG(Time(5, Time::kNSecInSec / 10 * 4 + 50)),
+ MACRO_DARG(Time(54, 500)) / 10);
+}
+
+TEST(TimeTest, Comparisons) {
+ EXPECT_TRUE(MACRO_DARG(Time(971, 254) > Time(971, 253)));
+ EXPECT_TRUE(MACRO_DARG(Time(971, 254) >= Time(971, 253)));
+ EXPECT_TRUE(MACRO_DARG(Time(971, 254) < Time(971, 255)));
+ EXPECT_TRUE(MACRO_DARG(Time(971, 254) <= Time(971, 255)));
+ EXPECT_TRUE(MACRO_DARG(Time(971, 254) >= Time(971, 253)));
+ EXPECT_TRUE(MACRO_DARG(Time(971, 254) <= Time(971, 254)));
+ EXPECT_TRUE(MACRO_DARG(Time(971, 254) >= Time(971, 254)));
+ EXPECT_TRUE(MACRO_DARG(Time(972, 254) > Time(971, 254)));
+ EXPECT_TRUE(MACRO_DARG(Time(971, 254) < Time(972, 254)));
+}
+
+TEST(TimeTest, Within) {
+ EXPECT_TRUE(MACRO_DARG(Time(55, 5000).IsWithin(Time(55, 4900), 100)));
+ EXPECT_FALSE(MACRO_DARG(Time(55, 5000).IsWithin(Time(55, 4900), 99)));
+ EXPECT_TRUE(MACRO_DARG(Time(5, 0).IsWithin(Time(4, Time::kNSecInSec - 200),
+ 250)));
+}
+
+TEST(TimeTest, Modulo) {
+ EXPECT_EQ(MACRO_DARG(Time(0, Time::kNSecInSec / 10 * 2)),
+ MACRO_DARG(Time(50, 0) % (Time::kNSecInSec / 10 * 3)));
+}
+
+TEST(TimeTest, InSeconds) {
+ EXPECT_EQ(MACRO_DARG(Time(2, Time::kNSecInSec / 100 * 55 - 1)),
+ Time::InSeconds(2.55));
+}
+
+#ifdef __VXWORKS__
+TEST(TimeTest, ToTicks) {
+ EXPECT_EQ(sysClkRateGet() / 100,
+ MACRO_DARG(Time(0, Time::kNSecInSec / 100).ToTicks()));
+}
+#endif
+
+TEST(TimeTest, InMS) {
+ Time t = Time::InMS(254971);
+ EXPECT_EQ(254, t.sec());
+ EXPECT_EQ(971000000, t.nsec());
+}
+
+TEST(TimeTest, InNS) {
+ Time t = Time::InNS(static_cast<int64_t>(973254111971ll));
+ EXPECT_EQ(973, t.sec());
+ EXPECT_EQ(254111971, t.nsec());
+}
+
+TEST(TimeTest, InUS) {
+ Time t = Time::InUS(254111971);
+ EXPECT_EQ(254, t.sec());
+ EXPECT_EQ(111971000, t.nsec());
+}
+
+TEST(TimeTest, ToMSec) {
+ Time t(254, 971000000);
+ EXPECT_EQ(254971, t.ToMSec());
+}
+
+} // namespace testing
+} // namespace time
+} // namespace aos
diff --git a/aos/common/type_traits.h b/aos/common/type_traits.h
new file mode 100644
index 0000000..9c06b02
--- /dev/null
+++ b/aos/common/type_traits.h
@@ -0,0 +1,38 @@
+#ifndef AOS_COMMON_TYPE_TRAITS_
+#define AOS_COMMON_TYPE_TRAITS_
+
+#ifndef __VXWORKS__
+#include <type_traits>
+#else
+#include "aos/crio/type_traits/type_traits"
+#endif
+
+namespace aos {
+
+// A class template that determines whether or not it is safe to pass a type
+// through the shared memory system (aka whether or not you can memcpy it).
+// Useful in combination with static_assert.
+// Always true when using the cRIO compiler.
+//
+// Doesn't need a trivial constructor because it's bytes only need to get
+// copied, so it does need to not require anything special to be cleaned up
+// (trivial destructor).
+// See also (3.9) [basic.types] in the C++11 standard.
+template<typename Tp>
+struct has_trivial_copy_assign : public std::integral_constant<bool,
+// This changed between 4.4.5 and 4.6.3. Unless somebody discovers otherwise,
+// 4.6 seems like a reasonable place to switch.
+#if ((__GNUC__ < 4) || (__GNUC_MINOR__ < 6)) && !defined(__clang__)
+ std::has_trivial_assign<Tp>::value> {};
+#else
+ std::has_trivial_copy_assign<Tp>::value> {};
+#endif
+template<typename Tp>
+struct shm_ok : public std::integral_constant<bool,
+ (std::has_trivial_copy_constructor<Tp>::value &&
+ aos::has_trivial_copy_assign<Tp>::value &&
+ std::has_trivial_destructor<Tp>::value)> {};
+
+} // namespace aos
+
+#endif
diff --git a/aos/common/type_traits_test.cpp b/aos/common/type_traits_test.cpp
new file mode 100644
index 0000000..aa104ee
--- /dev/null
+++ b/aos/common/type_traits_test.cpp
@@ -0,0 +1,98 @@
+#include "aos/common/type_traits.h"
+
+#include "gtest/gtest.h"
+
+namespace aos {
+namespace testing {
+
+class BadVirtualFunction {
+ virtual void Test() {}
+};
+TEST(TypeTraitsTest, VirtualFunction) {
+ EXPECT_FALSE(shm_ok<BadVirtualFunction>::value);
+}
+class BadPureVirtualFunction {
+ virtual void Test() = 0;
+};
+TEST(TypeTraitsTest, PureVirtualFunction) {
+ EXPECT_FALSE(shm_ok<BadPureVirtualFunction>::value);
+}
+class BadInheritedVirtual : public BadVirtualFunction {};
+TEST(TypeTraitsTest, InheritedVirtualFunction) {
+ EXPECT_FALSE(shm_ok<BadInheritedVirtual>::value);
+}
+class BadVirtualDestructor {
+ virtual ~BadVirtualDestructor();
+};
+TEST(TypeTraitsTest, VirtualDestructor) {
+ EXPECT_FALSE(shm_ok<BadVirtualDestructor>::value);
+}
+class Boring {};
+class BadVirtualBase : public virtual Boring {};
+TEST(TypeTraitsTest, VirtualBase) {
+ EXPECT_FALSE(shm_ok<BadVirtualBase>::value);
+}
+
+class GoodSimple {
+ public:
+ int test1, test2;
+ double test3;
+};
+// Make sure that it lets simple classes through and that the compiler isn't
+// completely nuts.
+TEST(TypeTraitsTest, Basic) {
+ EXPECT_TRUE(shm_ok<GoodSimple>::value);
+ GoodSimple test{5, 4, 34.2};
+ EXPECT_EQ(5, test.test1);
+ EXPECT_EQ(4, test.test2);
+ EXPECT_EQ(34.2, test.test3);
+ memset(&test, 0, sizeof(test));
+ EXPECT_EQ(0, test.test1);
+ EXPECT_EQ(0, test.test2);
+ EXPECT_EQ(0, test.test3);
+}
+
+class GoodWithConstructor {
+ public:
+ int val_;
+ GoodWithConstructor(int val) : val_(val) {}
+};
+// Make sure that it lets classes with constructors through.
+TEST(TypeTraitsTest, GoodWithConstructor) {
+ EXPECT_TRUE(shm_ok<GoodWithConstructor>::value);
+ GoodWithConstructor test(971);
+ EXPECT_EQ(971, test.val_);
+}
+
+class GoodPublicPrivateFunction {
+ public:
+ int32_t a_;
+ void set_a(int32_t a) { a_ = a; }
+ int32_t b() { return b_; }
+ void set_b(int32_t b) { b_ = b; }
+ private:
+ int32_t b_;
+};
+// Make sure that member functions still work.
+TEST(TypeTraitsTest, Function) {
+ EXPECT_TRUE(shm_ok<GoodPublicPrivateFunction>::value);
+ EXPECT_EQ(static_cast<unsigned>(8), sizeof(GoodPublicPrivateFunction)) <<
+ "The compiler did something weird, but it might not be a problem.";
+ GoodPublicPrivateFunction test;
+ test.a_ = 5;
+ test.set_b(971);
+ EXPECT_EQ(5, test.a_);
+ EXPECT_EQ(971, test.b());
+ test.set_a(74);
+ EXPECT_EQ(74, test.a_);
+ memset(&test, 0, sizeof(test));
+ EXPECT_EQ(0, test.a_);
+ EXPECT_EQ(0, test.b());
+ test.set_a(123);
+ test.set_b(254);
+ EXPECT_EQ(123, test.a_);
+ EXPECT_EQ(254, test.b());
+}
+
+} // namespace testing
+} // namespace aos
diff --git a/aos/common/unique_malloc_ptr.h b/aos/common/unique_malloc_ptr.h
new file mode 100644
index 0000000..696b9f3
--- /dev/null
+++ b/aos/common/unique_malloc_ptr.h
@@ -0,0 +1,29 @@
+#include <memory>
+
+namespace aos {
+
+namespace {
+
+template<typename T, void(*function)(T *)>
+void const_wrap(const T *ptr) {
+ function(const_cast<T *>(ptr));
+}
+
+// Wrapper function to deal with the differences between C and C++ (C++ doesn't
+// automatically convert T* to void* like C).
+template<typename T>
+void free_type(T *ptr) { ::free(reinterpret_cast<void *>(ptr)); }
+
+} // namespace
+
+// A std::unique_ptr that should get freed with a C-style free function
+// (free(2) by default).
+template<typename T, void(*function)(T *) = free_type>
+class unique_c_ptr : public std::unique_ptr<T, void(*)(const T *)> {
+ public:
+ unique_c_ptr(T *pointer)
+ : std::unique_ptr<T, void(*)(const T *)>(
+ pointer, const_wrap<T, function>) {}
+};
+
+} // namespace aos