Merge "Refactor logger_test a bit"
diff --git a/README.md b/README.md
index d54d963..f81d423 100644
--- a/README.md
+++ b/README.md
@@ -26,7 +26,8 @@
1. Install the required packages:
```console
apt-get update
-apt-get install bazel python
+# TODO(james): Ideally, we shouldn't need to be installing libtinfo5...
+apt-get install bazel python libtinfo5
```
2. Allow Bazel's sandboxing to work:
Follow the direction in `doc/frc971.conf`.
diff --git a/WORKSPACE b/WORKSPACE
index 9f087bc..dced1f0 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -60,11 +60,11 @@
)
load(
"//debian:gstreamer_amd64.bzl",
- gstreamer_amd64_debs = "files"
+ gstreamer_amd64_debs = "files",
)
load(
"//debian:gstreamer_armhf.bzl",
- gstreamer_armhf_debs = "files"
+ gstreamer_armhf_debs = "files",
)
load("//debian:packages.bzl", "generate_repositories_for_debs")
@@ -681,6 +681,7 @@
http_archive(
name = "halide_k8",
build_file = "@//debian:halide.BUILD",
+ sha256 = "c67185d50a99adba86f6b2cc43c7e2cf11bcdfba9052d05e764a89b456a50446",
strip_prefix = "halide/",
url = "http://www.frc971.org/Build-Dependencies/halide-linux-64-gcc53-800-65c26cba6a3eca2d08a0bccf113ca28746012cc3.tgz",
)
@@ -691,6 +692,7 @@
http_archive(
name = "halide_armhf",
build_file = "@//debian:halide.BUILD",
+ sha256 = "10564c559c9e04a173823413916d05fadd6e697d91bab21ddc5041190fa8f0f0",
strip_prefix = "halide/",
url = "http://www.frc971.org/Build-Dependencies/halide-arm32-linux-32-trunk-65c26cba6a3eca2d08a0bccf113ca28746012cc3.tgz",
)
@@ -718,3 +720,23 @@
sha256 = "c5ac4c604952c274a50636e244f0d091bd1de302032446f24f0e9e03ae9c76f7",
url = "http://www.frc971.org/Build-Dependencies/gstreamer_armhf.tar.gz",
)
+
+# Downloaded from:
+# https://files.pythonhosted.org/packages/64/a7/45e11eebf2f15bf987c3bc11d37dcc838d9dc81250e67e4c5968f6008b6c/Jinja2-2.11.2.tar.gz
+http_archive(
+ name = "python_jinja2",
+ build_file = "@//debian:python_jinja2.BUILD",
+ sha256 = "89aab215427ef59c34ad58735269eb58b1a5808103067f7bb9d5836c651b3bb0",
+ strip_prefix = "Jinja2-2.11.2",
+ url = "http://www.frc971.org/Build-Dependencies/Jinja2-2.11.2.tar.gz",
+)
+
+# Downloaded from:
+# https://files.pythonhosted.org/packages/b9/2e/64db92e53b86efccfaea71321f597fa2e1b2bd3853d8ce658568f7a13094/MarkupSafe-1.1.1.tar.gz
+http_archive(
+ name = "python_markupsafe",
+ build_file = "@//debian:python_markupsafe.BUILD",
+ sha256 = "29872e92839765e546828bb7754a68c418d927cd064fd4708fab9fe9c8bb116b",
+ strip_prefix = "MarkupSafe-1.1.1",
+ url = "http://www.frc971.org/Build-Dependencies/MarkupSafe-1.1.1.tar.gz",
+)
diff --git a/aos/BUILD b/aos/BUILD
index da558a4..d35e74c 100644
--- a/aos/BUILD
+++ b/aos/BUILD
@@ -526,3 +526,17 @@
"@com_github_google_glog//:glog",
],
)
+
+cc_library(
+ name = "ftrace",
+ srcs = [
+ "ftrace.cc",
+ ],
+ hdrs = [
+ "ftrace.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "@com_github_google_glog//:glog",
+ ],
+)
diff --git a/aos/aos_dump.cc b/aos/aos_dump.cc
index fd33eca..55ca57e 100644
--- a/aos/aos_dump.cc
+++ b/aos/aos_dump.cc
@@ -10,6 +10,38 @@
DEFINE_string(config, "./config.json", "File path of aos configuration");
DEFINE_int32(max_vector_size, 100,
"If positive, vectors longer than this will not be printed");
+DEFINE_bool(fetch, false,
+ "If true, fetch the current message on the channel first");
+
+namespace {
+
+void PrintMessage(const aos::Channel *channel, const aos::Context &context) {
+ // Print the flatbuffer out to stdout, both to remove the
+ // unnecessary cruft from glog and to allow the user to readily
+ // redirect just the logged output independent of any debugging
+ // information on stderr.
+ if (context.monotonic_remote_time != context.monotonic_event_time) {
+ std::cout << context.realtime_remote_time << " ("
+ << context.monotonic_remote_time << ") delivered "
+ << context.realtime_event_time << " ("
+ << context.monotonic_event_time << "): "
+ << aos::FlatbufferToJson(
+ channel->schema(),
+ static_cast<const uint8_t *>(context.data),
+ {false, static_cast<size_t>(FLAGS_max_vector_size)})
+ << '\n';
+ } else {
+ std::cout << context.realtime_event_time << " ("
+ << context.monotonic_event_time << "): "
+ << aos::FlatbufferToJson(
+ channel->schema(),
+ static_cast<const uint8_t *>(context.data),
+ {false, static_cast<size_t>(FLAGS_max_vector_size)})
+ << '\n';
+ }
+}
+
+} // namespace
int main(int argc, char **argv) {
aos::InitGoogle(&argc, &argv);
@@ -40,48 +72,48 @@
return 0;
}
- int found_channels = 0;
+ std::vector<const aos::Channel *> found_channels;
const flatbuffers::Vector<flatbuffers::Offset<aos::Channel>> *channels =
config_msg->channels();
+ bool found_exact = false;
for (const aos::Channel *channel : *channels) {
- if (channel->name()->c_str() == channel_name &&
- channel->type()->str().find(message_type) != std::string::npos) {
- event_loop.MakeRawWatcher(
- channel, [channel](const aos::Context &context, const void *message) {
- // Print the flatbuffer out to stdout, both to remove the
- // unnecessary cruft from glog and to allow the user to readily
- // redirect just the logged output independent of any debugging
- // information on stderr.
- if (context.monotonic_remote_time != context.monotonic_event_time) {
- std::cout << context.realtime_remote_time << " ("
- << context.monotonic_remote_time << ") delivered "
- << context.realtime_event_time << " ("
- << context.monotonic_event_time << "): "
- << aos::FlatbufferToJson(
- channel->schema(),
- static_cast<const uint8_t *>(message),
- FLAGS_max_vector_size)
- << '\n';
- } else {
- std::cout << context.realtime_event_time << " ("
- << context.monotonic_event_time << "): "
- << aos::FlatbufferToJson(
- channel->schema(),
- static_cast<const uint8_t *>(message),
- FLAGS_max_vector_size)
- << '\n';
- }
- });
- found_channels++;
+ if (channel->name()->c_str() != channel_name) {
+ continue;
}
+ if (channel->type()->string_view() == message_type) {
+ if (!found_exact) {
+ found_channels.clear();
+ found_exact = true;
+ }
+ } else if (!found_exact && channel->type()->string_view().find(
+ message_type) != std::string_view::npos) {
+ } else {
+ continue;
+ }
+ found_channels.push_back(channel);
}
- if (found_channels == 0) {
+ if (found_channels.empty()) {
LOG(FATAL) << "Could not find any channels with the given name and type.";
- } else if (found_channels > 1 && message_type.size() != 0) {
+ } else if (found_channels.size() > 1 && !message_type.empty()) {
LOG(FATAL) << "Multiple channels found with same type";
}
+ for (const aos::Channel *channel : found_channels) {
+ if (FLAGS_fetch) {
+ const std::unique_ptr<aos::RawFetcher> fetcher =
+ event_loop.MakeRawFetcher(channel);
+ if (fetcher->Fetch()) {
+ PrintMessage(channel, fetcher->context());
+ }
+ }
+
+ event_loop.MakeRawWatcher(channel, [channel](const aos::Context &context,
+ const void * /*message*/) {
+ PrintMessage(channel, context);
+ });
+ }
+
event_loop.Run();
::aos::Cleanup();
return 0;
diff --git a/aos/config_flattener.cc b/aos/config_flattener.cc
index 919028b..ee2f376 100644
--- a/aos/config_flattener.cc
+++ b/aos/config_flattener.cc
@@ -28,14 +28,16 @@
}
const std::string merged_config = FlatbufferToJson(
- &configuration::MergeConfiguration(config, schemas).message(), true);
+ &configuration::MergeConfiguration(config, schemas).message(),
+ {.multi_line = true});
// TODO(austin): Figure out how to squash the schemas onto 1 line so it is
// easier to read?
VLOG(1) << "Flattened config is " << merged_config;
util::WriteStringToFileOrDie(full_output, merged_config);
- util::WriteStringToFileOrDie(stripped_output,
- FlatbufferToJson(&config.message(), true));
+ util::WriteStringToFileOrDie(
+ stripped_output,
+ FlatbufferToJson(&config.message(), {.multi_line = true}));
return 0;
}
diff --git a/aos/configuration.cc b/aos/configuration.cc
index cef3876..b0ff973 100644
--- a/aos/configuration.cc
+++ b/aos/configuration.cc
@@ -12,6 +12,7 @@
#include <string_view>
#include "absl/container/btree_set.h"
+#include "absl/strings/str_cat.h"
#include "aos/configuration_generated.h"
#include "aos/flatbuffer_merge.h"
#include "aos/json_to_flatbuffer.h"
@@ -21,11 +22,6 @@
#include "gflags/gflags.h"
#include "glog/logging.h"
-DEFINE_string(
- override_hostname, "",
- "If set, this forces the hostname of this node to be the provided "
- "hostname.");
-
namespace aos {
// Define the compare and equal operators for Channel and Application so we can
@@ -457,11 +453,16 @@
}
FlatbufferDetachedBuffer<Configuration> MergeWithConfig(
+ const Configuration *config, const Flatbuffer<Configuration> &addition) {
+ return MergeConfiguration(MergeFlatBuffers(config, &addition.message()));
+}
+
+FlatbufferDetachedBuffer<Configuration> MergeWithConfig(
const Configuration *config, std::string_view json) {
FlatbufferDetachedBuffer<Configuration> addition =
JsonToFlatbuffer(json, Configuration::MiniReflectTypeTable());
- return MergeConfiguration(MergeFlatBuffers(config, &addition.message()));
+ return MergeWithConfig(config, addition);
}
const Channel *GetChannel(const Configuration *config, std::string_view name,
@@ -539,6 +540,12 @@
return FlatbufferToJson(cleaned_channel);
}
+std::string StrippedChannelToString(const Channel *channel) {
+ return absl::StrCat("{ \"name\": \"", channel->name()->string_view(),
+ "\", \"type\": \"", channel->type()->string_view(),
+ "\" }");
+}
+
FlatbufferDetachedBuffer<Configuration> MergeConfiguration(
const Flatbuffer<Configuration> &config,
const std::vector<aos::FlatbufferString<reflection::Schema>> &schemas) {
diff --git a/aos/configuration.h b/aos/configuration.h
index 9193fd5..a01a902 100644
--- a/aos/configuration.h
+++ b/aos/configuration.h
@@ -36,6 +36,8 @@
// returns the modified config.
FlatbufferDetachedBuffer<Configuration> MergeWithConfig(
const Configuration *config, std::string_view json);
+FlatbufferDetachedBuffer<Configuration> MergeWithConfig(
+ const Configuration *config, const Flatbuffer<Configuration> &addition);
// Returns the resolved location for a name, type, and application name. Returns
// nullptr if none is found.
@@ -113,6 +115,8 @@
// Prints a channel to json, but without the schema.
std::string CleanedChannelToString(const Channel *channel);
+// Prints out a channel to json, but only with the name and type.
+std::string StrippedChannelToString(const Channel *channel);
// Returns the node names that this node should be forwarding to.
std::vector<std::string_view> DestinationNodeNames(const Configuration *config,
diff --git a/aos/configuration_test.cc b/aos/configuration_test.cc
index aa6de8e..c800017 100644
--- a/aos/configuration_test.cc
+++ b/aos/configuration_test.cc
@@ -27,18 +27,19 @@
"{ \"name\": \"/foo\", \"type\": \".aos.bar\", \"max_size\": 5 }";
// And for multinode setups
const char *kExpectedMultinodeLocation =
- "{ \"name\": \"/foo\", \"type\": \".aos.bar\", \"max_size\": 5, \"source_node\": \"pi1\" }";
+ "{ \"name\": \"/foo\", \"type\": \".aos.bar\", \"max_size\": 5, "
+ "\"source_node\": \"pi1\" }";
// Tests that we can read and merge a configuration.
TEST_F(ConfigurationTest, ConfigMerge) {
FlatbufferDetachedBuffer<Configuration> config =
ReadConfig(kConfigPrefix + "config1.json");
- LOG(INFO) << "Read: " << FlatbufferToJson(config, true);
+ LOG(INFO) << "Read: " << FlatbufferToJson(config, {.multi_line = true});
EXPECT_EQ(
absl::StripSuffix(
util::ReadFileToStringOrDie(kConfigPrefix + "expected.json"), "\n"),
- FlatbufferToJson(config, true));
+ FlatbufferToJson(config, {.multi_line = true}));
}
// Tests that we can get back a ChannelIndex.
@@ -55,13 +56,13 @@
TEST_F(ConfigurationTest, ConfigMergeMultinode) {
FlatbufferDetachedBuffer<Configuration> config =
ReadConfig(kConfigPrefix + "config1_multinode.json");
- LOG(INFO) << "Read: " << FlatbufferToJson(config, true);
+ LOG(INFO) << "Read: " << FlatbufferToJson(config, {.multi_line = true});
- EXPECT_EQ(
- std::string(absl::StripSuffix(
- util::ReadFileToStringOrDie(kConfigPrefix + "expected_multinode.json"),
- "\n")),
- FlatbufferToJson(config, true));
+ EXPECT_EQ(std::string(absl::StripSuffix(
+ util::ReadFileToStringOrDie(kConfigPrefix +
+ "expected_multinode.json"),
+ "\n")),
+ FlatbufferToJson(config, {.multi_line = true}));
}
// Tests that we sort the entries in a config so we can look entries up.
@@ -69,7 +70,7 @@
FlatbufferDetachedBuffer<Configuration> config =
ReadConfig(kConfigPrefix + "backwards.json");
- LOG(INFO) << "Read: " << FlatbufferToJson(config, true);
+ LOG(INFO) << "Read: " << FlatbufferToJson(config, {.multi_line = true});
EXPECT_EQ(FlatbufferToJson(GetChannel(config, "/aos/robot_state",
"aos.RobotState", "app1", nullptr)),
@@ -115,7 +116,7 @@
TEST_F(ConfigurationTest, MergeWithConfig) {
FlatbufferDetachedBuffer<Configuration> config =
ReadConfig(kConfigPrefix + "config1.json");
- LOG(INFO) << "Read: " << FlatbufferToJson(config, true);
+ LOG(INFO) << "Read: " << FlatbufferToJson(config, {.multi_line = true});
FlatbufferDetachedBuffer<Configuration> updated_config =
MergeWithConfig(&config.message(),
@@ -129,11 +130,10 @@
]
})channel");
- EXPECT_EQ(
- absl::StripSuffix(util::ReadFileToStringOrDie(
- kConfigPrefix + "expected_merge_with.json"),
- "\n"),
- FlatbufferToJson(updated_config, true));
+ EXPECT_EQ(absl::StripSuffix(util::ReadFileToStringOrDie(
+ kConfigPrefix + "expected_merge_with.json"),
+ "\n"),
+ FlatbufferToJson(updated_config, {.multi_line = true}));
}
// Tests that we can lookup a location, complete with maps, from a merged
@@ -187,9 +187,9 @@
kExpectedMultinodeLocation);
// Tests that a root map/rename works with a node specific map.
- EXPECT_EQ(FlatbufferToJson(
- GetChannel(config, "/batman", ".aos.bar", "app1", pi1)),
- kExpectedMultinodeLocation);
+ EXPECT_EQ(
+ FlatbufferToJson(GetChannel(config, "/batman", ".aos.bar", "app1", pi1)),
+ kExpectedMultinodeLocation);
// Tests that a root map/rename fails with a node specific map for the wrong
// node.
@@ -427,9 +427,8 @@
})channel",
Channel::MiniReflectTypeTable()));
- FlatbufferDetachedBuffer<Channel> logged_on_both_channel (
- JsonToFlatbuffer(
- R"channel({
+ FlatbufferDetachedBuffer<Channel> logged_on_both_channel(JsonToFlatbuffer(
+ R"channel({
"name": "/test",
"type": "aos.examples.Ping",
"source_node": "bar",
@@ -441,7 +440,7 @@
}
]
})channel",
- Channel::MiniReflectTypeTable()));
+ Channel::MiniReflectTypeTable()));
FlatbufferDetachedBuffer<Node> foo_node(JsonToFlatbuffer(
R"node({
@@ -473,7 +472,7 @@
EXPECT_FALSE(ChannelMessageIsLoggedOnNode(¬_logged_channel.message(),
&foo_node.message()));
EXPECT_FALSE(ChannelMessageIsLoggedOnNode(¬_logged_channel.message(),
- &bar_node.message()));
+ &bar_node.message()));
EXPECT_FALSE(ChannelMessageIsLoggedOnNode(¬_logged_channel.message(),
&baz_node.message()));
@@ -567,9 +566,8 @@
})channel",
Channel::MiniReflectTypeTable()));
- FlatbufferDetachedBuffer<Channel> logged_on_both_channel (
- JsonToFlatbuffer(
- R"channel({
+ FlatbufferDetachedBuffer<Channel> logged_on_both_channel(JsonToFlatbuffer(
+ R"channel({
"name": "/test",
"type": "aos.examples.Ping",
"source_node": "bar",
@@ -581,7 +579,7 @@
}
]
})channel",
- Channel::MiniReflectTypeTable()));
+ Channel::MiniReflectTypeTable()));
FlatbufferDetachedBuffer<Node> foo_node(JsonToFlatbuffer(
R"node({
@@ -783,10 +781,10 @@
GetNodeFromHostname(&config.message(), "raspberrypi3"))
->name()
->string_view());
- EXPECT_EQ("pi2", CHECK_NOTNULL(
- GetNodeFromHostname(&config.message(), "other"))
- ->name()
- ->string_view());
+ EXPECT_EQ("pi2",
+ CHECK_NOTNULL(GetNodeFromHostname(&config.message(), "other"))
+ ->name()
+ ->string_view());
EXPECT_EQ(nullptr, GetNodeFromHostname(&config.message(), "raspberrypi4"));
EXPECT_EQ(nullptr, GetNodeFromHostname(&config.message(), "localhost"));
EXPECT_EQ(nullptr, GetNodeFromHostname(&config.message(), "3"));
diff --git a/aos/controls/BUILD b/aos/controls/BUILD
index c45a345..7172cbb 100644
--- a/aos/controls/BUILD
+++ b/aos/controls/BUILD
@@ -87,3 +87,31 @@
"//aos/util:log_interval",
],
)
+
+cc_library(
+ name = "quaternion_utils",
+ srcs = [
+ "quaternion_utils.cc",
+ ],
+ hdrs = [
+ "quaternion_utils.h",
+ ],
+ deps = [
+ "@com_github_google_glog//:glog",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_test(
+ name = "quarternion_utils_test",
+ srcs = [
+ "quaternion_utils_test.cc",
+ ],
+ deps = [
+ ":quaternion_utils",
+ "//aos/testing:googletest",
+ "//aos/testing:random_seed",
+ "@com_github_google_glog//:glog",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
diff --git a/aos/controls/quaternion_utils.cc b/aos/controls/quaternion_utils.cc
new file mode 100644
index 0000000..088c699
--- /dev/null
+++ b/aos/controls/quaternion_utils.cc
@@ -0,0 +1,139 @@
+#include "aos/controls/quaternion_utils.h"
+
+#include "Eigen/Dense"
+#include "Eigen/Geometry"
+
+namespace aos {
+namespace controls {
+
+Eigen::Matrix<double, 4, 1> ToQuaternionFromRotationVector(
+ const Eigen::Matrix<double, 3, 1> &X, const double max_angle_cap) {
+ const double unclipped_angle = X.norm();
+ const double angle_scalar =
+ (unclipped_angle > max_angle_cap) ? max_angle_cap / unclipped_angle : 1.0;
+ const double angle = unclipped_angle * angle_scalar;
+ const double half_angle = angle * 0.5;
+
+ const double half_angle_squared = half_angle * half_angle;
+
+ // sin(x)/x = 1
+ double sinx_x = 1.0;
+
+ // - x^2/3!
+ double value = half_angle_squared / 6.0;
+ sinx_x -= value;
+
+ // + x^4/5!
+ value = value * half_angle_squared / 20.0;
+ sinx_x += value;
+
+ // - x^6/7!
+ value = value * half_angle_squared / (6.0 * 7.0);
+ sinx_x -= value;
+
+ // + x^8/9!
+ value = value * half_angle_squared / (8.0 * 9.0);
+ sinx_x += value;
+
+ // - x^10/11!
+ value = value * half_angle_squared / (10.0 * 11.0);
+ sinx_x -= value;
+
+ // + x^12/13!
+ value = value * half_angle_squared / (12.0 * 13.0);
+ sinx_x += value;
+
+ // - x^14/15!
+ value = value * half_angle_squared / (14.0 * 15.0);
+ sinx_x -= value;
+
+ // + x^16/17!
+ value = value * half_angle_squared / (16.0 * 17.0);
+ sinx_x += value;
+
+ // To plot the residual in matplotlib, run:
+ // import numpy
+ // import scipy
+ // from matplotlib import pyplot
+ // x = numpy.arange(-numpy.pi, numpy.pi, 0.01)
+ // pyplot.plot(x, 1 - x**2 / scipy.misc.factorial(3) +
+ // x**4 / scipy.misc.factorial(5) -
+ // x**6 / scipy.misc.factorial(7) +
+ // x**8 / scipy.misc.factorial(9) -
+ // x ** 10 / scipy.misc.factorial(11) +
+ // x ** 12 / scipy.misc.factorial(13) -
+ // x ** 14 / scipy.misc.factorial(15) +
+ // x ** 16 / scipy.misc.factorial(17) -
+ // numpy.sin(x) / x)
+
+ const double scalar = sinx_x * 0.5;
+
+ Eigen::Matrix<double, 4, 1> result;
+ result.block<3, 1>(0, 0) = X * scalar * angle_scalar;
+ result(3, 0) = std::cos(half_angle);
+ return result;
+}
+
+inline Eigen::Matrix<double, 4, 1> MaybeFlipX(
+ const Eigen::Matrix<double, 4, 1> &X) {
+ if (X(3, 0) < 0.0) {
+ return -X;
+ } else {
+ return X;
+ }
+}
+
+Eigen::Matrix<double, 3, 1> ToRotationVectorFromQuaternion(
+ const Eigen::Matrix<double, 4, 1> &X) {
+ // TODO(austin): Verify we still need it.
+ const Eigen::Matrix<double, 4, 1> corrected_X = MaybeFlipX(X);
+ const double half_angle =
+ std::atan2(corrected_X.block<3, 1>(0, 0).norm(), corrected_X(3, 0));
+
+ const double half_angle_squared = half_angle * half_angle;
+
+ // TODO(austin): We are doing a division at the end of this. Do the taylor
+ // series expansion of x/sin(x) instead to avoid this.
+
+ // sin(x)/x = 1
+ double sinx_x = 1.0;
+
+ // - x^2/3!
+ double value = half_angle_squared / 6.0;
+ sinx_x -= value;
+
+ // + x^4/5!
+ value = value * half_angle_squared / 20.0;
+ sinx_x += value;
+
+ // - x^6/7!
+ value = value * half_angle_squared / (6.0 * 7.0);
+ sinx_x -= value;
+
+ // + x^8/9!
+ value = value * half_angle_squared / (8.0 * 9.0);
+ sinx_x += value;
+
+ // - x^10/11!
+ value = value * half_angle_squared / (10.0 * 11.0);
+ sinx_x -= value;
+
+ // + x^12/13!
+ value = value * half_angle_squared / (12.0 * 13.0);
+ sinx_x += value;
+
+ // - x^14/15!
+ value = value * half_angle_squared / (14.0 * 15.0);
+ sinx_x -= value;
+
+ // + x^16/17!
+ value = value * half_angle_squared / (16.0 * 17.0);
+ sinx_x += value;
+
+ const double scalar = 2.0 / sinx_x;
+
+ return corrected_X.block<3, 1>(0, 0) * scalar;
+}
+
+} // namespace controls
+} // namespace aos
diff --git a/aos/controls/quaternion_utils.h b/aos/controls/quaternion_utils.h
new file mode 100644
index 0000000..1455821
--- /dev/null
+++ b/aos/controls/quaternion_utils.h
@@ -0,0 +1,71 @@
+#ifndef AOS_CONTROLS_QUATERNION_UTILS_H_
+#define AOS_CONTROLS_QUATERNION_UTILS_H_
+
+#include "Eigen/Dense"
+#include "Eigen/Geometry"
+#include "glog/logging.h"
+
+namespace aos {
+namespace controls {
+
+// Function to compute the quaternion average of a bunch of quaternions. Each
+// column in the input matrix is a quaternion (optionally scaled by it's
+// weight).
+template <int SM>
+inline Eigen::Matrix<double, 4, 1> QuaternionMean(
+ Eigen::Matrix<double, 4, SM> input) {
+ // Algorithm to compute the average of a bunch of quaternions:
+ // http://www.acsu.buffalo.edu/~johnc/ave_quat07.pdf
+
+ Eigen::Matrix<double, 4, 4> m = input * input.transpose();
+
+ Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>> solver;
+ solver.compute(m);
+
+ Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>>::EigenvectorsType
+ eigenvectors = solver.eigenvectors();
+ Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>>::EigenvalueType eigenvalues =
+ solver.eigenvalues();
+
+ int max_index = 0;
+ double max_eigenvalue = 0.0;
+ for (int i = 0; i < 4; ++i) {
+ const double eigenvalue = std::abs(eigenvalues(i, 0));
+ if (eigenvalue > max_eigenvalue) {
+ max_eigenvalue = eigenvalue;
+ max_index = i;
+ }
+ }
+
+ // Assume that there shouldn't be any imaginary components to the eigenvector.
+ // I can't prove this is true, but everyone else seems to assume it...
+ // TODO(james): Handle this more rigorously.
+ for (int i = 0; i < 4; ++i) {
+ CHECK_LT(eigenvectors(i, max_index).imag(), 1e-4)
+ << eigenvectors(i, max_index);
+ }
+ return eigenvectors.col(max_index).real().normalized();
+}
+
+// Converts from a quaternion to a rotation vector, where the rotation vector's
+// direction represents the axis to rotate around and its magnitude represents
+// the number of radians to rotate.
+Eigen::Matrix<double, 3, 1> ToRotationVectorFromQuaternion(
+ const Eigen::Matrix<double, 4, 1> &X);
+
+inline Eigen::Matrix<double, 3, 1> ToRotationVectorFromQuaternion(
+ const Eigen::Quaternion<double> &X) {
+ return ToRotationVectorFromQuaternion(X.coeffs());
+}
+
+// Converts from a rotation vector to a quaternion. If you supply max_angle_cap,
+// then the rotation vector's magnitude will be clipped to be no more than
+// max_angle_cap before being converted to a quaternion.
+Eigen::Matrix<double, 4, 1> ToQuaternionFromRotationVector(
+ const Eigen::Matrix<double, 3, 1> &X,
+ const double max_angle_cap = std::numeric_limits<double>::infinity());
+
+} // namespace controls
+} // namespace aos
+
+#endif // AOS_CONTROLS_QUATERNION_UTILS_H_
diff --git a/aos/controls/quaternion_utils_test.cc b/aos/controls/quaternion_utils_test.cc
new file mode 100644
index 0000000..ec410db
--- /dev/null
+++ b/aos/controls/quaternion_utils_test.cc
@@ -0,0 +1,152 @@
+#include "Eigen/Dense"
+
+#include <random>
+
+#include "aos/controls/quaternion_utils.h"
+#include "aos/testing/random_seed.h"
+#include "glog/logging.h"
+#include "gtest/gtest.h"
+
+namespace aos {
+namespace controls {
+namespace testing {
+
+// Tests that small perturbations around a couple quaternions averaged out
+// return the original quaternion.
+TEST(DownEstimatorTest, QuaternionMean) {
+ Eigen::Matrix<double, 4, 7> vectors;
+ vectors.col(0) << 0, 0, 0, 1;
+ for (int i = 0; i < 3; ++i) {
+ Eigen::Matrix<double, 4, 1> perturbation;
+ perturbation.setZero();
+ perturbation(i, 0) = 0.1;
+
+ vectors.col(i * 2 + 1) = vectors.col(0) + perturbation;
+ vectors.col(i * 2 + 2) = vectors.col(0) - perturbation;
+ }
+
+ for (int i = 0; i < 7; ++i) {
+ vectors.col(i).normalize();
+ }
+
+ Eigen::Matrix<double, 4, 1> mean = QuaternionMean(vectors);
+
+ for (int i = 0; i < 4; ++i) {
+ EXPECT_NEAR(mean(i, 0), vectors(i, 0), 0.001) << ": Failed on index " << i;
+ }
+}
+
+// Tests that ToRotationVectorFromQuaternion works for a 0 rotation.
+TEST(DownEstimatorTest, ToRotationVectorFromQuaternionAtZero) {
+ Eigen::Matrix<double, 3, 1> vector = ToRotationVectorFromQuaternion(
+ Eigen::Quaternion<double>(
+ Eigen::AngleAxis<double>(0.0, Eigen::Vector3d::UnitX()))
+ .coeffs());
+
+ EXPECT_NEAR(0.0, (vector - Eigen::Vector3d::Zero()).norm(), 1e-4);
+}
+
+// Tests that ToRotationVectorFromQuaternion works for a real rotation.
+TEST(DownEstimatorTest, ToRotationVectorFromQuaternion) {
+ Eigen::Matrix<double, 3, 1> vector = ToRotationVectorFromQuaternion(
+ Eigen::Quaternion<double>(
+ Eigen::AngleAxis<double>(M_PI * 0.5, Eigen::Vector3d::UnitX()))
+ .coeffs());
+
+ EXPECT_NEAR(0.0, (vector - Eigen::Vector3d::UnitX() * M_PI * 0.5).norm(),
+ 1e-4);
+}
+
+// Tests that ToRotationVectorFromQuaternion works for a solution with negative
+// coefficients.
+TEST(DownEstimatorTest, ToRotationVectorFromQuaternionNegative) {
+ Eigen::Matrix<double, 3, 1> vector = ToRotationVectorFromQuaternion(
+ Eigen::Quaternion<double>(
+ -Eigen::Quaternion<double>(
+ Eigen::AngleAxis<double>(M_PI * 0.5, Eigen::Vector3d::UnitX()))
+ .coeffs())
+ .coeffs());
+
+ EXPECT_NEAR(0.0, (vector - Eigen::Vector3d::UnitX() * M_PI * 0.5).norm(),
+ 1e-4);
+}
+
+// Tests that ToQuaternionFromRotationVector works for a 0 rotation.
+TEST(DownEstimatorTest, ToQuaternionFromRotationVectorAtZero) {
+ Eigen::Matrix<double, 4, 1> quaternion =
+ ToQuaternionFromRotationVector(Eigen::Vector3d::Zero());
+
+ EXPECT_NEAR(
+ 0.0,
+ (quaternion - Eigen::Quaternion<double>(
+ Eigen::AngleAxis<double>(0.0, Eigen::Vector3d::UnitX()))
+ .coeffs())
+ .norm(),
+ 1e-4);
+}
+
+// Tests that ToQuaternionFromRotationVector works for a real rotation.
+TEST(DownEstimatorTest, ToQuaternionFromRotationVector) {
+ Eigen::Matrix<double, 4, 1> quaternion =
+ ToQuaternionFromRotationVector(Eigen::Vector3d::UnitX() * M_PI * 0.5);
+
+ EXPECT_NEAR(0.0,
+ (quaternion - Eigen::Quaternion<double>(
+ Eigen::AngleAxis<double>(
+ M_PI * 0.5, Eigen::Vector3d::UnitX()))
+ .coeffs())
+
+ .norm(),
+ 1e-4);
+}
+
+// Tests that ToQuaternionFromRotationVector correctly clips a rotation vector
+// that is too large in magnitude.
+TEST(DownEstimatorTest, ToQuaternionFromLargeRotationVector) {
+ const double kMaxAngle = 2.0;
+ const Eigen::Vector3d rotation_vector =
+ Eigen::Vector3d::UnitX() * kMaxAngle * 2.0;
+ const Eigen::Matrix<double, 3, 1> clipped_vector =
+ ToRotationVectorFromQuaternion(
+ ToQuaternionFromRotationVector(rotation_vector, kMaxAngle));
+
+ EXPECT_NEAR(0.0, (rotation_vector / 2.0 - clipped_vector).norm(), 1e-4);
+}
+
+// Tests that ToQuaternionFromRotationVector and ToRotationVectorFromQuaternion
+// works for random rotations.
+TEST(DownEstimatorTest, RandomQuaternions) {
+ std::mt19937 generator(aos::testing::RandomSeed());
+ std::uniform_real_distribution<double> random_scalar(-1.0, 1.0);
+
+ for (int i = 0; i < 1000; ++i) {
+ Eigen::Matrix<double, 3, 1> axis;
+ axis << random_scalar(generator), random_scalar(generator),
+ random_scalar(generator);
+ EXPECT_GE(axis.norm(), 1e-6);
+ axis.normalize();
+
+ const double angle = random_scalar(generator) * M_PI;
+
+ Eigen::Matrix<double, 4, 1> quaternion =
+ ToQuaternionFromRotationVector(axis * angle);
+
+ Eigen::Quaternion<double> answer(Eigen::AngleAxis<double>(angle, axis));
+
+ EXPECT_NEAR(quaternion(3, 0), std::cos(angle / 2.0), 1e-8);
+ EXPECT_NEAR(answer.w(), std::cos(angle / 2.0), 1e-8);
+
+ EXPECT_NEAR(1.0, (answer.coeffs() * quaternion.transpose()).norm(), 1e-6);
+
+ const Eigen::Matrix<double, 3, 1> recalculated_axis =
+ ToRotationVectorFromQuaternion(quaternion);
+
+ EXPECT_NEAR(std::abs(angle), recalculated_axis.norm(), 1e-8);
+
+ EXPECT_NEAR(0.0, (axis * angle - recalculated_axis).norm(), 1e-8);
+ }
+}
+
+} // namespace testing
+} // namespace controls
+} // namespace aos
diff --git a/aos/events/BUILD b/aos/events/BUILD
index d35f206..39a6a54 100644
--- a/aos/events/BUILD
+++ b/aos/events/BUILD
@@ -74,6 +74,7 @@
"//aos:configuration",
"//aos:configuration_fbs",
"//aos:flatbuffers",
+ "//aos:ftrace",
"//aos/ipc_lib:data_alignment",
"//aos/logging:implementations",
"//aos/time",
diff --git a/aos/events/event_loop.cc b/aos/events/event_loop.cc
index c03425e..e69fd0d 100644
--- a/aos/events/event_loop.cc
+++ b/aos/events/event_loop.cc
@@ -14,6 +14,7 @@
RawSender::RawSender(EventLoop *event_loop, const Channel *channel)
: event_loop_(event_loop),
channel_(channel),
+ ftrace_prefix_(configuration::StrippedChannelToString(channel)),
timing_(event_loop_->ChannelIndex(channel)) {
event_loop_->NewSender(this);
}
@@ -23,6 +24,7 @@
RawFetcher::RawFetcher(EventLoop *event_loop, const Channel *channel)
: event_loop_(event_loop),
channel_(channel),
+ ftrace_prefix_(configuration::StrippedChannelToString(channel)),
timing_(event_loop_->ChannelIndex(channel)) {
context_.monotonic_event_time = monotonic_clock::min_time;
context_.monotonic_remote_time = monotonic_clock::min_time;
@@ -440,12 +442,20 @@
namespace {
bool CompareEvents(const EventLoopEvent *first, const EventLoopEvent *second) {
- return first->event_time() > second->event_time();
+ if (first->event_time() > second->event_time()) {
+ return true;
+ }
+ if (first->event_time() < second->event_time()) {
+ return false;
+ }
+ return first->generation() > second->generation();
}
} // namespace
void EventLoop::AddEvent(EventLoopEvent *event) {
DCHECK(std::find(events_.begin(), events_.end(), event) == events_.end());
+ DCHECK(event->generation() == 0);
+ event->set_generation(++event_generation_);
events_.push_back(event);
std::push_heap(events_.begin(), events_.end(), CompareEvents);
}
@@ -453,6 +463,7 @@
void EventLoop::RemoveEvent(EventLoopEvent *event) {
auto e = std::find(events_.begin(), events_.end(), event);
if (e != events_.end()) {
+ DCHECK(event->generation() != 0);
events_.erase(e);
std::make_heap(events_.begin(), events_.end(), CompareEvents);
event->Invalidate();
diff --git a/aos/events/event_loop.h b/aos/events/event_loop.h
index 7634479..81dfe42 100644
--- a/aos/events/event_loop.h
+++ b/aos/events/event_loop.h
@@ -14,6 +14,7 @@
#include "aos/events/event_loop_generated.h"
#include "aos/events/timing_statistics.h"
#include "aos/flatbuffers.h"
+#include "aos/ftrace.h"
#include "aos/ipc_lib/data_alignment.h"
#include "aos/json_to_flatbuffer.h"
#include "aos/time/time.h"
@@ -92,10 +93,12 @@
virtual std::pair<bool, monotonic_clock::time_point> DoFetchNext() = 0;
virtual std::pair<bool, monotonic_clock::time_point> DoFetch() = 0;
- EventLoop *event_loop_;
- const Channel *channel_;
+ EventLoop *const event_loop_;
+ const Channel *const channel_;
+ const std::string ftrace_prefix_;
internal::RawFetcherTiming timing_;
+ Ftrace ftrace_;
};
// Raw version of sender. Sends a block of data. This is used for reflection
@@ -174,10 +177,12 @@
aos::realtime_clock::time_point realtime_remote_time,
uint32_t remote_queue_index) = 0;
- EventLoop *event_loop_;
- const Channel *channel_;
+ EventLoop *const event_loop_;
+ const Channel *const channel_;
+ const std::string ftrace_prefix_;
internal::RawSenderTiming timing_;
+ Ftrace ftrace_;
ChannelPreallocatedAllocator fbb_allocator_{nullptr, 0, nullptr};
};
@@ -221,6 +226,9 @@
: nullptr;
}
+ // Returns the channel this fetcher uses
+ const Channel *channel() const { return fetcher_->channel(); }
+
// Returns the context holding timestamps and other metadata about the
// message.
const Context &context() const { return fetcher_->context(); }
@@ -228,6 +236,9 @@
const T &operator*() const { return *get(); }
const T *operator->() const { return get(); }
+ // Returns true if this fetcher is valid and connected to a channel.
+ operator bool() const { return static_cast<bool>(fetcher_); }
+
private:
friend class EventLoop;
Fetcher(::std::unique_ptr<RawFetcher> fetcher)
@@ -358,6 +369,7 @@
std::string name_;
internal::TimerTiming timing_;
+ Ftrace ftrace_;
};
// Interface for phased loops. They are built on timers.
@@ -404,6 +416,7 @@
int cycles_elapsed_ = 0;
internal::TimerTiming timing_;
+ Ftrace ftrace_;
};
inline cpu_set_t MakeCpusetFromCpus(std::initializer_list<int> cpus) {
@@ -425,6 +438,14 @@
virtual monotonic_clock::time_point monotonic_now() = 0;
virtual realtime_clock::time_point realtime_now() = 0;
+ // Returns true if the channel exists in the configuration.
+ template <typename T>
+ bool HasChannel(const std::string_view channel_name) {
+ return configuration::GetChannel(configuration_, channel_name,
+ T::GetFullyQualifiedName(), name(),
+ node()) != nullptr;
+ }
+
// Note, it is supported to create:
// multiple fetchers, and (one sender or one watcher) per <name, type>
// tuple.
@@ -594,12 +615,18 @@
// watcher must exist before calling this.
WatcherState *GetWatcherState(const Channel *channel);
- // Returns a Sender's protected RawSender
+ // Returns a Sender's protected RawSender.
template <typename T>
static RawSender *GetRawSender(aos::Sender<T> *sender) {
return sender->sender_.get();
}
+ // Returns a Fetcher's protected RawFetcher.
+ template <typename T>
+ static RawFetcher *GetRawFetcher(aos::Fetcher<T> *fetcher) {
+ return fetcher->fetcher_.get();
+ }
+
// Context available for watchers, timers, and phased loops.
Context context_;
@@ -649,6 +676,7 @@
void ReserveEvents();
std::vector<EventLoopEvent *> events_;
+ size_t event_generation_ = 1;
// If true, don't send AOS_LOG to /aos
bool skip_logger_ = false;
diff --git a/aos/events/event_loop_event.h b/aos/events/event_loop_event.h
index 6438da1..808a4fe 100644
--- a/aos/events/event_loop_event.h
+++ b/aos/events/event_loop_event.h
@@ -12,21 +12,31 @@
virtual ~EventLoopEvent() {}
bool valid() const { return event_time_ != monotonic_clock::max_time; }
- void Invalidate() { event_time_ = monotonic_clock::max_time; }
+ void Invalidate() {
+ event_time_ = monotonic_clock::max_time;
+ generation_ = 0;
+ }
monotonic_clock::time_point event_time() const {
DCHECK(valid());
return event_time_;
}
-
- virtual void HandleEvent() = 0;
-
void set_event_time(monotonic_clock::time_point event_time) {
event_time_ = event_time;
}
+ // Internal book-keeping for EventLoop.
+ size_t generation() const {
+ DCHECK(valid());
+ return generation_;
+ }
+ void set_generation(size_t generation) { generation_ = generation; }
+
+ virtual void HandleEvent() = 0;
+
private:
monotonic_clock::time_point event_time_ = monotonic_clock::max_time;
+ size_t generation_ = 0;
};
// Adapter class to implement EventLoopEvent by calling HandleEvent on T.
@@ -34,7 +44,7 @@
class EventHandler final : public EventLoopEvent {
public:
EventHandler(T *t) : t_(t) {}
- ~EventHandler() = default;
+ ~EventHandler() override = default;
void HandleEvent() override { t_->HandleEvent(); }
private:
diff --git a/aos/events/event_loop_param_test.cc b/aos/events/event_loop_param_test.cc
index fbcb62e..dc7c5d0 100644
--- a/aos/events/event_loop_param_test.cc
+++ b/aos/events/event_loop_param_test.cc
@@ -604,6 +604,18 @@
"/test");
}
+// Verify that creating too many senders fails.
+TEST_P(AbstractEventLoopDeathTest, TooManySenders) {
+ auto loop = Make();
+ std::vector<aos::Sender<TestMessage>> senders;
+ for (int i = 0; i < 10; ++i) {
+ senders.emplace_back(loop->MakeSender<TestMessage>("/test"));
+ }
+ EXPECT_DEATH({ loop->MakeSender<TestMessage>("/test"); },
+ "Failed to create sender on \\{ \"name\": \"/test\", \"type\": "
+ "\"aos.TestMessage\" \\}, too many senders.");
+}
+
// Verify that we can't create a sender inside OnRun.
TEST_P(AbstractEventLoopDeathTest, SenderInOnRun) {
auto loop1 = MakePrimary();
@@ -755,7 +767,7 @@
// Confirm that we have the right number of reports, and the contents are
// sane.
- VLOG(1) << FlatbufferToJson(report, true);
+ VLOG(1) << FlatbufferToJson(report, {.multi_line = true});
EXPECT_EQ(report.message().name()->string_view(), "primary");
@@ -818,6 +830,48 @@
EXPECT_EQ(iteration_list.size(), 3);
}
+// Verify that we can disable a timer during execution of another timer
+// scheduled for the same time, with one ordering of creation for the timers.
+//
+// Also schedule some more events to reshuffle the heap in EventLoop used for
+// tracking events to change up the order. This used to segfault
+// SimulatedEventLoop.
+TEST_P(AbstractEventLoopTest, TimerDisableOther) {
+ for (bool creation_order : {true, false}) {
+ for (bool setup_order : {true, false}) {
+ for (int shuffle_events = 0; shuffle_events < 5; ++shuffle_events) {
+ auto loop = MakePrimary();
+ aos::TimerHandler *test_timer, *ender_timer;
+ if (creation_order) {
+ test_timer = loop->AddTimer([]() {});
+ ender_timer =
+ loop->AddTimer([&test_timer]() { test_timer->Disable(); });
+ } else {
+ ender_timer =
+ loop->AddTimer([&test_timer]() { test_timer->Disable(); });
+ test_timer = loop->AddTimer([]() {});
+ }
+
+ const auto start = loop->monotonic_now();
+
+ for (int i = 0; i < shuffle_events; ++i) {
+ loop->AddTimer([]() {})->Setup(start + std::chrono::milliseconds(10));
+ }
+
+ if (setup_order) {
+ test_timer->Setup(start + ::std::chrono::milliseconds(20));
+ ender_timer->Setup(start + ::std::chrono::milliseconds(20));
+ } else {
+ ender_timer->Setup(start + ::std::chrono::milliseconds(20));
+ test_timer->Setup(start + ::std::chrono::milliseconds(20));
+ }
+ EndEventLoop(loop.get(), ::std::chrono::milliseconds(40));
+ Run();
+ }
+ }
+ }
+}
+
// Verifies that the event loop implementations detect when Channel is not a
// pointer into confguration()
TEST_P(AbstractEventLoopDeathTest, InvalidChannel) {
@@ -1127,7 +1181,7 @@
}
}
- VLOG(1) << FlatbufferToJson(report, true);
+ VLOG(1) << FlatbufferToJson(report, {.multi_line = true});
EXPECT_EQ(report.message().name()->string_view(), "primary");
@@ -1191,7 +1245,7 @@
}
}
- LOG(INFO) << FlatbufferToJson(primary_report, true);
+ LOG(INFO) << FlatbufferToJson(primary_report, {.multi_line = true});
EXPECT_EQ(primary_report.message().name()->string_view(), "primary");
@@ -1273,7 +1327,7 @@
}
// Check the watcher report.
- VLOG(1) << FlatbufferToJson(primary_report, true);
+ VLOG(1) << FlatbufferToJson(primary_report, {.multi_line = true});
EXPECT_EQ(primary_report.message().name()->string_view(), "primary");
@@ -1343,7 +1397,7 @@
}
}
- VLOG(1) << FlatbufferToJson(primary_report, true);
+ VLOG(1) << FlatbufferToJson(primary_report, {.multi_line = true});
EXPECT_EQ(primary_report.message().name()->string_view(), "primary");
diff --git a/aos/events/event_loop_tmpl.h b/aos/events/event_loop_tmpl.h
index 5534c83..877a946 100644
--- a/aos/events/event_loop_tmpl.h
+++ b/aos/events/event_loop_tmpl.h
@@ -1,7 +1,11 @@
#ifndef AOS_EVENTS_EVENT_LOOP_TMPL_H_
#define AOS_EVENTS_EVENT_LOOP_TMPL_H_
+#include <inttypes.h>
+#include <stdint.h>
+
#include <type_traits>
+
#include "aos/events/event_loop.h"
#include "glog/logging.h"
@@ -72,6 +76,13 @@
if (result.first) {
timing_.fetcher->mutate_count(timing_.fetcher->count() + 1);
const monotonic_clock::time_point monotonic_time = result.second;
+ ftrace_.FormatMessage(
+ "%.*s: fetch next: now=%" PRId64 " event=%" PRId64 " queue=%" PRIu32,
+ static_cast<int>(ftrace_prefix_.size()), ftrace_prefix_.data(),
+ static_cast<int64_t>(monotonic_time.time_since_epoch().count()),
+ static_cast<int64_t>(
+ context_.monotonic_event_time.time_since_epoch().count()),
+ context_.queue_index);
const float latency =
std::chrono::duration_cast<std::chrono::duration<float>>(
monotonic_time - context_.monotonic_event_time)
@@ -79,6 +90,12 @@
timing_.latency.Add(latency);
return true;
}
+ ftrace_.FormatMessage(
+ "%.*s: fetch next: still event=%" PRId64 " queue=%" PRIu32,
+ static_cast<int>(ftrace_prefix_.size()), ftrace_prefix_.data(),
+ static_cast<int64_t>(
+ context_.monotonic_event_time.time_since_epoch().count()),
+ context_.queue_index);
return false;
}
@@ -87,6 +104,13 @@
if (result.first) {
timing_.fetcher->mutate_count(timing_.fetcher->count() + 1);
const monotonic_clock::time_point monotonic_time = result.second;
+ ftrace_.FormatMessage(
+ "%.*s: fetch latest: now=%" PRId64 " event=%" PRId64 " queue=%" PRIu32,
+ static_cast<int>(ftrace_prefix_.size()), ftrace_prefix_.data(),
+ static_cast<int64_t>(monotonic_time.time_since_epoch().count()),
+ static_cast<int64_t>(
+ context_.monotonic_event_time.time_since_epoch().count()),
+ context_.queue_index);
const float latency =
std::chrono::duration_cast<std::chrono::duration<float>>(
monotonic_time - context_.monotonic_event_time)
@@ -94,6 +118,12 @@
timing_.latency.Add(latency);
return true;
}
+ ftrace_.FormatMessage(
+ "%.*s: fetch latest: still event=%" PRId64 " queue=%" PRIu32,
+ static_cast<int>(ftrace_prefix_.size()), ftrace_prefix_.data(),
+ static_cast<int64_t>(
+ context_.monotonic_event_time.time_since_epoch().count()),
+ context_.queue_index);
return false;
}
@@ -105,6 +135,11 @@
remote_queue_index)) {
timing_.size.Add(size);
timing_.sender->mutate_count(timing_.sender->count() + 1);
+ ftrace_.FormatMessage(
+ "%.*s: sent internal: event=%" PRId64 " queue=%" PRIu32,
+ static_cast<int>(ftrace_prefix_.size()), ftrace_prefix_.data(),
+ static_cast<int64_t>(monotonic_sent_time().time_since_epoch().count()),
+ sent_queue_index());
return true;
}
return false;
@@ -119,6 +154,11 @@
remote_queue_index)) {
timing_.size.Add(size);
timing_.sender->mutate_count(timing_.sender->count() + 1);
+ ftrace_.FormatMessage(
+ "%.*s: sent external: event=%" PRId64 " queue=%" PRIu32,
+ static_cast<int>(ftrace_prefix_.size()), ftrace_prefix_.data(),
+ static_cast<int64_t>(monotonic_sent_time().time_since_epoch().count()),
+ sent_queue_index());
return true;
}
return false;
@@ -138,6 +178,11 @@
event_loop_->context_.size = 0;
event_loop_->context_.data = nullptr;
+ ftrace_.FormatMessage(
+ "timer: %.*s: start now=%" PRId64 " event=%" PRId64,
+ static_cast<int>(name_.size()), name_.data(),
+ static_cast<int64_t>(monotonic_start_time.time_since_epoch().count()),
+ static_cast<int64_t>(event_time.time_since_epoch().count()));
{
const float start_latency =
std::chrono::duration_cast<std::chrono::duration<float>>(
@@ -149,6 +194,10 @@
fn_();
const monotonic_clock::time_point monotonic_end_time = get_time();
+ ftrace_.FormatMessage(
+ "timer: %.*s: end now=%" PRId64, static_cast<int>(name_.size()),
+ name_.data(),
+ static_cast<int64_t>(monotonic_end_time.time_since_epoch().count()));
const float handler_latency =
std::chrono::duration_cast<std::chrono::duration<float>>(
@@ -176,6 +225,13 @@
// Compute how many cycles elapsed and schedule the next wakeup.
Reschedule(schedule, monotonic_start_time);
+ ftrace_.FormatMessage(
+ "phased: %.*s: start now=%" PRId64 " event=%" PRId64 " cycles=%d",
+ static_cast<int>(name_.size()), name_.data(),
+ static_cast<int64_t>(monotonic_start_time.time_since_epoch().count()),
+ static_cast<int64_t>(
+ phased_loop_.sleep_time().time_since_epoch().count()),
+ cycles_elapsed_);
{
const float start_latency =
std::chrono::duration_cast<std::chrono::duration<float>>(
@@ -190,6 +246,10 @@
cycles_elapsed_ = 0;
const monotonic_clock::time_point monotonic_end_time = get_time();
+ ftrace_.FormatMessage(
+ "phased: %.*s: end now=%" PRId64, static_cast<int>(name_.size()),
+ name_.data(),
+ static_cast<int64_t>(monotonic_end_time.time_since_epoch().count()));
const float handler_latency =
std::chrono::duration_cast<std::chrono::duration<float>>(
@@ -210,7 +270,9 @@
WatcherState(
EventLoop *event_loop, const Channel *channel,
std::function<void(const Context &context, const void *message)> fn)
- : channel_index_(event_loop->ChannelIndex(channel)), fn_(std::move(fn)) {}
+ : channel_index_(event_loop->ChannelIndex(channel)),
+ ftrace_prefix_(configuration::StrippedChannelToString(channel)),
+ fn_(std::move(fn)) {}
virtual ~WatcherState() {}
@@ -222,6 +284,13 @@
CheckChannelDataAlignment(context.data, context.size);
}
const monotonic_clock::time_point monotonic_start_time = get_time();
+ ftrace_.FormatMessage(
+ "%.*s: watcher start: now=%" PRId64 " event=%" PRId64 " queue=%" PRIu32,
+ static_cast<int>(ftrace_prefix_.size()), ftrace_prefix_.data(),
+ static_cast<int64_t>(monotonic_start_time.time_since_epoch().count()),
+ static_cast<int64_t>(
+ context.monotonic_event_time.time_since_epoch().count()),
+ context.queue_index);
{
const float start_latency =
std::chrono::duration_cast<std::chrono::duration<float>>(
@@ -233,6 +302,10 @@
fn_(context, context.data);
const monotonic_clock::time_point monotonic_end_time = get_time();
+ ftrace_.FormatMessage(
+ "%.*s: watcher end: now=%" PRId64,
+ static_cast<int>(ftrace_prefix_.size()), ftrace_prefix_.data(),
+ static_cast<int64_t>(monotonic_end_time.time_since_epoch().count()));
const float handler_latency =
std::chrono::duration_cast<std::chrono::duration<float>>(
@@ -250,12 +323,15 @@
protected:
const int channel_index_;
+ const std::string ftrace_prefix_;
- std::function<void(const Context &context, const void *message)> fn_;
+ const std::function<void(const Context &context, const void *message)> fn_;
internal::TimingStatistic wakeup_latency_;
internal::TimingStatistic handler_time_;
timing::Watcher *watcher_ = nullptr;
+
+ Ftrace ftrace_;
};
template <typename T>
diff --git a/aos/events/event_scheduler.cc b/aos/events/event_scheduler.cc
index a9ae7c0..f985eb1 100644
--- a/aos/events/event_scheduler.cc
+++ b/aos/events/event_scheduler.cc
@@ -14,6 +14,22 @@
}
void EventScheduler::Deschedule(EventScheduler::Token token) {
+ // We basically want to DCHECK some nontrivial logic. Guard it with NDEBUG to ensure the compiler
+ // realizes it's all unnecessary when not doing debug checks.
+#ifndef NDEBUG
+ {
+ bool found = false;
+ auto i = events_list_.begin();
+ while (i != events_list_.end()) {
+ if (i == token) {
+ CHECK(!found) << ": The same iterator is in the multimap twice??";
+ found = true;
+ }
+ ++i;
+ }
+ CHECK(found) << ": Trying to deschedule an event which is not scheduled";
+ }
+#endif
events_list_.erase(token);
}
diff --git a/aos/events/logging/BUILD b/aos/events/logging/BUILD
index 52ea5a8..b5a4377 100644
--- a/aos/events/logging/BUILD
+++ b/aos/events/logging/BUILD
@@ -62,6 +62,7 @@
srcs = [
"log_cat.cc",
],
+ visibility = ["//visibility:public"],
deps = [
":logger",
"//aos:configuration",
diff --git a/aos/events/logging/log_cat.cc b/aos/events/logging/log_cat.cc
index a938367..d3095cb 100644
--- a/aos/events/logging/log_cat.cc
+++ b/aos/events/logging/log_cat.cc
@@ -1,4 +1,10 @@
+#include <algorithm>
#include <iostream>
+#include <memory>
+#include <optional>
+#include <string>
+#include <string_view>
+#include <vector>
#include "aos/configuration.h"
#include "aos/events/logging/logger.h"
@@ -13,11 +19,47 @@
DEFINE_string(type, "",
"Channel type to match for printing out channels. Empty means no "
"type filter.");
+DEFINE_bool(fetch, false,
+ "If true, also print out the messages from before the start of the "
+ "log file");
DEFINE_bool(raw, false,
"If true, just print the data out unsorted and unparsed");
+DEFINE_bool(format_raw, true,
+ "If true and --raw is specified, print out raw data, but use the "
+ "schema to format the data.");
DEFINE_int32(max_vector_size, 100,
"If positive, vectors longer than this will not be printed");
+// Print the flatbuffer out to stdout, both to remove the unnecessary cruft from
+// glog and to allow the user to readily redirect just the logged output
+// independent of any debugging information on stderr.
+void PrintMessage(const std::string_view node_name, const aos::Channel *channel,
+ const aos::Context &context) {
+ if (context.monotonic_remote_time != context.monotonic_event_time) {
+ std::cout << node_name << context.realtime_event_time << " ("
+ << context.monotonic_event_time << ") sent "
+ << context.realtime_remote_time << " ("
+ << context.monotonic_remote_time << ") "
+ << channel->name()->c_str() << ' ' << channel->type()->c_str()
+ << ": "
+ << aos::FlatbufferToJson(
+ channel->schema(),
+ static_cast<const uint8_t *>(context.data),
+ {false, static_cast<size_t>(FLAGS_max_vector_size)})
+ << std::endl;
+ } else {
+ std::cout << node_name << context.realtime_event_time << " ("
+ << context.monotonic_event_time << ") "
+ << channel->name()->c_str() << ' ' << channel->type()->c_str()
+ << ": "
+ << aos::FlatbufferToJson(
+ channel->schema(),
+ static_cast<const uint8_t *>(context.data),
+ {false, static_cast<size_t>(FLAGS_max_vector_size)})
+ << std::endl;
+ }
+}
+
int main(int argc, char **argv) {
gflags::SetUsageMessage(
"Usage:\n"
@@ -45,9 +87,28 @@
if (!message) {
break;
}
+ const aos::Channel *channel =
+ reader.log_file_header()->configuration()->channels()->Get(
+ message.value().message().channel_index());
- std::cout << aos::FlatbufferToJson(message.value(), FLAGS_max_vector_size)
- << std::endl;
+ if (FLAGS_format_raw && message.value().message().data() != nullptr) {
+ std::cout << aos::configuration::StrippedChannelToString(channel) << " "
+ << aos::FlatbufferToJson(
+ message.value(),
+ {.multi_line = false, .max_vector_size = 4})
+ << ": "
+ << aos::FlatbufferToJson(
+ channel->schema(),
+ message.value().message().data()->data(),
+ {false, static_cast<size_t>(FLAGS_max_vector_size)})
+ << std::endl;
+ } else {
+ std::cout << aos::configuration::StrippedChannelToString(channel) << " "
+ << aos::FlatbufferToJson(
+ message.value(),
+ {false, static_cast<size_t>(FLAGS_max_vector_size)})
+ << std::endl;
+ }
}
return 0;
}
@@ -63,19 +124,28 @@
}
aos::logger::LogReader reader(logfiles);
- reader.Register();
+
+ aos::SimulatedEventLoopFactory event_loop_factory(reader.configuration());
+ reader.Register(&event_loop_factory);
std::vector<std::unique_ptr<aos::EventLoop>> printer_event_loops;
for (const aos::Node *node : reader.Nodes()) {
std::unique_ptr<aos::EventLoop> printer_event_loop =
- reader.event_loop_factory()->MakeEventLoop("printer", node);
+ event_loop_factory.MakeEventLoop("printer", node);
printer_event_loop->SkipTimingReport();
printer_event_loop->SkipAosLog();
+ struct MessageInfo {
+ std::string node_name;
+ std::unique_ptr<aos::RawFetcher> fetcher;
+ };
+ std::vector<MessageInfo> messages_before_start;
+
bool found_channel = false;
const flatbuffers::Vector<flatbuffers::Offset<aos::Channel>> *channels =
- reader.configuration()->channels();
+ printer_event_loop->configuration()->channels();
+
for (flatbuffers::uoffset_t i = 0; i < channels->size(); i++) {
const aos::Channel *channel = channels->Get(i);
const flatbuffers::string_view name = channel->name()->string_view();
@@ -93,37 +163,38 @@
: std::string(node->name()->string_view()) + " ";
CHECK_NOTNULL(channel->schema());
+
+ // Fetch the last message on this channel from before the log start
+ // time.
+ if (FLAGS_fetch) {
+ std::unique_ptr<aos::RawFetcher> fetcher =
+ printer_event_loop->MakeRawFetcher(channel);
+ if (fetcher->Fetch()) {
+ MessageInfo message{.node_name = node_name,
+ .fetcher = std::move(fetcher)};
+ // Insert it sorted into the vector so we can print in time order
+ // instead of channel order at the start.
+ auto it = std::lower_bound(
+ messages_before_start.begin(), messages_before_start.end(),
+ message, [](const MessageInfo &lhs, const MessageInfo &rhs) {
+ if (lhs.fetcher->context().monotonic_event_time <
+ rhs.fetcher->context().monotonic_event_time) {
+ return true;
+ }
+ if (lhs.fetcher->context().monotonic_event_time >
+ rhs.fetcher->context().monotonic_event_time) {
+ return false;
+ }
+ return lhs.fetcher->channel() < rhs.fetcher->channel();
+ });
+ messages_before_start.insert(it, std::move(message));
+ }
+ }
+
printer_event_loop->MakeRawWatcher(
channel, [channel, node_name](const aos::Context &context,
- const void *message) {
- // Print the flatbuffer out to stdout, both to remove the
- // unnecessary cruft from glog and to allow the user to readily
- // redirect just the logged output independent of any debugging
- // information on stderr.
- if (context.monotonic_remote_time !=
- context.monotonic_event_time) {
- std::cout << node_name << context.realtime_event_time << " ("
- << context.monotonic_event_time << ") sent "
- << context.realtime_remote_time << " ("
- << context.monotonic_remote_time << ") "
- << channel->name()->c_str() << ' '
- << channel->type()->c_str() << ": "
- << aos::FlatbufferToJson(
- channel->schema(),
- static_cast<const uint8_t *>(message),
- FLAGS_max_vector_size)
- << std::endl;
- } else {
- std::cout << node_name << context.realtime_event_time << " ("
- << context.monotonic_event_time << ") "
- << channel->name()->c_str() << ' '
- << channel->type()->c_str() << ": "
- << aos::FlatbufferToJson(
- channel->schema(),
- static_cast<const uint8_t *>(message),
- FLAGS_max_vector_size)
- << std::endl;
- }
+ const void * /*message*/) {
+ PrintMessage(node_name, channel, context);
});
found_channel = true;
}
@@ -132,10 +203,27 @@
if (!found_channel) {
LOG(FATAL) << "Could not find any channels";
}
+
+ // Print the messages from before the log start time.
+ // TODO(austin): Sort between nodes too when it becomes annoying enough.
+ for (const MessageInfo &message : messages_before_start) {
+ PrintMessage(message.node_name, message.fetcher->channel(),
+ message.fetcher->context());
+ }
printer_event_loops.emplace_back(std::move(printer_event_loop));
+
+ std::cout << std::endl;
+ std::cout << "Log starting at " << reader.realtime_start_time() << " ("
+ << reader.monotonic_start_time() << ")";
+ std::cout << std::endl << std::endl;
}
- reader.event_loop_factory()->Run();
+ if (FLAGS_fetch) {
+ // New line to separate fetched messages from non-fetched messages.
+ std::cout << std::endl;
+ }
+
+ event_loop_factory.Run();
aos::Cleanup();
return 0;
diff --git a/aos/events/logging/log_edit.cc b/aos/events/logging/log_edit.cc
index ff263f8..0dca904 100644
--- a/aos/events/logging/log_edit.cc
+++ b/aos/events/logging/log_edit.cc
@@ -18,8 +18,7 @@
"If provided, this is the path to the JSON with the log file header.");
int main(int argc, char **argv) {
- gflags::SetUsageMessage(
- R"(This tool lets us manipulate log files.)");
+ gflags::SetUsageMessage(R"(This tool lets us manipulate log files.)");
aos::InitGoogle(&argc, &argv);
if (!FLAGS_header.empty()) {
@@ -53,7 +52,8 @@
} else {
aos::logger::MessageReader reader(FLAGS_logfile);
aos::util::WriteStringToFileOrDie(
- FLAGS_header, aos::FlatbufferToJson(reader.log_file_header(), true));
+ FLAGS_header, aos::FlatbufferToJson(reader.log_file_header(),
+ {.multi_line = true}));
}
}
diff --git a/aos/events/logging/logger.h b/aos/events/logging/logger.h
index ce21598..c08d6d4 100644
--- a/aos/events/logging/logger.h
+++ b/aos/events/logging/logger.h
@@ -340,18 +340,25 @@
RemapLoggedChannel(name, T::GetFullyQualifiedName(), add_prefix);
}
+ template <typename T>
+ bool HasChannel(std::string_view name) {
+ return configuration::GetChannel(log_file_header()->configuration(), name,
+ T::GetFullyQualifiedName(), "",
+ nullptr) != nullptr;
+ }
+
SimulatedEventLoopFactory *event_loop_factory() {
return event_loop_factory_;
}
+ const LogFileHeader *log_file_header() const {
+ return &log_file_header_.message();
+ }
+
private:
const Channel *RemapChannel(const EventLoop *event_loop,
const Channel *channel);
- const LogFileHeader *log_file_header() const {
- return &log_file_header_.message();
- }
-
// Queues at least max_out_of_order_duration_ messages into channels_.
void QueueMessages();
// Handle constructing a configuration with all the additional remapped
diff --git a/aos/events/shm_event_loop.cc b/aos/events/shm_event_loop.cc
index ce34ad1..2383050 100644
--- a/aos/events/shm_event_loop.cc
+++ b/aos/events/shm_event_loop.cc
@@ -47,6 +47,8 @@
namespace aos {
+using namespace shm_event_loop_internal;
+
void SetShmBase(const std::string_view base) {
FLAGS_shm_base = std::string(base) + "/dev/shm/aos";
}
@@ -61,6 +63,31 @@
return ShmFolder(channel) + channel->type()->str() + ".v2";
}
+void PageFaultData(char *data, size_t size) {
+ // This just has to divide the actual page size. Being smaller will make this
+ // a bit slower than necessary, but not much. 1024 is a pretty conservative
+ // choice (most pages are probably 4096).
+ static constexpr size_t kPageSize = 1024;
+ const size_t pages = (size + kPageSize - 1) / kPageSize;
+ for (size_t i = 0; i < pages; ++i) {
+ char zero = 0;
+ // We need to ensure there's a writable pagetable entry, but avoid modifying
+ // the data.
+ //
+ // Even if you lock the data into memory, some kernels still seem to lazily
+ // create the actual pagetable entries. This means we need to somehow
+ // "write" to the page.
+ //
+ // Also, this takes place while other processes may be concurrently
+ // opening/initializing the memory, so we need to avoid corrupting that.
+ //
+ // This is the simplest operation I could think of which achieves that:
+ // "store 0 if it's already 0".
+ __atomic_compare_exchange_n(&data[i * kPageSize], &zero, 0, true,
+ __ATOMIC_RELAXED, __ATOMIC_RELAXED);
+ }
+}
+
class MMapedQueue {
public:
MMapedQueue(const Channel *channel,
@@ -82,7 +109,7 @@
// that fails, the file has already been created and we can open it
// normally.. Once the file has been created it wil never be deleted.
int fd = open(path.c_str(), O_RDWR | O_CREAT | O_EXCL,
- O_CLOEXEC | FLAGS_permissions);
+ O_CLOEXEC | FLAGS_permissions);
if (fd == -1 && errno == EEXIST) {
VLOG(1) << path << " already created.";
// File already exists.
@@ -112,13 +139,12 @@
data_ = mmap(NULL, size_, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);
PCHECK(data_ != MAP_FAILED);
PCHECK(close(fd) == 0);
+ PageFaultData(static_cast<char *>(data_), size_);
ipc_lib::InitializeLocklessQueueMemory(memory(), config_);
}
- ~MMapedQueue() {
- PCHECK(munmap(data_, size_) == 0);
- }
+ ~MMapedQueue() { PCHECK(munmap(data_, size_) == 0); }
ipc_lib::LocklessQueueMemory *memory() const {
return reinterpret_cast<ipc_lib::LocklessQueueMemory *>(data_);
@@ -160,13 +186,14 @@
}
}
-namespace internal {
+namespace shm_event_loop_internal {
class SimpleShmFetcher {
public:
- explicit SimpleShmFetcher(EventLoop *event_loop, const Channel *channel,
+ explicit SimpleShmFetcher(ShmEventLoop *event_loop, const Channel *channel,
bool copy_data)
- : channel_(channel),
+ : event_loop_(event_loop),
+ channel_(channel),
lockless_queue_memory_(
channel,
chrono::ceil<chrono::seconds>(chrono::nanoseconds(
@@ -237,9 +264,12 @@
"behind. "
<< configuration::CleanedChannelToString(channel_);
- CHECK(read_result != ipc_lib::LocklessQueue::ReadResult::TOO_OLD)
- << ": The next message is no longer available. "
- << configuration::CleanedChannelToString(channel_);
+ if (read_result == ipc_lib::LocklessQueue::ReadResult::TOO_OLD) {
+ event_loop_->SendTimingReport();
+ LOG(FATAL) << "The next message is no longer available. "
+ << configuration::CleanedChannelToString(channel_);
+ }
+
return read_result == ipc_lib::LocklessQueue::ReadResult::GOOD;
}
@@ -298,9 +328,12 @@
// We fell behind between when we read the index and read the value.
// This isn't worth recovering from since this means we went to sleep
// for a long time in the middle of this function.
- CHECK(read_result != ipc_lib::LocklessQueue::ReadResult::TOO_OLD)
- << ": The next message is no longer available. "
- << configuration::CleanedChannelToString(channel_);
+ if (read_result == ipc_lib::LocklessQueue::ReadResult::TOO_OLD) {
+ event_loop_->SendTimingReport();
+ LOG(FATAL) << "The next message is no longer available. "
+ << configuration::CleanedChannelToString(channel_);
+ }
+
return read_result == ipc_lib::LocklessQueue::ReadResult::GOOD;
}
@@ -316,6 +349,13 @@
return lockless_queue_memory_.GetSharedMemory();
}
+ absl::Span<char> GetPrivateMemory() const {
+ CHECK(copy_data());
+ return absl::Span<char>(
+ const_cast<SimpleShmFetcher *>(this)->data_storage_start(),
+ lockless_queue_.message_data_size());
+ }
+
private:
char *data_storage_start() {
if (!copy_data()) return nullptr;
@@ -323,6 +363,7 @@
}
bool copy_data() const { return static_cast<bool>(data_storage_); }
+ aos::ShmEventLoop *event_loop_;
const Channel *const channel_;
MMapedQueue lockless_queue_memory_;
ipc_lib::LocklessQueue lockless_queue_;
@@ -338,7 +379,7 @@
class ShmFetcher : public RawFetcher {
public:
- explicit ShmFetcher(EventLoop *event_loop, const Channel *channel)
+ explicit ShmFetcher(ShmEventLoop *event_loop, const Channel *channel)
: RawFetcher(event_loop, channel),
simple_shm_fetcher_(event_loop, channel, true) {}
@@ -360,6 +401,10 @@
return std::make_pair(false, monotonic_clock::min_time);
}
+ absl::Span<char> GetPrivateMemory() const {
+ return simple_shm_fetcher_.GetPrivateMemory();
+ }
+
private:
SimpleShmFetcher simple_shm_fetcher_;
};
@@ -374,10 +419,22 @@
event_loop->configuration()->channel_storage_duration()))),
lockless_queue_(lockless_queue_memory_.memory(),
lockless_queue_memory_.config()),
- lockless_queue_sender_(lockless_queue_.MakeSender()) {}
+ lockless_queue_sender_(
+ VerifySender(lockless_queue_.MakeSender(), channel)) {}
~ShmSender() override {}
+ static ipc_lib::LocklessQueue::Sender VerifySender(
+ std::optional<ipc_lib::LocklessQueue::Sender> &&sender,
+ const Channel *channel) {
+ if (sender) {
+ return std::move(sender.value());
+ }
+ LOG(FATAL) << "Failed to create sender on "
+ << configuration::CleanedChannelToString(channel)
+ << ", too many senders.";
+ }
+
void *data() override { return lockless_queue_sender_.Data(); }
size_t size() override { return lockless_queue_sender_.size(); }
bool DoSend(size_t length,
@@ -421,18 +478,18 @@
};
// Class to manage the state for a Watcher.
-class WatcherState : public aos::WatcherState {
+class ShmWatcherState : public WatcherState {
public:
- WatcherState(
+ ShmWatcherState(
ShmEventLoop *event_loop, const Channel *channel,
std::function<void(const Context &context, const void *message)> fn,
bool copy_data)
- : aos::WatcherState(event_loop, channel, std::move(fn)),
+ : WatcherState(event_loop, channel, std::move(fn)),
event_loop_(event_loop),
event_(this),
simple_shm_fetcher_(event_loop, channel, copy_data) {}
- ~WatcherState() override { event_loop_->RemoveEvent(&event_); }
+ ~ShmWatcherState() override { event_loop_->RemoveEvent(&event_); }
void Startup(EventLoop *event_loop) override {
simple_shm_fetcher_.PointAtNextQueueIndex();
@@ -477,14 +534,14 @@
bool has_new_data_ = false;
ShmEventLoop *event_loop_;
- EventHandler<WatcherState> event_;
+ EventHandler<ShmWatcherState> event_;
SimpleShmFetcher simple_shm_fetcher_;
};
// Adapter class to adapt a timerfd to a TimerHandler.
-class TimerHandlerState final : public TimerHandler {
+class ShmTimerHandler final : public TimerHandler {
public:
- TimerHandlerState(ShmEventLoop *shm_event_loop, ::std::function<void()> fn)
+ ShmTimerHandler(ShmEventLoop *shm_event_loop, ::std::function<void()> fn)
: TimerHandler(shm_event_loop, std::move(fn)),
shm_event_loop_(shm_event_loop),
event_(this) {
@@ -498,7 +555,7 @@
});
}
- ~TimerHandlerState() {
+ ~ShmTimerHandler() {
Disable();
shm_event_loop_->epoll_.DeleteFd(timerfd_.fd());
}
@@ -550,21 +607,22 @@
private:
ShmEventLoop *shm_event_loop_;
- EventHandler<TimerHandlerState> event_;
+ EventHandler<ShmTimerHandler> event_;
- TimerFd timerfd_;
+ internal::TimerFd timerfd_;
monotonic_clock::time_point base_;
monotonic_clock::duration repeat_offset_;
};
// Adapter class to the timerfd and PhasedLoop.
-class PhasedLoopHandler final : public ::aos::PhasedLoopHandler {
+class ShmPhasedLoopHandler final : public PhasedLoopHandler {
public:
- PhasedLoopHandler(ShmEventLoop *shm_event_loop, ::std::function<void(int)> fn,
- const monotonic_clock::duration interval,
- const monotonic_clock::duration offset)
- : aos::PhasedLoopHandler(shm_event_loop, std::move(fn), interval, offset),
+ ShmPhasedLoopHandler(ShmEventLoop *shm_event_loop,
+ ::std::function<void(int)> fn,
+ const monotonic_clock::duration interval,
+ const monotonic_clock::duration offset)
+ : PhasedLoopHandler(shm_event_loop, std::move(fn), interval, offset),
shm_event_loop_(shm_event_loop),
event_(this) {
shm_event_loop_->epoll_.OnReadable(
@@ -586,7 +644,7 @@
});
}
- ~PhasedLoopHandler() override {
+ ~ShmPhasedLoopHandler() override {
shm_event_loop_->epoll_.DeleteFd(timerfd_.fd());
shm_event_loop_->RemoveEvent(&event_);
}
@@ -604,11 +662,12 @@
}
ShmEventLoop *shm_event_loop_;
- EventHandler<PhasedLoopHandler> event_;
+ EventHandler<ShmPhasedLoopHandler> event_;
- TimerFd timerfd_;
+ internal::TimerFd timerfd_;
};
-} // namespace internal
+
+} // namespace shm_event_loop_internal
::std::unique_ptr<RawFetcher> ShmEventLoop::MakeRawFetcher(
const Channel *channel) {
@@ -619,14 +678,14 @@
"configuration.";
}
- return ::std::unique_ptr<RawFetcher>(new internal::ShmFetcher(this, channel));
+ return ::std::unique_ptr<RawFetcher>(new ShmFetcher(this, channel));
}
::std::unique_ptr<RawSender> ShmEventLoop::MakeRawSender(
const Channel *channel) {
TakeSender(channel);
- return ::std::unique_ptr<RawSender>(new internal::ShmSender(this, channel));
+ return ::std::unique_ptr<RawSender>(new ShmSender(this, channel));
}
void ShmEventLoop::MakeRawWatcher(
@@ -635,7 +694,7 @@
TakeWatcher(channel);
NewWatcher(::std::unique_ptr<WatcherState>(
- new internal::WatcherState(this, channel, std::move(watcher), true)));
+ new ShmWatcherState(this, channel, std::move(watcher), true)));
}
void ShmEventLoop::MakeRawNoArgWatcher(
@@ -643,7 +702,7 @@
std::function<void(const Context &context)> watcher) {
TakeWatcher(channel);
- NewWatcher(::std::unique_ptr<WatcherState>(new internal::WatcherState(
+ NewWatcher(::std::unique_ptr<WatcherState>(new ShmWatcherState(
this, channel,
[watcher](const Context &context, const void *) { watcher(context); },
false)));
@@ -651,16 +710,15 @@
TimerHandler *ShmEventLoop::AddTimer(::std::function<void()> callback) {
return NewTimer(::std::unique_ptr<TimerHandler>(
- new internal::TimerHandlerState(this, ::std::move(callback))));
+ new ShmTimerHandler(this, ::std::move(callback))));
}
PhasedLoopHandler *ShmEventLoop::AddPhasedLoop(
::std::function<void(int)> callback,
const monotonic_clock::duration interval,
const monotonic_clock::duration offset) {
- return NewPhasedLoop(
- ::std::unique_ptr<PhasedLoopHandler>(new internal::PhasedLoopHandler(
- this, ::std::move(callback), interval, offset)));
+ return NewPhasedLoop(::std::unique_ptr<PhasedLoopHandler>(
+ new ShmPhasedLoopHandler(this, ::std::move(callback), interval, offset)));
}
void ShmEventLoop::OnRun(::std::function<void()> on_run) {
@@ -670,8 +728,8 @@
void ShmEventLoop::HandleEvent() {
// Update all the times for handlers.
for (::std::unique_ptr<WatcherState> &base_watcher : watchers_) {
- internal::WatcherState *watcher =
- reinterpret_cast<internal::WatcherState *>(base_watcher.get());
+ ShmWatcherState *watcher =
+ reinterpret_cast<ShmWatcherState *>(base_watcher.get());
watcher->CheckForNewData();
}
@@ -848,8 +906,8 @@
}
for (::std::unique_ptr<WatcherState> &base_watcher : watchers_) {
- internal::WatcherState *watcher =
- reinterpret_cast<internal::WatcherState *>(base_watcher.get());
+ ShmWatcherState *watcher =
+ reinterpret_cast<ShmWatcherState *>(base_watcher.get());
watcher->UnregisterWakeup();
}
@@ -897,14 +955,19 @@
}
absl::Span<char> ShmEventLoop::GetWatcherSharedMemory(const Channel *channel) {
- internal::WatcherState *const watcher_state =
- static_cast<internal::WatcherState *>(GetWatcherState(channel));
+ ShmWatcherState *const watcher_state =
+ static_cast<ShmWatcherState *>(GetWatcherState(channel));
return watcher_state->GetSharedMemory();
}
absl::Span<char> ShmEventLoop::GetShmSenderSharedMemory(
const aos::RawSender *sender) const {
- return static_cast<const internal::ShmSender *>(sender)->GetSharedMemory();
+ return static_cast<const ShmSender *>(sender)->GetSharedMemory();
+}
+
+absl::Span<char> ShmEventLoop::GetShmFetcherPrivateMemory(
+ const aos::RawFetcher *fetcher) const {
+ return static_cast<const ShmFetcher *>(fetcher)->GetPrivateMemory();
}
pid_t ShmEventLoop::GetTid() { return syscall(SYS_gettid); }
diff --git a/aos/events/shm_event_loop.h b/aos/events/shm_event_loop.h
index 15759f4..57d3b98 100644
--- a/aos/events/shm_event_loop.h
+++ b/aos/events/shm_event_loop.h
@@ -9,16 +9,20 @@
#include "aos/events/event_loop.h"
#include "aos/events/event_loop_generated.h"
-namespace aos {
-namespace internal {
+DECLARE_string(application_name);
+DECLARE_string(shm_base);
-class WatcherState;
-class TimerHandlerState;
-class PhasedLoopHandler;
+namespace aos {
+namespace shm_event_loop_internal {
+
+class ShmWatcherState;
+class ShmTimerHandler;
+class ShmPhasedLoopHandler;
class ShmSender;
+class SimpleShmFetcher;
class ShmFetcher;
-} // namespace internal
+} // namespace shm_event_loop_internal
// Specialization of EventLoop that is built from queues running out of shared
// memory.
@@ -57,11 +61,10 @@
std::function<void(const Context &context)> watcher) override;
TimerHandler *AddTimer(std::function<void()> callback) override;
- aos::PhasedLoopHandler *AddPhasedLoop(
- std::function<void(int)> callback,
- const monotonic_clock::duration interval,
- const monotonic_clock::duration offset =
- std::chrono::seconds(0)) override;
+ PhasedLoopHandler *AddPhasedLoop(std::function<void(int)> callback,
+ const monotonic_clock::duration interval,
+ const monotonic_clock::duration offset =
+ std::chrono::seconds(0)) override;
void OnRun(std::function<void()> on_run) override;
@@ -82,18 +85,28 @@
// this.
absl::Span<char> GetWatcherSharedMemory(const Channel *channel);
- // Returns the local mapping of the shared memory used by the provided Sender
+ // Returns the local mapping of the shared memory used by the provided Sender.
template <typename T>
absl::Span<char> GetSenderSharedMemory(aos::Sender<T> *sender) const {
return GetShmSenderSharedMemory(GetRawSender(sender));
}
+ // Returns the local mapping of the private memory used by the provided
+ // Fetcher to hold messages.
+ template <typename T>
+ absl::Span<char> GetFetcherPrivateMemory(aos::Fetcher<T> *fetcher) const {
+ return GetShmFetcherPrivateMemory(GetRawFetcher(fetcher));
+ }
+
private:
- friend class internal::WatcherState;
- friend class internal::TimerHandlerState;
- friend class internal::PhasedLoopHandler;
- friend class internal::ShmSender;
- friend class internal::ShmFetcher;
+ friend class shm_event_loop_internal::ShmWatcherState;
+ friend class shm_event_loop_internal::ShmTimerHandler;
+ friend class shm_event_loop_internal::ShmPhasedLoopHandler;
+ friend class shm_event_loop_internal::ShmSender;
+ friend class shm_event_loop_internal::SimpleShmFetcher;
+ friend class shm_event_loop_internal::ShmFetcher;
+
+ using EventLoop::SendTimingReport;
static cpu_set_t DefaultAffinity() {
cpu_set_t result;
@@ -108,9 +121,13 @@
// Returns the TID of the event loop.
pid_t GetTid() override;
- // Private method to access the shared memory mapping of a ShmSender
+ // Private method to access the shared memory mapping of a ShmSender.
absl::Span<char> GetShmSenderSharedMemory(const aos::RawSender *sender) const;
+ // Private method to access the private memory mapping of a ShmFetcher.
+ absl::Span<char> GetShmFetcherPrivateMemory(
+ const aos::RawFetcher *fetcher) const;
+
std::vector<std::function<void()>> on_run_;
int priority_ = 0;
cpu_set_t affinity_ = DefaultAffinity();
diff --git a/aos/events/shm_event_loop_test.cc b/aos/events/shm_event_loop_test.cc
index 0c39704..d25e2f8 100644
--- a/aos/events/shm_event_loop_test.cc
+++ b/aos/events/shm_event_loop_test.cc
@@ -7,9 +7,7 @@
#include "gtest/gtest.h"
#include "aos/events/test_message_generated.h"
-
-DECLARE_string(shm_base);
-DECLARE_string(override_hostname);
+#include "aos/network/team_number.h"
namespace aos {
namespace testing {
@@ -207,11 +205,21 @@
auto generic_loop1 = factory.MakePrimary("primary");
ShmEventLoop *const loop1 = static_cast<ShmEventLoop *>(generic_loop1.get());
- // check that GetSenderSharedMemory returns non-null/non-empty memory span
+ // check that GetSenderSharedMemory returns non-null/non-empty memory span.
auto sender = loop1->MakeSender<TestMessage>("/test");
EXPECT_FALSE(loop1->GetSenderSharedMemory(&sender).empty());
}
+TEST(ShmEventLoopTest, GetFetcherPrivateMemory) {
+ ShmEventLoopTestFactory factory;
+ auto generic_loop1 = factory.MakePrimary("primary");
+ ShmEventLoop *const loop1 = static_cast<ShmEventLoop *>(generic_loop1.get());
+
+ // check that GetFetcherPrivateMemory returns non-null/non-empty memory span.
+ auto fetcher = loop1->MakeFetcher<TestMessage>("/test");
+ EXPECT_FALSE(loop1->GetFetcherPrivateMemory(&fetcher).empty());
+}
+
// TODO(austin): Test that missing a deadline with a timer recovers as expected.
} // namespace testing
diff --git a/aos/events/simulated_event_loop.cc b/aos/events/simulated_event_loop.cc
index 1241834..ee4e6d6 100644
--- a/aos/events/simulated_event_loop.cc
+++ b/aos/events/simulated_event_loop.cc
@@ -102,6 +102,19 @@
const Channel *channel() const { return channel_; }
+ void CountSenderCreated() {
+ if (sender_count_ >= channel()->num_senders()) {
+ LOG(FATAL) << "Failed to create sender on "
+ << configuration::CleanedChannelToString(channel())
+ << ", too many senders.";
+ }
+ ++sender_count_;
+ }
+ void CountSenderDestroyed() {
+ --sender_count_;
+ CHECK_GE(sender_count_, 0);
+ }
+
private:
const Channel *channel_;
@@ -114,6 +127,8 @@
EventScheduler *scheduler_;
ipc_lib::QueueIndex next_queue_index_;
+
+ int sender_count_ = 0;
};
namespace {
@@ -134,8 +149,10 @@
SimulatedSender(SimulatedChannel *simulated_channel, EventLoop *event_loop)
: RawSender(event_loop, simulated_channel->channel()),
simulated_channel_(simulated_channel),
- event_loop_(event_loop) {}
- ~SimulatedSender() {}
+ event_loop_(event_loop) {
+ simulated_channel_->CountSenderCreated();
+ }
+ ~SimulatedSender() { simulated_channel_->CountSenderDestroyed(); }
void *data() override {
if (!message_) {
diff --git a/aos/events/simulated_event_loop.h b/aos/events/simulated_event_loop.h
index 7718cfb..c8029de 100644
--- a/aos/events/simulated_event_loop.h
+++ b/aos/events/simulated_event_loop.h
@@ -2,6 +2,7 @@
#define AOS_EVENTS_SIMULATED_EVENT_LOOP_H_
#include <algorithm>
+#include <functional>
#include <map>
#include <memory>
#include <string_view>
diff --git a/aos/events/simulated_event_loop_test.cc b/aos/events/simulated_event_loop_test.cc
index 0b5dbec..513dc1b 100644
--- a/aos/events/simulated_event_loop_test.cc
+++ b/aos/events/simulated_event_loop_test.cc
@@ -72,9 +72,8 @@
[&counter]() { counter += 1; });
scheduler_scheduler.Run();
EXPECT_EQ(counter, 1);
- auto token =
- scheduler.Schedule(monotonic_clock::epoch() + chrono::seconds(2),
- [&counter]() { counter += 1; });
+ auto token = scheduler.Schedule(monotonic_clock::epoch() + chrono::seconds(2),
+ [&counter]() { counter += 1; });
scheduler.Deschedule(token);
scheduler_scheduler.Run();
EXPECT_EQ(counter, 1);
@@ -189,7 +188,7 @@
}
// Check the watcher report.
- VLOG(1) << FlatbufferToJson(primary_report, true);
+ VLOG(1) << FlatbufferToJson(primary_report, {.multi_line = true});
EXPECT_EQ(primary_report.message().name()->string_view(), "primary");
diff --git a/aos/flatbuffer_introspection.cc b/aos/flatbuffer_introspection.cc
index b72d059..48200b7 100644
--- a/aos/flatbuffer_introspection.cc
+++ b/aos/flatbuffer_introspection.cc
@@ -70,7 +70,8 @@
const reflection::Object *obj,
const flatbuffers::Vector<flatbuffers::Offset<reflection::Object>> *objects,
const flatbuffers::Vector<flatbuffers::Offset<reflection::Enum>> *enums,
- const ObjT *object, size_t max_vector_size, std::stringstream *out);
+ const ObjT *object, std::stringstream *out, JsonOptions json_options,
+ int tree_depth = 0);
// Get enum value name
const char *EnumToString(
@@ -114,13 +115,33 @@
}
}
+// Adds a newline and indents
+// Every increment in tree depth is two spaces
+void AddWrapping(std::stringstream *out, int tree_depth) {
+ *out << "\n";
+ for (int i = 0; i < tree_depth; i++) {
+ *out << " ";
+ }
+}
+
+// Detects if a field should trigger wrapping of the parent object.
+bool ShouldCauseWrapping(reflection::BaseType type) {
+ switch (type) {
+ case BaseType::Vector:
+ case BaseType::Obj:
+ return true;
+ default:
+ return false;
+ }
+}
+
// Print field in flatbuffer table. Field must be populated.
template <typename ObjT>
void FieldToString(
const ObjT *table, const reflection::Field *field,
const flatbuffers::Vector<flatbuffers::Offset<reflection::Object>> *objects,
const flatbuffers::Vector<flatbuffers::Offset<reflection::Enum>> *enums,
- size_t max_vector_size, std::stringstream *out) {
+ std::stringstream *out, JsonOptions json_options, int tree_depth) {
const reflection::Type *type = field->type();
switch (type->base_type()) {
@@ -183,14 +204,29 @@
flatbuffers::GetFieldAnyV(*table, *field);
reflection::BaseType elem_type = type->element();
- if (vector->size() > max_vector_size) {
+ if (vector->size() > json_options.max_vector_size) {
*out << "[ ... " << vector->size() << " elements ... ]";
break;
}
+
+ bool wrap = false;
+ const int child_tree_depth = tree_depth + 1;
+
+ if (json_options.multi_line) {
+ wrap = ShouldCauseWrapping(elem_type);
+ }
+
*out << '[';
for (flatbuffers::uoffset_t i = 0; i < vector->size(); ++i) {
if (i != 0) {
- *out << ", ";
+ if (wrap) {
+ *out << ",";
+ } else {
+ *out << ", ";
+ }
+ }
+ if (wrap) {
+ AddWrapping(out, child_tree_depth);
}
if (flatbuffers::IsInteger(elem_type)) {
IntOrEnumToString(
@@ -211,16 +247,19 @@
flatbuffers::GetAnyVectorElemAddressOf<
const flatbuffers::Struct>(
vector, i, objects->Get(type->index())->bytesize()),
- max_vector_size, out);
+ out, json_options, child_tree_depth);
} else {
ObjectToString(objects->Get(type->index()), objects, enums,
flatbuffers::GetAnyVectorElemPointer<
const flatbuffers::Table>(vector, i),
- max_vector_size, out);
+ out, json_options, child_tree_depth);
}
}
}
}
+ if (wrap) {
+ AddWrapping(out, tree_depth);
+ }
*out << ']';
} else {
*out << "null";
@@ -230,12 +269,12 @@
if (type->index() > -1 && type->index() < (int32_t)objects->size()) {
if (objects->Get(type->index())->is_struct()) {
ObjectToString(objects->Get(type->index()), objects, enums,
- flatbuffers::GetFieldStruct(*table, *field),
- max_vector_size, out);
+ flatbuffers::GetFieldStruct(*table, *field), out,
+ json_options, tree_depth);
} else if constexpr (std::is_same<flatbuffers::Table, ObjT>()) {
ObjectToString(objects->Get(type->index()), objects, enums,
- flatbuffers::GetFieldT(*table, *field),
- max_vector_size, out);
+ flatbuffers::GetFieldT(*table, *field), out,
+ json_options, tree_depth);
}
} else {
*out << "null";
@@ -253,40 +292,78 @@
const reflection::Object *obj,
const flatbuffers::Vector<flatbuffers::Offset<reflection::Object>> *objects,
const flatbuffers::Vector<flatbuffers::Offset<reflection::Enum>> *enums,
- const ObjT *object, size_t max_vector_size, std::stringstream *out) {
+ const ObjT *object, std::stringstream *out, JsonOptions json_options,
+ int tree_depth) {
static_assert(std::is_same<flatbuffers::Table, ObjT>() ||
std::is_same<flatbuffers::Struct, ObjT>(),
"Type must be either flatbuffer table or struct");
bool print_sep = false;
+
+ const int child_tree_depth = tree_depth + 1;
+
+ bool wrap = false;
+ if (json_options.multi_line) {
+ // Check whether this object has objects, vectors, or floats inside of it
+ for (const reflection::Field *field : *obj->fields()) {
+ if (ShouldCauseWrapping(field->type()->base_type())) {
+ wrap = true;
+ break;
+ }
+ }
+ }
+
*out << '{';
for (const reflection::Field *field : *obj->fields()) {
// Check whether this object has the field populated (even for structs,
// which should have all fields populated)
if (object->GetAddressOf(field->offset())) {
if (print_sep) {
- *out << ", ";
+ if (wrap) {
+ *out << ",";
+ } else {
+ *out << ", ";
+ }
} else {
print_sep = true;
}
+
+ if (wrap) {
+ AddWrapping(out, child_tree_depth);
+ }
+
*out << '"' << field->name()->c_str() << "\": ";
- FieldToString(object, field, objects, enums, max_vector_size, out);
+ FieldToString(object, field, objects, enums, out, json_options,
+ child_tree_depth);
}
}
+
+ if (wrap) {
+ AddWrapping(out, tree_depth);
+ }
+
*out << '}';
}
} // namespace
std::string FlatbufferToJson(const reflection::Schema *schema,
- const uint8_t *data, size_t max_vector_size) {
+ const uint8_t *data, JsonOptions json_options) {
+ CHECK(schema != nullptr) << ": Need to provide a schema";
+
+ // It is pretty common to get passed in a nullptr when a test fails. Rather
+ // than CHECK, return a more user friendly result.
+ if (data == nullptr) {
+ return "null";
+ }
+
const flatbuffers::Table *table = flatbuffers::GetAnyRoot(data);
const reflection::Object *obj = schema->root_table();
std::stringstream out;
- ObjectToString(obj, schema->objects(), schema->enums(), table,
- max_vector_size, &out);
+ ObjectToString(obj, schema->objects(), schema->enums(), table, &out,
+ json_options);
return out.str();
}
diff --git a/aos/flatbuffer_introspection_test.cc b/aos/flatbuffer_introspection_test.cc
index 5cc26fb..d771b55 100644
--- a/aos/flatbuffer_introspection_test.cc
+++ b/aos/flatbuffer_introspection_test.cc
@@ -357,9 +357,132 @@
builder.Finish(config_builder.Finish());
- std::string out = FlatbufferToJson(schema_, builder.GetBufferPointer(), 100);
+ std::string out =
+ FlatbufferToJson(schema_, builder.GetBufferPointer(),
+ {.multi_line = false, .max_vector_size = 100});
EXPECT_EQ(out, "{\"vector_foo_int\": [ ... 101 elements ... ]}");
}
+TEST_F(FlatbufferIntrospectionTest, MultilineTest) {
+ flatbuffers::FlatBufferBuilder builder;
+ ConfigurationBuilder config_builder(builder);
+
+ config_builder.add_foo_bool(true);
+ config_builder.add_foo_int(-20);
+
+ builder.Finish(config_builder.Finish());
+
+ std::string out = FlatbufferToJson(schema_, builder.GetBufferPointer(),
+ {.multi_line = true});
+
+ EXPECT_EQ(out,
+ "{\n"
+ " \"foo_bool\": true,\n"
+ " \"foo_int\": -20\n"
+ "}");
+}
+
+TEST_F(FlatbufferIntrospectionTest, MultilineStructTest) {
+ flatbuffers::FlatBufferBuilder builder;
+ ConfigurationBuilder config_builder(builder);
+
+ FooStructNested foo_struct2(10);
+ FooStruct foo_struct(5, foo_struct2);
+
+ config_builder.add_foo_struct(&foo_struct);
+
+ builder.Finish(config_builder.Finish());
+
+ std::string out = FlatbufferToJson(schema_, builder.GetBufferPointer(),
+ {.multi_line = true});
+
+ EXPECT_EQ(out,
+ "{\n"
+ " \"foo_struct\": {\n"
+ " \"foo_byte\": 5,\n"
+ " \"nested_struct\": {\"foo_byte\": 10}\n"
+ " }\n"
+ "}");
+}
+
+TEST_F(FlatbufferIntrospectionTest, MultilineVectorStructTest) {
+ flatbuffers::FlatBufferBuilder builder;
+
+ FooStructNested foo_struct2(1);
+
+ auto structs = builder.CreateVectorOfStructs(
+ std::vector<FooStruct>({{5, foo_struct2}, {10, foo_struct2}}));
+
+ ConfigurationBuilder config_builder(builder);
+ config_builder.add_vector_foo_struct(structs);
+
+ builder.Finish(config_builder.Finish());
+
+ std::string out = FlatbufferToJson(schema_, builder.GetBufferPointer(),
+ {.multi_line = true});
+
+ EXPECT_EQ(out,
+ "{\n"
+ " \"vector_foo_struct\": [\n"
+ " {\n"
+ " \"foo_byte\": 5,\n"
+ " \"nested_struct\": {\"foo_byte\": 1}\n"
+ " },\n"
+ " {\n"
+ " \"foo_byte\": 10,\n"
+ " \"nested_struct\": {\"foo_byte\": 1}\n"
+ " }\n"
+ " ]\n"
+ "}");
+}
+
+TEST_F(FlatbufferIntrospectionTest, MultilineVectorScalarTest) {
+ flatbuffers::FlatBufferBuilder builder;
+
+ // Flatbuffers don't like creating vectors simultaneously with table, so do
+ // first.
+ auto foo_ints =
+ builder.CreateVector<int32_t>({-300, -200, -100, 0, 100, 200, 300});
+
+ auto foo_floats =
+ builder.CreateVector<float>({0.0, 1.0 / 9.0, 2.0 / 9.0, 3.0 / 9.0});
+ auto foo_doubles =
+ builder.CreateVector<double>({0, 1.0 / 9.0, 2.0 / 9.0, 3.0 / 9.0});
+
+ ConfigurationBuilder config_builder(builder);
+
+ config_builder.add_vector_foo_int(foo_ints);
+ config_builder.add_vector_foo_float(foo_floats);
+ config_builder.add_vector_foo_double(foo_doubles);
+
+ builder.Finish(config_builder.Finish());
+
+ std::string out = FlatbufferToJson(schema_, builder.GetBufferPointer(),
+ {.multi_line = true});
+
+ EXPECT_EQ(out,
+ "{\n"
+ " \"vector_foo_double\": [0, 0.111111111111111, "
+ "0.222222222222222, 0.333333333333333],\n"
+ " \"vector_foo_float\": [0, 0.111111, 0.222222, 0.333333],\n"
+ " \"vector_foo_int\": [-300, -200, -100, 0, 100, 200, 300]\n"
+ "}");
+}
+
+// Tests that a nullptr buffer prints nullptr.
+TEST_F(FlatbufferIntrospectionTest, NullptrData) {
+ EXPECT_EQ("null", FlatbufferToJson(schema_, nullptr));
+}
+
+// Tests that a null schema gets caught.
+TEST(FlatbufferIntrospectionDeathTest, NullSchema) {
+ EXPECT_DEATH(
+ {
+ FlatbufferToJson(static_cast<const reflection::Schema *>(nullptr),
+ nullptr);
+ },
+ "Need to provide a schema");
+}
+
} // namespace testing
} // namespace aos
diff --git a/aos/ftrace.cc b/aos/ftrace.cc
new file mode 100644
index 0000000..af4964c
--- /dev/null
+++ b/aos/ftrace.cc
@@ -0,0 +1,45 @@
+#include "aos/ftrace.h"
+
+#include <stdarg.h>
+#include <stdio.h>
+
+namespace aos {
+
+Ftrace::~Ftrace() {
+ if (message_fd_ != -1) {
+ PCHECK(close(message_fd_) == 0);
+ }
+ if (message_fd_ != -1) {
+ PCHECK(close(on_fd_) == 0);
+ }
+}
+
+void Ftrace::FormatMessage(const char *format, ...) {
+ if (message_fd_ == -1) {
+ return;
+ }
+ char buffer[512];
+ va_list ap;
+ va_start(ap, format);
+ const int result = vsnprintf(buffer, sizeof(buffer), format, ap);
+ va_end(ap);
+ CHECK_LE(static_cast<size_t>(result), sizeof(buffer))
+ << ": Format string ended up too long: " << format;
+ WriteMessage(std::string_view(buffer, result));
+}
+
+void Ftrace::WriteMessage(std::string_view content) {
+ if (message_fd_ == -1) {
+ return;
+ }
+ const int result = write(message_fd_, content.data(), content.size());
+ if (result == -1 && errno == EBADF) {
+ // This just means tracing is turned off. Ignore it.
+ return;
+ }
+ PCHECK(result >= 0) << ": Failed to write ftrace message: " << content;
+ CHECK_EQ(static_cast<size_t>(result), content.size())
+ << ": Failed to write complete ftrace message: " << content;
+}
+
+} // namespace aos
diff --git a/aos/ftrace.h b/aos/ftrace.h
new file mode 100644
index 0000000..42147ad
--- /dev/null
+++ b/aos/ftrace.h
@@ -0,0 +1,51 @@
+#ifndef AOS_FTRACE_H_
+#define AOS_FTRACE_H_
+
+#include <fcntl.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <unistd.h>
+
+#include <string_view>
+
+#include "glog/logging.h"
+
+namespace aos {
+
+// Manages interacting with ftrace. Silently hides many errors, because they are
+// expected to occur when ftrace is not enabled, and we want calling code to
+// continue working in that case.
+class Ftrace {
+ public:
+ Ftrace()
+ : message_fd_(open("/sys/kernel/debug/tracing/trace_marker", O_WRONLY)),
+ on_fd_(open("/sys/kernel/debug/tracing/tracing_on", O_WRONLY)) {}
+ ~Ftrace();
+
+ // Writes a message with a printf-style format.
+ //
+ // Silently does nothing if tracing is disabled.
+ void FormatMessage(const char *format, ...)
+ __attribute__((format(__printf__, 2, 3)));
+
+ // Writes a preformatted message.
+ //
+ // Silently does nothing if tracing is disabled.
+ void WriteMessage(std::string_view content);
+
+ // Turns tracing off, or CHECK-fails if tracing is inaccessible. Does nothing
+ // if tracing is currently available but off.
+ void TurnOffOrDie() {
+ CHECK(on_fd_ != -1)
+ << ": Failed to open tracing_on earlier, cannot turn off tracing";
+ char zero = '0';
+ CHECK_EQ(write(on_fd_, &zero, 1), 1) << ": Failed to turn tracing off";
+ }
+
+ private:
+ int message_fd_, on_fd_;
+};
+
+} // namespace aos
+
+#endif // AOS_FTRACE_H_
diff --git a/aos/ipc_lib/lockless_queue.cc b/aos/ipc_lib/lockless_queue.cc
index ccbe887..02aebcb 100644
--- a/aos/ipc_lib/lockless_queue.cc
+++ b/aos/ipc_lib/lockless_queue.cc
@@ -562,7 +562,8 @@
}
if (sender_index_ == -1) {
- LOG(FATAL) << "Too many senders";
+ VLOG(1) << "Too many senders, starting to bail.";
+ return;
}
::aos::ipc_lib::Sender *s = memory_->GetSender(sender_index_);
@@ -573,13 +574,18 @@
}
LocklessQueue::Sender::~Sender() {
- if (memory_ != nullptr) {
+ if (valid()) {
death_notification_release(&(memory_->GetSender(sender_index_)->tid));
}
}
-LocklessQueue::Sender LocklessQueue::MakeSender() {
- return LocklessQueue::Sender(memory_);
+std::optional<LocklessQueue::Sender> LocklessQueue::MakeSender() {
+ LocklessQueue::Sender result = LocklessQueue::Sender(memory_);
+ if (result.valid()) {
+ return std::move(result);
+ } else {
+ return std::nullopt;
+ }
}
QueueIndex ZeroOrValid(QueueIndex index) {
diff --git a/aos/ipc_lib/lockless_queue.h b/aos/ipc_lib/lockless_queue.h
index 550485f..de80f3d 100644
--- a/aos/ipc_lib/lockless_queue.h
+++ b/aos/ipc_lib/lockless_queue.h
@@ -5,6 +5,7 @@
#include <sys/signalfd.h>
#include <sys/types.h>
#include <vector>
+#include <optional>
#include "aos/ipc_lib/aos_sync.h"
#include "aos/ipc_lib/data_alignment.h"
@@ -245,6 +246,11 @@
Sender(LocklessQueueMemory *memory);
+ // Returns true if this sender is valid. If it isn't valid, any of the
+ // other methods won't work. This is here to allow the lockless queue to
+ // only build a sender if there was one available.
+ bool valid() const { return sender_index_ != -1 && memory_ != nullptr; }
+
// Pointer to the backing memory.
LocklessQueueMemory *memory_ = nullptr;
@@ -252,8 +258,9 @@
int sender_index_ = -1;
};
- // Creates a sender.
- Sender MakeSender();
+ // Creates a sender. If we couldn't allocate a sender, returns nullopt.
+ // TODO(austin): Change the API if we find ourselves with more errors.
+ std::optional<Sender> MakeSender();
private:
LocklessQueueMemory *memory_ = nullptr;
diff --git a/aos/ipc_lib/lockless_queue_death_test.cc b/aos/ipc_lib/lockless_queue_death_test.cc
index 213c9e4..b7cdfee 100644
--- a/aos/ipc_lib/lockless_queue_death_test.cc
+++ b/aos/ipc_lib/lockless_queue_death_test.cc
@@ -521,7 +521,7 @@
LocklessQueue queue(
reinterpret_cast<aos::ipc_lib::LocklessQueueMemory *>(memory),
config);
- LocklessQueue::Sender sender = queue.MakeSender();
+ LocklessQueue::Sender sender = queue.MakeSender().value();
for (int i = 0; i < 2; ++i) {
char data[100];
size_t s = snprintf(data, sizeof(data), "foobar%d", i + 1);
@@ -555,7 +555,7 @@
LocklessQueue queue(memory, config);
// Building and destroying a sender will clean up the queue.
- { LocklessQueue::Sender sender = queue.MakeSender(); }
+ { LocklessQueue::Sender sender = queue.MakeSender().value(); }
if (print) {
printf("Cleaned up version:\n");
@@ -563,12 +563,12 @@
}
{
- LocklessQueue::Sender sender = queue.MakeSender();
+ LocklessQueue::Sender sender = queue.MakeSender().value();
{
// Make a second sender to confirm that the slot was freed.
// If the sender doesn't get cleaned up, this will fail.
LocklessQueue queue2(memory, config);
- queue2.MakeSender();
+ queue2.MakeSender().value();
}
// Send a message to make sure that the queue still works.
diff --git a/aos/ipc_lib/lockless_queue_test.cc b/aos/ipc_lib/lockless_queue_test.cc
index 109c2ea..65b2a15 100644
--- a/aos/ipc_lib/lockless_queue_test.cc
+++ b/aos/ipc_lib/lockless_queue_test.cc
@@ -194,17 +194,15 @@
}
// Tests that too many watchers dies like expected.
-TEST_F(LocklessQueueDeathTest, TooManySenders) {
- EXPECT_DEATH(
- {
- ::std::vector<::std::unique_ptr<LocklessQueue>> queues;
- ::std::vector<LocklessQueue::Sender> senders;
- for (size_t i = 0; i < config_.num_senders + 1; ++i) {
- queues.emplace_back(new LocklessQueue(get_memory(), config_));
- senders.emplace_back(queues.back()->MakeSender());
- }
- },
- "Too many senders");
+TEST_F(LocklessQueueTest, TooManySenders) {
+ ::std::vector<::std::unique_ptr<LocklessQueue>> queues;
+ ::std::vector<LocklessQueue::Sender> senders;
+ for (size_t i = 0; i < config_.num_senders; ++i) {
+ queues.emplace_back(new LocklessQueue(get_memory(), config_));
+ senders.emplace_back(queues.back()->MakeSender().value());
+ }
+ queues.emplace_back(new LocklessQueue(get_memory(), config_));
+ EXPECT_FALSE(queues.back()->MakeSender());
}
// Now, start 2 threads and have them receive the signals.
@@ -240,7 +238,7 @@
TEST_F(LocklessQueueTest, Send) {
LocklessQueue queue(get_memory(), config_);
- LocklessQueue::Sender sender = queue.MakeSender();
+ LocklessQueue::Sender sender = queue.MakeSender().value();
// Send enough messages to wrap.
for (int i = 0; i < 20000; ++i) {
diff --git a/aos/ipc_lib/queue_racer.cc b/aos/ipc_lib/queue_racer.cc
index 9bb0a70..f5b3d7e 100644
--- a/aos/ipc_lib/queue_racer.cc
+++ b/aos/ipc_lib/queue_racer.cc
@@ -143,7 +143,7 @@
::std::thread([this, &t, thread_index, &run, write_wrap_count]() {
// Build up a sender.
LocklessQueue queue(memory_, config_);
- LocklessQueue::Sender sender = queue.MakeSender();
+ LocklessQueue::Sender sender = queue.MakeSender().value();
// Signal that we are ready to start sending.
t.ready.Set();
diff --git a/aos/ipc_lib/signalfd_test.cc b/aos/ipc_lib/signalfd_test.cc
index b4fe74b..a50e9f6 100644
--- a/aos/ipc_lib/signalfd_test.cc
+++ b/aos/ipc_lib/signalfd_test.cc
@@ -60,15 +60,15 @@
// its signals.
TEST(SignalFdDeathTest, ExternalUnblockSignal) {
::aos::testing::EnableTestLogging();
- EXPECT_DEATH(
- {
- SignalFd signalfd({SIGUSR1});
- sigset_t test_mask;
- CHECK_EQ(0, sigemptyset(&test_mask));
- CHECK_EQ(0, sigaddset(&test_mask, SIGUSR1));
- PCHECK(sigprocmask(SIG_UNBLOCK, &test_mask, nullptr) == 0);
- },
- "Some other code unblocked one or more of our signals");
+ auto t = []() {
+ SignalFd signalfd({SIGUSR1});
+ sigset_t test_mask;
+ CHECK_EQ(0, sigemptyset(&test_mask));
+ CHECK_EQ(0, sigaddset(&test_mask, SIGUSR1));
+ PCHECK(sigprocmask(SIG_UNBLOCK, &test_mask, nullptr) == 0);
+ };
+ EXPECT_DEATH({ t(); },
+ "Some other code unblocked one or more of our signals");
}
} // namespace testing
diff --git a/aos/json_to_flatbuffer.cc b/aos/json_to_flatbuffer.cc
index fdab330..9980328 100644
--- a/aos/json_to_flatbuffer.cc
+++ b/aos/json_to_flatbuffer.cc
@@ -790,7 +790,7 @@
::std::string BufferFlatbufferToJson(const uint8_t *buffer,
const ::flatbuffers::TypeTable *typetable,
- bool multi_line, size_t max_vector_size) {
+ JsonOptions json_options) {
// It is pretty common to get passed in a nullptr when a test fails. Rather
// than CHECK, return a more user friendly result.
if (buffer == nullptr) {
@@ -798,7 +798,7 @@
}
return TableFlatbufferToJson(reinterpret_cast<const flatbuffers::Table *>(
flatbuffers::GetRoot<uint8_t>(buffer)),
- typetable, multi_line, max_vector_size);
+ typetable, json_options);
}
namespace {
@@ -925,15 +925,15 @@
::std::string TableFlatbufferToJson(const flatbuffers::Table *t,
const ::flatbuffers::TypeTable *typetable,
- bool multi_line, size_t max_vector_size) {
+ JsonOptions json_options) {
// It is pretty common to get passed in a nullptr when a test fails. Rather
// than CHECK, return a more user friendly result.
if (t == nullptr) {
return "null";
}
- TruncatingStringVisitor tostring_visitor(max_vector_size,
- multi_line ? "\n" : " ", true,
- multi_line ? " " : "", multi_line);
+ TruncatingStringVisitor tostring_visitor(
+ json_options.max_vector_size, json_options.multi_line ? "\n" : " ", true,
+ json_options.multi_line ? " " : "", json_options.multi_line);
flatbuffers::IterateObject(reinterpret_cast<const uint8_t *>(t), typetable,
&tostring_visitor);
return tostring_visitor.string();
diff --git a/aos/json_to_flatbuffer.h b/aos/json_to_flatbuffer.h
index 37560ec..aebd069 100644
--- a/aos/json_to_flatbuffer.h
+++ b/aos/json_to_flatbuffer.h
@@ -2,6 +2,7 @@
#define AOS_JSON_TO_FLATBUFFER_H_
#include <cstddef>
+#include <fstream>
#include <string>
#include <string_view>
@@ -33,53 +34,73 @@
JsonToFlatbuffer(data, T::MiniReflectTypeTable(), fbb).o);
}
+struct JsonOptions {
+ // controls if the Json is written ouut on multiple lines or one.
+ bool multi_line = false;
+ // the contents of vectors longer than max_vector_size will be skipped.
+ size_t max_vector_size = SIZE_MAX;
+};
+
// Converts a flatbuffer into a Json string.
-// multi_line controls if the Json is written out on multiple lines or one.
// The methods below are generally more useful than BufferFlatbufferToJson and
// TableFlatbufferToJson.
::std::string BufferFlatbufferToJson(const uint8_t *buffer,
const flatbuffers::TypeTable *typetable,
- bool multi_line = false,
- size_t max_vector_size = SIZE_MAX);
+ JsonOptions json_options = {});
::std::string TableFlatbufferToJson(const flatbuffers::Table *t,
const ::flatbuffers::TypeTable *typetable,
- bool multi_line,
- size_t max_vector_size = SIZE_MAX);
+ JsonOptions json_options = {});
// Converts a DetachedBuffer holding a flatbuffer to JSON.
inline ::std::string FlatbufferToJson(const flatbuffers::DetachedBuffer &buffer,
const flatbuffers::TypeTable *typetable,
- bool multi_line = false,
- size_t max_vector_size = SIZE_MAX) {
- return BufferFlatbufferToJson(buffer.data(), typetable, multi_line,
- max_vector_size);
+ JsonOptions json_options = {}) {
+ return BufferFlatbufferToJson(buffer.data(), typetable, json_options);
}
// Converts a Flatbuffer<T> holding a flatbuffer to JSON.
template <typename T>
inline ::std::string FlatbufferToJson(const Flatbuffer<T> &flatbuffer,
- bool multi_line = false,
- size_t max_vector_size = SIZE_MAX) {
- return BufferFlatbufferToJson(flatbuffer.data(),
- Flatbuffer<T>::MiniReflectTypeTable(),
- multi_line, max_vector_size);
+ JsonOptions json_options = {}) {
+ return BufferFlatbufferToJson(
+ flatbuffer.data(), Flatbuffer<T>::MiniReflectTypeTable(), json_options);
}
// Converts a flatbuffer::Table to JSON.
template <typename T>
-typename std::enable_if<std::is_base_of<flatbuffers::Table, T>::value,
- std::string>::
- type inline FlatbufferToJson(const T *flatbuffer, bool multi_line = false,
- size_t max_vector_size = SIZE_MAX) {
+typename std::enable_if<
+ std::is_base_of<flatbuffers::Table, T>::value,
+ std::string>::type inline FlatbufferToJson(const T *flatbuffer,
+ JsonOptions json_options = {}) {
return TableFlatbufferToJson(
reinterpret_cast<const flatbuffers::Table *>(flatbuffer),
- Flatbuffer<T>::MiniReflectTypeTable(), multi_line, max_vector_size);
+ Flatbuffer<T>::MiniReflectTypeTable(), json_options);
}
std::string FlatbufferToJson(const reflection::Schema *const schema,
const uint8_t *const data,
- size_t max_vector_size = SIZE_MAX);
+ JsonOptions json_options = {});
+
+// Writes a Flatbuffer to a file, or dies.
+template <typename T>
+inline void WriteFlatbufferToJson(const std::string_view filename,
+ const Flatbuffer<T> &msg) {
+ std::ofstream json_file(std::string(filename), std::ios::out);
+ CHECK(json_file) << ": Couldn't open " << filename;
+ json_file << FlatbufferToJson(msg);
+ json_file.close();
+}
+
+// Parses a file as JSON and returns the corresponding Flatbuffer, or dies.
+template <typename T>
+inline FlatbufferDetachedBuffer<T> JsonFileToFlatbuffer(
+ const std::string_view path) {
+ std::ifstream t{std::string(path)};
+ std::istream_iterator<char> start(t), end;
+ std::string result(start, end);
+ return FlatbufferDetachedBuffer<T>(JsonToFlatbuffer<T>(result));
+}
} // namespace aos
diff --git a/aos/json_to_flatbuffer_test.cc b/aos/json_to_flatbuffer_test.cc
index a9112f4..80dec7c 100644
--- a/aos/json_to_flatbuffer_test.cc
+++ b/aos/json_to_flatbuffer_test.cc
@@ -78,7 +78,6 @@
EXPECT_TRUE(JsonAndBack("{ \"foo_double\": 5.1 }"));
}
-
// Test what happens if you pass a field name that we don't know.
TEST_F(JsonToFlatbufferTest, InvalidFieldName) {
EXPECT_FALSE(JsonAndBack("{ \"foo\": 5 }"));
@@ -156,7 +155,8 @@
EXPECT_TRUE(JsonAndBack("{ \"vector_foo_string\": [ \"bar\", \"baz\" ] }"));
EXPECT_TRUE(JsonAndBack("{ \"vector_foo_string\": [ ] }"));
- EXPECT_TRUE(JsonAndBack("{ \"vector_foo_enum\": [ \"None\", \"UType\", \"Bool\" ] }"));
+ EXPECT_TRUE(JsonAndBack(
+ "{ \"vector_foo_enum\": [ \"None\", \"UType\", \"Bool\" ] }"));
EXPECT_TRUE(JsonAndBack("{ \"vector_foo_enum\": [ ] }"));
}
@@ -170,8 +170,7 @@
EXPECT_TRUE(JsonAndBack(
"{ \"apps\": [ { \"name\": \"woot\" }, { \"name\": \"wow\" } ] }"));
- EXPECT_TRUE(JsonAndBack(
- "{ \"apps\": [ { }, { } ] }"));
+ EXPECT_TRUE(JsonAndBack("{ \"apps\": [ { }, { } ] }"));
}
// Test that we can parse an empty message.
@@ -198,7 +197,8 @@
// Tests that multiple arrays get properly handled.
TEST_F(JsonToFlatbufferTest, NestedArrays) {
EXPECT_TRUE(
- JsonAndBack("{ \"vov\": { \"v\": [ { \"str\": [ \"a\", \"b\" ] }, { \"str\": [ \"c\", \"d\" ] } ] } }"));
+ JsonAndBack("{ \"vov\": { \"v\": [ { \"str\": [ \"a\", \"b\" ] }, { "
+ "\"str\": [ \"c\", \"d\" ] } ] } }"));
}
// TODO(austin): Missmatched values.
@@ -222,14 +222,20 @@
JsonToFlatbuffer(json_long.data(), ConfigurationTypeTable());
ASSERT_GT(fb_long.size(), 0);
- const std::string back_json_short =
- FlatbufferToJson(fb_short, ConfigurationTypeTable(), false, 100);
- const std::string back_json_long =
- FlatbufferToJson(fb_long, ConfigurationTypeTable(), false, 100);
+ const std::string back_json_short = FlatbufferToJson(
+ fb_short, ConfigurationTypeTable(), {.multi_line = false, .max_vector_size = 100});
+ const std::string back_json_long = FlatbufferToJson(
+ fb_long, ConfigurationTypeTable(), {.multi_line = false, .max_vector_size = 100});
EXPECT_EQ(json_short, back_json_short);
EXPECT_EQ("{ \"vector_foo_int\": [ ... 101 elements ... ] }", back_json_long);
}
+// Tests that a nullptr buffer prints nullptr.
+TEST_F(JsonToFlatbufferTest, NullptrData) {
+ EXPECT_EQ("null", TableFlatbufferToJson((const flatbuffers::Table *)(nullptr),
+ ConfigurationTypeTable()));
+}
+
} // namespace testing
} // namespace aos
diff --git a/aos/network/message_bridge_server_lib.cc b/aos/network/message_bridge_server_lib.cc
index 1b8d36f..93e4c90 100644
--- a/aos/network/message_bridge_server_lib.cc
+++ b/aos/network/message_bridge_server_lib.cc
@@ -177,9 +177,9 @@
// time out eventually. Need to sort that out.
}
-void ChannelState::AddPeer(
- const Connection *connection, int node_index,
- ServerConnection *server_connection_statistics, bool logged_remotely) {
+void ChannelState::AddPeer(const Connection *connection, int node_index,
+ ServerConnection *server_connection_statistics,
+ bool logged_remotely) {
peers_.emplace_back(connection, node_index, server_connection_statistics,
logged_remotely);
}
@@ -199,7 +199,7 @@
}
int ChannelState::NodeConnected(const Node *node, sctp_assoc_t assoc_id,
- int stream, SctpServer *server) {
+ int stream, SctpServer *server) {
for (ChannelState::Peer &peer : peers_) {
if (peer.connection->name()->string_view() == node->name()->string_view()) {
peer.sac_assoc_id = assoc_id;
@@ -445,11 +445,11 @@
}
ResetFilter(node_index);
VLOG(1) << "Resetting filters for " << node_index << " "
- << event_loop_->configuration()
- ->nodes()
- ->Get(node_index)
- ->name()
- ->string_view();
+ << event_loop_->configuration()
+ ->nodes()
+ ->Get(node_index)
+ ->name()
+ ->string_view();
} else if (message->header.rcvinfo.rcv_sid == kTimestampStream()) {
// Message delivery
const logger::MessageHeader *message_header =
diff --git a/aos/network/message_bridge_test.cc b/aos/network/message_bridge_test.cc
index 96003f7..8d52292 100644
--- a/aos/network/message_bridge_test.cc
+++ b/aos/network/message_bridge_test.cc
@@ -7,9 +7,7 @@
#include "aos/events/pong_generated.h"
#include "aos/network/message_bridge_client_lib.h"
#include "aos/network/message_bridge_server_lib.h"
-
-DECLARE_string(override_hostname);
-DECLARE_string(application_name);
+#include "aos/network/team_number.h"
namespace aos {
namespace message_bridge {
diff --git a/aos/network/sctp_client.cc b/aos/network/sctp_client.cc
index a29f841..91976cc 100644
--- a/aos/network/sctp_client.cc
+++ b/aos/network/sctp_client.cc
@@ -130,14 +130,14 @@
}
void SctpClient::SetPriorityScheduler(sctp_assoc_t assoc_id) {
- struct sctp_assoc_value scheduler;
- memset(&scheduler, 0, sizeof(scheduler));
- scheduler.assoc_id = assoc_id;
- scheduler.assoc_value = SCTP_SS_PRIO;
- if (setsockopt(fd(), IPPROTO_SCTP, SCTP_STREAM_SCHEDULER, &scheduler,
- sizeof(scheduler)) != 0) {
- PLOG(WARNING) << "Failed to set scheduler";
- }
+ struct sctp_assoc_value scheduler;
+ memset(&scheduler, 0, sizeof(scheduler));
+ scheduler.assoc_id = assoc_id;
+ scheduler.assoc_value = SCTP_SS_PRIO;
+ if (setsockopt(fd(), IPPROTO_SCTP, SCTP_STREAM_SCHEDULER, &scheduler,
+ sizeof(scheduler)) != 0) {
+ PLOG(WARNING) << "Failed to set scheduler";
+ }
}
} // namespace message_bridge
diff --git a/aos/network/team_number.cc b/aos/network/team_number.cc
index 8d4e69e..1f71946 100644
--- a/aos/network/team_number.cc
+++ b/aos/network/team_number.cc
@@ -1,15 +1,16 @@
#include "aos/network/team_number.h"
-#include <netinet/in.h>
#include <inttypes.h>
-#include <unistd.h>
+#include <netinet/in.h>
#include <stdlib.h>
-
-#include "glog/logging.h"
+#include <unistd.h>
#include "aos/util/string_to_num.h"
-DECLARE_string(override_hostname);
+DEFINE_string(
+ override_hostname, "",
+ "If set, this forces the hostname of this node to be the provided "
+ "hostname.");
namespace aos {
namespace network {
@@ -68,7 +69,7 @@
uint16_t DoGetTeamNumber() {
if (override_team != 0) {
- return override_team;
+ return override_team;
}
const char *override_number = getenv("AOS_TEAM_NUMBER");
@@ -134,8 +135,8 @@
if (second_separator == hostname.npos) {
return std::nullopt;
}
- const std::string number_string =
- hostname.substr(second_separator + 1, hostname.size() - second_separator - 1);
+ const std::string number_string = hostname.substr(
+ second_separator + 1, hostname.size() - second_separator - 1);
if (number_string.size() == 0) {
return std::nullopt;
}
diff --git a/aos/network/team_number.h b/aos/network/team_number.h
index 91a61f9..c2954b6 100644
--- a/aos/network/team_number.h
+++ b/aos/network/team_number.h
@@ -6,6 +6,10 @@
#include <optional>
#include <string>
+#include "glog/logging.h"
+
+DECLARE_string(override_hostname);
+
namespace aos {
namespace network {
diff --git a/aos/util/BUILD b/aos/util/BUILD
index f487892..b23b720 100644
--- a/aos/util/BUILD
+++ b/aos/util/BUILD
@@ -267,7 +267,7 @@
deps = [
":phased_loop",
"//aos/testing:googletest",
- "//aos/testing:test_logging",
+ "//aos/time",
],
)
diff --git a/aos/util/file.cc b/aos/util/file.cc
index b334ded..089efbc 100644
--- a/aos/util/file.cc
+++ b/aos/util/file.cc
@@ -66,5 +66,10 @@
PCHECK(result == 0) << ": Error creating " << folder;
}
+bool PathExists(std::string_view path) {
+ struct stat buffer;
+ return stat(path.data(), &buffer) == 0;
+}
+
} // namespace util
} // namespace aos
diff --git a/aos/util/file.h b/aos/util/file.h
index d6724af..9ee2fb4 100644
--- a/aos/util/file.h
+++ b/aos/util/file.h
@@ -17,6 +17,8 @@
void MkdirP(std::string_view path, mode_t mode);
+bool PathExists(std::string_view path);
+
} // namespace util
} // namespace aos
diff --git a/aos/util/file_test.cc b/aos/util/file_test.cc
index fa259e8..6851302 100644
--- a/aos/util/file_test.cc
+++ b/aos/util/file_test.cc
@@ -26,6 +26,19 @@
EXPECT_EQ(my_pid, stat.substr(0, my_pid.size()));
}
+// Tests that the PathExists function works under normal conditions.
+TEST(FileTest, PathExistsTest) {
+ const std::string tmpdir(getenv("TEST_TMPDIR"));
+ const std::string test_file = tmpdir + "/test_file";
+ // Make sure the test_file doesn't exist.
+ unlink(test_file.c_str());
+ EXPECT_FALSE(PathExists(test_file));
+
+ WriteStringToFileOrDie(test_file, "abc");
+
+ EXPECT_TRUE(PathExists(test_file));
+}
+
} // namespace testing
} // namespace util
} // namespace aos
diff --git a/aos/util/phased_loop.cc b/aos/util/phased_loop.cc
index 7369a93..1e520b1 100644
--- a/aos/util/phased_loop.cc
+++ b/aos/util/phased_loop.cc
@@ -37,20 +37,28 @@
}
int PhasedLoop::Iterate(const monotonic_clock::time_point now) {
- const monotonic_clock::time_point next_time =
- monotonic_clock::time_point(
- (((now - offset_).time_since_epoch() + monotonic_clock::duration(1)) /
- interval_) *
- interval_) +
- ((now.time_since_epoch() < offset_) ? monotonic_clock::zero()
- : interval_) +
- offset_;
+ auto next_time = monotonic_clock::epoch();
+ // Round up to the next whole interval, ignoring offset_.
+ {
+ const auto offset_now = (now - offset_).time_since_epoch();
+ monotonic_clock::duration prerounding;
+ if (now.time_since_epoch() >= offset_) {
+ // We're above 0, so rounding up means away from 0.
+ prerounding = offset_now + interval_;
+ } else {
+ // We're below 0, so rounding up means towards 0.
+ prerounding = offset_now + monotonic_clock::duration(1);
+ }
+ next_time += (prerounding / interval_) * interval_;
+ }
+ // Add offset_ back in.
+ next_time += offset_;
const monotonic_clock::duration difference = next_time - last_time_;
const int result = difference / interval_;
CHECK_EQ(
0, (next_time - offset_).time_since_epoch().count() % interval_.count());
- CHECK(next_time >= now);
+ CHECK(next_time > now);
CHECK(next_time - now <= interval_);
last_time_ = next_time;
return result;
diff --git a/aos/util/phased_loop_test.cc b/aos/util/phased_loop_test.cc
index 4a80522..1c766c4 100644
--- a/aos/util/phased_loop_test.cc
+++ b/aos/util/phased_loop_test.cc
@@ -2,13 +2,14 @@
#include "gtest/gtest.h"
-#include "aos/testing/test_logging.h"
+#include "aos/time/time.h"
namespace aos {
namespace time {
namespace testing {
using ::std::chrono::milliseconds;
+using ::std::chrono::nanoseconds;
typedef ::testing::Test PhasedLoopTest;
typedef PhasedLoopTest PhasedLoopDeathTest;
@@ -191,6 +192,31 @@
".*interval > monotonic_clock::duration\\(0\\).*");
}
+// Tests that every single value within two intervals of 0 works.
+// This is good at finding edge cases in the rounding.
+TEST_F(PhasedLoopTest, SweepingZero) {
+ for (int i = -30; i < -20; ++i) {
+ PhasedLoop loop(nanoseconds(20),
+ monotonic_clock::epoch() - nanoseconds(30));
+ EXPECT_EQ(1, loop.Iterate(monotonic_clock::epoch() + nanoseconds(i)));
+ }
+ for (int i = -20; i < 0; ++i) {
+ PhasedLoop loop(nanoseconds(20),
+ monotonic_clock::epoch() - nanoseconds(30));
+ EXPECT_EQ(2, loop.Iterate(monotonic_clock::epoch() + nanoseconds(i)));
+ }
+ for (int i = 0; i < 20; ++i) {
+ PhasedLoop loop(nanoseconds(20),
+ monotonic_clock::epoch() - nanoseconds(30));
+ EXPECT_EQ(3, loop.Iterate(monotonic_clock::epoch() + nanoseconds(i)));
+ }
+ for (int i = 20; i < 30; ++i) {
+ PhasedLoop loop(nanoseconds(20),
+ monotonic_clock::epoch() - nanoseconds(30));
+ EXPECT_EQ(4, loop.Iterate(monotonic_clock::epoch() + nanoseconds(i)));
+ }
+}
+
} // namespace testing
} // namespace time
} // namespace aos
diff --git a/build_tests/BUILD b/build_tests/BUILD
index 28dc677..e837390 100644
--- a/build_tests/BUILD
+++ b/build_tests/BUILD
@@ -124,3 +124,10 @@
srcs_version = "PY2AND3",
deps = ["@opencv_contrib_nonfree_amd64//:python_opencv"],
)
+
+py_test(
+ name = "python_jinja2",
+ srcs = ["python_jinja2.py"],
+ srcs_version = "PY2AND3",
+ deps = ["@python_jinja2"],
+)
diff --git a/build_tests/python_jinja2.py b/build_tests/python_jinja2.py
new file mode 100644
index 0000000..a926e31
--- /dev/null
+++ b/build_tests/python_jinja2.py
@@ -0,0 +1,4 @@
+#!/usr/bin/python3
+
+# Confirm that we can import jinja2.
+import jinja2
diff --git a/debian/python_jinja2.BUILD b/debian/python_jinja2.BUILD
new file mode 100644
index 0000000..1adab6a
--- /dev/null
+++ b/debian/python_jinja2.BUILD
@@ -0,0 +1,7 @@
+py_library(
+ name = "python_jinja2",
+ srcs = glob(["src/jinja2/*.py"]),
+ imports = ["src/"],
+ visibility = ["//visibility:public"],
+ deps = ["@python_markupsafe"],
+)
diff --git a/debian/python_markupsafe.BUILD b/debian/python_markupsafe.BUILD
new file mode 100644
index 0000000..87f10ea
--- /dev/null
+++ b/debian/python_markupsafe.BUILD
@@ -0,0 +1,6 @@
+py_library(
+ name = "python_markupsafe",
+ srcs = glob(["src/markupsafe/*.py"]),
+ imports = ["src/"],
+ visibility = ["//visibility:public"],
+)
diff --git a/documentation/tutorials/BUILD b/documentation/tutorials/BUILD
index 13516c1..5816444 100644
--- a/documentation/tutorials/BUILD
+++ b/documentation/tutorials/BUILD
@@ -6,10 +6,8 @@
"create-a-simple-program-for-running-a-motor",
"download-code-to-the-robot",
"make-a-drivebase-move",
- "send-and-receive-messages-on-queues",
"submitting-code-for-a-review",
"tune-an-autonomous",
- "generated-queue-code",
"using-bazel",
]
diff --git a/documentation/tutorials/generated-queue-code.md b/documentation/tutorials/generated-queue-code.md
deleted file mode 100644
index 8f00a31..0000000
--- a/documentation/tutorials/generated-queue-code.md
+++ /dev/null
@@ -1,246 +0,0 @@
-# Generated queue code
-
-This tutorial covers what the code that is generated from a .q file looks like.
-
-### The basics
-Each `.q` file generates a `.cc` and `.h` pair that can be #include'd in your c++
-code.
-
-We're going to use `//frc971/queues:gyro.q` as our example.
-
-## Package declaration
-`.q` files usually start with a `package` declaration. The `package` declaration
-directly converts to a set of namespace declarations. So
-
-```cpp
-package frc971.sensors;
-```
-
-generates the namespace definitions
-
-```cpp
-namespace frc971 {
-namespace sensors {
-
-// All of the code for the queue
-
-} // namespace sensors
-} // namespace frc971
-```
-
-## Message declarations
-Each message declared in the queue generates a class that can be instantiated in
-your code easily.
-
-For a simple example, lets use the `Uid` message in `//frc971/queues:gyro.q`:
-
-```cpp
-message Uid {
- uint32_t uid;
-};
-```
-
-Let's take a look at the actual class definition that is created. If you ever
-want to inspect the code that is generated from a .q file, you can take a look
-in `bazel-genfiles` after you build your code. So after running `bazel build
-//frc971/queues:gyro` there will be a `bazel-genfiles/frc971/queues/gyro.q.h`
-and `bazel-genfiles/frc971/queues/gyro.q.cc`.
-
-Here's the definition of the `Uid` class from
-`bazel-genfiles/frc971/queues/gyro.q.h` (comments mine):
-
-```cpp
-struct Uid : public ::aos::Message {
- // Used to identify queues uniquely even if they have the same name.
- enum { kQueueLength = 100, kHash = 0x0837c541 };
-
- // The actual data that we requested be part of the message.
- uint32_t uid;
-
- // Writes the message to a byte buffer.
- size_t Serialize(char *buffer) const;
- // Reads the message from a byte buffer.
- size_t Deserialize(const char *buffer);
- // Zeroes all of the fields in the message.
- void Zero();
- // Returns the size of message. Not used
- static size_t Size() { return 4 + ::aos::Message::Size(); }
- // Prints the contents of the message to a string that is human readable.
- size_t Print(char *buffer, size_t length) const;
-
- // Gets information about this message's type.
- static const ::aos::MessageType *GetType();
- static const ::aos::MessageType *DoGetType();
-
- // Default constructor, zeroes the struct.
- Uid();
- // Value constructor, sets the data at construction.
- Uid(uint32_t uid_in);
- bool EqualsNoTime(const Uid &other) const;
-};
-```
-
-## Queue declarations
-Every `queue` declaration in the `.q` file creates an `aos::Queue` variable that
-uses the message type given. So the declaration
-
-```cpp
-queue Uid gyro_part_id;
-```
-
-will generate a variable in `gyro.q.h`
-
-```cpp
-static UNUSED_VARIABLE::aos::Queue<Uid> &gyro_part_id;
-```
-
-which can then be used in any file that includes the header file.
-
-
-## QueueGroup declarations
-
-`queue_group` declares a set of queues and some associated helpers for them.
-There are two kinds of `queue_groups`, declarations and aliases. Here's an
-example of a `queue_group` definition:
-
-```cpp
-queue_group SuperstructureQueue {
- implements aos.control_loops.ControlLoop;
-
- message Goal {
- IntakeGoal intake;
-
- // Used to identify a position in the planned set of positions on the arm.
- uint32_t arm_goal_position;
- // If true, start the grab box sequence.
- bool grab_box;
-
- bool open_claw;
- bool close_claw;
-
- bool deploy_fork;
-
- bool hook_release;
-
- double voltage_winch;
-
- double open_threshold;
-
- bool disable_box_correct;
-
- bool trajectory_override;
- };
-
- message Status {
- // Are all the subsystems zeroed?
- bool zeroed;
-
- // If true, any of the subsystems have aborted.
- bool estopped;
-
- // Status of both intake sides.
- IntakeSideStatus left_intake;
- IntakeSideStatus right_intake;
-
- ArmStatus arm;
-
- double filtered_box_velocity;
- uint32_t rotation_state;
- };
-
- message Position {
- // Values of the series elastic encoders on the left side of the robot from
- // the rear perspective in radians.
- IntakeElasticSensors left_intake;
-
- // Values of the series elastic encoders on the right side of the robot from
- // the rear perspective in radians.
- IntakeElasticSensors right_intake;
-
- ArmPosition arm;
-
- // Value of the beam breaker sensor. This value is true if the beam is
- // broken, false if the beam isn't broken.
- bool claw_beambreak_triggered;
- // Value of the beambreak sensor detecting when the box has hit the frame
- // cutout.
- bool box_back_beambreak_triggered;
-
- // Distance to the box in meters.
- double box_distance;
- };
-
- message Output {
- // Voltage sent to the parts on the left side of the intake.
- IntakeVoltage left_intake;
-
- // Voltage sent to the parts on the right side of the intake.
- IntakeVoltage right_intake;
-
- // Voltage sent to the motors on the proximal joint of the arm.
- double voltage_proximal;
-
- // Voltage sent to the motors on the distal joint of the arm.
- double voltage_distal;
-
- // Voltage sent to the hanger. Positive pulls the robot up.
- double voltage_winch;
-
- // Clamped (when true) or unclamped (when false) status sent to the
- // pneumatic claw on the arm.
- bool claw_grabbed;
-
- // If true, release the arm brakes.
- bool release_arm_brake;
- // If true, release the hook
- bool hook_release;
- // If true, release the forks
- bool forks_release;
- };
-
- queue Goal goal;
- queue Output output;
- queue Status status;
- queue Position position;
-};
-```
-
-and aliases look like:
-
-```cpp
-queue_group SuperstructureQueue superstructure_queue;
-```
-
-The declaration type creates the definition of the queue group and what messages
-and queues it contains. The alias type creates a variable with the given name.
-
-This queue group results in the following code in
-`bazel-genfiles/y2018/control_loops/superstructure/superstructure.q.h`
-
-```cpp
-
-struct SuperstructureQueue_Goal : public ::aos::Message { /* ... */ }
-struct SuperstructureQueue_Output : public ::aos::Message { /* ... */ }
-struct SuperstructureQueue_Status : public ::aos::Message { /* ... */ }
-struct SuperstructureQueue_Position : public ::aos::Message { /* ... */ }
-class SuperstructureQueue : public ::aos::QueueGroup {
- public:
- typedef SuperstructureQueue_Goal Goal;
- ::aos::Queue<SuperstructureQueue_Goal> goal;
- typedef SuperstructureQueue_Output Output;
- ::aos::Queue<SuperstructureQueue_Output> output;
- typedef SuperstructureQueue_Status Status;
- ::aos::Queue<SuperstructureQueue_Status> status;
- typedef SuperstructureQueue_Position Position;
- ::aos::Queue<SuperstructureQueue_Position> position;
- SuperstructureQueue(const char *name, uint32_t hash, const char *goal_name,
- const char *output_name, const char *status_name,
- const char *position_name);
-};
-```
-
-and a definition of a variable:
-
-```cpp
-static UNUSED_VARIABLE SuperstructureQueue &superstructure_queue;
-```
diff --git a/documentation/tutorials/send-and-receive-messages-on-queues.md b/documentation/tutorials/send-and-receive-messages-on-queues.md
deleted file mode 100644
index bcdaac1..0000000
--- a/documentation/tutorials/send-and-receive-messages-on-queues.md
+++ /dev/null
@@ -1,74 +0,0 @@
-# How to send and receive messages on a Queue
-
-Let's say we have a `//y2018:basic_queue.q` file that has the following
-contents:
-
-```cpp
-package y2018;
-
-message BasicMessage {
- float value;
-};
-
-queue BasicMessage basic_queue;
-```
-
-and the corresponding entry in `//y2018/BUILD`
-
-```python
-queue_library(
- name = "basic_queue",
- srcs = [
- "basic_queue.q",
- ],
- visibility = ["//visibility:public"],
-)
-```
-
-The following examples assume that you've declared a dependency on
-`//y2018:basic_queue` in the relevant `BUILD` file.
-
-## Sending a message
-First, include the queue definition that is created from the `.q` file. This
-will make the `basic_queue` variable available.
-
-```cpp
-#include "y2018/basic_queue.q.h"
-```
-
-```cpp
-
-// Allocate a new message pointer to use. basic_queue is from the include
-// statement.
-auto new_basic_message = basic_queue.MakeMessage();
-new_goal.value = 0.5;
-
-// Send the message on the queue
-if (!new_basic_message.Send()) {
- LOG(ERROR, "Failed to send.\n");
-}
-```
-
-## Receiving a message
-
-Once again, we must include the queue header file to get access to the queue.
-
-
-```cpp
-#include "y2018/basic_queue.q.h"
-```
-
-Then we can receive a message.
-
-
-```cpp
-basic_queue.FetchLatest();
-if (basic_queue.get()) {
- // We have a valid message ready to be used
- LOG(INFO, "Got a BasicMessage from basic_queue with value %f\n",
- basic_queue->value);
-} else {
- // No new message was available
- LOG(ERROR, "No message was received\n");
-}
-```
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index ac2a2e8..ffa2c11 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -702,6 +702,7 @@
deps = [
":drivetrain_config",
":drivetrain_status_fbs",
+ "//aos/controls:quaternion_utils",
"//aos/events:event_loop",
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:runge_kutta",
@@ -718,6 +719,7 @@
],
deps = [
":drivetrain_test_lib",
+ "//aos/controls:quaternion_utils",
"//aos/testing:googletest",
"//aos/testing:random_seed",
"//frc971/control_loops/drivetrain:improved_down_estimator",
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index e03d581..bd358ca 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -138,6 +138,18 @@
static constexpr int kNInputs = 4;
// Number of previous samples to save.
static constexpr int kSaveSamples = 50;
+ // Whether we should completely rerun the entire stored history of
+ // kSaveSamples on every correction. Enabling this will increase overall CPU
+ // usage substantially; however, leaving it disabled makes it so that we are
+ // less likely to notice if processing camera frames is causing delays in the
+ // drivetrain.
+ // If we are having CPU issues, we have three easy avenues to improve things:
+ // (1) Reduce kSaveSamples (e.g., if all camera frames arive within
+ // 100 ms, then we can reduce kSaveSamples to be 25 (125 ms of samples)).
+ // (2) Don't actually rely on the ability to insert corrections into the
+ // timeline.
+ // (3) Set this to false.
+ static constexpr bool kFullRewindOnEverySample = false;
// Assume that all correction steps will have kNOutputs
// dimensions.
// TODO(james): Relax this assumption; relaxing it requires
@@ -182,19 +194,24 @@
X_hat_ = state;
have_zeroed_encoders_ = true;
P_ = P;
- observations_.PushFromBottom(
- {t,
- t,
- X_hat_,
- P_,
- Input::Zero(),
- Output::Zero(),
- {},
- [](const State &, const Input &) { return Output::Zero(); },
- [](const State &) {
- return Eigen::Matrix<Scalar, kNOutputs, kNStates>::Zero();
- },
- Eigen::Matrix<Scalar, kNOutputs, kNOutputs>::Identity()});
+ observations_.PushFromBottom({
+ t,
+ t,
+ X_hat_,
+ P_,
+ Input::Zero(),
+ Output::Zero(),
+ {},
+ [](const State &, const Input &) { return Output::Zero(); },
+ [](const State &) {
+ return Eigen::Matrix<Scalar, kNOutputs, kNStates>::Zero();
+ },
+ Eigen::Matrix<Scalar, kNOutputs, kNOutputs>::Identity(),
+ StateSquare::Identity(),
+ StateSquare::Zero(),
+ std::chrono::seconds(0),
+ State::Zero(),
+ });
}
// Correct with:
@@ -292,6 +309,29 @@
dt_config_.robot_radius;
}
+ // Returns the last state before the specified time.
+ // Returns nullopt if time is older than the oldest measurement.
+ std::optional<State> LastStateBeforeTime(
+ aos::monotonic_clock::time_point time) {
+ if (observations_.empty() || observations_.begin()->t > time) {
+ return std::nullopt;
+ }
+ for (const auto &observation : observations_) {
+ if (observation.t > time) {
+ // Note that observation.X_hat actually references the _previous_ X_hat.
+ return observation.X_hat;
+ }
+ }
+ return X_hat();
+ }
+
+ // Returns the most recent input vector.
+ Input MostRecentInput() {
+ CHECK(!observations_.empty());
+ Input U = observations_.top().U;
+ return U;
+ }
+
private:
struct Observation {
// Time when the observation was taken.
@@ -331,6 +371,16 @@
// The measurement noise matrix.
Eigen::Matrix<Scalar, kNOutputs, kNOutputs> R;
+ // Discretized A and Q to use on this update step. These will only be
+ // recalculated if the timestep changes.
+ StateSquare A_d;
+ StateSquare Q_d;
+ aos::monotonic_clock::duration discretization_time;
+
+ // A cached value indicating how much we change X_hat in the prediction step
+ // of this Observation.
+ State predict_update;
+
// In order to sort the observations in the PriorityQueue object, we
// need a comparison function.
friend bool operator<(const Observation &l, const Observation &r) {
@@ -453,45 +503,56 @@
return Xdot;
}
- void PredictImpl(const Input &U, std::chrono::nanoseconds dt, State *state,
+ void PredictImpl(Observation *obs, std::chrono::nanoseconds dt, State *state,
StateSquare *P) {
- StateSquare A_c = AForState(*state);
- StateSquare A_d;
- StateSquare Q_d;
- // TODO(james): By far the biggest CPU sink in the localization appears to
- // be this discretization--it's possible the spline code spikes higher,
- // but it doesn't create anywhere near the same sustained load. There
- // are a few potential options for optimizing this code, but none of
- // them are entirely trivial, e.g. we could:
- // -Reduce the number of states (this function grows at O(kNStates^3))
- // -Adjust the discretization function itself (there're a few things we
- // can tune there).
- // -Try to come up with some sort of lookup table or other way of
- // pre-calculating A_d and Q_d.
- // I also have to figure out how much we care about the precision of
- // some of these values--I don't think we care much, but we probably
- // do want to maintain some of the structure of the matrices.
- controls::DiscretizeQAFast(Q_continuous_, A_c, dt, &Q_d, &A_d);
+ // Only recalculate the discretization if the timestep has changed.
+ // Technically, this isn't quite correct, since the discretization will
+ // change depending on the current state. However, the slight loss of
+ // precision seems acceptable for the sake of significantly reducing CPU
+ // usage.
+ if (obs->discretization_time != dt) {
+ // TODO(james): By far the biggest CPU sink in the localization appears to
+ // be this discretization--it's possible the spline code spikes higher,
+ // but it doesn't create anywhere near the same sustained load. There
+ // are a few potential options for optimizing this code, but none of
+ // them are entirely trivial, e.g. we could:
+ // -Reduce the number of states (this function grows at O(kNStates^3))
+ // -Adjust the discretization function itself (there're a few things we
+ // can tune there).
+ // -Try to come up with some sort of lookup table or other way of
+ // pre-calculating A_d and Q_d.
+ // I also have to figure out how much we care about the precision of
+ // some of these values--I don't think we care much, but we probably
+ // do want to maintain some of the structure of the matrices.
+ const StateSquare A_c = AForState(*state);
+ controls::DiscretizeQAFast(Q_continuous_, A_c, dt, &obs->Q_d, &obs->A_d);
+ obs->discretization_time = dt;
- *state = RungeKuttaU(
- [this](const State &X, const Input &U) { return DiffEq(X, U); }, *state,
- U, aos::time::DurationInSeconds(dt));
+ obs->predict_update =
+ RungeKuttaU(
+ [this](const State &X, const Input &U) { return DiffEq(X, U); },
+ *state, obs->U, aos::time::DurationInSeconds(dt)) -
+ *state;
+ }
- StateSquare Ptemp = A_d * *P * A_d.transpose() + Q_d;
+ *state += obs->predict_update;
+
+ StateSquare Ptemp = obs->A_d * *P * obs->A_d.transpose() + obs->Q_d;
*P = Ptemp;
}
- void CorrectImpl(const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
- const Output &Z, const Output &expected_Z,
- const Eigen::Matrix<Scalar, kNOutputs, kNStates> &H,
- State *state, StateSquare *P) {
- Output err = Z - expected_Z;
- Eigen::Matrix<Scalar, kNStates, kNOutputs> PH = *P * H.transpose();
- Eigen::Matrix<Scalar, kNOutputs, kNOutputs> S = H * PH + R;
- Eigen::Matrix<Scalar, kNStates, kNOutputs> K = PH * S.inverse();
- *state += K * err;
- StateSquare Ptemp = (StateSquare::Identity() - K * H) * *P;
+ void CorrectImpl(Observation *obs, State *state, StateSquare *P) {
+ const Eigen::Matrix<Scalar, kNOutputs, kNStates> H = obs->dhdx(*state);
+ // Note: Technically, this does calculate P * H.transpose() twice. However,
+ // when I was mucking around with some things, I found that in practice
+ // putting everything into one expression and letting Eigen optimize it
+ // directly actually improved performance relative to precalculating P *
+ // H.transpose().
+ const Eigen::Matrix<Scalar, kNStates, kNOutputs> K =
+ *P * H.transpose() * (H * *P * H.transpose() + obs->R).inverse();
+ const StateSquare Ptemp = (StateSquare::Identity() - K * H) * *P;
*P = Ptemp;
+ *state += K * (obs->z - obs->h(*state, obs->U));
}
void ProcessObservation(Observation *obs, const std::chrono::nanoseconds dt,
@@ -499,14 +560,13 @@
*state = obs->X_hat;
*P = obs->P;
if (dt.count() != 0) {
- PredictImpl(obs->U, dt, state, P);
+ PredictImpl(obs, dt, state, P);
}
if (!(obs->h && obs->dhdx)) {
CHECK(obs->make_h);
obs->make_h(*state, *P, &obs->h, &obs->dhdx);
}
- CorrectImpl(obs->R, obs->z, obs->h(*state, obs->U), obs->dhdx(*state),
- state, P);
+ CorrectImpl(obs, state, P);
}
DrivetrainConfig<double> dt_config_;
@@ -547,9 +607,10 @@
"initialized.\n";
return;
}
- auto cur_it =
- observations_.PushFromBottom({t, t, State::Zero(), StateSquare::Zero(),
- Input::Zero(), z, make_h, h, dhdx, R});
+ auto cur_it = observations_.PushFromBottom(
+ {t, t, State::Zero(), StateSquare::Zero(), Input::Zero(), z, make_h, h,
+ dhdx, R, StateSquare::Identity(), StateSquare::Zero(),
+ std::chrono::seconds(0), State::Zero()});
if (cur_it == observations_.end()) {
VLOG(1) << "Camera dropped off of end with time of "
<< aos::time::DurationInSeconds(t.time_since_epoch())
@@ -560,7 +621,6 @@
<< "s.\n";
return;
}
-
// Now we populate any state information that depends on where the
// observation was inserted into the queue. X_hat and P must be populated
// from the values present in the observation *following* this one in
@@ -594,6 +654,12 @@
next_it->prev_t = cur_it->t;
cur_it->U = (U == nullptr) ? next_it->U : *U;
}
+
+ if (kFullRewindOnEverySample) {
+ next_it = observations_.begin();
+ cur_it = next_it++;
+ }
+
// Now we need to rerun the predict step from the previous to the new
// observation as well as every following correct/predict up to the current
// time.
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator.cc b/frc971/control_loops/drivetrain/improved_down_estimator.cc
index cdb1587..665bf32 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator.cc
+++ b/frc971/control_loops/drivetrain/improved_down_estimator.cc
@@ -3,139 +3,12 @@
#include "Eigen/Dense"
#include "Eigen/Geometry"
+#include "aos/controls/quaternion_utils.h"
+
namespace frc971 {
namespace control_loops {
namespace drivetrain {
-Eigen::Matrix<double, 4, 1> ToQuaternionFromRotationVector(
- const Eigen::Matrix<double, 3, 1> &X, const double max_angle_cap) {
- const double unclipped_angle = X.norm();
- const double angle_scalar =
- (unclipped_angle > max_angle_cap) ? max_angle_cap / unclipped_angle : 1.0;
- const double angle = unclipped_angle * angle_scalar;
- const double half_angle = angle * 0.5;
-
- const double half_angle_squared = half_angle * half_angle;
-
- // sin(x)/x = 1
- double sinx_x = 1.0;
-
- // - x^2/3!
- double value = half_angle_squared / 6.0;
- sinx_x -= value;
-
- // + x^4/5!
- value = value * half_angle_squared / 20.0;
- sinx_x += value;
-
- // - x^6/7!
- value = value * half_angle_squared / (6.0 * 7.0);
- sinx_x -= value;
-
- // + x^8/9!
- value = value * half_angle_squared / (8.0 * 9.0);
- sinx_x += value;
-
- // - x^10/11!
- value = value * half_angle_squared / (10.0 * 11.0);
- sinx_x -= value;
-
- // + x^12/13!
- value = value * half_angle_squared / (12.0 * 13.0);
- sinx_x += value;
-
- // - x^14/15!
- value = value * half_angle_squared / (14.0 * 15.0);
- sinx_x -= value;
-
- // + x^16/17!
- value = value * half_angle_squared / (16.0 * 17.0);
- sinx_x += value;
-
- // To plot the residual in matplotlib, run:
- // import numpy
- // import scipy
- // from matplotlib import pyplot
- // x = numpy.arange(-numpy.pi, numpy.pi, 0.01)
- // pyplot.plot(x, 1 - x**2 / scipy.misc.factorial(3) +
- // x**4 / scipy.misc.factorial(5) -
- // x**6 / scipy.misc.factorial(7) +
- // x**8 / scipy.misc.factorial(9) -
- // x ** 10 / scipy.misc.factorial(11) +
- // x ** 12 / scipy.misc.factorial(13) -
- // x ** 14 / scipy.misc.factorial(15) +
- // x ** 16 / scipy.misc.factorial(17) -
- // numpy.sin(x) / x)
-
- const double scalar = sinx_x * 0.5;
-
- Eigen::Matrix<double, 4, 1> result;
- result.block<3, 1>(0, 0) = X * scalar * angle_scalar;
- result(3, 0) = std::cos(half_angle);
- return result;
-}
-
-inline Eigen::Matrix<double, 4, 1> MaybeFlipX(
- const Eigen::Matrix<double, 4, 1> &X) {
- if (X(3, 0) < 0.0) {
- return -X;
- } else {
- return X;
- }
-}
-
-Eigen::Matrix<double, 3, 1> ToRotationVectorFromQuaternion(
- const Eigen::Matrix<double, 4, 1> &X) {
- // TODO(austin): Verify we still need it.
- const Eigen::Matrix<double, 4, 1> corrected_X = MaybeFlipX(X);
- const double half_angle =
- std::atan2(corrected_X.block<3, 1>(0, 0).norm(), corrected_X(3, 0));
-
- const double half_angle_squared = half_angle * half_angle;
-
- // TODO(austin): We are doing a division at the end of this. Do the taylor
- // series expansion of x/sin(x) instead to avoid this.
-
- // sin(x)/x = 1
- double sinx_x = 1.0;
-
- // - x^2/3!
- double value = half_angle_squared / 6.0;
- sinx_x -= value;
-
- // + x^4/5!
- value = value * half_angle_squared / 20.0;
- sinx_x += value;
-
- // - x^6/7!
- value = value * half_angle_squared / (6.0 * 7.0);
- sinx_x -= value;
-
- // + x^8/9!
- value = value * half_angle_squared / (8.0 * 9.0);
- sinx_x += value;
-
- // - x^10/11!
- value = value * half_angle_squared / (10.0 * 11.0);
- sinx_x -= value;
-
- // + x^12/13!
- value = value * half_angle_squared / (12.0 * 13.0);
- sinx_x += value;
-
- // - x^14/15!
- value = value * half_angle_squared / (14.0 * 15.0);
- sinx_x -= value;
-
- // + x^16/17!
- value = value * half_angle_squared / (16.0 * 17.0);
- sinx_x += value;
-
- const double scalar = 2.0 / sinx_x;
-
- return corrected_X.block<3, 1>(0, 0) * scalar;
-}
-
void QuaternionUkf::Predict(const Eigen::Matrix<double, 3, 1> &U,
const Eigen::Matrix<double, 3, 1> &measurement,
const aos::monotonic_clock::duration dt) {
@@ -183,7 +56,7 @@
// We now have the sigma points after the model update.
// Compute the mean of the transformed sigma point
- X_hat_ = Eigen::Quaternion<double>(QuaternionMean(Y));
+ X_hat_ = Eigen::Quaternion<double>(aos::controls::QuaternionMean(Y));
// And the covariance.
Eigen::Matrix<double, 3, 2 * 3 + 1> Wprime;
@@ -262,8 +135,9 @@
P_xz * P_vv.inverse();
// Update X_hat and the covariance P
- X_hat_ = X_hat_ * Eigen::Quaternion<double>(ToQuaternionFromRotationVector(
- K * (measurement - Z_hat_)));
+ X_hat_ = X_hat_ * Eigen::Quaternion<double>(
+ aos::controls::ToQuaternionFromRotationVector(
+ K * (measurement - Z_hat_)));
P_ = P_prior - K * P_vv * K.transpose();
}
@@ -317,9 +191,10 @@
for (int i = 0; i < 7; ++i) {
// Compute the error vector for each sigma point.
- Eigen::Matrix<double, 3, 1> Wprimei = ToRotationVectorFromQuaternion(
- Eigen::Quaternion<double>(mean).conjugate() *
- Eigen::Quaternion<double>(points.col(i)));
+ Eigen::Matrix<double, 3, 1> Wprimei =
+ aos::controls::ToRotationVectorFromQuaternion(
+ Eigen::Quaternion<double>(mean).conjugate() *
+ Eigen::Quaternion<double>(points.col(i)));
// Now, compute the contribution of this sigma point to P_prior.
P_prior += 1.0 / 6.0 * (Wprimei * Wprimei.transpose());
// Save the error for the cross-correlation matrix.
@@ -343,7 +218,7 @@
Eigen::Matrix<double, 4, 3 * 2 + 1> X;
for (int i = 0; i < 3; ++i) {
Eigen::Quaternion<double> perturbation(
- ToQuaternionFromRotationVector(S.col(i), M_PI_2));
+ aos::controls::ToQuaternionFromRotationVector(S.col(i), M_PI_2));
X.col(i * 2) = (mean * perturbation).coeffs();
X.col(i * 2 + 1) = (mean * perturbation.conjugate()).coeffs();
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator.h b/frc971/control_loops/drivetrain/improved_down_estimator.h
index c1c8e1b..457bbef 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator.h
+++ b/frc971/control_loops/drivetrain/improved_down_estimator.h
@@ -15,63 +15,6 @@
namespace control_loops {
namespace drivetrain {
-// Function to compute the quaternion average of a bunch of quaternions. Each
-// column in the input matrix is a quaternion (optionally scaled by it's
-// weight).
-template <int SM>
-Eigen::Matrix<double, 4, 1> QuaternionMean(
- Eigen::Matrix<double, 4, SM> input) {
- // Algorithm to compute the average of a bunch of quaternions:
- // http://www.acsu.buffalo.edu/~johnc/ave_quat07.pdf
-
- Eigen::Matrix<double, 4, 4> m = input * input.transpose();
-
- Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>> solver;
- solver.compute(m);
-
- Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>>::EigenvectorsType
- eigenvectors = solver.eigenvectors();
- Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>>::EigenvalueType eigenvalues =
- solver.eigenvalues();
-
- int max_index = 0;
- double max_eigenvalue = 0.0;
- for (int i = 0; i < 4; ++i) {
- const double eigenvalue = std::abs(eigenvalues(i, 0));
- if (eigenvalue > max_eigenvalue) {
- max_eigenvalue = eigenvalue;
- max_index = i;
- }
- }
-
- // Assume that there shouldn't be any imaginary components to the eigenvector.
- // I can't prove this is true, but everyone else seems to assume it...
- // TODO(james): Handle this more rigorously.
- for (int i = 0; i < 4; ++i) {
- CHECK_LT(eigenvectors(i, max_index).imag(), 1e-4)
- << eigenvectors(i, max_index);
- }
- return eigenvectors.col(max_index).real().normalized();
-}
-
-// Converts from a quaternion to a rotation vector, where the rotation vector's
-// direction represents the axis to rotate around and its magnitude represents
-// the number of radians to rotate.
-Eigen::Matrix<double, 3, 1> ToRotationVectorFromQuaternion(
- const Eigen::Matrix<double, 4, 1> &X);
-
-inline Eigen::Matrix<double, 3, 1> ToRotationVectorFromQuaternion(
- const Eigen::Quaternion<double> &X) {
- return ToRotationVectorFromQuaternion(X.coeffs());
-};
-
-// Converts from a rotation vector to a quaternion. If you supply max_angle_cap,
-// then the rotation vector's magnitude will be clipped to be no more than
-// max_angle_cap before being converted to a quaternion.
-Eigen::Matrix<double, 4, 1> ToQuaternionFromRotationVector(
- const Eigen::Matrix<double, 3, 1> &X,
- const double max_angle_cap = std::numeric_limits<double>::infinity());
-
// Generates the sigma points to use in the UKF given the current estimate and
// covariance.
Eigen::Matrix<double, 4, 3 * 2 + 1> GenerateSigmaPoints(
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
index 2edce5a..c4643d9 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
+++ b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
@@ -1,11 +1,12 @@
-#include "frc971/control_loops/runge_kutta.h"
+#include "frc971/control_loops/drivetrain/improved_down_estimator.h"
#include <Eigen/Geometry>
#include <random>
+#include "aos/controls/quaternion_utils.h"
#include "aos/testing/random_seed.h"
#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
-#include "frc971/control_loops/drivetrain/improved_down_estimator.h"
+#include "frc971/control_loops/runge_kutta.h"
#include "glog/logging.h"
#include "gtest/gtest.h"
@@ -220,31 +221,6 @@
<< dtukf.H(dtukf.X_hat().coeffs());
}
-// Tests that small perturbations around a couple quaternions averaged out
-// return the original quaternion.
-TEST(DownEstimatorTest, QuaternionMean) {
- Eigen::Matrix<double, 4, 7> vectors;
- vectors.col(0) << 0, 0, 0, 1;
- for (int i = 0; i < 3; ++i) {
- Eigen::Matrix<double, 4, 1> perturbation;
- perturbation.setZero();
- perturbation(i, 0) = 0.1;
-
- vectors.col(i * 2 + 1) = vectors.col(0) + perturbation;
- vectors.col(i * 2 + 2) = vectors.col(0) - perturbation;
- }
-
- for (int i = 0; i < 7; ++i) {
- vectors.col(i).normalize();
- }
-
- Eigen::Matrix<double, 4, 1> mean = drivetrain::QuaternionMean(vectors);
-
- for (int i = 0; i < 4; ++i) {
- EXPECT_NEAR(mean(i, 0), vectors(i, 0), 0.001) << ": Failed on index " << i;
- }
-}
-
// Tests that computing sigma points, and then computing the mean and covariance
// returns the original answer.
TEST(DownEstimatorTest, SigmaPoints) {
@@ -259,7 +235,7 @@
drivetrain::GenerateSigmaPoints(mean, covariance);
const Eigen::Matrix<double, 4, 1> calculated_mean =
- drivetrain::QuaternionMean(vectors);
+ aos::controls::QuaternionMean(vectors);
VLOG(1) << "actual mean: " << mean.coeffs();
VLOG(1) << "calculated mean: " << calculated_mean;
@@ -290,7 +266,7 @@
drivetrain::GenerateSigmaPoints(mean, covariance);
const Eigen::Matrix<double, 4, 1> calculated_mean =
- drivetrain::QuaternionMean(vectors);
+ aos::controls::QuaternionMean(vectors);
Eigen::Matrix<double, 3, 3 * 2 + 1> Wprime;
Eigen::Matrix<double, 3, 3> calculated_covariance =
@@ -308,120 +284,6 @@
"covariance.";
}
-// Tests that ToRotationVectorFromQuaternion works for a 0 rotation.
-TEST(DownEstimatorTest, ToRotationVectorFromQuaternionAtZero) {
- Eigen::Matrix<double, 3, 1> vector =
- drivetrain::ToRotationVectorFromQuaternion(
- Eigen::Quaternion<double>(
- Eigen::AngleAxis<double>(0.0, Eigen::Vector3d::UnitX()))
- .coeffs());
-
- EXPECT_NEAR(0.0, (vector - Eigen::Vector3d::Zero()).norm(), 1e-4);
-}
-
-// Tests that ToRotationVectorFromQuaternion works for a real rotation.
-TEST(DownEstimatorTest, ToRotationVectorFromQuaternion) {
- Eigen::Matrix<double, 3, 1> vector =
- drivetrain::ToRotationVectorFromQuaternion(
- Eigen::Quaternion<double>(
- Eigen::AngleAxis<double>(M_PI * 0.5, Eigen::Vector3d::UnitX()))
- .coeffs());
-
- EXPECT_NEAR(0.0, (vector - Eigen::Vector3d::UnitX() * M_PI * 0.5).norm(),
- 1e-4);
-}
-
-// Tests that ToRotationVectorFromQuaternion works for a solution with negative
-// coefficients.
-TEST(DownEstimatorTest, ToRotationVectorFromQuaternionNegative) {
- Eigen::Matrix<double, 3, 1> vector =
- drivetrain::ToRotationVectorFromQuaternion(
- Eigen::Quaternion<double>(
- -Eigen::Quaternion<double>(
- Eigen::AngleAxis<double>(M_PI * 0.5,
- Eigen::Vector3d::UnitX()))
- .coeffs())
- .coeffs());
-
- EXPECT_NEAR(0.0, (vector - Eigen::Vector3d::UnitX() * M_PI * 0.5).norm(),
- 1e-4);
-}
-
-// Tests that ToQuaternionFromRotationVector works for a 0 rotation.
-TEST(DownEstimatorTest, ToQuaternionFromRotationVectorAtZero) {
- Eigen::Matrix<double, 4, 1> quaternion =
- drivetrain::ToQuaternionFromRotationVector(Eigen::Vector3d::Zero());
-
- EXPECT_NEAR(0.0, (quaternion -
- Eigen::Quaternion<double>(
- Eigen::AngleAxis<double>(0.0, Eigen::Vector3d::UnitX()))
- .coeffs()).norm(),
- 1e-4);
-}
-
-// Tests that ToQuaternionFromRotationVector works for a real rotation.
-TEST(DownEstimatorTest, ToQuaternionFromRotationVector) {
- Eigen::Matrix<double, 4, 1> quaternion =
- drivetrain::ToQuaternionFromRotationVector(Eigen::Vector3d::UnitX() *
- M_PI * 0.5);
-
- EXPECT_NEAR(0.0, (quaternion -
- Eigen::Quaternion<double>(
- Eigen::AngleAxis<double>(
- M_PI * 0.5, Eigen::Vector3d::UnitX())).coeffs())
-
- .norm(),
- 1e-4);
-}
-
-// Tests that ToQuaternionFromRotationVector correctly clips a rotation vector
-// that is too large in magnitude.
-TEST(DownEstimatorTest, ToQuaternionFromLargeRotationVector) {
- const double kMaxAngle = 2.0;
- const Eigen::Vector3d rotation_vector =
- Eigen::Vector3d::UnitX() * kMaxAngle * 2.0;
- const Eigen::Matrix<double, 3, 1> clipped_vector =
- drivetrain::ToRotationVectorFromQuaternion(
- drivetrain::ToQuaternionFromRotationVector(rotation_vector,
- kMaxAngle));
-
- EXPECT_NEAR(0.0, (rotation_vector / 2.0 - clipped_vector).norm(), 1e-4);
-}
-
-// Tests that ToQuaternionFromRotationVector and ToRotationVectorFromQuaternion
-// works for random rotations.
-TEST(DownEstimatorTest, RandomQuaternions) {
- std::mt19937 generator(aos::testing::RandomSeed());
- std::uniform_real_distribution<double> random_scalar(-1.0, 1.0);
-
- for (int i = 0; i < 1000; ++i) {
- Eigen::Matrix<double, 3, 1> axis;
- axis << random_scalar(generator), random_scalar(generator),
- random_scalar(generator);
- EXPECT_GE(axis.norm(), 1e-6);
- axis.normalize();
-
- const double angle = random_scalar(generator) * M_PI;
-
- Eigen::Matrix<double, 4, 1> quaternion =
- drivetrain::ToQuaternionFromRotationVector(axis * angle);
-
- Eigen::Quaternion<double> answer(Eigen::AngleAxis<double>(angle, axis));
-
- EXPECT_NEAR(quaternion(3, 0), std::cos(angle / 2.0), 1e-8);
- EXPECT_NEAR(answer.w(), std::cos(angle / 2.0), 1e-8);
-
- EXPECT_NEAR(1.0, (answer.coeffs() * quaternion.transpose()).norm(), 1e-6);
-
- const Eigen::Matrix<double, 3, 1> recalculated_axis =
- drivetrain::ToRotationVectorFromQuaternion(quaternion);
-
- EXPECT_NEAR(std::abs(angle), recalculated_axis.norm(), 1e-8);
-
- EXPECT_NEAR(0.0, (axis * angle - recalculated_axis).norm(), 1e-8);
- }
-}
-
} // namespace testing
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/python/spline_graph.py b/frc971/control_loops/python/spline_graph.py
index 7885ec1..f6e3a0c 100755
--- a/frc971/control_loops/python/spline_graph.py
+++ b/frc971/control_loops/python/spline_graph.py
@@ -5,6 +5,7 @@
gi.require_version('Gtk', '3.0')
from gi.repository import Gdk, Gtk, GLib
import cairo
+import basic_window
class GridWindow(Gtk.Window):
def method_connect(self, event, cb):
@@ -79,6 +80,7 @@
self.drawing_area = GTK_Widget()
self.eventBox.add(self.drawing_area)
+ self.method_connect("delete-event", basic_window.quit_main_loop)
self.method_connect("key-release-event", self.key_press)
self.method_connect("button-release-event", self.button_press)
self.method_connect("configure-event", self.configure)
diff --git a/frc971/downloader.bzl b/frc971/downloader.bzl
index 11423e0..7f188dd 100644
--- a/frc971/downloader.bzl
+++ b/frc971/downloader.bzl
@@ -1,15 +1,15 @@
load("//frc971/downloader:downloader.bzl", "aos_downloader")
load("//tools/build_rules:label.bzl", "expand_label")
-
-def robot_downloader(start_binaries,
- name="download",
- binaries=[],
- data=[],
- dirs=None,
- default_target=None,
- restricted_to=["//tools:roborio"],
- target_type="roborio"):
+def robot_downloader(
+ start_binaries,
+ name = "download",
+ binaries = [],
+ data = [],
+ dirs = None,
+ default_target = None,
+ restricted_to = ["//tools:roborio"],
+ target_type = "roborio"):
"""Sets up the standard robot download targets.
Attrs:
@@ -19,30 +19,30 @@
"""
aos_downloader(
- name=name,
- start_srcs=([
+ name = name,
+ start_srcs = ([
"//aos:prime_start_binaries",
] if target_type == "roborio" else []) + start_binaries,
- srcs=[
+ srcs = [
"//aos:prime_binaries",
] + binaries + data,
- dirs=dirs,
- target_type=target_type,
- default_target=default_target,
- restricted_to=restricted_to,
+ dirs = dirs,
+ target_type = target_type,
+ default_target = default_target,
+ restricted_to = restricted_to,
)
aos_downloader(
- name=name + "_stripped",
- start_srcs=([
- "//aos:prime_start_binaries_stripped",
- ] if target_type == "roborio" else []) +
- [expand_label(binary) + ".stripped" for binary in start_binaries],
- srcs=[
+ name = name + "_stripped",
+ start_srcs = ([
+ "//aos:prime_start_binaries_stripped",
+ ] if target_type == "roborio" else []) +
+ [expand_label(binary) + ".stripped" for binary in start_binaries],
+ srcs = [
"//aos:prime_binaries_stripped",
] + [expand_label(binary) + ".stripped" for binary in binaries] + data,
- dirs=dirs,
- target_type=target_type,
- default_target=default_target,
- restricted_to=restricted_to,
+ dirs = dirs,
+ target_type = target_type,
+ default_target = default_target,
+ restricted_to = restricted_to,
)
diff --git a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
index c5a2d18..46b9b8b 100644
--- a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
+++ b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
@@ -383,7 +383,7 @@
// the target exactly. Instead, just run slightly past the target:
EXPECT_LT(
::std::abs(::aos::math::DiffAngle(M_PI, drivetrain_plant_.state()(2, 0))),
- 0.5);
+ 1.0);
EXPECT_GT(HPSlotLeft().abs_pos().x(), drivetrain_plant_.state().x());
EXPECT_NEAR(HPSlotLeft().abs_pos().y(), drivetrain_plant_.state().y(), 0.2);
}
diff --git a/y2020/control_loops/drivetrain/localizer.cc b/y2020/control_loops/drivetrain/localizer.cc
index f84be3e..7d7c83f 100644
--- a/y2020/control_loops/drivetrain/localizer.cc
+++ b/y2020/control_loops/drivetrain/localizer.cc
@@ -122,7 +122,7 @@
return best_data_match;
}
-void Localizer::Update(const ::Eigen::Matrix<double, 2, 1> &U,
+void Localizer::Update(const Eigen::Matrix<double, 2, 1> &U,
aos::monotonic_clock::time_point now,
double left_encoder, double right_encoder,
double gyro_rate, const Eigen::Vector3d &accel) {
@@ -251,24 +251,36 @@
AOS_LOG(WARNING, "Dropped image match due to heading mismatch.\n");
continue;
}
+ // In order to do the EKF correction, we determine the expected state based
+ // on the state at the time the image was captured; however, we insert the
+ // correction update itself at the current time. This is technically not
+ // quite correct, but saves substantial CPU usage by making it so that we
+ // don't have to constantly rewind the entire EKF history.
+ const std::optional<State> state_at_capture =
+ ekf_.LastStateBeforeTime(capture_time);
+ if (!state_at_capture.has_value()) {
+ AOS_LOG(WARNING, "Dropped image match due to age of image.\n");
+ continue;
+ }
+ const Input U = ekf_.MostRecentInput();
// For the correction step, instead of passing in the measurement directly,
// we pass in (0, 0, 0) as the measurement and then for the expected
// measurement (Zhat) we calculate the error between the implied and actual
// poses. This doesn't affect any of the math, it just makes the code a bit
// more convenient to write given the Correct() interface we already have.
ekf_.Correct(
- Eigen::Vector3f::Zero(), nullptr, {},
- [H, H_field_target, pose_robot_target](
- const State &X, const Input &) -> Eigen::Vector3f {
- const Eigen::Vector3f Z =
- CalculateImpliedPose(X, H_field_target, pose_robot_target);
+ Eigen::Vector3f::Zero(), &U, {},
+ [H, H_field_target, pose_robot_target, state_at_capture](
+ const State &, const Input &) -> Eigen::Vector3f {
+ const Eigen::Vector3f Z = CalculateImpliedPose(
+ state_at_capture.value(), H_field_target, pose_robot_target);
// Just in case we ever do encounter any, drop measurements if they
// have non-finite numbers.
if (!Z.allFinite()) {
AOS_LOG(WARNING, "Got measurement with infinites or NaNs.\n");
return Eigen::Vector3f::Zero();
}
- Eigen::Vector3f Zhat = H * X - Z;
+ Eigen::Vector3f Zhat = H * state_at_capture.value() - Z;
// Rewrap angle difference to put it back in range. Note that this
// component of the error is currently ignored (see definition of H
// above).
@@ -283,7 +295,7 @@
}
return Zhat;
},
- [H](const State &) { return H; }, R, capture_time);
+ [H](const State &) { return H; }, R, now);
}
}
diff --git a/y2020/vision/calibration.cc b/y2020/vision/calibration.cc
index 15f1f4c..6cf9e8e 100644
--- a/y2020/vision/calibration.cc
+++ b/y2020/vision/calibration.cc
@@ -96,7 +96,6 @@
namespace {
-
void ViewerMain() {
aos::FlatbufferDetachedBuffer<aos::Configuration> config =
aos::configuration::ReadConfig(FLAGS_config);
@@ -139,107 +138,104 @@
absl::StrCat("/pi", std::to_string(pi_number.value()), "/camera");
LOG(INFO) << "Connecting to channel " << channel;
- event_loop.MakeWatcher(
- channel, [&event_loop, &board, &all_charuco_ids, &all_charuco_corners,
- camera_matrix, dist_coeffs,
- eigen_camera_matrix](const CameraImage &image) {
- const aos::monotonic_clock::duration age =
- event_loop.monotonic_now() -
- event_loop.context().monotonic_event_time;
- const double age_double =
- std::chrono::duration_cast<std::chrono::duration<double>>(age)
- .count();
- if (age > std::chrono::milliseconds(100)) {
- LOG(INFO) << "Age: " << age_double << ", getting behind, skipping";
- return;
- }
- // Create color image:
- cv::Mat image_color_mat(cv::Size(image.cols(), image.rows()), CV_8UC2,
- (void *)image.data()->data());
- const cv::Size image_size(image.cols(), image.rows());
- cv::Mat rgb_image(image_size, CV_8UC3);
- cv::cvtColor(image_color_mat, rgb_image, CV_YUV2BGR_YUYV);
+ event_loop.MakeWatcher(channel, [&event_loop, &board, &all_charuco_ids,
+ &all_charuco_corners, camera_matrix,
+ dist_coeffs, eigen_camera_matrix](
+ const CameraImage &image) {
+ const aos::monotonic_clock::duration age =
+ event_loop.monotonic_now() - event_loop.context().monotonic_event_time;
+ const double age_double =
+ std::chrono::duration_cast<std::chrono::duration<double>>(age).count();
+ if (age > std::chrono::milliseconds(100)) {
+ LOG(INFO) << "Age: " << age_double << ", getting behind, skipping";
+ return;
+ }
+ // Create color image:
+ cv::Mat image_color_mat(cv::Size(image.cols(), image.rows()), CV_8UC2,
+ (void *)image.data()->data());
+ const cv::Size image_size(image.cols(), image.rows());
+ cv::Mat rgb_image(image_size, CV_8UC3);
+ cv::cvtColor(image_color_mat, rgb_image, CV_YUV2BGR_YUYV);
- std::vector<int> marker_ids;
- std::vector<std::vector<cv::Point2f>> marker_corners;
+ std::vector<int> marker_ids;
+ std::vector<std::vector<cv::Point2f>> marker_corners;
- cv::aruco::detectMarkers(rgb_image, board->dictionary, marker_corners,
- marker_ids);
+ cv::aruco::detectMarkers(rgb_image, board->dictionary, marker_corners,
+ marker_ids);
- std::vector<cv::Point2f> charuco_corners;
- std::vector<int> charuco_ids;
- // If at least one marker detected
- if (marker_ids.size() >= FLAGS_min_targets) {
- // Run everything twice, once with the calibration, and once without.
- // This lets us both calibrate, and also print out the pose real time
- // with the previous calibration.
- cv::aruco::interpolateCornersCharuco(marker_corners, marker_ids,
- rgb_image, board,
- charuco_corners, charuco_ids);
+ std::vector<cv::Point2f> charuco_corners;
+ std::vector<int> charuco_ids;
+ // If at least one marker detected
+ if (marker_ids.size() >= FLAGS_min_targets) {
+ // Run everything twice, once with the calibration, and once without.
+ // This lets us both calibrate, and also print out the pose real time
+ // with the previous calibration.
+ cv::aruco::interpolateCornersCharuco(marker_corners, marker_ids,
+ rgb_image, board, charuco_corners,
+ charuco_ids);
- std::vector<cv::Point2f> charuco_corners_with_calibration;
- std::vector<int> charuco_ids_with_calibration;
+ std::vector<cv::Point2f> charuco_corners_with_calibration;
+ std::vector<int> charuco_ids_with_calibration;
- cv::aruco::interpolateCornersCharuco(
- marker_corners, marker_ids, rgb_image, board,
- charuco_corners_with_calibration, charuco_ids_with_calibration,
- camera_matrix, dist_coeffs);
+ cv::aruco::interpolateCornersCharuco(
+ marker_corners, marker_ids, rgb_image, board,
+ charuco_corners_with_calibration, charuco_ids_with_calibration,
+ camera_matrix, dist_coeffs);
- cv::aruco::drawDetectedMarkers(rgb_image, marker_corners, marker_ids);
+ cv::aruco::drawDetectedMarkers(rgb_image, marker_corners, marker_ids);
- if (charuco_ids.size() >= FLAGS_min_targets) {
- cv::aruco::drawDetectedCornersCharuco(
- rgb_image, charuco_corners, charuco_ids, cv::Scalar(255, 0, 0));
+ if (charuco_ids.size() >= FLAGS_min_targets) {
+ cv::aruco::drawDetectedCornersCharuco(
+ rgb_image, charuco_corners, charuco_ids, cv::Scalar(255, 0, 0));
- cv::Vec3d rvec, tvec;
- bool valid = cv::aruco::estimatePoseCharucoBoard(
- charuco_corners_with_calibration, charuco_ids_with_calibration,
- board, camera_matrix, dist_coeffs, rvec, tvec);
+ cv::Vec3d rvec, tvec;
+ bool valid = cv::aruco::estimatePoseCharucoBoard(
+ charuco_corners_with_calibration, charuco_ids_with_calibration,
+ board, camera_matrix, dist_coeffs, rvec, tvec);
-
- // if charuco pose is valid
- if (valid) {
- LOG(INFO) << std::fixed << std::setprecision(6)
- << "Age: " << age_double << ", Pose is R:" << rvec
- << " T:" << tvec;
- cv::aruco::drawAxis(rgb_image, camera_matrix,
- dist_coeffs, rvec, tvec, 0.1);
- } else {
- LOG(INFO) << "Age: " << age_double << ", invalid pose";
- }
- } else {
- LOG(INFO) << "Age: " << age_double << ", no charuco IDs.";
- }
+ // if charuco pose is valid
+ if (valid) {
+ LOG(INFO) << std::fixed << std::setprecision(6)
+ << "Age: " << age_double << ", Pose is R:" << rvec
+ << " T:" << tvec;
+ cv::aruco::drawAxis(rgb_image, camera_matrix, dist_coeffs, rvec, tvec,
+ 0.1);
} else {
- LOG(INFO) << "Age: " << age_double << ", no marker IDs";
+ LOG(INFO) << "Age: " << age_double << ", invalid pose";
}
+ } else {
+ LOG(INFO) << "Age: " << age_double << ", no charuco IDs.";
+ }
+ } else {
+ LOG(INFO) << "Age: " << age_double << ", no marker IDs";
+ }
- cv::imshow("Display", rgb_image);
+ cv::imshow("Display", rgb_image);
- if (FLAGS_display_undistorted) {
- cv::Mat undistorted_rgb_image(image_size, CV_8UC3);
- cv::undistort(rgb_image, undistorted_rgb_image, camera_matrix,
- dist_coeffs);
+ if (FLAGS_display_undistorted) {
+ cv::Mat undistorted_rgb_image(image_size, CV_8UC3);
+ cv::undistort(rgb_image, undistorted_rgb_image, camera_matrix,
+ dist_coeffs);
- cv::imshow("Display undist", undistorted_rgb_image);
- }
+ cv::imshow("Display undist", undistorted_rgb_image);
+ }
- int keystroke = cv::waitKey(1);
- if ((keystroke & 0xFF) == static_cast<int>('c')) {
- if (charuco_ids.size() >= FLAGS_min_targets) {
- all_charuco_ids.emplace_back(std::move(charuco_ids));
- all_charuco_corners.emplace_back(std::move(charuco_corners));
- LOG(INFO) << "Image " << all_charuco_corners.size();
- }
+ int keystroke = cv::waitKey(1);
+ if ((keystroke & 0xFF) == static_cast<int>('c')) {
+ if (charuco_ids.size() >= FLAGS_min_targets) {
+ all_charuco_ids.emplace_back(std::move(charuco_ids));
+ all_charuco_corners.emplace_back(std::move(charuco_corners));
+ LOG(INFO) << "Image " << all_charuco_corners.size();
+ }
- if (all_charuco_ids.size() >= 50) {
- LOG(INFO) << "Got enough images to calibrate";
- event_loop.Exit();
- }
- } else if ((keystroke & 0xFF) == static_cast<int>('q')) {
- event_loop.Exit();
- }
- });
+ if (all_charuco_ids.size() >= 50) {
+ LOG(INFO) << "Got enough images to calibrate";
+ event_loop.Exit();
+ }
+ } else if ((keystroke & 0xFF) == static_cast<int>('q')) {
+ event_loop.Exit();
+ }
+ });
event_loop.Run();
@@ -302,10 +298,12 @@
pi_number.value(), time_ss.str());
LOG(INFO) << calibration_filename << " -> "
- << aos::FlatbufferToJson(camera_calibration, true);
+ << aos::FlatbufferToJson(camera_calibration,
+ {.multi_line = true});
aos::util::WriteStringToFileOrDie(
- calibration_filename, aos::FlatbufferToJson(camera_calibration, true));
+ calibration_filename,
+ aos::FlatbufferToJson(camera_calibration, {.multi_line = true}));
} else {
LOG(INFO) << "Skipping calibration due to not enough images.";
}
diff --git a/y2020/www/BUILD b/y2020/www/BUILD
index a8b0c67..1311b7d 100644
--- a/y2020/www/BUILD
+++ b/y2020/www/BUILD
@@ -5,8 +5,8 @@
ts_library(
name = "camera_main",
srcs = [
- "image_handler.ts",
"camera_main.ts",
+ "image_handler.ts",
],
visibility = ["//y2020:__subpackages__"],
deps = [
@@ -19,14 +19,14 @@
ts_library(
name = "field_main",
srcs = [
- "field_main.ts",
- "field_handler.ts",
"constants.ts",
+ "field_handler.ts",
+ "field_main.ts",
],
deps = [
"//aos/network/www:proxy",
- "//y2020/vision/sift:sift_ts_fbs",
"//frc971/control_loops/drivetrain:drivetrain_status_ts_fbs",
+ "//y2020/vision/sift:sift_ts_fbs",
],
)
@@ -42,10 +42,10 @@
rollup_bundle(
name = "field_main_bundle",
entry_point = "y2020/www/field_main",
+ visibility = ["//y2020:__subpackages__"],
deps = [
"field_main",
],
- visibility = ["//y2020:__subpackages__"],
)
filegroup(