Merge "Add roller velocity compensation"
diff --git a/LICENSE.txt b/LICENSE.txt
index 7d0feac..52cdf71 100644
--- a/LICENSE.txt
+++ b/LICENSE.txt
@@ -7,4 +7,4 @@
Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-Anyone who uses this software must notify the original authors through the form at <http://www.frc971.org/contact>.
+Anyone who uses this software must notify the original authors through the form at <http://frc971.org/contact>.
diff --git a/README.md b/README.md
index af5ae6e..395a802 100644
--- a/README.md
+++ b/README.md
@@ -2,36 +2,66 @@
This is FRC Team 971's main code repository. There are `README*` files throughout the source tree documenting specifics for their respective folders.
## Access to the code
-The main central location for our code is our [Gerrit](https://www.gerritcodereview.com/) server at https://software.frc971.org/gerrit. To get a copy of the code on your computer to work with, follow these steps:
- 1. Contact Michael Schuh to get an SVN account.
- 2. Go to Gerrit and create an account.
- 3. Contact Brian Silverman with your SVN username to get access to the code in Gerrit.
- 4. Go to [the 971-Robot-Code project in Gerrit](https://software.frc971.org/gerrit/#/admin/projects/971-Robot-Code) and run the command.
- Running the `clone with commit-msg hook` command will save you trouble later.
+The main central location for our code is our [Gerrit](https://www.gerritcodereview.com/) server at https://software.frc971.org/gerrit. To download a copy of the 971 code on your computer, follow these steps:
+ 1. Fill out an SVN account request form to get an SVN account.
+ 1. Mention that you are part of the software team and need Gerrit access
+ 2. It is recommended to use your firstname in all lowercase for your username
+ 2. Go to our [Gerrit server](https://software.frc971.org/gerrit) and create an account.
+ 3. Contact Brian Silverman or Stephan Massalt with your SVN username to get access to the code in Gerrit.
+ 4. When you log into Gerrit the first time, please [add your Email Address](http://software.frc971.org/gerrit/settings/#EmailAddresses)
+ 5. Add your SSH key to Gerrit in order to be able to check out code
+ 1. If you don't already have an ssh key, you can create one using `ssh-keygen`. This will create a public/private key pair-- the default location for your public key will be `~/.ssh/id_rsa.pub`
+ 2. Log into Gerrit and go to `Settings->SSH Keys` and paste your public key into the `New SSH Key` text box and clicking on `ADD NEW SSH KEY`
+ 6. Install `git`: `sudo apt install git`
+ 7. Go to [the 971-Robot-Code project in Gerrit](https://software.frc971.org/gerrit/#/admin/projects/971-Robot-Code) and run the command to Download the 971-Robot-Code repository.
+ 1. We recommend downloading the code via SSH using the `clone with commit-msg hook` command
+ 2. NOTE: Running with the option to `clone with commit-msg hook` will save you trouble later.
-To learn more about git, see git(1) (`man git` or [git(1)](http://manpages.debian.net/cgi-bin/man.cgi?query=git>) (especially the NOTES section).
+To learn more about git, open a terminal and run `man git`, or see [git(1)](https://manpages.debian.org/buster/git-man/git.1.en.html) (especially the NOTES section).
-## Code reviews
-We want all code to at least have a second person look over it before it gets merged into the `master` branch. Gerrit has [extensive documentation on starting reviews](https://software.frc971.org/gerrit/Documentation/user-upload.html). TL;DR: `git push origin HEAD:refs/for/master` and then click on the link to add reviewers.
-If you just upload a change without adding any reviewers, it might sit around for a long time before anybody else notices it.
-[git-review](http://manpages.debian.org/cgi-bin/man.cgi?query=git-review) can make the upload process simpler.
+
+## Prerequisites
+The instructions below assume you have the following:
+ 1. A host computer with an appropriate OS or VM to compile the 971 code using Bazel
+ 1. The currently supported operating system for building the code is amd64 Debian Buster.
+ 2. It is likely to work on any `x86\_64 GNU/Linux` system (e.g., Ubuntu 20.04), but that's not at all well-tested.
+ 2. Your favorite text editor installed, e.g., `vim`, `emacs`
+ 3. Access to the 971-Robot-Code repository and have downloaded the source code
+ 4. The ability to `ssh` into target CPU's like the roborio and Raspberry Pi
+
## Building the code
-The currently supported operating system for building the code is amd64 Debian Buster. It is likely to work on any x86\_64 GNU/Linux system, but that's not at all well-tested.
+We use [Bazel](http://bazel.io) to build the code. Bazel has [extensive docs](https://docs.bazel.build/versions/master/build-ref.html), including a nice [build encyclopedia reference](https://docs.bazel.build/versions/master/be/overview.html), and does a great job with fast, correct incremental rebuilds.
-We use [Bazel](http://bazel.io) to build the code. Bazel has [extensive](https://docs.bazel.build/versions/master/build-ref.html) [docs](https://docs.bazel.build/versions/master/be/overview.html) and does a nice job with fast, correct increment rebuilds.
+There are a couple options for building code that are given here-- setting up either your own computer, or using the frc971 build server.
### Steps to set up a computer to build the code:
- 0. Install any Bazel version. See [here](https://docs.bazel.build/versions/master/install-ubuntu.html)
- 1. Install the required packages:
-```console
-apt-get update
+ 1. Install any Bazel version.
+ 1. Check to see if the version of Linux you're running has an apt package for Bazel: `apt-cache search bazel` or just try `sudo apt install bazel`
+ 2. More likely, you'll need to install manually-- see [here](https://docs.bazel.build/versions/master/install-ubuntu.html). We recommend using `apt-key` instead of `gnupg` in setting up the key:
+ 1. Step 1: Add Bazel distribution URI as a package source
+ ```
+ sudo apt install curl apt-key
+ curl -fsSL https://bazel.build/bazel-release.pub.gpg | apt-key add -
+ echo "deb [arch=amd64] https://storage.googleapis.com/bazel-apt stable jdk1.8" | sudo tee /etc/apt/sources.list.d/bazel.list
+ ```
+ 2. Step 2: Install Bazel
+ ```
+ sudo apt update && sudo apt install bazel
+ ```
+
+ 2. Install the required packages:
+```sh
+sudo apt-get update
# TODO(james): Ideally, we shouldn't need to be installing libtinfo5...
-apt-get install bazel python libtinfo5
+sudo apt-get install python libtinfo5
```
- 2. Allow Bazel's sandboxing to work:
- Follow the direction in `doc/frc971.conf`.
+ 3. Change settings to allow Bazel's sandboxing to work-- follow the directions in `doc/frc971.conf`. For example, the commands to do this would be:
+ 1. `sudo cp doc/frc971.conf /etc/sysctl.d/`
+ 2. `sudo sysctl --system`
+
### Setting up access to a workspace on the build server
+In order to use the build server, you'll first need to get ssh access set up. (NOTE: you don't need to do any of the other setup steps done for your own computer, since things like `bazel`, `python`, etc. are already installed on the build server)
1. Use ssh-keygen to create a public and private key.
```console
# IMPORTANT These are the windows instructions.
@@ -51,106 +81,93 @@
```console
ssh [user]@build.frc971.org -p 2222 -i ./ssh/id_971_rsa -L 971:127.0.0.1:3389
```
- 5. So at this point you run the Remote Desktop app in windows.
-Once you get there, all you need to do is put `127.0.0.1:9971` for the computer name, and use your SVN usrname.
-Once you get connected accept the server certificate, and then enter your password that you gave Stephan. (Its either something unique or your SVN pwd)
-Then select the Default panel config.
-You can exit the Remote Desktop if you are good w/ the raw cmd line interface.
-And for future logins all you have to do is tunnel and then login using the app.
-Now if you have a graphical application that you are developing (ie spline UI), then you have to run the build command in the Remote Desktop application.
-
-# ONE VERY IMPORTANT LAST STEP
-In order for you to be able to commit, you need to run this command, replacing <YOUR EMAIL> w/ your email that is in gerrit.
+ 5. So at this point you run the Remote Desktop app in Windows. Once
+you get there, all you need to do is put `127.0.0.1:9971` for the
+computer name, and use your SVN username. Once you get connected,
+accept the server certificate and then enter your password that you
+gave Stephan. (It's either something unique or your SVN pwd) Then
+select the Default panel config. You can exit the Remote Desktop if
+you are good w/ the raw cmd line interface. And for future logins all
+you have to do is tunnel and then login using the app. Now if you
+have a graphical application that you are developing (e.g., spline
+UI), then you have to run the build command in the Remote Desktop
+application.
+ 6. Very important: In order for you to be able to commit, you need
+ to configure your email address in `git`. To do this, run the
+ following command, replacing `<YOUR EMAIL>` with the email that you are
+ using for Gerrit:
```console
git config --global user.email "<YOUR EMAIL>"
```
-One thing that also needs to be said is that you DO NOT need to do any of the installation steps, or the step w/ `frc971.conf`.
-If there are any questions, use #coding so that other people who may reach the same issue can refer back to that.
-Some people that you can @ would be Het <--Insert your name here if you are pingable-->
+If there are any questions, post to the #coding Slack channel so that other people who may reach the same issue can refer back to that.
-### Some useful Bazel commands:
- * Build and test everything (on the host system):
-```console
+
+### Bazel commands for building, testing, and deploying the code:
+ * Build and test everything (on the host system, for the roborio target-- note, this may take a while):
+```
bazel test //...
-bazel build --cpu=roborio //...
+bazel build --cpu=roborio -c opt //...
```
* Build the code for a specific robot:
```console
-bazel build --cpu=roborio -c opt //y2019/...
+# For the roborio:
+bazel build --cpu=roborio -c opt //y2020/...
```
- * Download code to a robot:
-```console
-# First add a host entry in your ~/.ssh/known_hosts file for the roboRIO.
-# Do this by ssh'ing into the machine. If you problems doing this, see
-# the notes below for more information on how to connect to the roboRIO.
-ssh admin@roboRIO-971-frc.local
-# If you see an error like:
-# subprocess.CalledProcessError: Command '['rsync' ...
-# ERROR: Non-zero return code '1' from command: Process exited with status 1
-# The above "ssh admin@roboRIO-971-frc.local" step has not been sucessfully completed.
-# If the roboRIO has been configued to use a static IP address like 10.9.71.2,
-# set the laptop to have an IP address on the 10.9.71.x subnet with a netmask
-# of 255.0.0.0. The ".x" is different than the .2 for the roboRIO or any other
-# device on the network. The driver station uses .5 or .6 so avoid those.
-# The radio uses .1 or .50 so avoid those too. If you are at the school,
-# disconnect from the student wireless network or try setting your netmask to
-# 255.255.255.0 if you want to be on both networks. The student wireless
-# network is on a 10.?.?.? subnet which can cause problems with connecting to
-# the robot.
-bazel run --cpu=roborio --compilation_mode=opt //y2018:download_stripped -- admin@roboRIO-971-frc.local
-# If this does not work, try
-bazel run --cpu=roborio --compilation_mode=opt //y2018:download_stripped -- admin@10.9.71.2
-# If this does not work, it probably means that the robot and laptop are on
-# different subnets. They need to be on the same subnet for the laptop to
-# connect to the robot. Connecting can be confirmed by using ping.
-ping roboRIO-971-frc.local
-# or
-ping 10.9.71.2
-# If this does not work, perhaps the roboRIO has not been configured to have
-# a static IP address. Use a USB cable to connect from a Windows laptop to
-# the roboRIO and use Internet Explorer (IE) to configure the roboRIO
-# to have a static IP address of 10.9.71.2. Inside IE, browse to
-# http://roborio-971-frc.local or http://172.22.11.2. Click on the "Ethernet"
-# icon on the left, select "Static" for the "Configure IPv4 Address" option.
-# Set the "IPv4 Address" to 10.9.71.2. Set the "Subnet Mask" to "255.0.0.0".
-# Finally click on "Save" at the bottom of the screen. If you have trouble
-# using an Ethernet cable, try using a USB cable. USB cables are much
-# more reliable for connecting than using a Ethernet cabe. USB cables work
-# for connecting to the robot on Windows and Linux computers.
-# Another option is to configure the laptop to have a link-local connection
-# by going using the "Network Settings" GUI. The laptop will then be on the
-# same subnet in the address range of 169.254.0.0 to 169.254.255.255.
-# James thinks this will only work over Ethernet (i.e., not USB; he is not
-# sure what will happen if you attempt this over USB), and if the robot
-# does *not* have a static IP address set and there is no DHCP server
-# assigning an IP address to the roboRIO. James says to also note that this
-# implies that the roboRIO will also have a 169.254.*.* IP addresss, and
-# that the only simple way to figure it out is to use mDNS.
-#
```
- * To configure a freshly imaged roboRIO:
+# For the raspberry pi:
+bazel build --cpu=armhf-debian -c opt //y2020/...
+```
+
+ * Configuring a roborio: Freshly imaged roboRIOs need to be configured to run the 971 code
+at startup. This is done by using the setup_roborio.sh script.
```console
-# Freshly imaged roboRIOs need to be configured to run the 971 code
-# at startup. This is done by using the setup_roborio.sh script.
bazel run -c opt //frc971/config:setup_roborio -- roboRIO-XXX-frc.local
```
-### Some other useful packages
+ * Download code to a robot:
+```console
+# For the roborio
+bazel run --cpu=roborio -c opt //y2020:download_stripped -- roboRIO-971-frc.local
+```
+This assumes the roborio is reachable at `roboRIO-971-frc.local`. If that does not work, you can try with a static IP address like `10.9.71.2` (see troubleshooting below)
+```console
+# For the raspberry pi's
+bazel run --cpu=armhf-debian -c opt //y2020:pi_download_stripped -- 10.9.71.101
+```
+NOTE:
+ 1. The raspberry pi's require that you have your ssh key installed on them in order to copy code over
+ 2. They are configured to use the IP addresses 10.X.71.Y, where X is 9, 79, 89, or 99 depending on the robot number (971, 7971, 8971, or 9971, respectively), and Y is 101, 102, etc for pi #1, #2, etc.
+
+ * Downloading specific targets to the robot
+ 1. Generally if you want to update what's running on the robot, you can use the `download_stripped` (or `pi_download_stripped`) targets. These will rsync only the changed files, and so are pretty efficient.
+ 2. If you have a need to compile a specific module, you can build stripped versions of the individual modules by adding "_stripped" to the module name. For example, to build the calibration code (`//y2020/vision:calibration`) for the pi (`armhf-debian`), run:
+ ```console
+ bazel run --cpu=armhf-debian -c opt //y2020/vision:calibration_stripped
+ ```
+ You will then need to manually copy the resulting file over to the robot.
+
+## Code reviews
+We want all code to at least have a second person look it over before it gets merged into the `master` branch. Gerrit has [extensive documentation on starting reviews](https://software.frc971.org/gerrit/Documentation/user-upload.html). There is also a good [intro User Guide](https://software.frc971.org/gerrit/Documentation/intro-user.html) and an [intro to working with Gerrit](https://gerrit-review.googlesource.com/Documentation/intro-gerrit-walkthrough.html) and [Gerrit workflows](https://docs.google.com/presentation/d/1C73UgQdzZDw0gzpaEqIC6SPujZJhqamyqO1XOHjH-uk)
+
+TL;DR: Make and commit your changes, do `git push origin HEAD:refs/for/master`, and then click on the provided link to add reviewers. If you just upload a change without adding any reviewers, it might sit around for a long time before anybody else notices it.
+
+[git-review](http://manpages.debian.org/cgi-bin/man.cgi?query=git-review) can make the upload process simpler.
+
+
+
+### Some other useful packages <TODO: Need to review these>
These aren't strictly necessary to build the code, but Michael found the
additional tools provided by these packages useful to install when working with
the code on May 13, 2018.
+
```console
# Get some useful packages including git and subversion.
apt-get update
- apt-get install git subversion ruby python vim-gtk3 subversion-tools
- apt-get install vim-doc git-doc git-gui git-svn exim4-doc-html ruby
- apt-get install python python-scipy python-matplotlib libpython-dev
- apt-get install bazel clang-format-3.8 clang-3.8 openjdk-8-jdk
- apt-get install gfortran libblas-dev liblapack-dev avahi-daemon
-# fix a key problem with llvm.
- wget -O - https://apt.llvm.org/llvm-snapshot.gpg.key|sudo apt-key add -
- apt-get update
+ apt-get install git git-gui vim-gtk3
+ apt-get install vim-doc git-doc exim4-doc-html yapf
+ apt-get install bazel clang-format
+ apt-get install python avahi-daemon
# Install apt-file so that packages can be searched
apt-get install apt-file
apt-file update
@@ -164,6 +181,30 @@
# understanding the revision history of the repository and viewing
# log messages and changes.
apt-get install gitg
+# Also may want to install `buildifier` for formatting BUILD files
+```
+
+### Creating ssh aliases
+
+It is also handy to alias logins to the raspberry pi's by adding lines like this to your ~/.ssh/config file:
+```console
+Host pi-7971-2
+ User pi
+ ForwardAgent yes
+ HostName 10.79.71.102
+ StrictHostKeyChecking no
+```
+or, for the roborio:
+```
+Host roborio-971
+ User admin
+ HostName 10.9.71.2
+ StrictHostKeyChecking no
+```
+This allows you to use the alias to `ping`, `ssh`, or run commands like:
+```
+# Download code to robot #7971's raspberry pi #2
+bazel run --cpu=armhf-debian -c opt //y2020:download_stripped -- pi-7971-2
```
### Roborio Kernel Traces
@@ -183,3 +224,63 @@
```
You can then scp the `trace.dat` file to your computer and run `kernelshark
trace.dat` (may require installing the `kernelshark` apt package).
+
+
+### Notes on troubleshooting network setup
+If the roboRIO has been configued to use a static IP address like
+10.9.71.2, set the laptop to have an IP address on the 10.9.71.x
+subnet with a netmask of 255.0.0.0. The ".x" is different than the
+.2 for the roboRIO or any other device on the network. The driver
+station uses .5 or .6 so avoid those. The radio uses .1 or .50 so
+avoid those too. A good choice might be in the 90-99 range. If you
+are at the school, disconnect from the student wireless network or
+try setting your netmask to 255.255.255.0 if you want to be on both
+networks. The student wireless network is on a 10.?.?.? subnet
+which can cause problems with connecting to the robot.
+
+If running Bazel on the `download_stripped` target does not work for
+the IP address you're using for the roborio or the raspberry pi, it
+probably means that the robot and laptop are on different subnets.
+They need to be on the same subnet for the laptop to connect to the
+robot. Connecting can be confirmed by using ping.
+```
+ping roboRIO-971-frc.local
+```
+or
+```
+ping 10.9.71.2
+```
+
+If this does not work, perhaps the roboRIO has not been configured to
+have a static IP address. Use a USB cable to connect from a Windows
+laptop to the roboRIO and use a web browser (Chrome is preferred,
+IE/Edge is not-- see [this technical
+note](https://docs.wpilib.org/en/stable/docs/software/roborio-info/roborio-web-dashboard.html))
+to configure the roboRIO to have a static IP address of 10.9.71.2.
+Browse to http://roborio-971-frc.local or http://172.22.11.2. Click
+on the "Ethernet" icon on the left, select "Static" for the "Configure
+IPv4 Address" option. Set the "IPv4 Address" to 10.9.71.2. Set the
+"Subnet Mask" to "255.0.0.0". Finally click on "Save" at the bottom
+of the screen. If you have trouble using an Ethernet cable, try using
+a USB cable (USB A->B).
+
+Another option is to configure the laptop to have a link-local connection
+by using the "Network Settings" GUI. The laptop will then be on the
+same subnet in the address range of 169.254.0.0 to 169.254.255.255.
+James thinks this will only work over Ethernet (i.e., not USB; he is not
+sure what will happen if you attempt this over USB), and if the robot
+does *not* have a static IP address set and there is no DHCP server
+assigning an IP address to the roboRIO. James says to also note that this
+implies that the roboRIO will also have a 169.254.*.* IP addresss, and
+that the only simple way to figure it out is to use mDNS.
+
+
+### Other resources
+ 1. Intro to [AOS](./aos/README.md), our robot Operating System
+ 2. Introductory example: [ping/pong](./aos/events/README.md)
+ 3. Example of [logging](./aos/events/README.md)
+
+TODOs:
+ 1. Add more on networking setup and troubleshooting
+ 2. Move Roborio Kernel Traces out of here, maybe into documentation/
+ 3. Currently requires `apt install libsigsegv2`, but this should be temporary
diff --git a/WORKSPACE b/WORKSPACE
index eb7ed28..9ddecff 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -70,6 +70,14 @@
"//debian:m4.bzl",
m4_debs = "files",
)
+load(
+ "//debian:lzma_amd64.bzl",
+ lzma_amd64_debs = "files",
+)
+load(
+ "//debian:lzma_arm64.bzl",
+ lzma_arm64_debs = "files",
+)
load("//debian:packages.bzl", "generate_repositories_for_debs")
generate_repositories_for_debs(python_debs)
@@ -106,6 +114,10 @@
generate_repositories_for_debs(m4_debs)
+generate_repositories_for_debs(lzma_amd64_debs)
+
+generate_repositories_for_debs(lzma_arm64_debs)
+
http_archive(
name = "python_repo",
build_file = "@//debian:python.BUILD",
@@ -753,3 +765,45 @@
sha256 = "ee8dfe664ac8c1d066bab64f71bd076a021875581b3cc47dac4a14a475f50b15",
url = "http://www.frc971.org/Build-Dependencies/m4.tar.gz",
)
+
+# //debian:lzma_amd64
+http_archive(
+ name = "lzma_amd64",
+ build_file_content = """
+cc_library(
+ name = "lib",
+ srcs = [
+ "usr/lib/x86_64-linux-gnu/liblzma.a",
+ ],
+ hdrs = glob([
+ "usr/include/lzma/*.h",
+ "usr/include/*.h",
+ ]),
+ strip_include_prefix = "usr/include",
+ visibility = ["//visibility:public"],
+)
+""",
+ sha256 = "e0ccaa7f793e44638e9f89570e00f146073a98a5928e0b547146c8184488bb19",
+ urls = ["http://www.frc971.org/Build-Dependencies/lzma_amd64.tar.gz"],
+)
+
+# //debian:lzma_arm64
+http_archive(
+ name = "lzma_arm64",
+ build_file_content = """
+cc_library(
+ name = "lib",
+ srcs = [
+ "usr/lib/aarch64-linux-gnu/liblzma.a",
+ ],
+ hdrs = glob([
+ "usr/include/lzma/*.h",
+ "usr/include/*.h",
+ ]),
+ strip_include_prefix = "usr/include",
+ visibility = ["//visibility:public"],
+)
+""",
+ sha256 = "18db35669ee49a5f8324a344071dd4ab553e716f385fb75747b909bd1de959f5",
+ urls = ["http://www.frc971.org/Build-Dependencies/lzma_arm64.tar.gz"],
+)
diff --git a/aos/BUILD b/aos/BUILD
index 286882f..04d77f3 100644
--- a/aos/BUILD
+++ b/aos/BUILD
@@ -274,6 +274,7 @@
],
visibility = ["//visibility:public"],
deps = [
+ ":thread_local",
"@com_github_google_glog//:glog",
],
)
@@ -453,6 +454,7 @@
visibility = ["//visibility:public"],
deps = [
"//aos:macros",
+ "//aos/containers:resizeable_buffer",
"@com_github_google_flatbuffers//:flatbuffers",
"@com_github_google_glog//:glog",
"@com_google_absl//absl/strings",
@@ -534,3 +536,23 @@
"@com_google_absl//absl/strings:str_format",
],
)
+
+cc_library(
+ name = "thread_local",
+ hdrs = [
+ "thread_local.h",
+ ],
+ visibility = ["//visibility:public"],
+)
+
+cc_test(
+ name = "realtime_test",
+ srcs = [
+ "realtime_test.cc",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":realtime",
+ "//aos/testing:googletest",
+ ],
+)
diff --git a/aos/README.md b/aos/README.md
new file mode 100644
index 0000000..cbf8a56
--- /dev/null
+++ b/aos/README.md
@@ -0,0 +1,35 @@
+See [Top level README](../README.md) for overall information
+
+### `aos_dump`
+
+For examples on viewing events and logging, see the [README.md](./events/README.md) file in `aos/events`
+
+
+### NOTES
+
+Some functions need to be in separate translation units in order for them to be guaranteed to work. As the C standard says,
+
+> Alternatively, an implementation might perform various optimizations
+> within each translation unit, such that the actual semantics would
+> agree with the abstract semantics only when making function calls
+> across translation unit boundaries. In such an implementation, at the
+> time of each function entry and function return where the calling
+> function and the called function are in different translation units,
+> the values of all externally linked objects and of all objects
+> accessible via pointers therein would agree with the abstract
+> semantics. Furthermore, at the time of each such function entry the
+> values of the parameters of the called function and of all objects
+> accessible via pointers therein would agree with the abstract
+> semantics. In this type of implementation, objects referred to by
+> interrupt service routines activated by the signal function would
+> require explicit specification of volatile storage, as well as other
+> implementation-defined restrictions.
+
+### FILES <TODO: I believe these are no longer correct>
+- `config/` has some configuration files
+ - `aos.conf` (currently in `aos` folder) has directions for setting up resource limits so you can run the code on any linux machine (the directions are there to keep them with the file)
+ - `setup_rc_caps.sh` (currently in `aos` folder) is a shell script (you have to run as root) that lets you run the realtime code without installing aos.conf for an individual file
+ - `starter` is an init.d file
+ - install it by putting it in /etc/init.d an running `update-rc.d starter defaults`
+ - restart it by running `invoke-rc.d starter restart` (doesn't always work very well...)
+ - the .config files are for building linux kernels
diff --git a/aos/README.txt b/aos/README.txt
deleted file mode 100644
index e0f9377..0000000
--- a/aos/README.txt
+++ /dev/null
@@ -1,34 +0,0 @@
-see ../README.txt for overall information
-
-[FILES]
-config/ has some configuration files
- aos.conf has directions for setting up resource limits so you can run the code on any linux machine (the directions are there to keep them with the file)
- setup_rc_caps.sh is a shell script (you have to run as root) that lets you run the realtime code without installing aos.conf for an individual file
- starter is an init.d file
- install it by putting it in /etc/init.d an running "update-rc.d starter defaults"
- restart it by running "invoke-rc.d starter restart" (doesn't always work very well...)
- the .config files are for building linux kernels
-
-linux/ has code that only runs on linux systems
-
-common/ is where stuff that runs on all platforms is
-common/input/ is where the framework code for reading stuff into the queues system is
-common/output/ is where the framework for writing things out of the queues system is
-common/messages is where the c++ wrappers for "queues" are
-
-[NOTES]
-Some functions need to be in separate translation units in order for them to be guaranteed to work. As the C standard says,
- Alternatively, an implementation might perform various optimizations within each translation unit, such
- that the actual semantics would agree with the abstract semantics only when making function calls across
- translation unit boundaries. In such an implementation, at the time of each function entry and function
- return where the calling function and the called function are in different translation units, the values of all
- externally linked objects and of all objects accessible via pointers therein would agree with the abstract
- semantics. Furthermore, at the time of each such function entry the values of the parameters of the called
- function and of all objects accessible via pointers therein would agree with the abstract semantics. In this
- type of implementation, objects referred to by interrupt service routines activated by the signal function
- would require explicit specification of volatile storage, as well as other implementation-defined
- restrictions.
-
-The C++ namespace aos is used for all of the aos code. The crio and linux_code namespaces are for APIs that only make sense on one platform or the other.
-
-Almost everything in aos/ is thread-safe. Files with a main() might not be, and some pieces of code explicitly say at their declaration that they aren't. Also, code which shares non-const pointers (including this) is not generally thread-safe. Other than those exceptions, everything is assumed to be thread-safe so the comments don't say so explicitly.
diff --git a/aos/aos_dump.cc b/aos/aos_dump.cc
index 055700f..a0cca5d 100644
--- a/aos/aos_dump.cc
+++ b/aos/aos_dump.cc
@@ -14,6 +14,9 @@
"If true, fetch the current message on the channel first");
DEFINE_bool(pretty, false,
"If true, pretty print the messages on multiple lines");
+DEFINE_bool(all, false,
+ "If true, print out the channels for all nodes, not just the "
+ "channels which are visible on this node.");
DEFINE_bool(print_timestamps, true, "If true, timestamps are printed.");
DEFINE_uint64(count, 0,
"If >0, aos_dump will exit after printing this many messages.");
@@ -75,8 +78,11 @@
if (argc == 1) {
std::cout << "Channels:\n";
for (const aos::Channel *channel : *config_msg->channels()) {
- std::cout << channel->name()->c_str() << ' ' << channel->type()->c_str()
- << '\n';
+ if (FLAGS_all || aos::configuration::ChannelIsReadableOnNode(
+ channel, event_loop.node())) {
+ std::cout << channel->name()->c_str() << ' ' << channel->type()->c_str()
+ << '\n';
+ }
}
return 0;
}
@@ -86,6 +92,10 @@
config_msg->channels();
bool found_exact = false;
for (const aos::Channel *channel : *channels) {
+ if (!FLAGS_all && !aos::configuration::ChannelIsReadableOnNode(
+ channel, event_loop.node())) {
+ continue;
+ }
if (channel->name()->c_str() != channel_name) {
continue;
}
diff --git a/aos/config.bzl b/aos/config.bzl
index b87fff2..a11efd5 100644
--- a/aos/config.bzl
+++ b/aos/config.bzl
@@ -18,6 +18,7 @@
def _aos_config_impl(ctx):
config = ctx.actions.declare_file(ctx.label.name + ".json")
stripped_config = ctx.actions.declare_file(ctx.label.name + ".stripped.json")
+ binary_config = ctx.actions.declare_file(ctx.label.name + ".bfbs")
flatbuffers_depset = depset(
ctx.files.flatbuffers,
@@ -31,21 +32,22 @@
all_files = flatbuffers_depset.to_list() + src_depset.to_list()
ctx.actions.run(
- outputs = [config, stripped_config],
+ outputs = [config, stripped_config, binary_config],
inputs = all_files,
arguments = [
config.path,
stripped_config.path,
+ binary_config.path,
(ctx.label.workspace_root or ".") + "/" + ctx.files.src[0].short_path,
ctx.bin_dir.path,
] + [f.path for f in flatbuffers_depset.to_list()],
progress_message = "Flattening config",
executable = ctx.executable._config_flattener,
)
- runfiles = ctx.runfiles(files = [config, stripped_config])
+ runfiles = ctx.runfiles(files = [config, stripped_config, binary_config])
return [
DefaultInfo(
- files = depset([config, stripped_config]),
+ files = depset([config, stripped_config, binary_config]),
runfiles = runfiles,
),
AosConfigInfo(
diff --git a/aos/config_flattener.cc b/aos/config_flattener.cc
index d39cd50..e4bd192 100644
--- a/aos/config_flattener.cc
+++ b/aos/config_flattener.cc
@@ -11,37 +11,50 @@
namespace aos {
int Main(int argc, char **argv) {
- CHECK_GE(argc, 5) << ": Too few arguments";
+ CHECK_GE(argc, 6) << ": Too few arguments";
const char *full_output = argv[1];
const char *stripped_output = argv[2];
- const char *config_path = argv[3];
+ const char *binary_output = argv[3];
+ const char *config_path = argv[4];
// In order to support not only importing things by absolute path, but also
// importing the outputs of genrules (rather than just manually written
// files), we need to tell ReadConfig where the generated files directory is.
- const char *bazel_outs_directory = argv[4];
+ const char *bazel_outs_directory = argv[5];
VLOG(1) << "Reading " << config_path;
FlatbufferDetachedBuffer<Configuration> config =
configuration::ReadConfig(config_path, {bazel_outs_directory});
+ for (const Channel *channel : *config.message().channels()) {
+ if (channel->max_size() % alignof(flatbuffers::largest_scalar_t) != 0) {
+ LOG(FATAL) << "max_size() (" << channel->max_size()
+ << ") is not a multiple of alignment ("
+ << alignof(flatbuffers::largest_scalar_t) << ") for channel "
+ << configuration::CleanedChannelToString(channel) << ".";
+ }
+ }
+
std::vector<aos::FlatbufferString<reflection::Schema>> schemas;
- for (int i = 5; i < argc; ++i) {
+ for (int i = 6; i < argc; ++i) {
schemas.emplace_back(util::ReadFileToStringOrDie(argv[i]));
}
- const std::string merged_config = FlatbufferToJson(
- &configuration::MergeConfiguration(config, schemas).message(),
- {.multi_line = true});
+ aos::FlatbufferDetachedBuffer<Configuration> merged_config =
+ configuration::MergeConfiguration(config, schemas);
+
+ const std::string merged_config_json =
+ FlatbufferToJson(&merged_config.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);
+ VLOG(1) << "Flattened config is " << merged_config_json;
+ util::WriteStringToFileOrDie(full_output, merged_config_json);
util::WriteStringToFileOrDie(
stripped_output,
FlatbufferToJson(&config.message(), {.multi_line = true}));
+ aos::WriteFlatbufferToFile(binary_output, merged_config);
return 0;
}
diff --git a/aos/configuration.cc b/aos/configuration.cc
index c74234e..adba7dc 100644
--- a/aos/configuration.cc
+++ b/aos/configuration.cc
@@ -23,6 +23,44 @@
#include "glog/logging.h"
namespace aos {
+namespace {
+bool EndsWith(std::string_view str, std::string_view end) {
+ if (str.size() < end.size()) {
+ return false;
+ }
+ if (str.substr(str.size() - end.size(), end.size()) != end) {
+ return false;
+ }
+ return true;
+}
+
+std::string MaybeReplaceExtension(std::string_view filename,
+ std::string_view extension,
+ std::string_view replacement) {
+ if (!EndsWith(filename, extension)) {
+ return std::string(filename);
+ }
+ filename.remove_suffix(extension.size());
+ return absl::StrCat(filename, replacement);
+}
+
+FlatbufferDetachedBuffer<Configuration> ReadConfigFile(std::string_view path,
+ bool binary) {
+ if (binary) {
+ FlatbufferVector<Configuration> config =
+ FileToFlatbuffer<Configuration>(path);
+ return CopySpanAsDetachedBuffer(config.span());
+ }
+
+ flatbuffers::DetachedBuffer buffer = JsonToFlatbuffer(
+ util::ReadFileToStringOrDie(path), ConfigurationTypeTable());
+
+ CHECK_GT(buffer.size(), 0u) << ": Failed to parse JSON file";
+
+ return FlatbufferDetachedBuffer<Configuration>(std::move(buffer));
+}
+
+} // namespace
// Define the compare and equal operators for Channel and Application so we can
// insert them in the btree below.
@@ -95,19 +133,30 @@
FlatbufferDetachedBuffer<Configuration> ReadConfig(
const std::string_view path, absl::btree_set<std::string> *visited_paths,
const std::vector<std::string_view> &extra_import_paths) {
+ std::string binary_path = MaybeReplaceExtension(path, ".json", ".bfbs");
+ bool binary_path_exists = util::PathExists(binary_path);
std::string raw_path(path);
- if (!util::PathExists(path)) {
- const bool path_is_absolute = path.size() > 0 && path[0] == '/';
+ // For each .json file, look and see if we can find a .bfbs file next to it
+ // with the same base name. If we can, assume it is the same and use it
+ // instead. It is much faster to load .bfbs files than .json files.
+ if (!binary_path_exists && !util::PathExists(raw_path)) {
+ const bool path_is_absolute = raw_path.size() > 0 && raw_path[0] == '/';
if (path_is_absolute) {
CHECK(extra_import_paths.empty())
<< "Can't specify extra import paths if attempting to read a config "
"file from an absolute path (path is "
- << path << ").";
+ << raw_path << ").";
}
bool found_path = false;
for (const auto &import_path : extra_import_paths) {
raw_path = std::string(import_path) + "/" + std::string(path);
+ binary_path = MaybeReplaceExtension(raw_path, ".json", ".bfbs");
+ binary_path_exists = util::PathExists(binary_path);
+ if (binary_path_exists) {
+ found_path = true;
+ break;
+ }
if (util::PathExists(raw_path)) {
found_path = true;
break;
@@ -115,12 +164,9 @@
}
CHECK(found_path) << ": Failed to find file " << path << ".";
}
- flatbuffers::DetachedBuffer buffer = JsonToFlatbuffer(
- util::ReadFileToStringOrDie(raw_path), ConfigurationTypeTable());
- CHECK_GT(buffer.size(), 0u) << ": Failed to parse JSON file";
-
- FlatbufferDetachedBuffer<Configuration> config(std::move(buffer));
+ FlatbufferDetachedBuffer<Configuration> config = ReadConfigFile(
+ binary_path_exists ? binary_path : raw_path, binary_path_exists);
// Depth first. Take the following example:
//
@@ -153,8 +199,10 @@
// config. That means that it needs to be merged into the imported configs,
// not the other way around.
- const std::string absolute_path = AbsolutePath(raw_path);
- // Track that we have seen this file before recursing.
+ const std::string absolute_path =
+ AbsolutePath(binary_path_exists ? binary_path : raw_path);
+ // Track that we have seen this file before recursing. Track the path we
+ // actually loaded (which should be consistent if imported twice).
if (!visited_paths->insert(absolute_path).second) {
for (const auto &visited_path : *visited_paths) {
LOG(INFO) << "Already visited: " << visited_path;
@@ -272,6 +320,102 @@
}
}
+void ValidateConfiguration(const Flatbuffer<Configuration> &config) {
+ // No imports should be left.
+ CHECK(!config.message().has_imports());
+
+ // Check that if there is a node list, all the source nodes are filled out and
+ // valid, and all the destination nodes are valid (and not the source). This
+ // is a basic consistency check.
+ if (config.message().has_channels()) {
+ const Channel *last_channel = nullptr;
+ for (const Channel *c : *config.message().channels()) {
+ CHECK(c->has_name());
+ CHECK(c->has_type());
+ if (c->name()->string_view().back() == '/') {
+ LOG(FATAL) << "Channel names can't end with '/'";
+ }
+ if (c->name()->string_view().find("//") != std::string_view::npos) {
+ LOG(FATAL) << ": Invalid channel name " << c->name()->string_view()
+ << ", can't use //.";
+ }
+ for (const char data : c->name()->string_view()) {
+ if (data >= '0' && data <= '9') {
+ continue;
+ }
+ if (data >= 'a' && data <= 'z') {
+ continue;
+ }
+ if (data >= 'A' && data <= 'Z') {
+ continue;
+ }
+ if (data == '-' || data == '_' || data == '/') {
+ continue;
+ }
+ LOG(FATAL) << "Invalid channel name " << c->name()->string_view()
+ << ", can only use [-a-zA-Z0-9_/]";
+ }
+
+ // Make sure everything is sorted while we are here... If this fails,
+ // there will be a bunch of weird errors.
+ if (last_channel != nullptr) {
+ CHECK(CompareChannels(
+ last_channel,
+ std::make_pair(c->name()->string_view(), c->type()->string_view())))
+ << ": Channels not sorted!";
+ }
+ last_channel = c;
+ }
+ }
+
+ if (config.message().has_nodes() && config.message().has_channels()) {
+ for (const Channel *c : *config.message().channels()) {
+ CHECK(c->has_source_node()) << ": Channel " << FlatbufferToJson(c)
+ << " is missing \"source_node\"";
+ CHECK(GetNode(&config.message(), c->source_node()->string_view()) !=
+ nullptr)
+ << ": Channel " << FlatbufferToJson(c)
+ << " has an unknown \"source_node\"";
+
+ if (c->has_destination_nodes()) {
+ for (const Connection *connection : *c->destination_nodes()) {
+ CHECK(connection->has_name());
+ CHECK(GetNode(&config.message(), connection->name()->string_view()) !=
+ nullptr)
+ << ": Channel " << FlatbufferToJson(c)
+ << " has an unknown \"destination_nodes\" "
+ << connection->name()->string_view();
+
+ switch (connection->timestamp_logger()) {
+ case LoggerConfig::LOCAL_LOGGER:
+ case LoggerConfig::NOT_LOGGED:
+ CHECK(!connection->has_timestamp_logger_nodes());
+ break;
+ case LoggerConfig::REMOTE_LOGGER:
+ case LoggerConfig::LOCAL_AND_REMOTE_LOGGER:
+ CHECK(connection->has_timestamp_logger_nodes());
+ CHECK_GT(connection->timestamp_logger_nodes()->size(), 0u);
+ for (const flatbuffers::String *timestamp_logger_node :
+ *connection->timestamp_logger_nodes()) {
+ CHECK(GetNode(&config.message(),
+ timestamp_logger_node->string_view()) != nullptr)
+ << ": Channel " << FlatbufferToJson(c)
+ << " has an unknown \"timestamp_logger_node\""
+ << connection->name()->string_view();
+ }
+ break;
+ }
+
+ CHECK_NE(connection->name()->string_view(),
+ c->source_node()->string_view())
+ << ": Channel " << FlatbufferToJson(c)
+ << " is forwarding data to itself";
+ }
+ }
+ }
+ }
+}
+
} // namespace
FlatbufferDetachedBuffer<Configuration> MergeConfiguration(
@@ -403,83 +547,7 @@
FlatbufferDetachedBuffer<Configuration> result =
MergeFlatBuffers(modified_config, auto_merge_config);
- // Check that if there is a node list, all the source nodes are filled out and
- // valid, and all the destination nodes are valid (and not the source). This
- // is a basic consistency check.
- if (result.message().has_channels()) {
- for (const Channel *c : *result.message().channels()) {
- if (c->name()->string_view().back() == '/') {
- LOG(FATAL) << "Channel names can't end with '/'";
- }
- if(c->name()->string_view().find("//")!= std::string_view::npos) {
- LOG(FATAL) << ": Invalid channel name " << c->name()->string_view()
- << ", can't use //.";
- }
- for (const char data : c->name()->string_view()) {
- if (data >= '0' && data <= '9') {
- continue;
- }
- if (data >= 'a' && data <= 'z') {
- continue;
- }
- if (data >= 'A' && data <= 'Z') {
- continue;
- }
- if (data == '-' || data == '_' || data == '/') {
- continue;
- }
- LOG(FATAL) << "Invalid channel name " << c->name()->string_view()
- << ", can only use [-a-zA-Z0-9_/]";
- }
- }
- }
-
- if (result.message().has_nodes() && result.message().has_channels()) {
- for (const Channel *c : *result.message().channels()) {
- CHECK(c->has_source_node()) << ": Channel " << FlatbufferToJson(c)
- << " is missing \"source_node\"";
- CHECK(GetNode(&result.message(), c->source_node()->string_view()) !=
- nullptr)
- << ": Channel " << FlatbufferToJson(c)
- << " has an unknown \"source_node\"";
-
- if (c->has_destination_nodes()) {
- for (const Connection *connection : *c->destination_nodes()) {
- CHECK(connection->has_name());
- CHECK(GetNode(&result.message(), connection->name()->string_view()) !=
- nullptr)
- << ": Channel " << FlatbufferToJson(c)
- << " has an unknown \"destination_nodes\" "
- << connection->name()->string_view();
-
- switch (connection->timestamp_logger()) {
- case LoggerConfig::LOCAL_LOGGER:
- case LoggerConfig::NOT_LOGGED:
- CHECK(!connection->has_timestamp_logger_nodes());
- break;
- case LoggerConfig::REMOTE_LOGGER:
- case LoggerConfig::LOCAL_AND_REMOTE_LOGGER:
- CHECK(connection->has_timestamp_logger_nodes());
- CHECK_GT(connection->timestamp_logger_nodes()->size(), 0u);
- for (const flatbuffers::String *timestamp_logger_node :
- *connection->timestamp_logger_nodes()) {
- CHECK(GetNode(&result.message(),
- timestamp_logger_node->string_view()) != nullptr)
- << ": Channel " << FlatbufferToJson(c)
- << " has an unknown \"timestamp_logger_node\""
- << connection->name()->string_view();
- }
- break;
- }
-
- CHECK_NE(connection->name()->string_view(),
- c->source_node()->string_view())
- << ": Channel " << FlatbufferToJson(c)
- << " is forwarding data to itself";
- }
- }
- }
- }
+ ValidateConfiguration(result);
return result;
}
@@ -489,7 +557,17 @@
const std::vector<std::string_view> &import_paths) {
// We only want to read a file once. So track the visited files in a set.
absl::btree_set<std::string> visited_paths;
- return MergeConfiguration(ReadConfig(path, &visited_paths, import_paths));
+ FlatbufferDetachedBuffer<Configuration> read_config =
+ ReadConfig(path, &visited_paths, import_paths);
+
+ // If we only read one file, and it had a .bfbs extension, it has to be a
+ // fully formatted config. Do a quick verification and return it.
+ if (visited_paths.size() == 1 && EndsWith(*visited_paths.begin(), ".bfbs")) {
+ ValidateConfiguration(read_config);
+ return read_config;
+ }
+
+ return MergeConfiguration(read_config);
}
FlatbufferDetachedBuffer<Configuration> MergeWithConfig(
@@ -803,6 +881,8 @@
if (node == nullptr) {
return true;
}
+ CHECK(channel->has_source_node()) << FlatbufferToJson(channel);
+ CHECK(node->has_name()) << FlatbufferToJson(node);
return (CHECK_NOTNULL(channel)->source_node()->string_view() ==
node->name()->string_view());
}
@@ -975,5 +1055,40 @@
return result;
}
+std::vector<const Node *> TimestampNodes(const Configuration *config,
+ const Node *my_node) {
+ if (!configuration::MultiNode(config)) {
+ CHECK(my_node == nullptr);
+ return std::vector<const Node *>{};
+ }
+
+ std::set<const Node *> timestamp_logger_nodes;
+ for (const Channel *channel : *config->channels()) {
+ if (!configuration::ChannelIsSendableOnNode(channel, my_node)) {
+ continue;
+ }
+ if (!channel->has_destination_nodes()) {
+ continue;
+ }
+ for (const Connection *connection : *channel->destination_nodes()) {
+ const Node *other_node =
+ configuration::GetNode(config, connection->name()->string_view());
+
+ if (configuration::ConnectionDeliveryTimeIsLoggedOnNode(connection,
+ my_node)) {
+ VLOG(1) << "Timestamps are logged from "
+ << FlatbufferToJson(other_node);
+ timestamp_logger_nodes.insert(other_node);
+ }
+ }
+ }
+
+ std::vector<const Node *> result;
+ for (const Node *node : timestamp_logger_nodes) {
+ result.emplace_back(node);
+ }
+ return result;
+}
+
} // namespace configuration
} // namespace aos
diff --git a/aos/configuration.fbs b/aos/configuration.fbs
index 9a24c8a..cdf21b8 100644
--- a/aos/configuration.fbs
+++ b/aos/configuration.fbs
@@ -17,29 +17,29 @@
table Connection {
// Node name to forward to.
- name:string;
+ name:string (id: 0);
// How the delivery timestamps for this connection should be logged. Do we
// log them with the local logger (i.e. the logger running on the node that
// this message is delivered to)? Do we log them on another node (a central
// logging node)? Do we log them in both places redundantly?
- timestamp_logger:LoggerConfig = LOCAL_LOGGER;
+ timestamp_logger:LoggerConfig = LOCAL_LOGGER (id: 1);
// If the corresponding delivery timestamps for this channel are logged
// remotely, which node should be responsible for logging the data. Note:
// for now, this can only be the source node. Empty implies the node this
// connection is connecting to (i.e. name).
- timestamp_logger_nodes:[string];
+ timestamp_logger_nodes:[string] (id: 2);
// Priority to forward data with.
- priority:ushort = 100;
+ priority:ushort = 100 (id: 3);
// Time to live in nanoseconds before the message is dropped.
// A value of 0 means no timeout, i.e. reliable. When a client connects, the
// latest message from this channel will be sent regardless.
// TODO(austin): We can retry more than just the last message on reconnect
// if we want. This is an unlikely scenario though.
- time_to_live:uint = 0;
+ time_to_live:uint = 0 (id: 4);
}
enum ReadMethod : ubyte {
@@ -53,46 +53,46 @@
// subscribed from. The tuple of name, type is the identifying information.
table Channel {
// Name of the channel.
- name:string;
+ name:string (id: 0);
// Type name of the flatbuffer.
- type:string;
+ type:string (id: 1);
// Max frequency in messages/sec of the data published on this channel.
- frequency:int = 100;
+ frequency:int = 100 (id: 2);
// Max size of the data being published. (This will hopefully be
// automatically computed in the future.)
- max_size:int = 1000;
+ max_size:int = 1000 (id: 3);
// Sets the maximum number of senders on a channel.
- num_senders:int = 10;
+ num_senders:int = 10 (id: 4);
// Sets the maximum number of watchers on a channel.
- num_watchers:int = 10;
+ num_watchers:int = 10 (id: 5);
// The schema for the data sent on this channel.
- schema:reflection.Schema;
+ schema:reflection.Schema (id: 6);
// The source node name for the data sent on this channel.
// If nodes is populated below, this needs to also be populated.
- source_node:string;
+ source_node:string (id: 7);
// The destination nodes for data sent on this channel.
// This only needs to be populated if this message is getting forwarded.
- destination_nodes:[Connection];
+ destination_nodes:[Connection] (id: 8);
// What service is responsible for logging this channel:
- logger:LoggerConfig = LOCAL_LOGGER;
+ logger:LoggerConfig = LOCAL_LOGGER (id: 9);
// If the channel is logged remotely, which node should be responsible for
// logging the data. Note: this requires that the data is forwarded to the
// node responsible for logging it. Empty implies the node this connection
// is connecting to (i.e. name).
- logger_nodes:[string];
+ logger_nodes:[string] (id: 10);
// The way messages are read from shared memory for this channel.
- read_method:ReadMethod = COPY;
+ read_method:ReadMethod = COPY (id: 11);
// Sets the maximum number of senders on a channel.
//
// Currently, this must be set if and only if read_method is PIN.
- num_readers:int;
+ num_readers:int (id: 12);
}
// Table to support renaming channel names.
@@ -102,15 +102,15 @@
// wildcard. Anything with the same prefix will match, and anything matching
// the * will get preserved on rename. This supports moving subfolders.
// Node specific matches are also supported.
- match:Channel;
+ match:Channel (id: 0);
// The channel to merge in.
- rename:Channel;
+ rename:Channel (id: 1);
}
// Application specific information.
table Application {
// Name of the application.
- name:string;
+ name:string (id: 0);
// List of maps to apply for this specific application. Application specific
// maps are applied in reverse order, and before the global maps.
// For example
@@ -126,29 +126,37 @@
// ]
//
// will map "/foo" to "/baz", even if there is a global list of maps.
- maps:[Map];
+ maps:[Map] (id: 1);
// The node that this application will be started on.
// TODO(austin): Teach starter how to use this for starting applications.
- node:string;
+ nodes:[string] (id: 2);
+
+ // The user to run this application as. If this field is unset, run it as
+ // the current user of the application starter.
+ user:string (id: 3);
}
// Per node data and connection information.
table Node {
// Short name for the node. This provides a short hand to make it easy to
// setup forwarding rules as part of the channel setup.
- name:string;
+ name:string (id: 0);
// Hostname used to identify and connect to the node.
- hostname:string;
+ hostname:string (id: 1);
// Port to serve forwarded data from.
- port:ushort = 9971;
+ port:ushort = 9971 (id: 2);
// An alternative to hostname which allows specifying multiple hostnames,
// any of which will match this node.
//
// Don't specify a hostname in multiple nodes in the same configuration.
- hostnames:[string];
+ hostnames:[string] (id: 3);
+
+ // An arbitrary list of strings representing properties of each node. These
+ // can be used to store information about roles.
+ tags:[string] (id: 4);
}
// Overall configuration datastructure for the pubsub.
diff --git a/aos/configuration.h b/aos/configuration.h
index fbd9cff..5579334 100644
--- a/aos/configuration.h
+++ b/aos/configuration.h
@@ -8,7 +8,7 @@
#include <string_view>
-#include "aos/configuration_generated.h"
+#include "aos/configuration_generated.h" // IWYU pragma: export
#include "aos/flatbuffers.h"
namespace aos {
@@ -135,6 +135,10 @@
std::vector<std::string_view> SourceNodeNames(const Configuration *config,
const Node *my_node);
+// Returns the all nodes that are logging timestamps on our node.
+std::vector<const Node *> TimestampNodes(const Configuration *config,
+ const Node *my_node);
+
// TODO(austin): GetSchema<T>(const Flatbuffer<Configuration> &config);
} // namespace configuration
diff --git a/aos/containers/BUILD b/aos/containers/BUILD
index 3e24fab..9e527cd 100644
--- a/aos/containers/BUILD
+++ b/aos/containers/BUILD
@@ -56,3 +56,25 @@
"//aos/testing:googletest",
],
)
+
+cc_library(
+ name = "resizeable_buffer",
+ hdrs = [
+ "resizeable_buffer.h",
+ ],
+ deps = [
+ "@com_github_google_glog//:glog",
+ ],
+)
+
+cc_test(
+ name = "resizeable_buffer_test",
+ srcs = [
+ "resizeable_buffer_test.cc",
+ ],
+ deps = [
+ ":resizeable_buffer",
+ "//aos/testing:googletest",
+ "@com_google_absl//absl/types:span",
+ ],
+)
diff --git a/aos/containers/resizeable_buffer.h b/aos/containers/resizeable_buffer.h
new file mode 100644
index 0000000..b94f8cc
--- /dev/null
+++ b/aos/containers/resizeable_buffer.h
@@ -0,0 +1,101 @@
+#ifndef AOS_CONTAINERS_RESIZEABLE_BUFFER_H_
+#define AOS_CONTAINERS_RESIZEABLE_BUFFER_H_
+
+#include <stdlib.h>
+
+#include <memory>
+
+#include "glog/logging.h"
+
+namespace aos {
+
+// Kind of like a subset of vector<uint8_t>, but with less destructor calls.
+// When building unoptimized, especially with sanitizers, the vector<uint8_t>
+// version ends up being really slow in tests.
+class ResizeableBuffer {
+ public:
+ ResizeableBuffer() = default;
+
+ ResizeableBuffer(const ResizeableBuffer &other) { *this = other; }
+ ResizeableBuffer(ResizeableBuffer &&other) { *this = std::move(other); }
+ ResizeableBuffer &operator=(const ResizeableBuffer &other) {
+ resize(other.size());
+ memcpy(storage_.get(), other.storage_.get(), size());
+ return *this;
+ }
+ ResizeableBuffer &operator=(ResizeableBuffer &&other) {
+ std::swap(storage_, other.storage_);
+ std::swap(size_, other.size_);
+ std::swap(capacity_, other.capacity_);
+ return *this;
+ }
+
+ uint8_t *data() { return static_cast<uint8_t *>(storage_.get()); }
+ const uint8_t *data() const {
+ return static_cast<const uint8_t *>(storage_.get());
+ }
+
+ uint8_t *begin() { return data(); }
+ const uint8_t *begin() const { return data(); }
+ uint8_t *end() { return data() + size(); }
+ const uint8_t *end() const { return data() + size(); }
+
+ size_t size() const { return size_; }
+ size_t capacity() const { return capacity_; }
+
+ void resize(size_t new_size) {
+ if (new_size > capacity_) {
+ Allocate(new_size);
+ }
+ size_ = new_size;
+ }
+
+ void erase_front(size_t count) {
+ if (count == 0) {
+ return;
+ }
+ CHECK_LE(count, size_);
+ memmove(static_cast<void *>(data()), static_cast<void *>(data() + count),
+ size_ - count);
+ size_ -= count;
+ }
+
+ void push_back(uint8_t x) {
+ if (size_ == capacity_) {
+ Allocate(std::max<size_t>(16, size_ * 2));
+ }
+ *end() = x;
+ ++size_;
+ CHECK_LE(size_, capacity_);
+ }
+
+ private:
+ // We need this silly function because C++ is bad, and extensions to it which
+ // we work with make it a true nightmare.
+ //
+ // (a) You can't easily write out the signature of free because it depends on
+ // whether exceptions are enabled or not. You could use decltype, but see (b).
+ //
+ // (b) You can't easily write &free because CUDA overloads it with a
+ // __device__ version. You could cast to the appropriate version, but see (a).
+ //
+ // There's probably some kind of SFINAE thing which could find the matching
+ // signature from a set of choices, and then we could just
+ // static_cast<TheSignature>(&free). However, that sounds like a nightmare,
+ // especially because it has to conditionally enable the part mentioning CUDA
+ // identifiers in the preprocessor. This little function is way simpler.
+ static void DoFree(void *p) { free(p); }
+
+ void Allocate(size_t new_capacity) {
+ void *const old = storage_.release();
+ storage_.reset(CHECK_NOTNULL(realloc(old, new_capacity)));
+ capacity_ = new_capacity;
+ }
+
+ std::unique_ptr<void, decltype(&DoFree)> storage_{nullptr, &DoFree};
+ size_t size_ = 0, capacity_ = 0;
+};
+
+} // namespace aos
+
+#endif // AOS_CONTAINERS_RESIZEABLE_BUFFER_H_
diff --git a/aos/containers/resizeable_buffer_test.cc b/aos/containers/resizeable_buffer_test.cc
new file mode 100644
index 0000000..1d27a39
--- /dev/null
+++ b/aos/containers/resizeable_buffer_test.cc
@@ -0,0 +1,95 @@
+#include "aos/containers/resizeable_buffer.h"
+
+#include <numeric>
+
+#include "absl/types/span.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+
+namespace aos::testing {
+
+// Tests using resize to do some stuff with a ResizeableBuffer.
+TEST(ResizeableBufferTest, Resize) {
+ ResizeableBuffer buffer;
+ EXPECT_EQ(buffer.size(), 0u);
+ EXPECT_EQ(buffer.capacity(), 0u);
+
+ buffer.resize(3);
+ EXPECT_EQ(buffer.size(), 3u);
+ EXPECT_GE(buffer.capacity(), 3u);
+ EXPECT_EQ(buffer.begin() + 3, buffer.end());
+
+ buffer.resize(5);
+ EXPECT_EQ(buffer.size(), 5u);
+ EXPECT_GE(buffer.capacity(), 5u);
+ EXPECT_EQ(buffer.begin() + 5, buffer.end());
+
+ buffer.resize(2);
+ EXPECT_EQ(buffer.size(), 2u);
+ EXPECT_GE(buffer.capacity(), 2u);
+ EXPECT_EQ(buffer.begin() + 2, buffer.end());
+}
+
+// Tests using erase_front on a ResizeableBuffer.
+TEST(ResizeableBufferTest, EraseFront) {
+ ResizeableBuffer buffer;
+ EXPECT_EQ(buffer.size(), 0u);
+ EXPECT_EQ(buffer.capacity(), 0u);
+
+ buffer.resize(5);
+ for (int i = 0; i < 5; ++i) {
+ buffer.begin()[i] = '1' + i;
+ }
+ EXPECT_THAT(absl::Span<uint8_t>(buffer.begin(), buffer.size()),
+ ::testing::ElementsAre('1', '2', '3', '4', '5'));
+ buffer.erase_front(2);
+ EXPECT_THAT(absl::Span<uint8_t>(buffer.begin(), buffer.size()),
+ ::testing::ElementsAre('3', '4', '5'));
+}
+
+// Tests using push_back starting from an empty ResizeableBuffer.
+TEST(ResizeableBufferTest, PushBackEmpty) {
+ ResizeableBuffer buffer;
+ buffer.push_back('1');
+ buffer.push_back('2');
+ buffer.push_back('3');
+ EXPECT_THAT(absl::Span<uint8_t>(buffer.begin(), buffer.size()),
+ ::testing::ElementsAre('1', '2', '3'));
+}
+
+// Tests using push_back starting from a non-empty ResizeableBuffer.
+TEST(ResizeableBufferTest, PushBackNonEmpty) {
+ ResizeableBuffer buffer;
+ EXPECT_EQ(buffer.size(), 0u);
+ EXPECT_EQ(buffer.capacity(), 0u);
+
+ buffer.resize(2);
+ for (int i = 0; i < 2; ++i) {
+ buffer.begin()[i] = '1' + i;
+ }
+ buffer.push_back('3');
+ buffer.push_back('4');
+ buffer.push_back('5');
+ EXPECT_THAT(absl::Span<uint8_t>(buffer.begin(), buffer.size()),
+ ::testing::ElementsAre('1', '2', '3', '4', '5'));
+}
+
+// Tests using push_back starting from a large non-empty ResizeableBuffer.
+TEST(ResizeableBufferTest, PushBackLargeNonEmpty) {
+ ResizeableBuffer buffer;
+ EXPECT_EQ(buffer.size(), 0u);
+ EXPECT_EQ(buffer.capacity(), 0u);
+
+ buffer.resize(27);
+ for (int i = 0; i < 27; ++i) {
+ buffer.begin()[i] = '1' + i;
+ }
+ buffer.push_back('1' + 27);
+ buffer.push_back('1' + 28);
+ std::vector<char> expected(29);
+ std::iota(expected.begin(), expected.end(), '1');
+ EXPECT_THAT(absl::Span<uint8_t>(buffer.begin(), buffer.size()),
+ ::testing::ElementsAreArray(expected));
+}
+
+} // namespace aos::testing
diff --git a/aos/events/BUILD b/aos/events/BUILD
index 5912ac6..7c1abd0 100644
--- a/aos/events/BUILD
+++ b/aos/events/BUILD
@@ -260,6 +260,7 @@
deps = [
":event_loop",
":test_message_fbs",
+ "//aos:realtime",
"//aos/testing:googletest",
],
)
@@ -271,6 +272,7 @@
shard_count = 4,
deps = [
":event_loop_param_test",
+ ":message_counter",
":ping_lib",
":pong_lib",
":simulated_event_loop",
@@ -307,6 +309,7 @@
":aos_logging",
":event_loop",
":simple_channel",
+ "//aos:realtime",
"//aos/events/logging:logger_fbs",
"//aos/ipc_lib:index",
"//aos/network:message_bridge_client_status",
@@ -341,3 +344,12 @@
"//aos/logging:log_message_fbs",
],
)
+
+cc_library(
+ name = "message_counter",
+ hdrs = ["message_counter.h"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":event_loop",
+ ],
+)
diff --git a/aos/events/README.md b/aos/events/README.md
new file mode 100644
index 0000000..875e1b7
--- /dev/null
+++ b/aos/events/README.md
@@ -0,0 +1,74 @@
+## How to run ping & pong
+
+Running ping<->pong is a nice way to test that you can run some basic code and shows how messaging can work between two nodes
+
+### Setup real-time niceties:
+ 1. Add the following lines to `/etc/security/limits.d/rt.conf`, replacing "USERNAME" with the username you're running under. You'll probably need to do this as root, e.g., `sudo nano /etc/security/limits.d/rt.conf`
+```
+USERNAME - nice -20
+USERNAME - rtprio 95
+USERNAME - memlock unlimited
+```
+
+ 2. Reboot your machine to pick up the changes
+
+### Compile and run the code
+ 1. Compile the code for ping and pong, as well as aos_dump for looking at the messages. We'll assume throughout that you're running from the top level directory of the 971 code.
+ ```
+ bazel build -c opt //aos/events:ping //aos/events:pong //aos:aos_dump
+ ```
+
+ 2. In 2 separate windows, run the ping and pong commands using the `pingpong_config.json` config file:
+ 1. `bazel-bin/aos/events/ping --config bazel-bin/aos/events/pingpong_config.json`
+ 2. `bazel-bin/aos/events/pong --config bazel-bin/aos/events/pingpong_config.json`
+
+ 3. In a third window, explore the message stream using `aos_dump`. Some things you can do:
+ 1. List the channels:
+ `bazel-bin/aos/aos_dump --config bazel-bin/aos/events/pingpong_config.json`
+ 2. Listen to a specific topic on a channel-- copy one of the channels listed in the first step and put it at the end of the aos_dump command (e.g., "/test aos.examples.Ping")
+ `bazel-bin/aos/aos_dump --config bazel-bin/aos/events/pingpong_config.json /test aos.examples.Ping`
+ 3. Listen to multiple topics on a channel (e.g., all the topics published on "/test")
+ `bazel-bin/aos/aos_dump --config bazel-bin/aos/events/pingpong_config.json /test`
+
+
+NOTE: To make life easier, you can alias `aos_dump` to include the path and the config file (you may want to specify the full path to the aos_dump executable so it can be run from anywhere)
+```
+alias aos_dump='bazel-bin/aos/aos_dump --config bazel-bin/aos/events/pingpong_config.json'
+```
+
+## Logging
+
+In addition to running ping and pong, this is a good example to explore event logging.
+
+ 1. Start by compiling the code:
+ ```
+ bazel build -c opt //aos/events/logging:logger_main
+ ```
+
+ 2. Create a folder for the log files, e.g.,
+ ```
+ mkdir /tmp/log_folder
+ ```
+
+ 3. Run the command to log the data:
+ ```
+ bazel-bin/aos/events/logging/logger_main --config bazel-bin/aos/events/pingpong_config.json --logging_folder /tmp/log_folder/
+ ```
+
+A log file should be created in /tmp/log_folder, with a name something like `fbs_log-001.bfbs`.
+
+If you're running ping and pong at the same time, you should be able to watch the log file grow in size as events are being logged. For example, running `ls -lh /tmp/log_folder/*` will list the files and their sizes. Doing this periodically (e.g., every 15-30 seconds) should show the new log file growing in size.
+
+ 4. View the contents of the log file (using `log_cat` functionality found in the `logging` sub-directory):
+ 1. Compile `log_cat`:
+ ```
+ bazel build -c opt //aos/events/logging:log_cat
+ ```
+
+ 2. Run the binary pointed at the recorded event log (this assumes you're running from the base directory of the repository and that the file is named `fbs_log-001.bfbs`):
+ ```
+ bazel-bin/aos/events/logging/log_cat /tmp/log_folder/fbs_log-001.bfbs
+ ```
+
+## EXERCISE:
+ 1. Modify code in ping and pong to see a difference in their behavior, e.g., increment the counter by 2's instead of 1
diff --git a/aos/events/channel_preallocated_allocator.h b/aos/events/channel_preallocated_allocator.h
index 5ca370b..c4f0eca 100644
--- a/aos/events/channel_preallocated_allocator.h
+++ b/aos/events/channel_preallocated_allocator.h
@@ -34,17 +34,30 @@
~ChannelPreallocatedAllocator() override { CHECK(!is_allocated_); }
// TODO(austin): Read the contract for these.
- uint8_t *allocate(size_t /*size*/) override {
+ uint8_t *allocate(size_t size) override {
if (is_allocated_) {
- LOG(FATAL) << "Can't allocate more memory with a fixed size allocator. "
- "Increase the memory reserved.";
+ LOG(FATAL) << "Can't allocate more memory with a fixed size allocator on "
+ "channel "
+ << configuration::CleanedChannelToString(channel_);
}
+ CHECK_LE(size, size_)
+ << ": Tried to allocate more space than available on channel "
+ << configuration::CleanedChannelToString(channel_);
+
is_allocated_ = true;
return data_;
}
- void deallocate(uint8_t *, size_t) override { is_allocated_ = false; }
+ void deallocate(uint8_t *data, size_t size) override {
+ CHECK_EQ(data, data_)
+ << ": Deallocating data not allocated here on channel "
+ << configuration::CleanedChannelToString(channel_);
+ CHECK_LE(size, size_)
+ << ": Tried to deallocate more space than available on channel "
+ << configuration::CleanedChannelToString(channel_);
+ is_allocated_ = false;
+ }
uint8_t *reallocate_downward(uint8_t * /*old_p*/, size_t /*old_size*/,
size_t new_size, size_t /*in_use_back*/,
diff --git a/aos/events/event_loop.cc b/aos/events/event_loop.cc
index 5c7e49d..639e337 100644
--- a/aos/events/event_loop.cc
+++ b/aos/events/event_loop.cc
@@ -10,6 +10,16 @@
"Period in milliseconds to publish timing reports at.");
namespace aos {
+namespace {
+void CheckAlignment(const Channel *channel) {
+ if (channel->max_size() % alignof(flatbuffers::largest_scalar_t) != 0) {
+ LOG(FATAL) << "max_size() (" << channel->max_size()
+ << ") is not a multiple of alignment ("
+ << alignof(flatbuffers::largest_scalar_t) << ") for channel "
+ << configuration::CleanedChannelToString(channel) << ".";
+ }
+}
+} // namespace
RawSender::RawSender(EventLoop *event_loop, const Channel *channel)
: event_loop_(event_loop),
@@ -120,6 +130,8 @@
}
void EventLoop::NewFetcher(RawFetcher *fetcher) {
+ CheckAlignment(fetcher->channel());
+
fetchers_.emplace_back(fetcher);
UpdateTimingReport();
}
@@ -144,6 +156,8 @@
CHECK(!is_running()) << ": Cannot add new objects while running.";
ChannelIndex(channel);
+ CheckAlignment(channel);
+
CHECK(taken_senders_.find(channel) == taken_senders_.end())
<< ": " << configuration::CleanedChannelToString(channel)
<< " is already being used.";
@@ -163,6 +177,8 @@
CHECK(!is_running()) << ": Cannot add new objects while running.";
ChannelIndex(channel);
+ CheckAlignment(channel);
+
CHECK(taken_watchers_.find(channel) == taken_watchers_.end())
<< ": Channel " << configuration::CleanedChannelToString(channel)
<< " is already being used.";
diff --git a/aos/events/event_loop.h b/aos/events/event_loop.h
index 1e7d25b..dc458e7 100644
--- a/aos/events/event_loop.h
+++ b/aos/events/event_loop.h
@@ -79,9 +79,10 @@
// underlying flatbuffer.
template <typename T>
FlatbufferVector<T> CopyFlatBuffer() const {
- return FlatbufferVector<T>(
- std::vector<uint8_t>(reinterpret_cast<const uint8_t *>(data),
- reinterpret_cast<const uint8_t *>(data) + size));
+ ResizeableBuffer buffer;
+ buffer.resize(size);
+ memcpy(buffer.data(), data, size);
+ return FlatbufferVector<T>(std::move(buffer));
}
};
@@ -752,6 +753,6 @@
} // namespace aos
-#include "aos/events/event_loop_tmpl.h"
+#include "aos/events/event_loop_tmpl.h" // IWYU pragma: export
#endif // AOS_EVENTS_EVENT_LOOP_H
diff --git a/aos/events/event_loop_param_test.cc b/aos/events/event_loop_param_test.cc
index 993011f..d92927b 100644
--- a/aos/events/event_loop_param_test.cc
+++ b/aos/events/event_loop_param_test.cc
@@ -5,6 +5,7 @@
#include <unordered_set>
#include "aos/flatbuffer_merge.h"
+#include "aos/realtime.h"
#include "glog/logging.h"
#include "gmock/gmock.h"
#include "gtest/gtest.h"
@@ -1090,6 +1091,28 @@
EXPECT_EQ(iteration_list.size(), 3);
}
+// Verify that a timer can disable itself.
+//
+// TODO(Brian): Do something similar with phased loops, both with a quick
+// handler and a handler that would miss a cycle except it got deferred. Current
+// behavior doing that is a mess.
+TEST_P(AbstractEventLoopTest, TimerDisableSelf) {
+ auto loop = MakePrimary();
+
+ int count = 0;
+ aos::TimerHandler *test_timer;
+ test_timer = loop->AddTimer([&count, &test_timer]() {
+ ++count;
+ test_timer->Disable();
+ });
+
+ test_timer->Setup(loop->monotonic_now(), ::std::chrono::milliseconds(20));
+ EndEventLoop(loop.get(), ::std::chrono::milliseconds(80));
+ Run();
+
+ EXPECT_EQ(count, 1);
+}
+
// 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.
//
@@ -1158,6 +1181,35 @@
"Channel pointer not found in configuration\\(\\)->channels\\(\\)");
}
+// Verifies that the event loop implementations detect when Channel has an
+// invalid alignment.
+TEST_P(AbstractEventLoopDeathTest, InvalidChannelAlignment) {
+ const char *const kError = "multiple of alignment";
+ InvalidChannelAlignment();
+
+ auto loop = MakePrimary();
+
+ const Channel *channel = configuration::GetChannel(
+ loop->configuration(), "/test", "aos.TestMessage", "", nullptr);
+
+ EXPECT_DEATH({ loop->MakeRawSender(channel); }, kError);
+ EXPECT_DEATH({ loop->MakeSender<TestMessage>("/test"); }, kError);
+
+ EXPECT_DEATH({ loop->MakeRawFetcher(channel); }, kError);
+ EXPECT_DEATH({ loop->MakeFetcher<TestMessage>("/test"); }, kError);
+
+ EXPECT_DEATH(
+ { loop->MakeRawWatcher(channel, [](const Context &, const void *) {}); },
+ kError);
+ EXPECT_DEATH({ loop->MakeRawNoArgWatcher(channel, [](const Context &) {}); },
+ kError);
+
+ EXPECT_DEATH({ loop->MakeNoArgWatcher<TestMessage>("/test", []() {}); },
+ kError);
+ EXPECT_DEATH({ loop->MakeWatcher("/test", [](const TestMessage &) {}); },
+ kError);
+}
+
// Verify that the send time on a message is roughly right when using a watcher.
TEST_P(AbstractEventLoopTest, MessageSendTime) {
auto loop1 = MakePrimary();
@@ -1898,6 +1950,120 @@
aos::Sender<TestMessage> sender = loop1->MakeSender<TestMessage>("/test");
}
+// Tests that a non-realtime event loop timer is marked non-realtime.
+TEST_P(AbstractEventLoopTest, NonRealtimeEventLoopTimer) {
+ auto loop1 = MakePrimary();
+
+ // Add a timer to actually quit.
+ auto test_timer = loop1->AddTimer([this]() {
+ CheckNotRealtime();
+ this->Exit();
+ });
+
+ loop1->OnRun([&test_timer, &loop1]() {
+ CheckNotRealtime();
+ test_timer->Setup(loop1->monotonic_now(), ::std::chrono::milliseconds(100));
+ });
+
+ Run();
+}
+
+// Tests that a realtime event loop timer is marked realtime.
+TEST_P(AbstractEventLoopTest, RealtimeEventLoopTimer) {
+ auto loop1 = MakePrimary();
+
+ loop1->SetRuntimeRealtimePriority(1);
+
+ // Add a timer to actually quit.
+ auto test_timer = loop1->AddTimer([this]() {
+ CheckRealtime();
+ this->Exit();
+ });
+
+ loop1->OnRun([&test_timer, &loop1]() {
+ CheckRealtime();
+ test_timer->Setup(loop1->monotonic_now(), ::std::chrono::milliseconds(100));
+ });
+
+ Run();
+}
+
+// Tests that a non-realtime event loop phased loop is marked non-realtime.
+TEST_P(AbstractEventLoopTest, NonRealtimeEventLoopPhasedLoop) {
+ auto loop1 = MakePrimary();
+
+ // Add a timer to actually quit.
+ loop1->AddPhasedLoop(
+ [this](int) {
+ CheckNotRealtime();
+ this->Exit();
+ },
+ chrono::seconds(1), chrono::seconds(0));
+
+ Run();
+}
+
+// Tests that a realtime event loop phased loop is marked realtime.
+TEST_P(AbstractEventLoopTest, RealtimeEventLoopPhasedLoop) {
+ auto loop1 = MakePrimary();
+
+ loop1->SetRuntimeRealtimePriority(1);
+
+ // Add a timer to actually quit.
+ loop1->AddPhasedLoop(
+ [this](int) {
+ CheckRealtime();
+ this->Exit();
+ },
+ chrono::seconds(1), chrono::seconds(0));
+
+ Run();
+}
+
+// Tests that a non-realtime event loop watcher is marked non-realtime.
+TEST_P(AbstractEventLoopTest, NonRealtimeEventLoopWatcher) {
+ auto loop1 = MakePrimary();
+ auto loop2 = Make();
+
+ aos::Sender<TestMessage> sender = loop2->MakeSender<TestMessage>("/test");
+
+ loop1->OnRun([&]() {
+ aos::Sender<TestMessage>::Builder msg = sender.MakeBuilder();
+ TestMessage::Builder builder = msg.MakeBuilder<TestMessage>();
+ ASSERT_TRUE(msg.Send(builder.Finish()));
+ });
+
+ loop1->MakeWatcher("/test", [&](const TestMessage &) {
+ CheckNotRealtime();
+ this->Exit();
+ });
+
+ Run();
+}
+
+// Tests that a realtime event loop watcher is marked realtime.
+TEST_P(AbstractEventLoopTest, RealtimeEventLoopWatcher) {
+ auto loop1 = MakePrimary();
+ auto loop2 = Make();
+
+ loop1->SetRuntimeRealtimePriority(1);
+
+ aos::Sender<TestMessage> sender = loop2->MakeSender<TestMessage>("/test");
+
+ loop1->OnRun([&]() {
+ aos::Sender<TestMessage>::Builder msg = sender.MakeBuilder();
+ TestMessage::Builder builder = msg.MakeBuilder<TestMessage>();
+ ASSERT_TRUE(msg.Send(builder.Finish()));
+ });
+
+ loop1->MakeWatcher("/test", [&](const TestMessage &) {
+ CheckRealtime();
+ this->Exit();
+ });
+
+ Run();
+}
+
// Tests that watchers fail when created on the wrong node.
TEST_P(AbstractEventLoopDeathTest, NodeWatcher) {
EnableNodes("them");
diff --git a/aos/events/event_loop_param_test.h b/aos/events/event_loop_param_test.h
index 57d0525..85e802b 100644
--- a/aos/events/event_loop_param_test.h
+++ b/aos/events/event_loop_param_test.h
@@ -59,6 +59,35 @@
// Advances time by sleeping. Can't be called from inside a loop.
virtual void SleepFor(::std::chrono::nanoseconds duration) = 0;
+ // Sets the config to a config with a max size with an invalid alignment.
+ void InvalidChannelAlignment() {
+ flatbuffer_ = JsonToFlatbuffer<Configuration>(R"config({
+ "channels": [
+ {
+ "name": "/aos",
+ "type": "aos.logging.LogMessageFbs"
+ },
+ {
+ "name": "/aos",
+ "type": "aos.timing.Report"
+ },
+ {
+ "name": "/test",
+ "type": "aos.TestMessage",
+ "max_size": 13
+ },
+ {
+ "name": "/test1",
+ "type": "aos.TestMessage"
+ },
+ {
+ "name": "/test2",
+ "type": "aos.TestMessage"
+ }
+ ]
+})config");
+ }
+
void PinReads() {
static const std::string kJson = R"config({
"channels": [
@@ -245,6 +274,8 @@
return factory_->MakePrimary(name);
}
+ void InvalidChannelAlignment() { factory_->InvalidChannelAlignment(); }
+
void EnableNodes(std::string_view my_node) { factory_->EnableNodes(my_node); }
void Run() { return factory_->Run(); }
diff --git a/aos/events/event_loop_tmpl.h b/aos/events/event_loop_tmpl.h
index 5ca1067..9e16d20 100644
--- a/aos/events/event_loop_tmpl.h
+++ b/aos/events/event_loop_tmpl.h
@@ -259,8 +259,8 @@
.count();
timing_.handler_time.Add(handler_latency);
- // If the handler too too long so we blew by the previous deadline, we
- // want to just try for the next deadline. Rescuedule.
+ // If the handler took too long so we blew by the previous deadline, we
+ // want to just try for the next deadline. Reschedule.
if (monotonic_end_time > phased_loop_.sleep_time()) {
Reschedule(schedule, monotonic_end_time);
}
diff --git a/aos/events/logging/BUILD b/aos/events/logging/BUILD
index 56bef43..b7f3b5a 100644
--- a/aos/events/logging/BUILD
+++ b/aos/events/logging/BUILD
@@ -14,22 +14,163 @@
cc_library(
name = "logfile_utils",
srcs = [
+ "logfile_sorting.cc",
"logfile_utils.cc",
],
hdrs = [
+ "logfile_sorting.h",
"logfile_utils.h",
],
visibility = ["//visibility:public"],
deps = [
+ ":buffer_encoder",
+ ":uuid",
":logger_fbs",
"//aos:configuration",
"//aos:flatbuffer_merge",
+ "//aos:flatbuffers",
+ "//aos/containers:resizeable_buffer",
"//aos/events:event_loop",
"//aos/util:file",
"@com_github_gflags_gflags//:gflags",
"@com_github_google_flatbuffers//:flatbuffers",
"@com_github_google_glog//:glog",
"@com_google_absl//absl/types:span",
+ ] + select({
+ "//tools:cpu_k8": [":lzma_encoder"],
+ "//tools:cpu_aarch64": [":lzma_encoder"],
+ "//conditions:default": [],
+ }),
+)
+
+cc_binary(
+ name = "logfile_utils_out_of_space_test_runner",
+ testonly = True,
+ srcs = [
+ "logfile_utils_out_of_space_test_runner.cc",
+ ],
+ deps = [
+ ":logfile_utils",
+ "//aos:init",
+ "@com_github_gflags_gflags//:gflags",
+ "@com_github_google_glog//:glog",
+ ],
+)
+
+sh_test(
+ name = "logfile_utils_out_of_space_test",
+ srcs = [
+ "logfile_utils_out_of_space_test.sh",
+ ],
+ data = [
+ ":logfile_utils_out_of_space_test_runner",
+ ],
+ deps = [
+ "@bazel_tools//tools/bash/runfiles",
+ ],
+)
+
+cc_library(
+ name = "buffer_encoder",
+ srcs = [
+ "buffer_encoder.cc",
+ ],
+ hdrs = [
+ "buffer_encoder.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":logger_fbs",
+ "//aos:configuration_fbs",
+ "@com_github_google_flatbuffers//:flatbuffers",
+ "@com_github_google_glog//:glog",
+ "@com_google_absl//absl/types:span",
+ ],
+)
+
+cc_test(
+ name = "buffer_encoder_test",
+ srcs = [
+ "buffer_encoder_test.cc",
+ ],
+ shard_count = 4,
+ deps = [
+ ":buffer_encoder",
+ ":buffer_encoder_param_test",
+ "//aos/testing:googletest",
+ "@com_github_google_glog//:glog",
+ ],
+)
+
+# TODO(Brian): Properly restrict this to specific platforms and
+# un-conditionalize the srcs and hdrs once we have support for that which
+# interacts with select properly.
+cc_library(
+ name = "lzma_encoder",
+ srcs = select({
+ "//tools:cpu_k8": [
+ "lzma_encoder.cc",
+ ],
+ "//tools:cpu_aarch64": [
+ "lzma_encoder.cc",
+ ],
+ "//conditions:default": [],
+ }),
+ hdrs = select({
+ "//tools:cpu_k8": [
+ "lzma_encoder.h",
+ ],
+ "//tools:cpu_aarch64": [
+ "lzma_encoder.h",
+ ],
+ "//conditions:default": [],
+ }),
+ visibility = ["//visibility:public"],
+ deps = [
+ ":buffer_encoder",
+ ":logger_fbs",
+ "//aos:configuration_fbs",
+ "//aos/containers:resizeable_buffer",
+ "//third_party:lzma",
+ "@com_github_google_flatbuffers//:flatbuffers",
+ "@com_github_google_glog//:glog",
+ "@com_google_absl//absl/types:span",
+ ],
+)
+
+cc_test(
+ name = "lzma_encoder_test",
+ srcs = [
+ "lzma_encoder_test.cc",
+ ],
+ restricted_to = [
+ "//tools:k8",
+ "//tools:aarch64",
+ ],
+ shard_count = 4,
+ deps = [
+ ":buffer_encoder_param_test",
+ ":lzma_encoder",
+ "//aos/testing:googletest",
+ "@com_github_google_glog//:glog",
+ ],
+)
+
+cc_library(
+ name = "buffer_encoder_param_test",
+ testonly = True,
+ srcs = [
+ "buffer_encoder_param_test.cc",
+ ],
+ hdrs = [
+ "buffer_encoder_param_test.h",
+ ],
+ deps = [
+ ":logfile_utils",
+ ":logger_fbs",
+ "//aos/testing:googletest",
+ "//aos/testing:random_seed",
+ "@com_github_google_glog//:glog",
],
)
@@ -56,6 +197,7 @@
"//aos/network:team_number",
"//aos/network:timestamp_filter",
"//aos/time",
+ "//aos/util:file",
"//third_party/gmp",
"@com_github_google_flatbuffers//:flatbuffers",
"@com_google_absl//absl/strings",
@@ -153,12 +295,15 @@
":multinode_pingpong_config",
"//aos/events:pingpong_config",
],
+ shard_count = 5,
deps = [
":logger",
+ "//aos/events:message_counter",
"//aos/events:ping_lib",
"//aos/events:pong_lib",
"//aos/events:simulated_event_loop",
"//aos/testing:googletest",
+ "//aos/testing:tmpdir",
],
)
@@ -176,3 +321,20 @@
"//aos/testing:googletest",
],
)
+
+cc_test(
+ name = "logfile_utils_test",
+ srcs = ["logfile_utils_test.cc"],
+ deps = [
+ ":logfile_utils",
+ ":test_message_fbs",
+ "//aos/testing:googletest",
+ "//aos/testing:tmpdir",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "test_message_fbs",
+ srcs = ["test_message.fbs"],
+ gen_reflections = 1,
+)
diff --git a/aos/events/logging/buffer_encoder.cc b/aos/events/logging/buffer_encoder.cc
new file mode 100644
index 0000000..10a7ed1
--- /dev/null
+++ b/aos/events/logging/buffer_encoder.cc
@@ -0,0 +1,65 @@
+#include "aos/events/logging/buffer_encoder.h"
+
+#include <fcntl.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+
+#include "glog/logging.h"
+
+namespace aos::logger {
+
+void DummyEncoder::Encode(flatbuffers::DetachedBuffer &&in) {
+ CHECK(in.data()) << ": Encode called with nullptr.";
+
+ total_bytes_ += in.size();
+ queue_.emplace_back(std::move(in));
+}
+
+void DummyEncoder::Clear(const int n) {
+ CHECK_GE(n, 0);
+ CHECK_LE(static_cast<size_t>(n), queue_size());
+ queue_.erase(queue_.begin(), queue_.begin() + n);
+}
+
+std::vector<absl::Span<const uint8_t>> DummyEncoder::queue() const {
+ std::vector<absl::Span<const uint8_t>> queue;
+ queue.reserve(queue_.size());
+ for (const auto &buffer : queue_) {
+ queue.emplace_back(buffer.data(), buffer.size());
+ }
+ return queue;
+}
+
+size_t DummyEncoder::queued_bytes() const {
+ size_t bytes = 0;
+ for (const auto &buffer : queue_) {
+ bytes += buffer.size();
+ }
+ return bytes;
+}
+
+DummyDecoder::DummyDecoder(std::string_view filename)
+ : fd_(open(std::string(filename).c_str(), O_RDONLY | O_CLOEXEC)) {
+ PCHECK(fd_ != -1) << ": Failed to open " << filename;
+}
+
+DummyDecoder::~DummyDecoder() {
+ int status = close(fd_);
+ if (status != 0) {
+ PLOG(ERROR) << "DummyDecoder: Failed to close file";
+ }
+}
+
+size_t DummyDecoder::Read(uint8_t *begin, uint8_t *end) {
+ if (end_of_file_) {
+ return 0;
+ }
+ const ssize_t count = read(fd_, begin, end - begin);
+ PCHECK(count >= 0) << ": Failed to read from file";
+ if (count == 0) {
+ end_of_file_ = true;
+ }
+ return static_cast<size_t>(count);
+}
+
+} // namespace aos::logger
diff --git a/aos/events/logging/buffer_encoder.h b/aos/events/logging/buffer_encoder.h
new file mode 100644
index 0000000..cc16fea
--- /dev/null
+++ b/aos/events/logging/buffer_encoder.h
@@ -0,0 +1,108 @@
+#ifndef AOS_EVENTS_LOGGING_BUFFER_ENCODER_H_
+#define AOS_EVENTS_LOGGING_BUFFER_ENCODER_H_
+
+#include "absl/types/span.h"
+#include "flatbuffers/flatbuffers.h"
+
+#include "aos/events/logging/logger_generated.h"
+
+namespace aos::logger {
+
+// Interface to encode DetachedBuffers as they are written to a file.
+//
+// The interface to a file is represented as a series of buffers, appropriate to
+// pass to writev(2). The backing storage for these is managed internally so
+// subclasses can handle resizing/moving as desired. Implementations must
+// enqueue as many of these backing buffers as demanded, leaving it up to the
+// caller to write them to a file when desired.
+class DetachedBufferEncoder {
+ public:
+ virtual ~DetachedBufferEncoder() = default;
+
+ // Encodes and enqueues the given DetachedBuffer.
+ //
+ // Note that in may be moved-from, or it may be left unchanged, depending on
+ // what makes the most sense for a given implementation.
+ virtual void Encode(flatbuffers::DetachedBuffer &&in) = 0;
+
+ // If this returns true, the encoder may be bypassed by writing directly to
+ // the file.
+ virtual bool may_bypass() const { return false; }
+
+ // Finalizes the encoding process. After this, queue_size() represents the
+ // full extent of data which will be written to this file.
+ //
+ // Encode may not be called after this method.
+ virtual void Finish() = 0;
+
+ // Clears the first n encoded buffers from the queue.
+ virtual void Clear(int n) = 0;
+
+ // Returns a view of the queue of encoded buffers.
+ virtual std::vector<absl::Span<const uint8_t>> queue() const = 0;
+
+ // Returns the total number of of bytes currently queued up.
+ virtual size_t queued_bytes() const = 0;
+
+ // Returns the cumulative number of bytes which have been queued. This
+ // includes data which has been removed via Clear.
+ virtual size_t total_bytes() const = 0;
+
+ // Returns the number of elements in the queue.
+ virtual size_t queue_size() const = 0;
+};
+
+// This class does not encode the data. It just claims ownership of the raw data
+// and queues it up as is.
+class DummyEncoder final : public DetachedBufferEncoder {
+ public:
+ ~DummyEncoder() override = default;
+
+ // No encoding happens, the raw data is queued up as is.
+ void Encode(flatbuffers::DetachedBuffer &&in) final;
+ bool may_bypass() const final { return true; }
+ void Finish() final {}
+ void Clear(int n) final;
+ std::vector<absl::Span<const uint8_t>> queue() const final;
+ size_t queued_bytes() const final;
+ size_t total_bytes() const final { return total_bytes_; }
+ size_t queue_size() const final { return queue_.size(); }
+
+ private:
+ std::vector<flatbuffers::DetachedBuffer> queue_;
+ size_t total_bytes_ = 0;
+};
+
+// Interface to decode chunks of data. Implementations of this interface will
+// manage opening, reading, and closing the file stream.
+class DataDecoder {
+ public:
+ virtual ~DataDecoder() = default;
+
+ // Reads data into the given range. Returns the number of bytes read.
+ //
+ // Returns less than end-begin if all bytes have been read. Otherwise, this
+ // will always fill the whole range.
+ virtual size_t Read(uint8_t *begin, uint8_t *end) = 0;
+};
+
+// Simply reads the contents of the file into the target buffer.
+class DummyDecoder final : public DataDecoder {
+ public:
+ explicit DummyDecoder(std::string_view filename);
+ ~DummyDecoder() override;
+
+ size_t Read(uint8_t *begin, uint8_t *end) final;
+
+ private:
+ // File descriptor for the log file.
+ int fd_;
+
+ // Cached bit for if we have reached the end of the file. Otherwise we will
+ // hammer on the kernel asking for more data each time we send.
+ bool end_of_file_ = false;
+};
+
+} // namespace aos::logger
+
+#endif // AOS_EVENTS_LOGGING_BUFFER_ENCODER_H_
diff --git a/aos/events/logging/buffer_encoder_param_test.cc b/aos/events/logging/buffer_encoder_param_test.cc
new file mode 100644
index 0000000..825696e
--- /dev/null
+++ b/aos/events/logging/buffer_encoder_param_test.cc
@@ -0,0 +1,92 @@
+#include "aos/events/logging/buffer_encoder_param_test.h"
+
+#include "gmock/gmock.h"
+
+namespace aos::logger::testing {
+
+// Verifies that Clear affects the sizes as expected.
+TEST_P(BufferEncoderTest, ClearAndSizes) {
+ const auto encoder = MakeEncoder();
+
+ std::uniform_int_distribution<int> quantity_distribution(500, 600);
+ const int first_quantity = quantity_distribution(*random_number_generator());
+ const int second_quantity = quantity_distribution(*random_number_generator());
+
+ const auto first_buffers = CreateAndEncode(first_quantity, encoder.get());
+ const auto first_size = encoder->queued_bytes();
+ // We want at least 2 buffers to ensure this test is actually validating the
+ // behavior.
+ ASSERT_GT(first_size, 2u) << ": Encoder chunk size is too big for this test";
+ ASSERT_EQ(encoder->total_bytes(), first_size);
+ ASSERT_EQ(TotalSize(encoder->queue()), first_size);
+ ASSERT_EQ(encoder->queue_size(), encoder->queue().size());
+
+ encoder->Clear(encoder->queue_size());
+ ASSERT_EQ(encoder->queued_bytes(), 0);
+ ASSERT_EQ(encoder->total_bytes(), first_size);
+ ASSERT_EQ(encoder->queue().size(), 0);
+ ASSERT_EQ(encoder->queue_size(), 0);
+
+ const auto second_buffers = CreateAndEncode(second_quantity, encoder.get());
+ const auto second_size = encoder->queued_bytes();
+ ASSERT_GT(second_size, 2u) << ": Encoder chunk size is too big for this test";
+ ASSERT_EQ(encoder->total_bytes(), first_size + second_size);
+ ASSERT_EQ(TotalSize(encoder->queue()), second_size);
+ ASSERT_EQ(encoder->queue_size(), encoder->queue().size());
+}
+
+// Runs data in randomly-chunked sizes through the encoder and decoder to verify
+// it comes back out the same.
+TEST_P(BufferEncoderTest, RoundTrip) {
+ std::uniform_int_distribution<int> quantity_distribution(20, 60);
+ const char *const test_dir = CHECK_NOTNULL(getenv("TEST_TMPDIR"));
+ const std::string file_path = std::string(test_dir) + "/foo";
+
+ std::vector<std::vector<uint8_t>> encoded_buffers;
+ {
+ const int encode_chunks = quantity_distribution(*random_number_generator());
+ const auto encoder = MakeEncoder();
+ encoded_buffers = CreateAndEncode(encode_chunks, encoder.get());
+ encoder->Finish();
+
+ std::ofstream output_file(file_path, std::ios::binary);
+ for (auto span : encoder->queue()) {
+ output_file.write(reinterpret_cast<const char *>(span.data()),
+ span.size());
+ }
+ }
+
+ const size_t total_encoded_size = TotalSize(encoded_buffers);
+
+ // Try decoding in multiple random chunkings.
+ for (int i = 0; i < 20; ++i) {
+ const auto decoder = MakeDecoder(file_path);
+ std::vector<std::vector<uint8_t>> decoded_buffers;
+ size_t total_decoded_size = 0;
+ while (true) {
+ const int chunk_size = quantity_distribution(*random_number_generator());
+ std::vector<uint8_t> chunk(chunk_size);
+ const size_t read_result =
+ decoder->Read(chunk.data(), chunk.data() + chunk_size);
+ if (read_result + total_decoded_size != total_encoded_size) {
+ // We didn't read anything, so we should've read the complete chunk.
+ ASSERT_EQ(read_result, chunk_size);
+ }
+ // Eventually we'll get here, once the decoder is really sure it's done.
+ if (read_result == 0) {
+ // Sanity check the math in the test code.
+ CHECK_EQ(total_decoded_size, TotalSize(decoded_buffers));
+ // Bail out because we're done.
+ break;
+ }
+ // If we're at the end, trim off the 0s so our comparison later works out.
+ chunk.resize(read_result);
+ total_decoded_size += read_result;
+ decoded_buffers.emplace_back(std::move(chunk));
+ }
+ ASSERT_THAT(Flatten(decoded_buffers),
+ ::testing::Eq(Flatten(encoded_buffers)));
+ }
+}
+
+} // namespace aos::logger::testing
diff --git a/aos/events/logging/buffer_encoder_param_test.h b/aos/events/logging/buffer_encoder_param_test.h
new file mode 100644
index 0000000..fd1a00c
--- /dev/null
+++ b/aos/events/logging/buffer_encoder_param_test.h
@@ -0,0 +1,114 @@
+#ifndef AOS_EVENTS_LOGGING_BUFFER_ENCODER_PARAM_TEST_H_
+#define AOS_EVENTS_LOGGING_BUFFER_ENCODER_PARAM_TEST_H_
+
+#include <functional>
+#include <memory>
+#include <random>
+#include <vector>
+
+#include "glog/logging.h"
+#include "gtest/gtest.h"
+
+#include "aos/events/logging/logfile_utils.h"
+#include "aos/events/logging/logger_generated.h"
+#include "aos/testing/random_seed.h"
+
+namespace aos::logger::testing {
+
+// Contains some helpful infrastructure for testing DetachedBufferEncoder
+// implementations.
+class BufferEncoderBaseTest : public ::testing::Test {
+ public:
+ // Creates and encodes n random buffers, returning a copy of the encoded data.
+ std::vector<std::vector<uint8_t>> CreateAndEncode(
+ int n, DetachedBufferEncoder *encoder) {
+ std::vector<std::vector<uint8_t>> result;
+ for (int i = 0; i < n; i++) {
+ flatbuffers::DetachedBuffer buffer = CreateRandomBuffer();
+ result.emplace_back(buffer.data(), buffer.data() + buffer.size());
+ encoder->Encode(std::move(buffer));
+ }
+ return result;
+ }
+
+ // Returns the total size of a vector full of objects with a size() method.
+ template <typename T>
+ static size_t TotalSize(const std::vector<T> &buffers) {
+ size_t result = 0;
+ for (const auto &v : buffers) {
+ result += v.size();
+ }
+ return result;
+ }
+
+ // Returns a flattened copy of a vector of sequences.
+ template <typename T>
+ static std::vector<typename T::value_type> Flatten(
+ const std::vector<T> &buffers) {
+ std::vector<typename T::value_type> result;
+ for (const auto &buffer : buffers) {
+ result.insert(result.end(), buffer.begin(), buffer.end());
+ }
+ return result;
+ }
+
+ void Reseed(int seed) {
+ // This algorithm allows a reasonable --runs_per_test to cover new seeds.
+ random_number_generator_ =
+ std::mt19937(::aos::testing::RandomSeed() + 1000 * seed);
+ }
+
+ std::mt19937 *random_number_generator() { return &random_number_generator_; }
+
+ private:
+ flatbuffers::DetachedBuffer CreateRandomBuffer() {
+ std::uniform_int_distribution<int> length_distribution(2800, 3000);
+ const size_t length = length_distribution(random_number_generator_);
+
+ flatbuffers::FlatBufferBuilder fbb;
+ uint8_t *data;
+ const flatbuffers::Offset<flatbuffers::Vector<uint8_t>> data_offset =
+ fbb.CreateUninitializedVector(length, &data);
+ {
+ std::independent_bits_engine<std::mt19937, CHAR_BIT, uint8_t> engine(
+ std::ref(random_number_generator_));
+ const uint8_t byte = engine();
+ std::fill(data, data + length, byte);
+ }
+
+ MessageHeader::Builder builder(fbb);
+ builder.add_data(data_offset);
+ fbb.FinishSizePrefixed(builder.Finish());
+
+ return fbb.Release();
+ }
+
+ std::mt19937 random_number_generator_{
+ std::mt19937(::aos::testing::RandomSeed())};
+};
+
+// Tests some generic properties for any DetachedBufferEncoder+DataDecoder
+// implementation pair.
+//
+// First and second test parameters are methods to create instances of the
+// encoder and decoder. Third test parameter is a random seed, to allow testing
+// many different data patterns.
+class BufferEncoderTest
+ : public BufferEncoderBaseTest,
+ public ::testing::WithParamInterface<std::tuple<
+ std::function<std::unique_ptr<DetachedBufferEncoder>()>,
+ std::function<std::unique_ptr<DataDecoder>(std::string_view)>, int>> {
+ public:
+ BufferEncoderTest() { Reseed(std::get<2>(GetParam())); }
+
+ std::unique_ptr<DetachedBufferEncoder> MakeEncoder() const {
+ return std::get<0>(GetParam())();
+ }
+ std::unique_ptr<DataDecoder> MakeDecoder(std::string_view filename) const {
+ return std::get<1>(GetParam())(filename);
+ }
+};
+
+} // namespace aos::logger::testing
+
+#endif // AOS_EVENTS_LOGGING_BUFFER_ENCODER_PARAM_TEST_H_
diff --git a/aos/events/logging/buffer_encoder_test.cc b/aos/events/logging/buffer_encoder_test.cc
new file mode 100644
index 0000000..a9c2bbb
--- /dev/null
+++ b/aos/events/logging/buffer_encoder_test.cc
@@ -0,0 +1,106 @@
+#include "aos/events/logging/buffer_encoder.h"
+
+#include <algorithm>
+#include <fstream>
+#include <string>
+
+#include "glog/logging.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+
+#include "aos/events/logging/buffer_encoder_param_test.h"
+
+namespace aos::logger::testing {
+
+class DummyEncoderTest : public BufferEncoderBaseTest {};
+
+// Tests that buffers are enqueued without any changes.
+TEST_F(DummyEncoderTest, QueuesBuffersAsIs) {
+ DummyEncoder encoder;
+ const auto expected = CreateAndEncode(100, &encoder);
+
+ auto queue = encoder.queue();
+ EXPECT_THAT(queue, ::testing::ElementsAreArray(expected));
+}
+
+// Checks that DummyDecoder can read into a buffer.
+TEST(DummyDecoderTest, ReadsIntoExactBuffer) {
+ static const std::string kTestString{"Just some random words."};
+
+ const char *const test_dir = CHECK_NOTNULL(getenv("TEST_TMPDIR"));
+ const std::string file_path = std::string(test_dir) + "/foo";
+ std::ofstream(file_path, std::ios::binary) << kTestString;
+
+ // Read the contents of the file into the buffer.
+ DummyDecoder dummy_decoder(file_path.c_str());
+ std::vector<uint8_t> buffer(kTestString.size());
+ const size_t count = dummy_decoder.Read(&*buffer.begin(), &*buffer.end());
+ ASSERT_EQ(std::string(buffer.data(), buffer.data() + count), kTestString);
+
+ for (int i = 0; i < 10; ++i) {
+ // Verify that there is no more data to read from the file.
+ ASSERT_EQ(dummy_decoder.Read(&*buffer.begin(), &*buffer.end()), 0);
+ }
+}
+
+// Checks that DummyDecoder can read into a buffer that can accommodate all the
+// data in the file.
+TEST(DummyDecoderTest, ReadsIntoLargerBuffer) {
+ static const std::string kTestString{"Just some random words."};
+
+ const char *const test_dir = CHECK_NOTNULL(getenv("TEST_TMPDIR"));
+ const std::string file_path = std::string(test_dir) + "/foo";
+ std::ofstream(file_path, std::ios::binary) << kTestString;
+
+ DummyDecoder dummy_decoder(file_path.c_str());
+ std::vector<uint8_t> buffer(100);
+ const size_t count = dummy_decoder.Read(&*buffer.begin(), &*buffer.end());
+ buffer.resize(count);
+ ASSERT_EQ(std::string(buffer.data(), buffer.data() + count), kTestString);
+
+ // Verify that there is no more data to read from the file.
+ ASSERT_EQ(dummy_decoder.Read(&*buffer.begin(), &*buffer.end()), 0);
+}
+
+// Checks that DummyDecoder can repeatedly read the contents of the file into a
+// smaller buffer until there is no more to read.
+TEST(DummyDecoderTest, ReadsRepeatedlyIntoSmallerBuffer) {
+ static const std::string kTestString{"Just some random words."};
+
+ const char *const test_dir = CHECK_NOTNULL(getenv("TEST_TMPDIR"));
+ const std::string file_path = std::string(test_dir) + "/foo";
+ std::ofstream(file_path, std::ios::binary) << kTestString;
+
+ DummyDecoder dummy_decoder(file_path.c_str());
+ std::vector<uint8_t> buffer((kTestString.size() + 1) / 2);
+
+ {
+ // Read into our buffer once, and verify the contents.
+ const size_t count = dummy_decoder.Read(&*buffer.begin(), &*buffer.end());
+ ASSERT_EQ(std::string(buffer.data(), buffer.data() + count),
+ kTestString.substr(0, buffer.size()));
+ }
+
+ {
+ // Read into the same buffer again, and verify the contents.
+ const size_t count = dummy_decoder.Read(&*buffer.begin(), &*buffer.end());
+ ASSERT_EQ(
+ std::string(buffer.data(), buffer.data() + count),
+ kTestString.substr(buffer.size(), kTestString.size() - buffer.size()));
+ }
+
+ // Verify that there is no more data to read from the file.
+ ASSERT_EQ(dummy_decoder.Read(&*buffer.begin(), &*buffer.end()), 0);
+}
+
+INSTANTIATE_TEST_CASE_P(
+ Dummy, BufferEncoderTest,
+ ::testing::Combine(::testing::Values([]() {
+ return std::make_unique<DummyEncoder>();
+ }),
+ ::testing::Values([](std::string_view filename) {
+ return std::make_unique<DummyDecoder>(filename);
+ }),
+ ::testing::Range(0, 100)));
+
+} // namespace aos::logger::testing
diff --git a/aos/events/logging/eigen_mpq.h b/aos/events/logging/eigen_mpq.h
index 1463648..370d77e 100644
--- a/aos/events/logging/eigen_mpq.h
+++ b/aos/events/logging/eigen_mpq.h
@@ -8,8 +8,7 @@
// TypeTraits for mpq_class. This is only really enough to use inverse().
template <>
-struct NumTraits<mpq_class>
- : GenericNumTraits<mpq_class> {
+struct NumTraits<mpq_class> : GenericNumTraits<mpq_class> {
typedef mpq_class Real;
typedef mpq_class Literal;
typedef mpq_class NonInteger;
diff --git a/aos/events/logging/log_cat.cc b/aos/events/logging/log_cat.cc
index f406d24..0afdc03 100644
--- a/aos/events/logging/log_cat.cc
+++ b/aos/events/logging/log_cat.cc
@@ -127,7 +127,7 @@
unsorted_logfiles.emplace_back(std::string(argv[i]));
}
- std::vector<std::vector<std::string>> logfiles =
+ const std::vector<aos::logger::LogFile> logfiles =
aos::logger::SortParts(unsorted_logfiles);
aos::logger::LogReader reader(logfiles);
diff --git a/aos/events/logging/log_edit.cc b/aos/events/logging/log_edit.cc
index 4078f9b..02a1402 100644
--- a/aos/events/logging/log_edit.cc
+++ b/aos/events/logging/log_edit.cc
@@ -15,46 +15,48 @@
"If true, replace the header on the log file with the JSON header.");
DEFINE_string(
header, "",
- "If provided, this is the path to the JSON with the log file header.");
+ "If provided, this is the path to the JSON with the log file header. If "
+ "not provided, _header.json will be appended to --logfile.");
int main(int argc, char **argv) {
gflags::SetUsageMessage(R"(This tool lets us manipulate log files.)");
aos::InitGoogle(&argc, &argv);
- if (!FLAGS_header.empty()) {
- if (FLAGS_replace) {
- const ::std::string header_json =
- aos::util::ReadFileToStringOrDie(FLAGS_header);
- flatbuffers::FlatBufferBuilder fbb;
- fbb.ForceDefaults(true);
- flatbuffers::Offset<aos::logger::LogFileHeader> header =
- aos::JsonToFlatbuffer<aos::logger::LogFileHeader>(header_json, &fbb);
+ std::string header =
+ FLAGS_header.empty() ? (FLAGS_logfile + "_header.json") : FLAGS_header;
- fbb.FinishSizePrefixed(header);
+ if (FLAGS_replace) {
+ const ::std::string header_json = aos::util::ReadFileToStringOrDie(header);
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.ForceDefaults(true);
+ flatbuffers::Offset<aos::logger::LogFileHeader> header =
+ aos::JsonToFlatbuffer<aos::logger::LogFileHeader>(header_json, &fbb);
- const std::string orig_path = FLAGS_logfile + ".orig";
- PCHECK(rename(FLAGS_logfile.c_str(), orig_path.c_str()) == 0);
+ fbb.FinishSizePrefixed(header);
- aos::logger::SpanReader span_reader(orig_path);
- CHECK(!span_reader.ReadMessage().empty()) << ": Empty header, aborting";
+ const std::string orig_path = FLAGS_logfile + ".orig";
+ PCHECK(rename(FLAGS_logfile.c_str(), orig_path.c_str()) == 0);
- aos::logger::DetachedBufferWriter buffer_writer(FLAGS_logfile);
- buffer_writer.QueueSizedFlatbuffer(&fbb);
+ aos::logger::SpanReader span_reader(orig_path);
+ CHECK(!span_reader.ReadMessage().empty()) << ": Empty header, aborting";
- while (true) {
- absl::Span<const uint8_t> msg_data = span_reader.ReadMessage();
- if (msg_data == absl::Span<const uint8_t>()) {
- break;
- }
+ aos::logger::DetachedBufferWriter buffer_writer(
+ FLAGS_logfile, std::make_unique<aos::logger::DummyEncoder>());
+ buffer_writer.QueueSizedFlatbuffer(&fbb);
- buffer_writer.WriteSizedFlatbuffer(msg_data);
+ while (true) {
+ absl::Span<const uint8_t> msg_data = span_reader.ReadMessage();
+ if (msg_data == absl::Span<const uint8_t>()) {
+ break;
}
- } else {
- aos::logger::MessageReader reader(FLAGS_logfile);
- aos::util::WriteStringToFileOrDie(
- FLAGS_header, aos::FlatbufferToJson(reader.log_file_header(),
- {.multi_line = true}));
+
+ buffer_writer.QueueSpan(msg_data);
}
+ } else {
+ aos::logger::MessageReader reader(FLAGS_logfile);
+ aos::util::WriteStringToFileOrDie(
+ header,
+ aos::FlatbufferToJson(reader.log_file_header(), {.multi_line = true}));
}
return 0;
diff --git a/aos/events/logging/log_namer.cc b/aos/events/logging/log_namer.cc
index def0842..411e666 100644
--- a/aos/events/logging/log_namer.cc
+++ b/aos/events/logging/log_namer.cc
@@ -32,11 +32,12 @@
const Node *node) {
CHECK_EQ(node, this->node());
UpdateHeader(header, uuid_, part_number_);
- data_writer_->WriteSizedFlatbuffer(header->full_span());
+ data_writer_->QueueSpan(header->full_span());
}
DetachedBufferWriter *LocalLogNamer::MakeWriter(const Channel *channel) {
- CHECK(configuration::ChannelIsSendableOnNode(channel, node()));
+ CHECK(configuration::ChannelIsSendableOnNode(channel, node()))
+ << ": " << configuration::CleanedChannelToString(channel);
return data_writer_.get();
}
@@ -47,7 +48,7 @@
++part_number_;
*data_writer_ = std::move(*OpenDataWriter());
UpdateHeader(header, uuid_, part_number_);
- data_writer_->WriteSizedFlatbuffer(header->full_span());
+ data_writer_->QueueSpan(header->full_span());
}
DetachedBufferWriter *LocalLogNamer::MakeTimestampWriter(
@@ -70,25 +71,31 @@
MultiNodeLogNamer::MultiNodeLogNamer(std::string_view base_name,
const Configuration *configuration,
const Node *node)
- : LogNamer(node),
- base_name_(base_name),
- configuration_(configuration),
- uuid_(UUID::Random()),
- data_writer_(OpenDataWriter()) {}
+ : LogNamer(node), base_name_(base_name), configuration_(configuration) {}
+
+MultiNodeLogNamer::~MultiNodeLogNamer() {
+ if (!ran_out_of_space_) {
+ // This handles renaming temporary files etc.
+ Close();
+ }
+}
void MultiNodeLogNamer::WriteHeader(
aos::SizePrefixedFlatbufferDetachedBuffer<LogFileHeader> *header,
const Node *node) {
if (node == this->node()) {
- UpdateHeader(header, uuid_, part_number_);
- data_writer_->WriteSizedFlatbuffer(header->full_span());
+ if (!data_writer_.writer) {
+ OpenDataWriter();
+ }
+ UpdateHeader(header, data_writer_.uuid, data_writer_.part_number);
+ data_writer_.writer->QueueSpan(header->full_span());
} else {
for (std::pair<const Channel *const, DataWriter> &data_writer :
data_writers_) {
if (node == data_writer.second.node) {
UpdateHeader(header, data_writer.second.uuid,
data_writer.second.part_number);
- data_writer.second.writer->WriteSizedFlatbuffer(header->full_span());
+ data_writer.second.writer->QueueSpan(header->full_span());
}
}
}
@@ -98,10 +105,12 @@
const Node *node,
aos::SizePrefixedFlatbufferDetachedBuffer<LogFileHeader> *header) {
if (node == this->node()) {
- ++part_number_;
- *data_writer_ = std::move(*OpenDataWriter());
- UpdateHeader(header, uuid_, part_number_);
- data_writer_->WriteSizedFlatbuffer(header->full_span());
+ if (data_writer_.writer) {
+ ++data_writer_.part_number;
+ }
+ OpenDataWriter();
+ UpdateHeader(header, data_writer_.uuid, data_writer_.part_number);
+ data_writer_.writer->QueueSpan(header->full_span());
} else {
for (std::pair<const Channel *const, DataWriter> &data_writer :
data_writers_) {
@@ -110,7 +119,7 @@
data_writer.second.rotate(data_writer.first, &data_writer.second);
UpdateHeader(header, data_writer.second.uuid,
data_writer.second.part_number);
- data_writer.second.writer->WriteSizedFlatbuffer(header->full_span());
+ data_writer.second.writer->QueueSpan(header->full_span());
}
}
}
@@ -135,7 +144,10 @@
// Now, sort out if this is data generated on this node, or not. It is
// generated if it is sendable on this node.
if (configuration::ChannelIsSendableOnNode(channel, this->node())) {
- return data_writer_.get();
+ if (!data_writer_.writer) {
+ OpenDataWriter();
+ }
+ return data_writer_.writer.get();
}
// Ok, we have data that is being forwarded to us that we are supposed to
@@ -188,49 +200,163 @@
DetachedBufferWriter *MultiNodeLogNamer::MakeTimestampWriter(
const Channel *channel) {
- const bool log_delivery_times =
- (this->node() == nullptr)
- ? false
- : configuration::ConnectionDeliveryTimeIsLoggedOnNode(
- channel, this->node(), this->node());
+ bool log_delivery_times = false;
+ if (this->node() != nullptr) {
+ log_delivery_times = configuration::ConnectionDeliveryTimeIsLoggedOnNode(
+ channel, this->node(), this->node());
+ }
if (!log_delivery_times) {
return nullptr;
}
- return data_writer_.get();
+ if (!data_writer_.writer) {
+ OpenDataWriter();
+ }
+ return data_writer_.writer.get();
+}
+
+void MultiNodeLogNamer::Close() {
+ for (std::pair<const Channel *const, DataWriter> &data_writer :
+ data_writers_) {
+ CloseWriter(&data_writer.second.writer);
+ data_writer.second.writer.reset();
+ }
+ CloseWriter(&data_writer_.writer);
+ data_writer_.writer.reset();
+}
+
+void MultiNodeLogNamer::ResetStatistics() {
+ for (std::pair<const Channel *const, DataWriter> &data_writer :
+ data_writers_) {
+ data_writer.second.writer->ResetStatistics();
+ }
+ if (data_writer_.writer) {
+ data_writer_.writer->ResetStatistics();
+ }
+ max_write_time_ = std::chrono::nanoseconds::zero();
+ max_write_time_bytes_ = -1;
+ max_write_time_messages_ = -1;
+ total_write_time_ = std::chrono::nanoseconds::zero();
+ total_write_count_ = 0;
+ total_write_messages_ = 0;
+ total_write_bytes_ = 0;
}
void MultiNodeLogNamer::OpenForwardedTimestampWriter(const Channel *channel,
DataWriter *data_writer) {
std::string filename =
- absl::StrCat(base_name_, "_timestamps", channel->name()->string_view(),
- "/", channel->type()->string_view(), ".part",
- data_writer->part_number, ".bfbs");
-
- if (!data_writer->writer) {
- data_writer->writer = std::make_unique<DetachedBufferWriter>(filename);
- } else {
- *data_writer->writer = DetachedBufferWriter(filename);
- }
+ absl::StrCat("timestamps", channel->name()->string_view(), "/",
+ channel->type()->string_view(), ".part",
+ data_writer->part_number, ".bfbs", extension_);
+ CreateBufferWriter(filename, &data_writer->writer);
}
void MultiNodeLogNamer::OpenWriter(const Channel *channel,
DataWriter *data_writer) {
const std::string filename = absl::StrCat(
- base_name_, "_", channel->source_node()->string_view(), "_data",
+ CHECK_NOTNULL(channel->source_node())->string_view(), "_data",
channel->name()->string_view(), "/", channel->type()->string_view(),
- ".part", data_writer->part_number, ".bfbs");
- if (!data_writer->writer) {
- data_writer->writer = std::make_unique<DetachedBufferWriter>(filename);
- } else {
- *data_writer->writer = DetachedBufferWriter(filename);
+ ".part", data_writer->part_number, ".bfbs", extension_);
+ CreateBufferWriter(filename, &data_writer->writer);
+}
+
+void MultiNodeLogNamer::OpenDataWriter() {
+ std::string name;
+ if (node() != nullptr) {
+ name = absl::StrCat(name, node()->name()->string_view(), "_");
+ }
+ absl::StrAppend(&name, "data.part", data_writer_.part_number, ".bfbs",
+ extension_);
+ CreateBufferWriter(name, &data_writer_.writer);
+}
+
+void MultiNodeLogNamer::CreateBufferWriter(
+ std::string_view path, std::unique_ptr<DetachedBufferWriter> *destination) {
+ if (ran_out_of_space_) {
+ // Refuse to open any new files, which might skip data. Any existing files
+ // are in the same folder, which means they're on the same filesystem, which
+ // means they're probably going to run out of space and get stuck too.
+ return;
+ }
+ const std::string_view separator = base_name_.back() == '/' ? "" : "_";
+ const std::string filename =
+ absl::StrCat(base_name_, separator, path, temp_suffix_);
+ if (!destination->get()) {
+ if (ran_out_of_space_) {
+ *destination = std::make_unique<DetachedBufferWriter>(
+ DetachedBufferWriter::already_out_of_space_t());
+ return;
+ }
+ *destination =
+ std::make_unique<DetachedBufferWriter>(filename, encoder_factory_());
+ if (!destination->get()->ran_out_of_space()) {
+ all_filenames_.emplace_back(path);
+ }
+ return;
+ }
+
+ CloseWriter(destination);
+ if (ran_out_of_space_) {
+ *destination->get() =
+ DetachedBufferWriter(DetachedBufferWriter::already_out_of_space_t());
+ return;
+ }
+
+ *destination->get() = DetachedBufferWriter(filename, encoder_factory_());
+ if (!destination->get()->ran_out_of_space()) {
+ all_filenames_.emplace_back(path);
}
}
-std::unique_ptr<DetachedBufferWriter> MultiNodeLogNamer::OpenDataWriter() {
- return std::make_unique<DetachedBufferWriter>(
- absl::StrCat(base_name_, "_", node()->name()->string_view(), "_data.part",
- part_number_, ".bfbs"));
+void MultiNodeLogNamer::RenameTempFile(DetachedBufferWriter *destination) {
+ if (temp_suffix_.empty()) {
+ return;
+ }
+ const std::string current_filename = std::string(destination->filename());
+ CHECK(current_filename.size() > temp_suffix_.size());
+ const std::string final_filename =
+ current_filename.substr(0, current_filename.size() - temp_suffix_.size());
+ const int result = rename(current_filename.c_str(), final_filename.c_str());
+ if (result != 0) {
+ if (errno == ENOSPC) {
+ ran_out_of_space_ = true;
+ return;
+ } else {
+ PLOG(FATAL) << "Renaming " << current_filename << " to " << final_filename
+ << " failed";
+ }
+ }
+}
+
+void MultiNodeLogNamer::CloseWriter(
+ std::unique_ptr<DetachedBufferWriter> *writer_pointer) {
+ DetachedBufferWriter *const writer = writer_pointer->get();
+ if (!writer) {
+ return;
+ }
+ const bool was_open = writer->is_open();
+ writer->Close();
+
+ if (writer->max_write_time() > max_write_time_) {
+ max_write_time_ = writer->max_write_time();
+ max_write_time_bytes_ = writer->max_write_time_bytes();
+ max_write_time_messages_ = writer->max_write_time_messages();
+ }
+ total_write_time_ += writer->total_write_time();
+ total_write_count_ += writer->total_write_count();
+ total_write_messages_ += writer->total_write_messages();
+ total_write_bytes_ += writer->total_write_bytes();
+
+ if (writer->ran_out_of_space()) {
+ ran_out_of_space_ = true;
+ writer->acknowledge_out_of_space();
+ }
+ if (was_open) {
+ RenameTempFile(writer);
+ } else {
+ CHECK(access(std::string(writer->filename()).c_str(), F_OK) == -1)
+ << ": File should not exist: " << writer->filename();
+ }
}
} // namespace logger
diff --git a/aos/events/logging/log_namer.h b/aos/events/logging/log_namer.h
index cec7ad8..5384ab2 100644
--- a/aos/events/logging/log_namer.h
+++ b/aos/events/logging/log_namer.h
@@ -88,6 +88,7 @@
base_name_(base_name),
uuid_(UUID::Random()),
data_writer_(OpenDataWriter()) {}
+ ~LocalLogNamer() override = default;
void WriteHeader(
aos::SizePrefixedFlatbufferDetachedBuffer<LogFileHeader> *header,
@@ -108,7 +109,8 @@
// Creates a new data writer with the new part number.
std::unique_ptr<DetachedBufferWriter> OpenDataWriter() {
return std::make_unique<DetachedBufferWriter>(
- absl::StrCat(base_name_, ".part", part_number_, ".bfbs"));
+ absl::StrCat(base_name_, ".part", part_number_, ".bfbs"),
+ std::make_unique<aos::logger::DummyEncoder>());
}
const std::string base_name_;
@@ -122,6 +124,40 @@
public:
MultiNodeLogNamer(std::string_view base_name,
const Configuration *configuration, const Node *node);
+ ~MultiNodeLogNamer() override;
+
+ std::string_view base_name() const { return base_name_; }
+
+ // If temp_suffix is set, then this will write files under names beginning
+ // with the specified suffix, and then rename them to the desired name after
+ // they are fully written.
+ //
+ // This is useful to enable incremental copying of the log files.
+ //
+ // Defaults to writing directly to the final filename.
+ void set_temp_suffix(std::string_view temp_suffix) {
+ temp_suffix_ = temp_suffix;
+ }
+
+ // Sets the function for creating encoders.
+ //
+ // Defaults to just creating DummyEncoders.
+ void set_encoder_factory(
+ std::function<std::unique_ptr<DetachedBufferEncoder>()> encoder_factory) {
+ encoder_factory_ = std::move(encoder_factory);
+ }
+
+ // Sets an additional file extension.
+ //
+ // Defaults to nothing.
+ void set_extension(std::string_view extension) { extension_ = extension; }
+
+ // A list of all the filenames we've written.
+ //
+ // This only includes the part after base_name().
+ const std::vector<std::string> &all_filenames() const {
+ return all_filenames_;
+ }
void WriteHeader(
aos::SizePrefixedFlatbufferDetachedBuffer<LogFileHeader> *header,
@@ -133,11 +169,105 @@
DetachedBufferWriter *MakeWriter(const Channel *channel) override;
- DetachedBufferWriter *MakeForwardedTimestampWriter(
- const Channel *channel, const Node *node) override;
+ DetachedBufferWriter *MakeForwardedTimestampWriter(const Channel *channel,
+ const Node *node) override;
DetachedBufferWriter *MakeTimestampWriter(const Channel *channel) override;
+ // Indicates that at least one file ran out of space. Once this happens, we
+ // stop trying to open new files, to avoid writing any files with holes from
+ // previous parts.
+ //
+ // Besides this function, this object will silently stop logging data when
+ // this occurs. If you want to ensure log files are complete, you must call
+ // this method.
+ bool ran_out_of_space() const {
+ return accumulate_data_writers<bool>(
+ ran_out_of_space_, [](bool x, const DataWriter &data_writer) {
+ return x ||
+ (data_writer.writer && data_writer.writer->ran_out_of_space());
+ });
+ }
+
+ // Returns the maximum total_bytes() value for all existing
+ // DetachedBufferWriters.
+ //
+ // Returns 0 if no files are open.
+ size_t maximum_total_bytes() const {
+ return accumulate_data_writers<size_t>(
+ 0, [](size_t x, const DataWriter &data_writer) {
+ return std::max(x, data_writer.writer->total_bytes());
+ });
+ }
+
+ // Closes all existing log files. No more data may be written after this.
+ //
+ // This may set ran_out_of_space().
+ void Close();
+
+ // Accessors for various statistics. See the identically-named methods in
+ // DetachedBufferWriter for documentation. These are aggregated across all
+ // past and present DetachedBufferWriters.
+ std::chrono::nanoseconds max_write_time() const {
+ return accumulate_data_writers(
+ max_write_time_,
+ [](std::chrono::nanoseconds x, const DataWriter &data_writer) {
+ return std::max(x, data_writer.writer->max_write_time());
+ });
+ }
+ int max_write_time_bytes() const {
+ return std::get<0>(accumulate_data_writers(
+ std::make_tuple(max_write_time_bytes_, max_write_time_),
+ [](std::tuple<int, std::chrono::nanoseconds> x,
+ const DataWriter &data_writer) {
+ if (data_writer.writer->max_write_time() > std::get<1>(x)) {
+ return std::make_tuple(data_writer.writer->max_write_time_bytes(),
+ data_writer.writer->max_write_time());
+ }
+ return x;
+ }));
+ }
+ int max_write_time_messages() const {
+ return std::get<0>(accumulate_data_writers(
+ std::make_tuple(max_write_time_messages_, max_write_time_),
+ [](std::tuple<int, std::chrono::nanoseconds> x,
+ const DataWriter &data_writer) {
+ if (data_writer.writer->max_write_time() > std::get<1>(x)) {
+ return std::make_tuple(
+ data_writer.writer->max_write_time_messages(),
+ data_writer.writer->max_write_time());
+ }
+ return x;
+ }));
+ }
+ std::chrono::nanoseconds total_write_time() const {
+ return accumulate_data_writers(
+ total_write_time_,
+ [](std::chrono::nanoseconds x, const DataWriter &data_writer) {
+ return x + data_writer.writer->total_write_time();
+ });
+ }
+ int total_write_count() const {
+ return accumulate_data_writers(
+ total_write_count_, [](int x, const DataWriter &data_writer) {
+ return x + data_writer.writer->total_write_count();
+ });
+ }
+ int total_write_messages() const {
+ return accumulate_data_writers(
+ total_write_messages_, [](int x, const DataWriter &data_writer) {
+ return x + data_writer.writer->total_write_messages();
+ });
+ }
+ int total_write_bytes() const {
+ return accumulate_data_writers(
+ total_write_bytes_, [](int x, const DataWriter &data_writer) {
+ return x + data_writer.writer->total_write_bytes();
+ });
+ }
+
+ void ResetStatistics();
+
private:
// Files to write remote data to. We want one per channel. Maps the channel
// to the writer, Node, and part number.
@@ -145,7 +275,7 @@
std::unique_ptr<DetachedBufferWriter> writer = nullptr;
const Node *node;
size_t part_number = 0;
- UUID uuid = UUID::Random();
+ const UUID uuid = UUID::Random();
std::function<void(const Channel *, DataWriter *)> rotate;
};
@@ -157,16 +287,50 @@
void OpenWriter(const Channel *channel, DataWriter *data_writer);
// Opens the main data writer file for this node responsible for data_writer_.
- std::unique_ptr<DetachedBufferWriter> OpenDataWriter();
+ void OpenDataWriter();
+
+ void CreateBufferWriter(std::string_view path,
+ std::unique_ptr<DetachedBufferWriter> *destination);
+
+ void RenameTempFile(DetachedBufferWriter *destination);
+
+ void CloseWriter(std::unique_ptr<DetachedBufferWriter> *writer_pointer);
+
+ // A version of std::accumulate which operates over all of our DataWriters.
+ template <typename T, typename BinaryOperation>
+ T accumulate_data_writers(T t, BinaryOperation op) const {
+ for (const std::pair<const Channel *const, DataWriter> &data_writer :
+ data_writers_) {
+ t = op(std::move(t), data_writer.second);
+ }
+ if (data_writer_.writer) {
+ t = op(std::move(t), data_writer_);
+ }
+ return t;
+ }
const std::string base_name_;
const Configuration *const configuration_;
- const UUID uuid_;
- size_t part_number_ = 0;
+ bool ran_out_of_space_ = false;
+ std::vector<std::string> all_filenames_;
+
+ std::string temp_suffix_;
+ std::function<std::unique_ptr<DetachedBufferEncoder>()> encoder_factory_ =
+ []() { return std::make_unique<DummyEncoder>(); };
+ std::string extension_;
+
+ // Storage for statistics from previously-rotated DetachedBufferWriters.
+ std::chrono::nanoseconds max_write_time_ = std::chrono::nanoseconds::zero();
+ int max_write_time_bytes_ = -1;
+ int max_write_time_messages_ = -1;
+ std::chrono::nanoseconds total_write_time_ = std::chrono::nanoseconds::zero();
+ int total_write_count_ = 0;
+ int total_write_messages_ = 0;
+ int total_write_bytes_ = 0;
// File to write both delivery timestamps and local data to.
- std::unique_ptr<DetachedBufferWriter> data_writer_;
+ DataWriter data_writer_;
std::map<const Channel *, DataWriter> data_writers_;
};
diff --git a/aos/events/logging/logfile_sorting.cc b/aos/events/logging/logfile_sorting.cc
new file mode 100644
index 0000000..d3f7f38
--- /dev/null
+++ b/aos/events/logging/logfile_sorting.cc
@@ -0,0 +1,281 @@
+#include "aos/events/logging/logfile_sorting.h"
+
+#include <algorithm>
+#include <map>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "aos/events/logging/logfile_utils.h"
+#include "aos/flatbuffers.h"
+#include "aos/time/time.h"
+
+namespace aos {
+namespace logger {
+namespace chrono = std::chrono;
+
+std::vector<LogFile> SortParts(const std::vector<std::string> &parts) {
+ // Start by grouping all parts by UUID, and extracting the part index.
+ // Datastructure to hold all the info extracted from a set of parts which go
+ // together so we can sort them afterwords.
+ struct UnsortedLogParts {
+ // Start times.
+ aos::monotonic_clock::time_point monotonic_start_time;
+ aos::realtime_clock::time_point realtime_start_time;
+
+ // Node to save.
+ std::string node;
+
+ // Pairs of the filename and the part index for sorting.
+ std::vector<std::pair<std::string, int>> parts;
+ };
+
+ // Struct to hold both the node, and the parts associated with it.
+ struct UnsortedLogPartsMap {
+ std::string logger_node;
+ aos::monotonic_clock::time_point monotonic_start_time =
+ aos::monotonic_clock::min_time;
+ aos::realtime_clock::time_point realtime_start_time =
+ aos::realtime_clock::min_time;
+
+ std::map<std::string, UnsortedLogParts> unsorted_parts;
+ };
+
+ // Map holding the log_event_uuid -> second map. The second map holds the
+ // parts_uuid -> list of parts for sorting.
+ std::map<std::string, UnsortedLogPartsMap> parts_list;
+
+ // Sort part files without UUIDs and part indexes as well. Extract everything
+ // useful from the log in the first pass, then sort later.
+ struct UnsortedOldParts {
+ // Part information with everything but the list of parts.
+ LogParts parts;
+
+ // Tuple of time for the data and filename needed for sorting after
+ // extracting.
+ std::vector<std::pair<monotonic_clock::time_point, std::string>>
+ unsorted_parts;
+ };
+
+ // A list of all the old parts which we don't know how to sort using uuids.
+ // There are enough of these in the wild that this is worth supporting.
+ std::vector<UnsortedOldParts> old_parts;
+
+ // Now extract everything into our datastructures above for sorting.
+ for (const std::string &part : parts) {
+ FlatbufferVector<LogFileHeader> log_header = ReadHeader(part);
+
+ const monotonic_clock::time_point monotonic_start_time(
+ chrono::nanoseconds(log_header.message().monotonic_start_time()));
+ const realtime_clock::time_point realtime_start_time(
+ chrono::nanoseconds(log_header.message().realtime_start_time()));
+
+ const std::string_view node =
+ log_header.message().has_node()
+ ? log_header.message().node()->name()->string_view()
+ : "";
+
+ const std::string_view logger_node =
+ log_header.message().has_logger_node()
+ ? log_header.message().logger_node()->name()->string_view()
+ : "";
+
+ // Looks like an old log. No UUID, index, and also single node. We have
+ // little to no multi-node log files in the wild without part UUIDs and
+ // indexes which we care much about.
+ if (!log_header.message().has_parts_uuid() &&
+ !log_header.message().has_parts_index() &&
+ !log_header.message().has_node()) {
+ FlatbufferVector<MessageHeader> first_message = ReadNthMessage(part, 0);
+ const monotonic_clock::time_point first_message_time(
+ chrono::nanoseconds(first_message.message().monotonic_sent_time()));
+
+ // Find anything with a matching start time. They all go together.
+ auto result = std::find_if(
+ old_parts.begin(), old_parts.end(),
+ [&](const UnsortedOldParts &parts) {
+ return parts.parts.monotonic_start_time == monotonic_start_time &&
+ parts.parts.realtime_start_time == realtime_start_time;
+ });
+
+ if (result == old_parts.end()) {
+ old_parts.emplace_back();
+ old_parts.back().parts.monotonic_start_time = monotonic_start_time;
+ old_parts.back().parts.realtime_start_time = realtime_start_time;
+ old_parts.back().unsorted_parts.emplace_back(
+ std::make_pair(first_message_time, part));
+ } else {
+ result->unsorted_parts.emplace_back(
+ std::make_pair(first_message_time, part));
+ }
+ continue;
+ }
+
+ CHECK(log_header.message().has_log_event_uuid());
+ CHECK(log_header.message().has_parts_uuid());
+ CHECK(log_header.message().has_parts_index());
+
+ CHECK_EQ(log_header.message().has_logger_node(),
+ log_header.message().has_node());
+
+ const std::string log_event_uuid =
+ log_header.message().log_event_uuid()->str();
+ const std::string parts_uuid = log_header.message().parts_uuid()->str();
+ int32_t parts_index = log_header.message().parts_index();
+
+ auto log_it = parts_list.find(log_event_uuid);
+ if (log_it == parts_list.end()) {
+ log_it =
+ parts_list
+ .insert(std::make_pair(log_event_uuid, UnsortedLogPartsMap()))
+ .first;
+ log_it->second.logger_node = logger_node;
+ } else {
+ CHECK_EQ(log_it->second.logger_node, logger_node);
+ }
+
+ if (node == log_it->second.logger_node) {
+ if (log_it->second.monotonic_start_time ==
+ aos::monotonic_clock::min_time) {
+ log_it->second.monotonic_start_time = monotonic_start_time;
+ log_it->second.realtime_start_time = realtime_start_time;
+ } else {
+ CHECK_EQ(log_it->second.monotonic_start_time, monotonic_start_time);
+ CHECK_EQ(log_it->second.realtime_start_time, realtime_start_time);
+ }
+ }
+
+ auto it = log_it->second.unsorted_parts.find(parts_uuid);
+ if (it == log_it->second.unsorted_parts.end()) {
+ it = log_it->second.unsorted_parts
+ .insert(std::make_pair(parts_uuid, UnsortedLogParts()))
+ .first;
+ it->second.monotonic_start_time = monotonic_start_time;
+ it->second.realtime_start_time = realtime_start_time;
+ it->second.node = std::string(node);
+ }
+
+ // First part might be min_time. If it is, try to put a better time on it.
+ if (it->second.monotonic_start_time == monotonic_clock::min_time) {
+ it->second.monotonic_start_time = monotonic_start_time;
+ } else if (monotonic_start_time != monotonic_clock::min_time) {
+ CHECK_EQ(it->second.monotonic_start_time, monotonic_start_time);
+ }
+ if (it->second.realtime_start_time == realtime_clock::min_time) {
+ it->second.realtime_start_time = realtime_start_time;
+ } else if (realtime_start_time != realtime_clock::min_time) {
+ CHECK_EQ(it->second.realtime_start_time, realtime_start_time);
+ }
+
+ it->second.parts.emplace_back(std::make_pair(part, parts_index));
+ }
+
+ CHECK_NE(old_parts.empty(), parts_list.empty())
+ << ": Can't have a mix of old and new parts.";
+
+ // Now reformat old_parts to be in the right datastructure to report.
+ if (!old_parts.empty()) {
+ std::vector<LogFile> result;
+ for (UnsortedOldParts &p : old_parts) {
+ // Sort by the oldest message in each file.
+ std::sort(
+ p.unsorted_parts.begin(), p.unsorted_parts.end(),
+ [](const std::pair<monotonic_clock::time_point, std::string> &a,
+ const std::pair<monotonic_clock::time_point, std::string> &b) {
+ return a.first < b.first;
+ });
+ LogFile log_file;
+ for (std::pair<monotonic_clock::time_point, std::string> &f :
+ p.unsorted_parts) {
+ p.parts.parts.emplace_back(std::move(f.second));
+ }
+ log_file.parts.emplace_back(std::move(p.parts));
+ log_file.monotonic_start_time = log_file.parts[0].monotonic_start_time;
+ log_file.realtime_start_time = log_file.parts[0].realtime_start_time;
+ result.emplace_back(std::move(log_file));
+ }
+
+ return result;
+ }
+
+ // Now, sort them and produce the final vector form.
+ std::vector<LogFile> result;
+ result.reserve(parts_list.size());
+ for (std::pair<const std::string, UnsortedLogPartsMap> &logs : parts_list) {
+ LogFile new_file;
+ new_file.log_event_uuid = logs.first;
+ new_file.logger_node = logs.second.logger_node;
+ new_file.monotonic_start_time = logs.second.monotonic_start_time;
+ new_file.realtime_start_time = logs.second.realtime_start_time;
+ for (std::pair<const std::string, UnsortedLogParts> &parts :
+ logs.second.unsorted_parts) {
+ LogParts new_parts;
+ new_parts.monotonic_start_time = parts.second.monotonic_start_time;
+ new_parts.realtime_start_time = parts.second.realtime_start_time;
+ new_parts.log_event_uuid = logs.first;
+ new_parts.parts_uuid = parts.first;
+ new_parts.node = std::move(parts.second.node);
+
+ std::sort(parts.second.parts.begin(), parts.second.parts.end(),
+ [](const std::pair<std::string, int> &a,
+ const std::pair<std::string, int> &b) {
+ return a.second < b.second;
+ });
+ new_parts.parts.reserve(parts.second.parts.size());
+ for (std::pair<std::string, int> &p : parts.second.parts) {
+ new_parts.parts.emplace_back(std::move(p.first));
+ }
+ new_file.parts.emplace_back(std::move(new_parts));
+ }
+ result.emplace_back(std::move(new_file));
+ }
+ return result;
+}
+
+std::ostream &operator<<(std::ostream &stream, const LogFile &file) {
+ stream << "{";
+ if (!file.log_event_uuid.empty()) {
+ stream << "\"log_event_uuid\": \"" << file.log_event_uuid << "\", ";
+ }
+ if (!file.logger_node.empty()) {
+ stream << "\"logger_node\": \"" << file.logger_node << "\", ";
+ }
+ stream << "\"monotonic_start_time\": " << file.monotonic_start_time
+ << ", \"realtime_start_time\": " << file.realtime_start_time << ", [";
+ stream << "\"parts\": [";
+ for (size_t i = 0; i < file.parts.size(); ++i) {
+ if (i != 0u) {
+ stream << ", ";
+ }
+ stream << file.parts[i];
+ }
+ stream << "]}";
+ return stream;
+}
+std::ostream &operator<<(std::ostream &stream, const LogParts &parts) {
+ stream << "{";
+ if (!parts.log_event_uuid.empty()) {
+ stream << "\"log_event_uuid\": \"" << parts.log_event_uuid << "\", ";
+ }
+ if (!parts.parts_uuid.empty()) {
+ stream << "\"parts_uuid\": \"" << parts.parts_uuid << "\", ";
+ }
+ if (!parts.node.empty()) {
+ stream << "\"node\": \"" << parts.node << "\", ";
+ }
+ stream << "\"monotonic_start_time\": " << parts.monotonic_start_time
+ << ", \"realtime_start_time\": " << parts.realtime_start_time << ", [";
+
+ for (size_t i = 0; i < parts.parts.size(); ++i) {
+ if (i != 0u) {
+ stream << ", ";
+ }
+ stream << parts.parts[i];
+ }
+
+ stream << "]}";
+ return stream;
+}
+
+} // namespace logger
+} // namespace aos
diff --git a/aos/events/logging/logfile_sorting.h b/aos/events/logging/logfile_sorting.h
new file mode 100644
index 0000000..50bfbd7
--- /dev/null
+++ b/aos/events/logging/logfile_sorting.h
@@ -0,0 +1,59 @@
+#ifndef AOS_EVENTS_LOGGING_LOGFILE_SORTING_H_
+#define AOS_EVENTS_LOGGING_LOGFILE_SORTING_H_
+
+#include <iostream>
+#include <vector>
+#include <string>
+
+#include "aos/events/logging/uuid.h"
+#include "aos/time/time.h"
+
+namespace aos {
+namespace logger {
+
+// Datastructure to hold ordered parts.
+struct LogParts {
+ // Monotonic and realtime start times for this set of log files. For log
+ // files which started out unknown and then became known, this is the known
+ // start time.
+ aos::monotonic_clock::time_point monotonic_start_time;
+ aos::realtime_clock::time_point realtime_start_time;
+
+ // UUIDs if available.
+ std::string log_event_uuid;
+ std::string parts_uuid;
+
+ // The node this represents, or empty if we are in a single node world.
+ std::string node;
+
+ // Pre-sorted list of parts.
+ std::vector<std::string> parts;
+};
+
+// Datastructure to hold parts from the same run of the logger which have no
+// ordering constraints relative to each other.
+struct LogFile {
+ // The UUID tying them all together (if available)
+ std::string log_event_uuid;
+
+ // The node the logger was running on (if available)
+ std::string logger_node;
+
+ // The start time on the logger node.
+ aos::monotonic_clock::time_point monotonic_start_time;
+ aos::realtime_clock::time_point realtime_start_time;
+
+ // All the parts, unsorted.
+ std::vector<LogParts> parts;
+};
+
+std::ostream &operator<<(std::ostream &stream, const LogFile &file);
+std::ostream &operator<<(std::ostream &stream, const LogParts &parts);
+
+// Takes a bunch of parts and sorts them based on part_uuid and part_index.
+std::vector<LogFile> SortParts(const std::vector<std::string> &parts);
+
+} // namespace logger
+} // namespace aos
+
+#endif // AOS_EVENTS_LOGGING_LOGFILE_SORTING_H_
diff --git a/aos/events/logging/logfile_utils.cc b/aos/events/logging/logfile_utils.cc
index 0403423..908e5e1 100644
--- a/aos/events/logging/logfile_utils.cc
+++ b/aos/events/logging/logfile_utils.cc
@@ -1,47 +1,66 @@
#include "aos/events/logging/logfile_utils.h"
#include <fcntl.h>
-#include <limits.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/uio.h>
-#include <vector>
+#include <algorithm>
+#include <climits>
#include "absl/strings/escaping.h"
#include "aos/configuration.h"
-#include "aos/events/logging/logger_generated.h"
#include "aos/flatbuffer_merge.h"
#include "aos/util/file.h"
#include "flatbuffers/flatbuffers.h"
#include "gflags/gflags.h"
#include "glog/logging.h"
-DEFINE_int32(flush_size, 1000000,
+#if defined(__x86_64__)
+#define ENABLE_LZMA 1
+#elif defined(__aarch64__)
+#define ENABLE_LZMA 1
+#else
+#define ENABLE_LZMA 0
+#endif
+
+#if ENABLE_LZMA
+#include "aos/events/logging/lzma_encoder.h"
+#endif
+
+DEFINE_int32(flush_size, 128000,
"Number of outstanding bytes to allow before flushing to disk.");
-namespace aos {
-namespace logger {
+namespace aos::logger {
namespace chrono = std::chrono;
-DetachedBufferWriter::DetachedBufferWriter(std::string_view filename)
- : filename_(filename) {
- util::MkdirP(filename, 0777);
- fd_ = open(std::string(filename).c_str(),
- O_RDWR | O_CLOEXEC | O_CREAT | O_EXCL, 0774);
- VLOG(1) << "Opened " << filename << " for writing";
- PCHECK(fd_ != -1) << ": Failed to open " << filename << " for writing";
+DetachedBufferWriter::DetachedBufferWriter(
+ std::string_view filename, std::unique_ptr<DetachedBufferEncoder> encoder)
+ : filename_(filename), encoder_(std::move(encoder)) {
+ if (!util::MkdirPIfSpace(filename, 0777)) {
+ ran_out_of_space_ = true;
+ } else {
+ fd_ = open(std::string(filename).c_str(),
+ O_RDWR | O_CLOEXEC | O_CREAT | O_EXCL, 0774);
+ if (fd_ == -1 && errno == ENOSPC) {
+ ran_out_of_space_ = true;
+ } else {
+ PCHECK(fd_ != -1) << ": Failed to open " << filename << " for writing";
+ VLOG(1) << "Opened " << filename << " for writing";
+ }
+ }
}
DetachedBufferWriter::~DetachedBufferWriter() {
- Flush();
- PLOG_IF(ERROR, close(fd_) == -1) << " Failed to close logfile";
- VLOG(1) << "Closed " << filename_;
+ Close();
+ if (ran_out_of_space_) {
+ CHECK(acknowledge_ran_out_of_space_)
+ << ": Unacknowledged out of disk space, log file was not completed";
+ }
}
-DetachedBufferWriter::DetachedBufferWriter(
- DetachedBufferWriter &&other) {
+DetachedBufferWriter::DetachedBufferWriter(DetachedBufferWriter &&other) {
*this = std::move(other);
}
@@ -51,77 +70,144 @@
DetachedBufferWriter &DetachedBufferWriter::operator=(
DetachedBufferWriter &&other) {
std::swap(filename_, other.filename_);
+ std::swap(encoder_, other.encoder_);
std::swap(fd_, other.fd_);
- std::swap(queued_size_, other.queued_size_);
- std::swap(written_size_, other.written_size_);
- std::swap(queue_, other.queue_);
+ std::swap(ran_out_of_space_, other.ran_out_of_space_);
+ std::swap(acknowledge_ran_out_of_space_, other.acknowledge_ran_out_of_space_);
std::swap(iovec_, other.iovec_);
+ std::swap(max_write_time_, other.max_write_time_);
+ std::swap(max_write_time_bytes_, other.max_write_time_bytes_);
+ std::swap(max_write_time_messages_, other.max_write_time_messages_);
+ std::swap(total_write_time_, other.total_write_time_);
+ std::swap(total_write_count_, other.total_write_count_);
+ std::swap(total_write_messages_, other.total_write_messages_);
+ std::swap(total_write_bytes_, other.total_write_bytes_);
return *this;
}
-void DetachedBufferWriter::QueueSizedFlatbuffer(
- flatbuffers::FlatBufferBuilder *fbb) {
- QueueSizedFlatbuffer(fbb->Release());
+void DetachedBufferWriter::QueueSpan(absl::Span<const uint8_t> span) {
+ if (ran_out_of_space_) {
+ // We don't want any later data to be written after space becomes
+ // available, so refuse to write anything more once we've dropped data
+ // because we ran out of space.
+ VLOG(1) << "Ignoring span: " << span.size();
+ return;
+ }
+
+ if (encoder_->may_bypass() && span.size() > 4096u) {
+ // Over this threshold, we'll assume it's cheaper to add an extra
+ // syscall to write the data immediately instead of copying it to
+ // enqueue.
+
+ // First, flush everything.
+ while (encoder_->queue_size() > 0u) {
+ Flush();
+ }
+
+ // Then, write it directly.
+ const auto start = aos::monotonic_clock::now();
+ const ssize_t written = write(fd_, span.data(), span.size());
+ const auto end = aos::monotonic_clock::now();
+ HandleWriteReturn(written, span.size());
+ UpdateStatsForWrite(end - start, written, 1);
+ } else {
+ encoder_->Encode(CopySpanAsDetachedBuffer(span));
+ }
+
+ FlushAtThreshold();
}
-void DetachedBufferWriter::WriteSizedFlatbuffer(
- absl::Span<const uint8_t> span) {
- // Cheat aggressively... Write out the queued up data, and then write this
- // data once without buffering. It is hard to make a DetachedBuffer out of
- // this data, and we don't want to worry about lifetimes.
- Flush();
- iovec_.clear();
- iovec_.reserve(1);
-
- struct iovec n;
- n.iov_base = const_cast<uint8_t *>(span.data());
- n.iov_len = span.size();
- iovec_.emplace_back(n);
-
- const ssize_t written = writev(fd_, iovec_.data(), iovec_.size());
-
- PCHECK(written == static_cast<ssize_t>(n.iov_len))
- << ": Wrote " << written << " expected " << n.iov_len;
- written_size_ += written;
-}
-
-void DetachedBufferWriter::QueueSizedFlatbuffer(
- flatbuffers::DetachedBuffer &&buffer) {
- queued_size_ += buffer.size();
- queue_.emplace_back(std::move(buffer));
-
- // Flush if we are at the max number of iovs per writev, or have written
- // enough data. Otherwise writev will fail with an invalid argument.
- if (queued_size_ > static_cast<size_t>(FLAGS_flush_size) ||
- queue_.size() == IOV_MAX) {
+void DetachedBufferWriter::Close() {
+ if (fd_ == -1) {
+ return;
+ }
+ encoder_->Finish();
+ while (encoder_->queue_size() > 0) {
Flush();
}
+ if (close(fd_) == -1) {
+ if (errno == ENOSPC) {
+ ran_out_of_space_ = true;
+ } else {
+ PLOG(ERROR) << "Closing log file failed";
+ }
+ }
+ fd_ = -1;
+ VLOG(1) << "Closed " << filename_;
}
void DetachedBufferWriter::Flush() {
- if (queue_.size() == 0u) {
+ const auto queue = encoder_->queue();
+ if (queue.empty()) {
return;
}
- iovec_.clear();
- iovec_.reserve(queue_.size());
- size_t counted_size = 0;
- for (size_t i = 0; i < queue_.size(); ++i) {
- struct iovec n;
- n.iov_base = queue_[i].data();
- n.iov_len = queue_[i].size();
- counted_size += n.iov_len;
- iovec_.emplace_back(std::move(n));
+ if (ran_out_of_space_) {
+ // We don't want any later data to be written after space becomes available,
+ // so refuse to write anything more once we've dropped data because we ran
+ // out of space.
+ VLOG(1) << "Ignoring queue: " << queue.size();
+ encoder_->Clear(queue.size());
+ return;
}
- CHECK_EQ(counted_size, queued_size_);
+
+ iovec_.clear();
+ const size_t iovec_size = std::min<size_t>(queue.size(), IOV_MAX);
+ iovec_.resize(iovec_size);
+ size_t counted_size = 0;
+ for (size_t i = 0; i < iovec_size; ++i) {
+ iovec_[i].iov_base = const_cast<uint8_t *>(queue[i].data());
+ iovec_[i].iov_len = queue[i].size();
+ counted_size += iovec_[i].iov_len;
+ }
+
+ const auto start = aos::monotonic_clock::now();
const ssize_t written = writev(fd_, iovec_.data(), iovec_.size());
+ const auto end = aos::monotonic_clock::now();
+ HandleWriteReturn(written, counted_size);
- PCHECK(written == static_cast<ssize_t>(queued_size_))
- << ": Wrote " << written << " expected " << queued_size_;
- written_size_ += written;
+ encoder_->Clear(iovec_size);
- queued_size_ = 0;
- queue_.clear();
- // TODO(austin): Handle partial writes in some way other than crashing...
+ UpdateStatsForWrite(end - start, written, iovec_size);
+}
+
+void DetachedBufferWriter::HandleWriteReturn(ssize_t write_return,
+ size_t write_size) {
+ if (write_return == -1 && errno == ENOSPC) {
+ ran_out_of_space_ = true;
+ return;
+ }
+ PCHECK(write_return >= 0) << ": write failed";
+ if (write_return < static_cast<ssize_t>(write_size)) {
+ // Sometimes this happens instead of ENOSPC. On a real filesystem, this
+ // never seems to happen in any other case. If we ever want to log to a
+ // socket, this will happen more often. However, until we get there, we'll
+ // just assume it means we ran out of space.
+ ran_out_of_space_ = true;
+ return;
+ }
+}
+
+void DetachedBufferWriter::UpdateStatsForWrite(
+ aos::monotonic_clock::duration duration, ssize_t written, int iovec_size) {
+ if (duration > max_write_time_) {
+ max_write_time_ = duration;
+ max_write_time_bytes_ = written;
+ max_write_time_messages_ = iovec_size;
+ }
+ total_write_time_ += duration;
+ ++total_write_count_;
+ total_write_messages_ += iovec_size;
+ total_write_bytes_ += written;
+}
+
+void DetachedBufferWriter::FlushAtThreshold() {
+ // Flush if we are at the max number of iovs per writev, because there's no
+ // point queueing up any more data in memory. Also flush once we have enough
+ // data queued up.
+ while (encoder_->queued_bytes() > static_cast<size_t>(FLAGS_flush_size) ||
+ encoder_->queue_size() >= IOV_MAX) {
+ Flush();
+ }
}
flatbuffers::Offset<MessageHeader> PackMessage(
@@ -186,10 +272,17 @@
return message_header_builder.Finish();
}
-SpanReader::SpanReader(std::string_view filename)
- : filename_(filename),
- fd_(open(std::string(filename).c_str(), O_RDONLY | O_CLOEXEC)) {
- PCHECK(fd_ != -1) << ": Failed to open " << filename;
+SpanReader::SpanReader(std::string_view filename) : filename_(filename) {
+ static const std::string_view kXz = ".xz";
+ if (filename.substr(filename.size() - kXz.size()) == kXz) {
+#if ENABLE_LZMA
+ decoder_ = std::make_unique<LzmaDecoder>(filename);
+#else
+ LOG(FATAL) << "Reading xz-compressed files not supported on this platform";
+#endif
+ } else {
+ decoder_ = std::make_unique<DummyDecoder>(filename);
+ }
}
absl::Span<const uint8_t> SpanReader::ReadMessage() {
@@ -227,36 +320,14 @@
return absl::Span<const uint8_t>(data_ptr, data_size);
}
-bool SpanReader::MessageAvailable() {
- // Are we big enough to read the size?
- if (data_.size() - consumed_data_ < sizeof(flatbuffers::uoffset_t)) {
- return false;
- }
-
- // Then, are we big enough to read the full message?
- const size_t data_size =
- flatbuffers::GetPrefixedSize(data_.data() + consumed_data_) +
- sizeof(flatbuffers::uoffset_t);
- if (data_.size() < consumed_data_ + data_size) {
- return false;
- }
-
- return true;
-}
-
bool SpanReader::ReadBlock() {
- if (end_of_file_) {
- return false;
- }
-
- // Appends 256k. This is enough that the read call is efficient. We don't
- // want to spend too much time reading small chunks because the syscalls for
- // that will be expensive.
+ // This is the amount of data we grab at a time. Doing larger chunks minimizes
+ // syscalls and helps decompressors batch things more efficiently.
constexpr size_t kReadSize = 256 * 1024;
// Strip off any unused data at the front.
if (consumed_data_ != 0) {
- data_.erase(data_.begin(), data_.begin() + consumed_data_);
+ data_.erase_front(consumed_data_);
consumed_data_ = 0;
}
@@ -265,15 +336,14 @@
// This should automatically grow the backing store. It won't shrink if we
// get a small chunk later. This reduces allocations when we want to append
// more data.
- data_.resize(data_.size() + kReadSize);
+ data_.resize(starting_size + kReadSize);
- ssize_t count = read(fd_, &data_[starting_size], kReadSize);
- data_.resize(starting_size + std::max(count, static_cast<ssize_t>(0)));
+ const size_t count =
+ decoder_->Read(data_.begin() + starting_size, data_.end());
+ data_.resize(starting_size + count);
if (count == 0) {
- end_of_file_ = true;
return false;
}
- PCHECK(count > 0);
return true;
}
@@ -287,8 +357,10 @@
<< ": Failed to read header from: " << filename;
// And copy the config so we have it forever, removing the size prefix.
- std::vector<uint8_t> data(
- config_data.begin() + sizeof(flatbuffers::uoffset_t), config_data.end());
+ ResizeableBuffer data;
+ data.resize(config_data.size() - sizeof(flatbuffers::uoffset_t));
+ memcpy(data.data(), config_data.begin() + sizeof(flatbuffers::uoffset_t),
+ data.size());
return FlatbufferVector<LogFileHeader>(std::move(data));
}
@@ -304,9 +376,11 @@
<< ": Failed to read data from: " << filename;
}
- // And copy the data so we have it forever.
- std::vector<uint8_t> data(data_span.begin() + sizeof(flatbuffers::uoffset_t),
- data_span.end());
+ // And copy the config so we have it forever, removing the size prefix.
+ ResizeableBuffer data;
+ data.resize(data_span.size() - sizeof(flatbuffers::uoffset_t));
+ memcpy(data.data(), data_span.begin() + sizeof(flatbuffers::uoffset_t),
+ data.size());
return FlatbufferVector<MessageHeader>(std::move(data));
}
@@ -321,8 +395,11 @@
<< ": Failed to read header from: " << filename;
// And copy the header data so we have it forever.
- std::vector<uint8_t> header_data_copy(
- header_data.begin() + sizeof(flatbuffers::uoffset_t), header_data.end());
+ ResizeableBuffer header_data_copy;
+ header_data_copy.resize(header_data.size() - sizeof(flatbuffers::uoffset_t));
+ memcpy(header_data_copy.data(),
+ header_data.begin() + sizeof(flatbuffers::uoffset_t),
+ header_data_copy.size());
raw_log_file_header_ =
FlatbufferVector<LogFileHeader>(std::move(header_data_copy));
@@ -339,8 +416,12 @@
return std::nullopt;
}
- FlatbufferVector<MessageHeader> result{std::vector<uint8_t>(
- msg_data.begin() + sizeof(flatbuffers::uoffset_t), msg_data.end())};
+ ResizeableBuffer result_buffer;
+ result_buffer.resize(msg_data.size() - sizeof(flatbuffers::uoffset_t));
+ memcpy(result_buffer.data(),
+ msg_data.begin() + sizeof(flatbuffers::uoffset_t),
+ result_buffer.size());
+ FlatbufferVector<MessageHeader> result(std::move(result_buffer));
const monotonic_clock::time_point timestamp = monotonic_clock::time_point(
chrono::nanoseconds(result.message().monotonic_sent_time()));
@@ -409,10 +490,10 @@
// Now compare that the headers match.
if (!CompareFlatBuffer(message_reader.raw_log_file_header(),
log_file_header_)) {
- if (message_reader.log_file_header()->has_logger_uuid() &&
- log_file_header_.message().has_logger_uuid() &&
- message_reader.log_file_header()->logger_uuid()->string_view() !=
- log_file_header_.message().logger_uuid()->string_view()) {
+ if (message_reader.log_file_header()->has_log_event_uuid() &&
+ log_file_header_.message().has_log_event_uuid() &&
+ message_reader.log_file_header()->log_event_uuid()->string_view() !=
+ log_file_header_.message().log_event_uuid()->string_view()) {
LOG(FATAL) << "Logger UUIDs don't match between log file chunks "
<< filenames_[0] << " and " << filenames_[i]
<< ", this is not supported.";
@@ -532,6 +613,7 @@
// but we then want to find the next message to read. The conservative
// answer is to immediately trigger a second requeue to get things moving.
time_to_queue_ = monotonic_start_time();
+ CHECK_NE(time_to_queue_, monotonic_clock::min_time);
QueueMessages(time_to_queue_);
}
@@ -566,8 +648,8 @@
chrono::nanoseconds(header.monotonic_sent_time()));
if (VLOG_IS_ON(2)) {
- LOG(INFO) << MaybeNodeName(target_node_) << "Queued " << this
- << " " << filename() << " ttq: " << time_to_queue_ << " now "
+ LOG(INFO) << MaybeNodeName(target_node_) << "Queued " << this << " "
+ << filename() << " ttq: " << time_to_queue_ << " now "
<< newest_timestamp() << " start time "
<< monotonic_start_time() << " " << FlatbufferToJson(&header);
} else if (VLOG_IS_ON(1)) {
@@ -1062,6 +1144,8 @@
timestamp.realtime_event_time =
realtime_clock::time_point(chrono::nanoseconds(
std::get<2>(oldest_timestamp).message().realtime_sent_time()));
+ timestamp.queue_index =
+ std::get<2>(oldest_timestamp).message().queue_index();
// Consistency check.
CHECK_EQ(timestamp.monotonic_event_time, std::get<0>(oldest_timestamp));
@@ -1164,6 +1248,7 @@
timestamp.realtime_event_time =
realtime_clock::time_point(chrono::nanoseconds(
std::get<2>(oldest_message).message().realtime_sent_time()));
+ timestamp.queue_index = std::get<2>(oldest_message).message().queue_index();
timestamp.remote_queue_index = 0xffffffff;
CHECK_EQ(std::get<0>(oldest_message), timestamp.monotonic_event_time);
@@ -1230,9 +1315,8 @@
(reader->node() != nullptr) && (target_node != nullptr) &&
(reader->node()->has_name() && target_node->has_name());
const bool node_names_identical =
- both_have_name &&
- (reader->node()->name()->string_view() ==
- target_node->name()->string_view());
+ both_have_name && (reader->node()->name()->string_view() ==
+ target_node->name()->string_view());
if (both_null || node_names_identical) {
if (!found_node) {
found_node = true;
@@ -1522,5 +1606,4 @@
return "";
}
-} // namespace logger
-} // namespace aos
+} // namespace aos::logger
diff --git a/aos/events/logging/logfile_utils.h b/aos/events/logging/logfile_utils.h
index 9534860..60bd39c 100644
--- a/aos/events/logging/logfile_utils.h
+++ b/aos/events/logging/logfile_utils.h
@@ -15,12 +15,14 @@
#include <vector>
#include "absl/types/span.h"
+#include "aos/containers/resizeable_buffer.h"
#include "aos/events/event_loop.h"
+#include "aos/events/logging/buffer_encoder.h"
#include "aos/events/logging/logger_generated.h"
+#include "aos/flatbuffers.h"
#include "flatbuffers/flatbuffers.h"
-namespace aos {
-namespace logger {
+namespace aos::logger {
enum class LogType : uint8_t {
// The message originated on this node and should be logged here.
@@ -37,10 +39,17 @@
};
// This class manages efficiently writing a sequence of detached buffers to a
-// file. It queues them up and batches the write operation.
+// file. It encodes them, queues them up, and batches the write operation.
class DetachedBufferWriter {
public:
- DetachedBufferWriter(std::string_view filename);
+ // Marker struct for one of our constructor overloads.
+ struct already_out_of_space_t {};
+
+ DetachedBufferWriter(std::string_view filename,
+ std::unique_ptr<DetachedBufferEncoder> encoder);
+ // Creates a dummy instance which won't even open a file. It will act as if
+ // opening the file ran out of space immediately.
+ DetachedBufferWriter(already_out_of_space_t) : ran_out_of_space_(true) {}
DetachedBufferWriter(DetachedBufferWriter &&other);
DetachedBufferWriter(const DetachedBufferWriter &) = delete;
@@ -51,43 +60,119 @@
std::string_view filename() const { return filename_; }
- // Rewrites a location in a file (relative to the start) to have new data in
- // it. The main use case is updating start times after a log file starts.
- void RewriteLocation(off64_t offset, absl::Span<const uint8_t> data);
+ // This will be true until Close() is called, unless the file couldn't be
+ // created due to running out of space.
+ bool is_open() const { return fd_ != -1; }
- // TODO(austin): Snappy compress the log file if it ends with .snappy!
+ // Queues up a finished FlatBufferBuilder to be encoded and written.
+ //
+ // Triggers a flush if there's enough data queued up.
+ //
+ // Steals the detached buffer from it.
+ void QueueSizedFlatbuffer(flatbuffers::FlatBufferBuilder *fbb) {
+ QueueSizedFlatbuffer(fbb->Release());
+ }
+ // May steal the backing storage of buffer, or may leave it alone.
+ void QueueSizedFlatbuffer(flatbuffers::DetachedBuffer &&buffer) {
+ if (ran_out_of_space_) {
+ return;
+ }
+ encoder_->Encode(std::move(buffer));
+ FlushAtThreshold();
+ }
- // Queues up a finished FlatBufferBuilder to be written. Steals the detached
- // buffer from it.
- void QueueSizedFlatbuffer(flatbuffers::FlatBufferBuilder *fbb);
- // Queues up a detached buffer directly.
- void QueueSizedFlatbuffer(flatbuffers::DetachedBuffer &&buffer);
- // Writes a Span. This is not terribly optimized right now.
- void WriteSizedFlatbuffer(absl::Span<const uint8_t> span);
+ // Queues up data in span. May copy or may write it to disk immediately.
+ void QueueSpan(absl::Span<const uint8_t> span);
- // Triggers data to be provided to the kernel and written.
- void Flush();
+ // Indicates we got ENOSPC when trying to write. After this returns true, no
+ // further data is written.
+ bool ran_out_of_space() const { return ran_out_of_space_; }
- // Returns the number of bytes written.
- size_t written_size() const { return written_size_; }
+ // To avoid silently failing to write logfiles, you must call this before
+ // destruction if ran_out_of_space() is true and the situation has been
+ // handled.
+ void acknowledge_out_of_space() {
+ CHECK(ran_out_of_space_);
+ acknowledge_ran_out_of_space_ = true;
+ }
- // Returns the number of bytes written or currently queued.
- size_t total_size() const { return written_size_ + queued_size_; }
+ // Fully flushes and closes the underlying file now. No additional data may be
+ // enqueued after calling this.
+ //
+ // This will be performed in the destructor automatically.
+ //
+ // Note that this may set ran_out_of_space().
+ void Close();
+
+ // Returns the total number of bytes written and currently queued.
+ size_t total_bytes() const { return encoder_->total_bytes(); }
+
+ // The maximum time for a single write call, or 0 if none have been performed.
+ std::chrono::nanoseconds max_write_time() const { return max_write_time_; }
+ // The number of bytes in the longest write call, or -1 if none have been
+ // performed.
+ int max_write_time_bytes() const { return max_write_time_bytes_; }
+ // The number of buffers in the longest write call, or -1 if none have been
+ // performed.
+ int max_write_time_messages() const { return max_write_time_messages_; }
+ // The total time spent in write calls.
+ std::chrono::nanoseconds total_write_time() const {
+ return total_write_time_;
+ }
+ // The total number of writes which have been performed.
+ int total_write_count() const { return total_write_count_; }
+ // The total number of messages which have been written.
+ int total_write_messages() const { return total_write_messages_; }
+ // The total number of bytes which have been written.
+ int total_write_bytes() const { return total_write_bytes_; }
+ void ResetStatistics() {
+ max_write_time_ = std::chrono::nanoseconds::zero();
+ max_write_time_bytes_ = -1;
+ max_write_time_messages_ = -1;
+ total_write_time_ = std::chrono::nanoseconds::zero();
+ total_write_count_ = 0;
+ total_write_messages_ = 0;
+ total_write_bytes_ = 0;
+ }
private:
+ // Performs a single writev call with as much of the data we have queued up as
+ // possible.
+ //
+ // This will normally take all of the data we have queued up, unless an
+ // encoder has spit out a big enough chunk all at once that we can't manage
+ // all of it.
+ void Flush();
+
+ // write_return is what write(2) or writev(2) returned. write_size is the
+ // number of bytes we expected it to write.
+ void HandleWriteReturn(ssize_t write_return, size_t write_size);
+
+ void UpdateStatsForWrite(aos::monotonic_clock::duration duration,
+ ssize_t written, int iovec_size);
+
+ // Flushes data if we've reached the threshold to do that as part of normal
+ // operation.
+ void FlushAtThreshold();
+
std::string filename_;
+ std::unique_ptr<DetachedBufferEncoder> encoder_;
int fd_ = -1;
+ bool ran_out_of_space_ = false;
+ bool acknowledge_ran_out_of_space_ = false;
- // Size of all the data in the queue.
- size_t queued_size_ = 0;
- size_t written_size_ = 0;
-
- // List of buffers to flush.
- std::vector<flatbuffers::DetachedBuffer> queue_;
// List of iovecs to use with writev. This is a member variable to avoid
// churn.
std::vector<struct iovec> iovec_;
+
+ std::chrono::nanoseconds max_write_time_ = std::chrono::nanoseconds::zero();
+ int max_write_time_bytes_ = -1;
+ int max_write_time_messages_ = -1;
+ std::chrono::nanoseconds total_write_time_ = std::chrono::nanoseconds::zero();
+ int total_write_count_ = 0;
+ int total_write_messages_ = 0;
+ int total_write_bytes_ = 0;
};
// Packes a message pointed to by the context into a MessageHeader.
@@ -104,67 +189,33 @@
public:
SpanReader(std::string_view filename);
- ~SpanReader() { close(fd_); }
-
std::string_view filename() const { return filename_; }
// Returns a span with the data for a message from the log file, excluding
// the size.
absl::Span<const uint8_t> ReadMessage();
- // Returns true if there is a full message available in the buffer, or if we
- // will have to read more data from disk.
- bool MessageAvailable();
-
private:
// TODO(austin): Optimization:
// Allocate the 256k blocks like we do today. But, refcount them with
// shared_ptr pointed to by the messageheader that is returned. This avoids
// the copy. Need to do more benchmarking.
+ // And (Brian): Consider just mmapping the file and handing out refcounted
+ // pointers into that too.
// Reads a chunk of data into data_. Returns false if no data was read.
bool ReadBlock();
const std::string filename_;
- // File descriptor for the log file.
- int fd_ = -1;
+ // File reader and data decoder.
+ std::unique_ptr<DataDecoder> decoder_;
- // Allocator which doesn't zero initialize memory.
- template <typename T>
- struct DefaultInitAllocator {
- typedef T value_type;
-
- template <typename U>
- void construct(U *p) {
- ::new (static_cast<void *>(p)) U;
- }
-
- template <typename U, typename... Args>
- void construct(U *p, Args &&... args) {
- ::new (static_cast<void *>(p)) U(std::forward<Args>(args)...);
- }
-
- T *allocate(std::size_t n) {
- return reinterpret_cast<T *>(::operator new(sizeof(T) * n));
- }
-
- template <typename U>
- void deallocate(U *p, std::size_t /*n*/) {
- ::operator delete(static_cast<void *>(p));
- }
- };
-
- // Vector to read into. This uses an allocator which doesn't zero
- // initialize the memory.
- std::vector<uint8_t, DefaultInitAllocator<uint8_t>> data_;
+ // Vector to read into.
+ ResizeableBuffer data_;
// Amount of data consumed already in data_.
size_t consumed_data_ = 0;
-
- // Cached bit for if we have reached the end of the file. Otherwise we will
- // hammer on the kernel asking for more data each time we send.
- bool end_of_file_ = false;
};
// Class which handles reading the header and messages from the log file. This
@@ -450,6 +501,7 @@
monotonic_clock::time_point monotonic_event_time =
monotonic_clock::min_time;
realtime_clock::time_point realtime_event_time = realtime_clock::min_time;
+ uint32_t queue_index = 0xffffffff;
monotonic_clock::time_point monotonic_remote_time =
monotonic_clock::min_time;
@@ -668,7 +720,6 @@
// a single node.
std::string MaybeNodeName(const Node *);
-} // namespace logger
-} // namespace aos
+} // namespace aos::logger
#endif // AOS_EVENTS_LOGGING_LOGFILE_UTILS_H_
diff --git a/aos/events/logging/logfile_utils_out_of_space_test.sh b/aos/events/logging/logfile_utils_out_of_space_test.sh
new file mode 100755
index 0000000..f412e0d
--- /dev/null
+++ b/aos/events/logging/logfile_utils_out_of_space_test.sh
@@ -0,0 +1,46 @@
+#!/bin/bash
+
+# Tests DetachedBufferWriter's behavior when running out of disk space. We do
+# this by creating a small tmpfs in a new mount namespace, and then running a
+# test binary in that namespace. There's no easy way for a process with user
+# code to run itself in a new mount+user namespace, so we do that outside the
+# test process itself.
+
+# --- begin runfiles.bash initialization v2 ---
+# Copy-pasted from the Bazel Bash runfiles library v2.
+set -uo pipefail; f=bazel_tools/tools/bash/runfiles/runfiles.bash
+source "${RUNFILES_DIR:-/dev/null}/$f" 2>/dev/null || \
+source "$(grep -sm1 "^$f " "${RUNFILES_MANIFEST_FILE:-/dev/null}" | cut -f2- -d' ')" 2>/dev/null || \
+source "$0.runfiles/$f" 2>/dev/null || \
+source "$(grep -sm1 "^$f " "$0.runfiles_manifest" | cut -f2- -d' ')" 2>/dev/null || \
+source "$(grep -sm1 "^$f " "$0.exe.runfiles_manifest" | cut -f2- -d' ')" 2>/dev/null || \
+ { echo>&2 "ERROR: cannot find $f"; exit 1; }; f=; set -e
+# --- end runfiles.bash initialization v2 ---
+
+set -euo pipefail
+
+TMPFS="${TEST_TMPDIR}/small_tmpfs"
+rm -rf "${TMPFS}"
+mkdir "${TMPFS}"
+
+function test {
+ SIZE="$1"
+ echo "Running test with ${SIZE}..." >&2
+ unshare --mount --map-root-user bash <<END
+set -euo pipefail
+
+mount -t tmpfs tmpfs -o size=${SIZE} "${TMPFS}"
+
+exec "$(rlocation org_frc971/aos/events/logging/logfile_utils_out_of_space_test_runner)" -tmpfs "${TMPFS}"
+END
+ echo "Finished test with ${SIZE}" >&2
+}
+
+# Run out of space exactly at the beginning of a block.
+test 81920
+
+# Run out of space 1 byte into a block.
+test 81921
+
+# Run out of space in the middle of a block.
+test 87040
diff --git a/aos/events/logging/logfile_utils_out_of_space_test_runner.cc b/aos/events/logging/logfile_utils_out_of_space_test_runner.cc
new file mode 100644
index 0000000..3010a6f
--- /dev/null
+++ b/aos/events/logging/logfile_utils_out_of_space_test_runner.cc
@@ -0,0 +1,30 @@
+// See :logfile_utils_out_of_space_test for usage and details.
+
+#include <array>
+
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+
+#include "aos/events/logging/logfile_utils.h"
+#include "aos/init.h"
+
+DECLARE_int32(flush_size);
+DEFINE_string(tmpfs, "", "tmpfs with the desired size");
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ FLAGS_flush_size = 1;
+ CHECK(!FLAGS_tmpfs.empty()) << ": Must specify a tmpfs location";
+
+ aos::logger::DetachedBufferWriter writer(
+ FLAGS_tmpfs + "/file", std::make_unique<aos::logger::DummyEncoder>());
+ std::array<uint8_t, 10240> data;
+ data.fill(0);
+ for (int i = 0; i < 8; ++i) {
+ writer.QueueSpan(data);
+ CHECK(!writer.ran_out_of_space()) << ": " << i;
+ }
+ writer.QueueSpan(data);
+ CHECK(writer.ran_out_of_space());
+ writer.acknowledge_out_of_space();
+}
diff --git a/aos/events/logging/logfile_utils_test.cc b/aos/events/logging/logfile_utils_test.cc
new file mode 100644
index 0000000..f6412c3
--- /dev/null
+++ b/aos/events/logging/logfile_utils_test.cc
@@ -0,0 +1,95 @@
+#include "aos/events/logging/logfile_utils.h"
+
+#include <chrono>
+#include <string>
+
+#include "aos/events/logging/test_message_generated.h"
+#include "aos/flatbuffers.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/testing/tmpdir.h"
+#include "gtest/gtest.h"
+
+namespace aos {
+namespace logger {
+namespace testing {
+namespace chrono = std::chrono;
+
+// Creates a size prefixed flatbuffer from json.
+template <typename T>
+SizePrefixedFlatbufferDetachedBuffer<T> JsonToSizedFlatbuffer(
+ const std::string_view data) {
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.ForceDefaults(true);
+ fbb.FinishSizePrefixed(JsonToFlatbuffer<T>(data, &fbb));
+ return fbb.Release();
+}
+
+// Tests that we can write and read 2 flatbuffers to file.
+TEST(SpanReaderTest, ReadWrite) {
+ const std::string logfile = aos::testing::TestTmpDir() + "/log.bfbs";
+ unlink(logfile.c_str());
+
+ const aos::SizePrefixedFlatbufferDetachedBuffer<TestMessage> m1 =
+ JsonToSizedFlatbuffer<TestMessage>(
+ R"({ "value": 1 })");
+ const aos::SizePrefixedFlatbufferDetachedBuffer<TestMessage> m2 =
+ JsonToSizedFlatbuffer<TestMessage>(
+ R"({ "value": 2 })");
+
+ {
+ DetachedBufferWriter writer(logfile, std::make_unique<DummyEncoder>());
+ writer.QueueSpan(m1.full_span());
+ writer.QueueSpan(m2.full_span());
+ }
+
+ SpanReader reader(logfile);
+
+ EXPECT_EQ(reader.filename(), logfile);
+ EXPECT_EQ(reader.ReadMessage(), m1.full_span());
+ EXPECT_EQ(reader.ReadMessage(), m2.full_span());
+ EXPECT_EQ(reader.ReadMessage(), absl::Span<const uint8_t>());
+}
+
+// Tests that we can actually parse the resulting messages at a basic level
+// through MessageReader.
+TEST(MessageReaderTest, ReadWrite) {
+ const std::string logfile = aos::testing::TestTmpDir() + "/log.bfbs";
+ unlink(logfile.c_str());
+
+ const aos::SizePrefixedFlatbufferDetachedBuffer<LogFileHeader> config =
+ JsonToSizedFlatbuffer<LogFileHeader>(
+ R"({ "max_out_of_order_duration": 100000000 })");
+ const aos::SizePrefixedFlatbufferDetachedBuffer<MessageHeader> m1 =
+ JsonToSizedFlatbuffer<MessageHeader>(
+ R"({ "channel_index": 0, "monotonic_sent_time": 1 })");
+ const aos::SizePrefixedFlatbufferDetachedBuffer<MessageHeader> m2 =
+ JsonToSizedFlatbuffer<MessageHeader>(
+ R"({ "channel_index": 0, "monotonic_sent_time": 2 })");
+
+ {
+ DetachedBufferWriter writer(logfile, std::make_unique<DummyEncoder>());
+ writer.QueueSpan(config.full_span());
+ writer.QueueSpan(m1.full_span());
+ writer.QueueSpan(m2.full_span());
+ }
+
+ MessageReader reader(logfile);
+
+ EXPECT_EQ(reader.filename(), logfile);
+
+ EXPECT_EQ(
+ reader.max_out_of_order_duration(),
+ std::chrono::nanoseconds(config.message().max_out_of_order_duration()));
+ EXPECT_EQ(reader.newest_timestamp(), monotonic_clock::min_time);
+ EXPECT_TRUE(reader.ReadMessage());
+ EXPECT_EQ(reader.newest_timestamp(),
+ monotonic_clock::time_point(chrono::nanoseconds(1)));
+ EXPECT_TRUE(reader.ReadMessage());
+ EXPECT_EQ(reader.newest_timestamp(),
+ monotonic_clock::time_point(chrono::nanoseconds(2)));
+ EXPECT_FALSE(reader.ReadMessage());
+}
+
+} // namespace testing
+} // namespace logger
+} // namespace aos
diff --git a/aos/events/logging/logger.cc b/aos/events/logging/logger.cc
index 424153a..134c202 100644
--- a/aos/events/logging/logger.cc
+++ b/aos/events/logging/logger.cc
@@ -11,11 +11,13 @@
#include "absl/strings/escaping.h"
#include "absl/types/span.h"
#include "aos/events/event_loop.h"
+#include "aos/events/logging/logfile_sorting.h"
#include "aos/events/logging/logger_generated.h"
#include "aos/events/logging/uuid.h"
#include "aos/flatbuffer_merge.h"
#include "aos/network/team_number.h"
#include "aos/time/time.h"
+#include "aos/util/file.h"
#include "flatbuffers/flatbuffers.h"
#include "third_party/gmp/gmpxx.h"
@@ -33,68 +35,49 @@
namespace aos {
namespace logger {
+namespace {
+// Helper to safely read a header, or CHECK.
+FlatbufferVector<LogFileHeader> MaybeReadHeaderOrDie(
+ const std::vector<std::vector<std::string>> &filenames) {
+ CHECK_GE(filenames.size(), 1u) << ": Empty filenames list";
+ CHECK_GE(filenames[0].size(), 1u) << ": Empty filenames list";
+ return ReadHeader(filenames[0][0]);
+}
namespace chrono = std::chrono;
+} // namespace
-
-Logger::Logger(std::string_view base_name, EventLoop *event_loop,
- std::chrono::milliseconds polling_period)
- : Logger(base_name, event_loop, event_loop->configuration(),
- polling_period) {}
-Logger::Logger(std::string_view base_name, EventLoop *event_loop,
- const Configuration *configuration,
- std::chrono::milliseconds polling_period)
- : Logger(std::make_unique<LocalLogNamer>(base_name, event_loop->node()),
- event_loop, configuration, polling_period) {}
-Logger::Logger(std::unique_ptr<LogNamer> log_namer, EventLoop *event_loop,
- std::chrono::milliseconds polling_period)
- : Logger(std::move(log_namer), event_loop, event_loop->configuration(),
- polling_period) {}
-
-Logger::Logger(std::unique_ptr<LogNamer> log_namer, EventLoop *event_loop,
- const Configuration *configuration,
- std::chrono::milliseconds polling_period)
+Logger::Logger(EventLoop *event_loop, const Configuration *configuration,
+ std::function<bool(const Channel *)> should_log)
: event_loop_(event_loop),
- uuid_(UUID::Random()),
- log_namer_(std::move(log_namer)),
configuration_(configuration),
+ boot_uuid_(
+ util::ReadFileToStringOrDie("/proc/sys/kernel/random/boot_id")),
name_(network::GetHostname()),
- timer_handler_(event_loop_->AddTimer([this]() { DoLogData(); })),
- polling_period_(polling_period),
+ timer_handler_(event_loop_->AddTimer(
+ [this]() { DoLogData(event_loop_->monotonic_now()); })),
server_statistics_fetcher_(
configuration::MultiNode(event_loop_->configuration())
? event_loop_->MakeFetcher<message_bridge::ServerStatistics>(
"/aos")
: aos::Fetcher<message_bridge::ServerStatistics>()) {
- VLOG(1) << "Starting logger for " << FlatbufferToJson(event_loop_->node());
- int channel_index = 0;
+ VLOG(1) << "Creating logger for " << FlatbufferToJson(event_loop_->node());
- // Find all the nodes which are logging timestamps on our node.
- std::set<const Node *> timestamp_logger_nodes;
- for (const Channel *channel : *configuration_->channels()) {
- if (!configuration::ChannelIsSendableOnNode(channel, event_loop_->node()) ||
- !channel->has_destination_nodes()) {
- continue;
- }
- for (const Connection *connection : *channel->destination_nodes()) {
- const Node *other_node = configuration::GetNode(
- configuration_, connection->name()->string_view());
-
- if (configuration::ConnectionDeliveryTimeIsLoggedOnNode(
- connection, event_loop_->node())) {
- VLOG(1) << "Timestamps are logged from "
- << FlatbufferToJson(other_node);
- timestamp_logger_nodes.insert(other_node);
- }
- }
- }
+ // Find all the nodes which are logging timestamps on our node. This may
+ // over-estimate if should_log is specified.
+ std::vector<const Node *> timestamp_logger_nodes =
+ configuration::TimestampNodes(configuration_, event_loop_->node());
std::map<const Channel *, const Node *> timestamp_logger_channels;
// Now that we have all the nodes accumulated, make remote timestamp loggers
// for them.
for (const Node *node : timestamp_logger_nodes) {
+ // Note: since we are doing a find using the event loop channel, we need to
+ // make sure this channel pointer is part of the event loop configuration,
+ // not configuration_. This only matters when configuration_ !=
+ // event_loop->configuration();
const Channel *channel = configuration::GetChannel(
- configuration_,
+ event_loop->configuration(),
absl::StrCat("/aos/remote_timestamps/", node->name()->string_view()),
logger::MessageHeader::GetFullyQualifiedName(), event_loop_->name(),
event_loop_->node());
@@ -104,44 +87,56 @@
<< event_loop_->node()->name()->string_view()
<< " but can't find channel /aos/remote_timestamps/"
<< node->name()->string_view();
+ if (!should_log(channel)) {
+ continue;
+ }
timestamp_logger_channels.insert(std::make_pair(channel, node));
}
- const size_t our_node_index = configuration::GetNodeIndex(
- configuration_, event_loop_->node());
+ const size_t our_node_index =
+ configuration::GetNodeIndex(configuration_, event_loop_->node());
- for (const Channel *config_channel : *configuration_->channels()) {
+ for (size_t channel_index = 0;
+ channel_index < configuration_->channels()->size(); ++channel_index) {
+ const Channel *const config_channel =
+ configuration_->channels()->Get(channel_index);
// The MakeRawFetcher method needs a channel which is in the event loop
// configuration() object, not the configuration_ object. Go look that up
// from the config.
const Channel *channel = aos::configuration::GetChannel(
event_loop_->configuration(), config_channel->name()->string_view(),
config_channel->type()->string_view(), "", event_loop_->node());
+ CHECK(channel != nullptr)
+ << ": Failed to look up channel "
+ << aos::configuration::CleanedChannelToString(config_channel);
+ if (!should_log(channel)) {
+ continue;
+ }
FetcherStruct fs;
fs.node_index = our_node_index;
+ fs.channel_index = channel_index;
+ fs.channel = channel;
+
const bool is_local =
configuration::ChannelIsSendableOnNode(channel, event_loop_->node());
const bool is_readable =
configuration::ChannelIsReadableOnNode(channel, event_loop_->node());
- const bool log_message = configuration::ChannelMessageIsLoggedOnNode(
- channel, event_loop_->node()) &&
- is_readable;
+ const bool is_logged = configuration::ChannelMessageIsLoggedOnNode(
+ channel, event_loop_->node());
+ const bool log_message = is_logged && is_readable;
- const bool log_delivery_times =
- (event_loop_->node() == nullptr)
- ? false
- : configuration::ConnectionDeliveryTimeIsLoggedOnNode(
- channel, event_loop_->node(), event_loop_->node());
+ bool log_delivery_times = false;
+ if (event_loop_->node() != nullptr) {
+ log_delivery_times = configuration::ConnectionDeliveryTimeIsLoggedOnNode(
+ channel, event_loop_->node(), event_loop_->node());
+ }
// Now, detect a MessageHeader timestamp logger where we should just log the
// contents to a file directly.
const bool log_contents = timestamp_logger_channels.find(channel) !=
timestamp_logger_channels.end();
- const Node *timestamp_node =
- log_contents ? timestamp_logger_channels.find(channel)->second
- : nullptr;
if (log_message || log_delivery_times || log_contents) {
fs.fetcher = event_loop->MakeRawFetcher(channel);
@@ -150,11 +145,11 @@
if (log_delivery_times) {
VLOG(1) << " Delivery times";
- fs.timestamp_writer = log_namer_->MakeTimestampWriter(channel);
+ fs.wants_timestamp_writer = true;
}
if (log_message) {
VLOG(1) << " Data";
- fs.writer = log_namer_->MakeWriter(channel);
+ fs.wants_writer = true;
if (!is_local) {
fs.log_type = LogType::kLogRemoteMessage;
}
@@ -162,49 +157,96 @@
if (log_contents) {
VLOG(1) << "Timestamp logger channel "
<< configuration::CleanedChannelToString(channel);
- fs.contents_writer =
- log_namer_->MakeForwardedTimestampWriter(channel, timestamp_node);
+ fs.timestamp_node = timestamp_logger_channels.find(channel)->second;
+ fs.wants_contents_writer = true;
fs.node_index =
- configuration::GetNodeIndex(configuration_, timestamp_node);
+ configuration::GetNodeIndex(configuration_, fs.timestamp_node);
}
- fs.channel_index = channel_index;
- fs.written = false;
fetchers_.emplace_back(std::move(fs));
}
- ++channel_index;
}
+ // When we are logging remote timestamps, we need to be able to translate from
+ // the channel index that the event loop uses to the channel index in the
+ // config in the log file.
+ event_loop_to_logged_channel_index_.resize(
+ event_loop->configuration()->channels()->size(), -1);
+ for (size_t event_loop_channel_index = 0;
+ event_loop_channel_index <
+ event_loop->configuration()->channels()->size();
+ ++event_loop_channel_index) {
+ const Channel *event_loop_channel =
+ event_loop->configuration()->channels()->Get(event_loop_channel_index);
+
+ const Channel *logged_channel = aos::configuration::GetChannel(
+ configuration_, event_loop_channel->name()->string_view(),
+ event_loop_channel->type()->string_view(), "",
+ configuration::GetNode(configuration_, event_loop_->node()));
+
+ if (logged_channel != nullptr) {
+ event_loop_to_logged_channel_index_[event_loop_channel_index] =
+ configuration::ChannelIndex(configuration_, logged_channel);
+ }
+ }
+}
+
+Logger::~Logger() {
+ if (log_namer_) {
+ // If we are replaying a log file, or in simulation, we want to force the
+ // last bit of data to be logged. The easiest way to deal with this is to
+ // poll everything as we go to destroy the class, ie, shut down the logger,
+ // and write it to disk.
+ StopLogging(event_loop_->monotonic_now());
+ }
+}
+
+void Logger::StartLogging(std::unique_ptr<LogNamer> log_namer,
+ std::string_view log_start_uuid) {
+ CHECK(!log_namer_) << ": Already logging";
+ log_namer_ = std::move(log_namer);
+ log_event_uuid_ = UUID::Random();
+ log_start_uuid_ = log_start_uuid;
+ VLOG(1) << "Starting logger for " << FlatbufferToJson(event_loop_->node());
+
+ // We want to do as much work as possible before the initial Fetch. Time
+ // between that and actually starting to log opens up the possibility of
+ // falling off the end of the queue during that time.
+
+ for (FetcherStruct &f : fetchers_) {
+ if (f.wants_writer) {
+ f.writer = log_namer_->MakeWriter(f.channel);
+ }
+ if (f.wants_timestamp_writer) {
+ f.timestamp_writer = log_namer_->MakeTimestampWriter(f.channel);
+ }
+ if (f.wants_contents_writer) {
+ f.contents_writer = log_namer_->MakeForwardedTimestampWriter(
+ f.channel, CHECK_NOTNULL(f.timestamp_node));
+ }
+ }
+
+ CHECK(node_state_.empty());
node_state_.resize(configuration::MultiNode(configuration_)
? configuration_->nodes()->size()
: 1u);
for (const Node *node : log_namer_->nodes()) {
- const int node_index =
- configuration::GetNodeIndex(configuration_, node);
+ const int node_index = configuration::GetNodeIndex(configuration_, node);
node_state_[node_index].log_file_header = MakeHeader(node);
}
- // When things start, we want to log the header, then the most recent
- // messages available on each fetcher to capture the previous state, then
- // start polling.
- event_loop_->OnRun([this]() { StartLogging(); });
-}
-
-Logger::~Logger() {
- // If we are replaying a log file, or in simulation, we want to force the last
- // bit of data to be logged. The easiest way to deal with this is to poll
- // everything as we go to destroy the class, ie, shut down the logger, and
- // write it to disk.
- DoLogData();
-}
-
-void Logger::StartLogging() {
// Grab data from each channel right before we declare the log file started
// so we can capture the latest message on each channel. This lets us have
// non periodic messages with configuration that now get logged.
for (FetcherStruct &f : fetchers_) {
- f.written = !f.fetcher->Fetch();
+ const auto start = event_loop_->monotonic_now();
+ const bool got_new = f.fetcher->Fetch();
+ const auto end = event_loop_->monotonic_now();
+ RecordFetchResult(start, end, got_new, &f);
+
+ // If there is a message, we want to write it.
+ f.written = f.fetcher->context().data == nullptr;
}
// Clear out any old timestamps in case we are re-starting logging.
@@ -221,6 +263,28 @@
polling_period_);
}
+std::unique_ptr<LogNamer> Logger::StopLogging(
+ aos::monotonic_clock::time_point end_time) {
+ CHECK(log_namer_) << ": Not logging right now";
+
+ if (end_time != aos::monotonic_clock::min_time) {
+ LogUntil(end_time);
+ }
+ timer_handler_->Disable();
+
+ for (FetcherStruct &f : fetchers_) {
+ f.writer = nullptr;
+ f.timestamp_writer = nullptr;
+ f.contents_writer = nullptr;
+ }
+ node_state_.clear();
+
+ log_event_uuid_ = UUID::Zero();
+ log_start_uuid_ = std::string();
+
+ return std::move(log_namer_);
+}
+
void Logger::WriteHeader() {
if (configuration::MultiNode(configuration_)) {
server_statistics_fetcher_.Fetch();
@@ -238,8 +302,7 @@
last_synchronized_time_ = monotonic_start_time;
for (const Node *node : log_namer_->nodes()) {
- const int node_index =
- configuration::GetNodeIndex(configuration_, node);
+ const int node_index = configuration::GetNodeIndex(configuration_, node);
MaybeUpdateTimestamp(node, node_index, monotonic_start_time,
realtime_start_time);
log_namer_->WriteHeader(&node_state_[node_index].log_file_header, node);
@@ -258,8 +321,7 @@
}
for (const Node *node : log_namer_->nodes()) {
- const int node_index =
- configuration::GetNodeIndex(configuration_, node);
+ const int node_index = configuration::GetNodeIndex(configuration_, node);
if (MaybeUpdateTimestamp(
node, node_index,
server_statistics_fetcher_.context().monotonic_event_time,
@@ -354,22 +416,36 @@
// TODO(austin): Compress this much more efficiently. There are a bunch of
// duplicated schemas.
- flatbuffers::Offset<aos::Configuration> configuration_offset =
+ const flatbuffers::Offset<aos::Configuration> configuration_offset =
CopyFlatBuffer(configuration_, &fbb);
- flatbuffers::Offset<flatbuffers::String> name_offset =
+ const flatbuffers::Offset<flatbuffers::String> name_offset =
fbb.CreateString(name_);
- flatbuffers::Offset<flatbuffers::String> logger_uuid_offset =
- fbb.CreateString(uuid_.string_view());
+ CHECK(log_event_uuid_ != UUID::Zero());
+ const flatbuffers::Offset<flatbuffers::String> log_event_uuid_offset =
+ fbb.CreateString(log_event_uuid_.string_view());
- flatbuffers::Offset<flatbuffers::String> parts_uuid_offset =
+ const flatbuffers::Offset<flatbuffers::String> logger_instance_uuid_offset =
+ fbb.CreateString(logger_instance_uuid_.string_view());
+
+ flatbuffers::Offset<flatbuffers::String> log_start_uuid_offset;
+ if (!log_start_uuid_.empty()) {
+ log_start_uuid_offset = fbb.CreateString(log_start_uuid_);
+ }
+
+ const flatbuffers::Offset<flatbuffers::String> boot_uuid_offset =
+ fbb.CreateString(boot_uuid_);
+
+ const flatbuffers::Offset<flatbuffers::String> parts_uuid_offset =
fbb.CreateString("00000000-0000-4000-8000-000000000000");
flatbuffers::Offset<Node> node_offset;
+ flatbuffers::Offset<Node> logger_node_offset;
if (configuration::MultiNode(configuration_)) {
node_offset = CopyFlatBuffer(node, &fbb);
+ logger_node_offset = CopyFlatBuffer(event_loop_->node(), &fbb);
}
aos::logger::LogFileHeader::Builder log_file_header_builder(fbb);
@@ -379,6 +455,7 @@
// Only add the node if we are running in a multinode configuration.
if (node != nullptr) {
log_file_header_builder.add_node(node_offset);
+ log_file_header_builder.add_logger_node(logger_node_offset);
}
log_file_header_builder.add_configuration(configuration_offset);
@@ -388,8 +465,7 @@
// message. Report back 3x to be extra safe, and because the cost isn't
// huge on the read side.
log_file_header_builder.add_max_out_of_order_duration(
- std::chrono::duration_cast<std::chrono::nanoseconds>(3 * polling_period_)
- .count());
+ std::chrono::nanoseconds(3 * polling_period_).count());
log_file_header_builder.add_monotonic_start_time(
std::chrono::duration_cast<std::chrono::nanoseconds>(
@@ -402,7 +478,12 @@
.count());
}
- log_file_header_builder.add_logger_uuid(logger_uuid_offset);
+ log_file_header_builder.add_log_event_uuid(log_event_uuid_offset);
+ log_file_header_builder.add_logger_instance_uuid(logger_instance_uuid_offset);
+ if (!log_start_uuid_offset.IsNull()) {
+ log_file_header_builder.add_log_start_uuid(log_start_uuid_offset);
+ }
+ log_file_header_builder.add_boot_uuid(boot_uuid_offset);
log_file_header_builder.add_parts_uuid(parts_uuid_offset);
log_file_header_builder.add_parts_index(0);
@@ -411,10 +492,26 @@
return fbb.Release();
}
+void Logger::ResetStatisics() {
+ max_message_fetch_time_ = std::chrono::nanoseconds::zero();
+ max_message_fetch_time_channel_ = -1;
+ max_message_fetch_time_size_ = -1;
+ total_message_fetch_time_ = std::chrono::nanoseconds::zero();
+ total_message_fetch_count_ = 0;
+ total_message_fetch_bytes_ = 0;
+ total_nop_fetch_time_ = std::chrono::nanoseconds::zero();
+ total_nop_fetch_count_ = 0;
+ max_copy_time_ = std::chrono::nanoseconds::zero();
+ max_copy_time_channel_ = -1;
+ max_copy_time_size_ = -1;
+ total_copy_time_ = std::chrono::nanoseconds::zero();
+ total_copy_count_ = 0;
+ total_copy_bytes_ = 0;
+}
+
void Logger::Rotate() {
for (const Node *node : log_namer_->nodes()) {
- const int node_index =
- configuration::GetNodeIndex(configuration_, node);
+ const int node_index = configuration::GetNodeIndex(configuration_, node);
log_namer_->Rotate(node, &node_state_[node_index].log_file_header);
}
}
@@ -426,214 +523,185 @@
for (FetcherStruct &f : fetchers_) {
while (true) {
if (f.written) {
- if (!f.fetcher->FetchNext()) {
+ const auto start = event_loop_->monotonic_now();
+ const bool got_new = f.fetcher->FetchNext();
+ const auto end = event_loop_->monotonic_now();
+ RecordFetchResult(start, end, got_new, &f);
+ if (!got_new) {
VLOG(2) << "No new data on "
<< configuration::CleanedChannelToString(
f.fetcher->channel());
break;
- } else {
- f.written = false;
}
+ f.written = false;
}
- CHECK(!f.written);
-
// TODO(james): Write tests to exercise this logic.
- if (f.fetcher->context().monotonic_event_time < t) {
- if (f.writer != nullptr) {
- // Write!
- flatbuffers::FlatBufferBuilder fbb(f.fetcher->context().size +
- max_header_size_);
- fbb.ForceDefaults(true);
-
- fbb.FinishSizePrefixed(PackMessage(&fbb, f.fetcher->context(),
- f.channel_index, f.log_type));
-
- VLOG(2) << "Writing data as node "
- << FlatbufferToJson(event_loop_->node()) << " for channel "
- << configuration::CleanedChannelToString(f.fetcher->channel())
- << " to " << f.writer->filename() << " data "
- << FlatbufferToJson(
- flatbuffers::GetSizePrefixedRoot<MessageHeader>(
- fbb.GetBufferPointer()));
-
- max_header_size_ = std::max(
- max_header_size_, fbb.GetSize() - f.fetcher->context().size);
- f.writer->QueueSizedFlatbuffer(&fbb);
- }
-
- if (f.timestamp_writer != nullptr) {
- // And now handle timestamps.
- flatbuffers::FlatBufferBuilder fbb;
- fbb.ForceDefaults(true);
-
- fbb.FinishSizePrefixed(PackMessage(&fbb, f.fetcher->context(),
- f.channel_index,
- LogType::kLogDeliveryTimeOnly));
-
- VLOG(2) << "Writing timestamps as node "
- << FlatbufferToJson(event_loop_->node()) << " for channel "
- << configuration::CleanedChannelToString(f.fetcher->channel())
- << " to " << f.timestamp_writer->filename() << " timestamp "
- << FlatbufferToJson(
- flatbuffers::GetSizePrefixedRoot<MessageHeader>(
- fbb.GetBufferPointer()));
-
- f.timestamp_writer->QueueSizedFlatbuffer(&fbb);
- }
-
- if (f.contents_writer != nullptr) {
- // And now handle the special message contents channel. Copy the
- // message into a FlatBufferBuilder and save it to disk.
- // TODO(austin): We can be more efficient here when we start to
- // care...
- flatbuffers::FlatBufferBuilder fbb;
- fbb.ForceDefaults(true);
-
- const MessageHeader *msg =
- flatbuffers::GetRoot<MessageHeader>(f.fetcher->context().data);
-
- logger::MessageHeader::Builder message_header_builder(fbb);
-
- // Note: this must match the same order as MessageBridgeServer and
- // PackMessage. We want identical headers to have identical
- // on-the-wire formats to make comparing them easier.
- message_header_builder.add_channel_index(msg->channel_index());
-
- message_header_builder.add_queue_index(msg->queue_index());
- message_header_builder.add_monotonic_sent_time(
- msg->monotonic_sent_time());
- message_header_builder.add_realtime_sent_time(
- msg->realtime_sent_time());
-
- message_header_builder.add_monotonic_remote_time(
- msg->monotonic_remote_time());
- message_header_builder.add_realtime_remote_time(
- msg->realtime_remote_time());
- message_header_builder.add_remote_queue_index(
- msg->remote_queue_index());
-
- fbb.FinishSizePrefixed(message_header_builder.Finish());
-
- f.contents_writer->QueueSizedFlatbuffer(&fbb);
- }
-
- f.written = true;
- } else {
+ if (f.fetcher->context().monotonic_event_time >= t) {
break;
}
+ if (f.writer != nullptr) {
+ // Write!
+ const auto start = event_loop_->monotonic_now();
+ flatbuffers::FlatBufferBuilder fbb(f.fetcher->context().size +
+ max_header_size_);
+ fbb.ForceDefaults(true);
+
+ fbb.FinishSizePrefixed(PackMessage(&fbb, f.fetcher->context(),
+ f.channel_index, f.log_type));
+ const auto end = event_loop_->monotonic_now();
+ RecordCreateMessageTime(start, end, &f);
+
+ VLOG(2) << "Writing data as node "
+ << FlatbufferToJson(event_loop_->node()) << " for channel "
+ << configuration::CleanedChannelToString(f.fetcher->channel())
+ << " to " << f.writer->filename() << " data "
+ << FlatbufferToJson(
+ flatbuffers::GetSizePrefixedRoot<MessageHeader>(
+ fbb.GetBufferPointer()));
+
+ max_header_size_ = std::max(max_header_size_,
+ fbb.GetSize() - f.fetcher->context().size);
+ f.writer->QueueSizedFlatbuffer(&fbb);
+ }
+
+ if (f.timestamp_writer != nullptr) {
+ // And now handle timestamps.
+ const auto start = event_loop_->monotonic_now();
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.ForceDefaults(true);
+
+ fbb.FinishSizePrefixed(PackMessage(&fbb, f.fetcher->context(),
+ f.channel_index,
+ LogType::kLogDeliveryTimeOnly));
+ const auto end = event_loop_->monotonic_now();
+ RecordCreateMessageTime(start, end, &f);
+
+ VLOG(2) << "Writing timestamps as node "
+ << FlatbufferToJson(event_loop_->node()) << " for channel "
+ << configuration::CleanedChannelToString(f.fetcher->channel())
+ << " to " << f.timestamp_writer->filename() << " timestamp "
+ << FlatbufferToJson(
+ flatbuffers::GetSizePrefixedRoot<MessageHeader>(
+ fbb.GetBufferPointer()));
+
+ f.timestamp_writer->QueueSizedFlatbuffer(&fbb);
+ }
+
+ if (f.contents_writer != nullptr) {
+ const auto start = event_loop_->monotonic_now();
+ // And now handle the special message contents channel. Copy the
+ // message into a FlatBufferBuilder and save it to disk.
+ // TODO(austin): We can be more efficient here when we start to
+ // care...
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.ForceDefaults(true);
+
+ const MessageHeader *msg =
+ flatbuffers::GetRoot<MessageHeader>(f.fetcher->context().data);
+
+ logger::MessageHeader::Builder message_header_builder(fbb);
+
+ // TODO(austin): This needs to check the channel_index and confirm
+ // that it should be logged before squirreling away the timestamp to
+ // disk. We don't want to log irrelevant timestamps.
+
+ // Note: this must match the same order as MessageBridgeServer and
+ // PackMessage. We want identical headers to have identical
+ // on-the-wire formats to make comparing them easier.
+
+ // Translate from the channel index that the event loop uses to the
+ // channel index in the log file.
+ message_header_builder.add_channel_index(
+ event_loop_to_logged_channel_index_[msg->channel_index()]);
+
+ message_header_builder.add_queue_index(msg->queue_index());
+ message_header_builder.add_monotonic_sent_time(
+ msg->monotonic_sent_time());
+ message_header_builder.add_realtime_sent_time(
+ msg->realtime_sent_time());
+
+ message_header_builder.add_monotonic_remote_time(
+ msg->monotonic_remote_time());
+ message_header_builder.add_realtime_remote_time(
+ msg->realtime_remote_time());
+ message_header_builder.add_remote_queue_index(
+ msg->remote_queue_index());
+
+ fbb.FinishSizePrefixed(message_header_builder.Finish());
+ const auto end = event_loop_->monotonic_now();
+ RecordCreateMessageTime(start, end, &f);
+
+ f.contents_writer->QueueSizedFlatbuffer(&fbb);
+ }
+
+ f.written = true;
}
}
last_synchronized_time_ = t;
}
-void Logger::DoLogData() {
- // We want to guarentee that messages aren't out of order by more than
+void Logger::DoLogData(const monotonic_clock::time_point end_time) {
+ // We want to guarantee that messages aren't out of order by more than
// max_out_of_order_duration. To do this, we need sync points. Every write
// cycle should be a sync point.
- const monotonic_clock::time_point monotonic_now =
- event_loop_->monotonic_now();
do {
// Move the sync point up by at most polling_period. This forces one sync
// per iteration, even if it is small.
- LogUntil(
- std::min(last_synchronized_time_ + polling_period_, monotonic_now));
+ LogUntil(std::min(last_synchronized_time_ + polling_period_, end_time));
+
+ on_logged_period_();
// If we missed cycles, we could be pretty far behind. Spin until we are
// caught up.
- } while (last_synchronized_time_ + polling_period_ < monotonic_now);
+ } while (last_synchronized_time_ + polling_period_ < end_time);
}
-std::vector<std::vector<std::string>> SortParts(
- const std::vector<std::string> &parts) {
- // Start by grouping all parts by UUID, and extracting the part index.
- std::map<std::string, std::vector<std::pair<std::string, int>>> parts_list;
-
- // Sort part files without UUIDs and part indexes as well. Extract everything
- // useful from the log in the first pass, then sort later.
- struct LogPart {
- std::string filename;
- monotonic_clock::time_point start_time;
- monotonic_clock::time_point first_message_time;
- };
-
- std::vector<LogPart> old_parts;
-
- for (const std::string &part : parts) {
- FlatbufferVector<LogFileHeader> log_header = ReadHeader(part);
-
- // Looks like an old log. No UUID, index, and also single node. We have
- // little to no multi-node log files in the wild without part UUIDs and
- // indexes which we care much about.
- if (!log_header.message().has_parts_uuid() &&
- !log_header.message().has_parts_index() &&
- !log_header.message().has_node()) {
- LogPart log_part;
- log_part.filename = part;
- log_part.start_time = monotonic_clock::time_point(
- chrono::nanoseconds(log_header.message().monotonic_start_time()));
- FlatbufferVector<MessageHeader> first_message = ReadNthMessage(part, 0);
- log_part.first_message_time = monotonic_clock::time_point(
- chrono::nanoseconds(first_message.message().monotonic_sent_time()));
- old_parts.emplace_back(std::move(log_part));
- continue;
- }
-
- CHECK(log_header.message().has_parts_uuid());
- CHECK(log_header.message().has_parts_index());
-
- const std::string parts_uuid = log_header.message().parts_uuid()->str();
- auto it = parts_list.find(parts_uuid);
- if (it == parts_list.end()) {
- it = parts_list
- .insert(std::make_pair(
- parts_uuid, std::vector<std::pair<std::string, int>>{}))
- .first;
- }
- it->second.emplace_back(
- std::make_pair(part, log_header.message().parts_index()));
+void Logger::RecordFetchResult(aos::monotonic_clock::time_point start,
+ aos::monotonic_clock::time_point end,
+ bool got_new, FetcherStruct *fetcher) {
+ const auto duration = end - start;
+ if (!got_new) {
+ ++total_nop_fetch_count_;
+ total_nop_fetch_time_ += duration;
+ return;
}
-
- CHECK_NE(old_parts.empty(), parts_list.empty())
- << ": Can't have a mix of old and new parts.";
-
- if (!old_parts.empty()) {
- // Confirm they all have the same start time. Old loggers always used the
- // same start time.
- for (const LogPart &p : old_parts) {
- CHECK_EQ(old_parts[0].start_time, p.start_time);
- }
- // Sort by the oldest message in each file.
- std::sort(old_parts.begin(), old_parts.end(),
- [](const LogPart &a, const LogPart &b) {
- return a.first_message_time < b.first_message_time;
- });
-
- // Produce the final form.
- std::vector<std::string> sorted_old_parts;
- sorted_old_parts.reserve(old_parts.size());
- for (LogPart &p : old_parts) {
- sorted_old_parts.emplace_back(std::move(p.filename));
- }
- return std::vector<std::vector<std::string>>{std::move(sorted_old_parts)};
+ ++total_message_fetch_count_;
+ total_message_fetch_bytes_ += fetcher->fetcher->context().size;
+ total_message_fetch_time_ += duration;
+ if (duration > max_message_fetch_time_) {
+ max_message_fetch_time_ = duration;
+ max_message_fetch_time_channel_ = fetcher->channel_index;
+ max_message_fetch_time_size_ = fetcher->fetcher->context().size;
}
+}
- // Now, sort them and produce the final vector form.
+void Logger::RecordCreateMessageTime(aos::monotonic_clock::time_point start,
+ aos::monotonic_clock::time_point end,
+ FetcherStruct *fetcher) {
+ const auto duration = end - start;
+ total_copy_time_ += duration;
+ ++total_copy_count_;
+ total_copy_bytes_ += fetcher->fetcher->context().size;
+ if (duration > max_copy_time_) {
+ max_copy_time_ = duration;
+ max_copy_time_channel_ = fetcher->channel_index;
+ max_copy_time_size_ = fetcher->fetcher->context().size;
+ }
+}
+
+std::vector<std::vector<std::string>> ToLogReaderVector(
+ const std::vector<LogFile> &log_files) {
std::vector<std::vector<std::string>> result;
- result.reserve(parts_list.size());
- for (auto &part : parts_list) {
- std::sort(part.second.begin(), part.second.end(),
- [](const std::pair<std::string, int> &a,
- const std::pair<std::string, int> &b) {
- return a.second < b.second;
- });
- std::vector<std::string> result_line;
- result_line.reserve(part.second.size());
- for (std::pair<std::string, int> &p : part.second) {
- result_line.emplace_back(std::move(p.first));
+ for (const LogFile &log_file : log_files) {
+ for (const LogParts &log_parts : log_file.parts) {
+ std::vector<std::string> parts;
+ for (const std::string &part : log_parts.parts) {
+ parts.emplace_back(part);
+ }
+ result.emplace_back(std::move(parts));
}
- result.emplace_back(std::move(result_line));
}
return result;
}
@@ -648,13 +716,35 @@
: LogReader(std::vector<std::vector<std::string>>{filenames},
replay_configuration) {}
+// TODO(austin): Make this the base and kill the others. This has much better
+// context for sorting.
+LogReader::LogReader(const std::vector<LogFile> &log_files,
+ const Configuration *replay_configuration)
+ : LogReader(ToLogReaderVector(log_files), replay_configuration) {}
+
LogReader::LogReader(const std::vector<std::vector<std::string>> &filenames,
const Configuration *replay_configuration)
: filenames_(filenames),
- log_file_header_(ReadHeader(filenames[0][0])),
+ log_file_header_(MaybeReadHeaderOrDie(filenames)),
replay_configuration_(replay_configuration) {
MakeRemappedConfig();
+ // Remap all existing remote timestamp channels. They will be recreated, and
+ // the data logged isn't relevant anymore.
+ for (const Node *node : configuration::GetNodes(logged_configuration())) {
+ std::vector<const Node *> timestamp_logger_nodes =
+ configuration::TimestampNodes(logged_configuration(), node);
+ for (const Node *remote_node : timestamp_logger_nodes) {
+ const std::string channel = absl::StrCat(
+ "/aos/remote_timestamps/", remote_node->name()->string_view());
+ CHECK(HasChannel<logger::MessageHeader>(channel, node))
+ << ": Failed to find {\"name\": \"" << channel << "\", \"type\": \""
+ << logger::MessageHeader::GetFullyQualifiedName() << "\"} for node "
+ << node->name()->string_view();
+ RemapLoggedChannel<logger::MessageHeader>(channel, node);
+ }
+ }
+
if (replay_configuration) {
CHECK_EQ(configuration::MultiNode(configuration()),
configuration::MultiNode(replay_configuration))
@@ -721,7 +811,8 @@
return configuration::GetNodes(remapped_configuration_);
}
-monotonic_clock::time_point LogReader::monotonic_start_time(const Node *node) {
+monotonic_clock::time_point LogReader::monotonic_start_time(
+ const Node *node) const {
State *state =
states_[configuration::GetNodeIndex(configuration(), node)].get();
CHECK(state != nullptr) << ": Unknown node " << FlatbufferToJson(node);
@@ -729,7 +820,8 @@
return state->monotonic_start_time();
}
-realtime_clock::time_point LogReader::realtime_start_time(const Node *node) {
+realtime_clock::time_point LogReader::realtime_start_time(
+ const Node *node) const {
State *state =
states_[configuration::GetNodeIndex(configuration(), node)].get();
CHECK(state != nullptr) << ": Unknown node " << FlatbufferToJson(node);
@@ -745,6 +837,7 @@
void LogReader::Register(SimulatedEventLoopFactory *event_loop_factory) {
event_loop_factory_ = event_loop_factory;
+ remapped_configuration_ = event_loop_factory_->configuration();
for (const Node *node : configuration::GetNodes(configuration())) {
const size_t node_index =
@@ -752,10 +845,22 @@
states_[node_index] =
std::make_unique<State>(std::make_unique<ChannelMerger>(filenames_));
State *state = states_[node_index].get();
-
- Register(state->SetNodeEventLoopFactory(
+ state->set_event_loop(state->SetNodeEventLoopFactory(
event_loop_factory_->GetNodeEventLoopFactory(node)));
+
+ state->SetChannelCount(logged_configuration()->channels()->size());
}
+
+ // Register after making all the State objects so we can build references
+ // between them.
+ for (const Node *node : configuration::GetNodes(configuration())) {
+ const size_t node_index =
+ configuration::GetNodeIndex(configuration(), node);
+ State *state = states_[node_index].get();
+
+ Register(state->event_loop());
+ }
+
if (live_nodes_ == 0) {
LOG(FATAL)
<< "Don't have logs from any of the nodes in the replay config--are "
@@ -1094,7 +1199,6 @@
}
}
-
void LogReader::Register(EventLoop *event_loop) {
State *state =
states_[configuration::GetNodeIndex(configuration(), event_loop->node())]
@@ -1110,29 +1214,46 @@
const bool has_data = state->SetNode();
- state->SetChannelCount(logged_configuration()->channels()->size());
+ for (size_t logged_channel_index = 0;
+ logged_channel_index < logged_configuration()->channels()->size();
+ ++logged_channel_index) {
+ const Channel *channel = RemapChannel(
+ event_loop,
+ logged_configuration()->channels()->Get(logged_channel_index));
- for (size_t i = 0; i < logged_configuration()->channels()->size(); ++i) {
- const Channel *channel =
- RemapChannel(event_loop, logged_configuration()->channels()->Get(i));
-
- NodeEventLoopFactory *channel_target_event_loop_factory = nullptr;
message_bridge::NoncausalOffsetEstimator *filter = nullptr;
+ aos::Sender<MessageHeader> *remote_timestamp_sender = nullptr;
+
+ State *source_state = nullptr;
if (!configuration::ChannelIsSendableOnNode(channel, event_loop->node()) &&
configuration::ChannelIsReadableOnNode(channel, event_loop->node())) {
- const Node *target_node = configuration::GetNode(
+ // We've got a message which is being forwarded to this node.
+ const Node *source_node = configuration::GetNode(
event_loop->configuration(), channel->source_node()->string_view());
- filter = GetFilter(event_loop->node(), target_node);
+ filter = GetFilter(event_loop->node(), source_node);
- if (event_loop_factory_ != nullptr) {
- channel_target_event_loop_factory =
- event_loop_factory_->GetNodeEventLoopFactory(target_node);
+ // Delivery timestamps are supposed to be logged back on the source node.
+ // Configure remote timestamps to be sent.
+ const bool delivery_time_is_logged =
+ configuration::ConnectionDeliveryTimeIsLoggedOnNode(
+ channel, event_loop->node(), source_node);
+
+ source_state =
+ states_[configuration::GetNodeIndex(configuration(), source_node)]
+ .get();
+
+ if (delivery_time_is_logged) {
+ remote_timestamp_sender =
+ source_state->RemoteTimestampSender(event_loop->node());
}
}
- state->SetChannel(i, event_loop->MakeRawSender(channel), filter,
- channel_target_event_loop_factory);
+ state->SetChannel(
+ logged_channel_index,
+ configuration::ChannelIndex(event_loop->configuration(), channel),
+ event_loop->MakeRawSender(channel), filter, remote_timestamp_sender,
+ source_state);
}
// If we didn't find any log files with data in them, we won't ever get a
@@ -1261,10 +1382,7 @@
// TODO(austin): std::move channel_data in and make that efficient in
// simulation.
state->Send(channel_index, channel_data.message().data()->Data(),
- channel_data.message().data()->size(),
- channel_timestamp.monotonic_remote_time,
- channel_timestamp.realtime_remote_time,
- channel_timestamp.remote_queue_index);
+ channel_data.message().data()->size(), channel_timestamp);
} else if (state->at_end() && !ignore_missing_data_) {
// We are at the end of the log file and found missing data. Finish
// reading the rest of the log file and call it quits. We don't want
@@ -1318,9 +1436,8 @@
});
for (size_t i = 0; i < states_.size(); ++i) {
- VLOG(1) << MaybeNodeName(
- states_[i]->event_loop()->node())
- << "before " << states_[i]->monotonic_now();
+ VLOG(1) << MaybeNodeName(states_[i]->event_loop()->node()) << "before "
+ << states_[i]->monotonic_now();
}
UpdateOffsets();
@@ -1328,9 +1445,8 @@
<< state->monotonic_now();
for (size_t i = 0; i < states_.size(); ++i) {
- VLOG(1) << MaybeNodeName(
- states_[i]->event_loop()->node())
- << "after " << states_[i]->monotonic_now();
+ VLOG(1) << MaybeNodeName(states_[i]->event_loop()->node()) << "after "
+ << states_[i]->monotonic_now();
}
// TODO(austin): We should be perfect.
@@ -1421,6 +1537,50 @@
<< type;
}
+void LogReader::RemapLoggedChannel(std::string_view name, std::string_view type,
+ const Node *node,
+ std::string_view add_prefix) {
+ VLOG(1) << "Node is " << aos::FlatbufferToJson(node);
+ const Channel *remapped_channel =
+ configuration::GetChannel(logged_configuration(), name, type, "", node);
+ CHECK(remapped_channel != nullptr) << ": Failed to find {\"name\": \"" << name
+ << "\", \"type\": \"" << type << "\"}";
+ VLOG(1) << "Original {\"name\": \"" << name << "\", \"type\": \"" << type
+ << "\"}";
+ VLOG(1) << "Remapped "
+ << aos::configuration::StrippedChannelToString(remapped_channel);
+
+ // We want to make /spray on node 0 go to /0/spray by snooping the maps. And
+ // we want it to degrade if the heuristics fail to just work.
+ //
+ // The easiest way to do this is going to be incredibly specific and verbose.
+ // Look up /spray, to /0/spray. Then, prefix the result with /original to get
+ // /original/0/spray. Then, create a map from /original/spray to
+ // /original/0/spray for just the type we were asked for.
+ if (name != remapped_channel->name()->string_view()) {
+ MapT new_map;
+ new_map.match = std::make_unique<ChannelT>();
+ new_map.match->name = absl::StrCat(add_prefix, name);
+ new_map.match->type = type;
+ if (node != nullptr) {
+ new_map.match->source_node = node->name()->str();
+ }
+ new_map.rename = std::make_unique<ChannelT>();
+ new_map.rename->name =
+ absl::StrCat(add_prefix, remapped_channel->name()->string_view());
+ maps_.emplace_back(std::move(new_map));
+ }
+
+ const size_t channel_index =
+ configuration::ChannelIndex(logged_configuration(), remapped_channel);
+ CHECK_EQ(0u, remapped_channels_.count(channel_index))
+ << "Already remapped channel "
+ << configuration::CleanedChannelToString(remapped_channel);
+ remapped_channels_[channel_index] =
+ absl::StrCat(add_prefix, remapped_channel->name()->string_view());
+ MakeRemappedConfig();
+}
+
void LogReader::MakeRemappedConfig() {
for (std::unique_ptr<State> &state : states_) {
if (state) {
@@ -1489,10 +1649,46 @@
&new_config_fbb));
}
// Create the Configuration containing the new channels that we want to add.
- const auto new_name_vector_offsets =
+ const auto new_channel_vector_offsets =
new_config_fbb.CreateVector(channel_offsets);
+
+ // Now create the new maps.
+ std::vector<flatbuffers::Offset<Map>> map_offsets;
+ for (const MapT &map : maps_) {
+ const flatbuffers::Offset<flatbuffers::String> match_name_offset =
+ new_config_fbb.CreateString(map.match->name);
+ const flatbuffers::Offset<flatbuffers::String> match_type_offset =
+ new_config_fbb.CreateString(map.match->type);
+ const flatbuffers::Offset<flatbuffers::String> rename_name_offset =
+ new_config_fbb.CreateString(map.rename->name);
+ flatbuffers::Offset<flatbuffers::String> match_source_node_offset;
+ if (!map.match->source_node.empty()) {
+ match_source_node_offset =
+ new_config_fbb.CreateString(map.match->source_node);
+ }
+ Channel::Builder match_builder(new_config_fbb);
+ match_builder.add_name(match_name_offset);
+ match_builder.add_type(match_type_offset);
+ if (!map.match->source_node.empty()) {
+ match_builder.add_source_node(match_source_node_offset);
+ }
+ const flatbuffers::Offset<Channel> match_offset = match_builder.Finish();
+
+ Channel::Builder rename_builder(new_config_fbb);
+ rename_builder.add_name(rename_name_offset);
+ const flatbuffers::Offset<Channel> rename_offset = rename_builder.Finish();
+
+ Map::Builder map_builder(new_config_fbb);
+ map_builder.add_match(match_offset);
+ map_builder.add_rename(rename_offset);
+ map_offsets.emplace_back(map_builder.Finish());
+ }
+
+ const auto new_maps_offsets = new_config_fbb.CreateVector(map_offsets);
+
ConfigurationBuilder new_config_builder(new_config_fbb);
- new_config_builder.add_channels(new_name_vector_offsets);
+ new_config_builder.add_channels(new_channel_vector_offsets);
+ new_config_builder.add_maps(new_maps_offsets);
new_config_fbb.Finish(new_config_builder.Finish());
const FlatbufferDetachedBuffer<Configuration> new_name_config =
new_config_fbb.Release();
@@ -1548,18 +1744,148 @@
void LogReader::State::SetChannelCount(size_t count) {
channels_.resize(count);
+ remote_timestamp_senders_.resize(count);
filters_.resize(count);
- channel_target_event_loop_factory_.resize(count);
+ channel_source_state_.resize(count);
+ factory_channel_index_.resize(count);
+ queue_index_map_.resize(count);
}
void LogReader::State::SetChannel(
- size_t channel, std::unique_ptr<RawSender> sender,
+ size_t logged_channel_index, size_t factory_channel_index,
+ std::unique_ptr<RawSender> sender,
message_bridge::NoncausalOffsetEstimator *filter,
- NodeEventLoopFactory *channel_target_event_loop_factory) {
- channels_[channel] = std::move(sender);
- filters_[channel] = filter;
- channel_target_event_loop_factory_[channel] =
- channel_target_event_loop_factory;
+ aos::Sender<MessageHeader> *remote_timestamp_sender, State *source_state) {
+ channels_[logged_channel_index] = std::move(sender);
+ filters_[logged_channel_index] = filter;
+ remote_timestamp_senders_[logged_channel_index] = remote_timestamp_sender;
+
+ if (source_state) {
+ channel_source_state_[logged_channel_index] = source_state;
+
+ if (remote_timestamp_sender != nullptr) {
+ source_state->queue_index_map_[logged_channel_index] =
+ std::make_unique<std::vector<State::SentTimestamp>>();
+ }
+ }
+
+ factory_channel_index_[logged_channel_index] = factory_channel_index;
+}
+
+bool LogReader::State::Send(
+ size_t channel_index, const void *data, size_t size,
+ const TimestampMerger::DeliveryTimestamp &delivery_timestamp) {
+ aos::RawSender *sender = channels_[channel_index].get();
+ uint32_t remote_queue_index = 0xffffffff;
+
+ if (remote_timestamp_senders_[channel_index] != nullptr) {
+ std::vector<SentTimestamp> *queue_index_map =
+ CHECK_NOTNULL(CHECK_NOTNULL(channel_source_state_[channel_index])
+ ->queue_index_map_[channel_index]
+ .get());
+
+ SentTimestamp search;
+ search.monotonic_event_time = delivery_timestamp.monotonic_remote_time;
+ search.realtime_event_time = delivery_timestamp.realtime_remote_time;
+ search.queue_index = delivery_timestamp.remote_queue_index;
+
+ // Find the sent time if available.
+ auto element = std::lower_bound(
+ queue_index_map->begin(), queue_index_map->end(), search,
+ [](SentTimestamp a, SentTimestamp b) {
+ if (b.monotonic_event_time < a.monotonic_event_time) {
+ return false;
+ }
+ if (b.monotonic_event_time > a.monotonic_event_time) {
+ return true;
+ }
+
+ if (b.queue_index < a.queue_index) {
+ return false;
+ }
+ if (b.queue_index > a.queue_index) {
+ return true;
+ }
+
+ CHECK_EQ(a.realtime_event_time, b.realtime_event_time);
+ return false;
+ });
+
+ // TODO(austin): Be a bit more principled here, but we will want to do that
+ // after the logger rewrite. We hit this when one node finishes, but the
+ // other node isn't done yet. So there is no send time, but there is a
+ // receive time.
+ if (element != queue_index_map->end()) {
+ CHECK_EQ(element->monotonic_event_time,
+ delivery_timestamp.monotonic_remote_time);
+ CHECK_EQ(element->realtime_event_time,
+ delivery_timestamp.realtime_remote_time);
+ CHECK_EQ(element->queue_index, delivery_timestamp.remote_queue_index);
+
+ remote_queue_index = element->actual_queue_index;
+ }
+ }
+
+ // Send! Use the replayed queue index here instead of the logged queue index
+ // for the remote queue index. This makes re-logging work.
+ const bool sent =
+ sender->Send(data, size, delivery_timestamp.monotonic_remote_time,
+ delivery_timestamp.realtime_remote_time, remote_queue_index);
+ if (!sent) return false;
+
+ if (queue_index_map_[channel_index]) {
+ SentTimestamp timestamp;
+ timestamp.monotonic_event_time = delivery_timestamp.monotonic_event_time;
+ timestamp.realtime_event_time = delivery_timestamp.realtime_event_time;
+ timestamp.queue_index = delivery_timestamp.queue_index;
+ timestamp.actual_queue_index = sender->sent_queue_index();
+ queue_index_map_[channel_index]->emplace_back(timestamp);
+ } else if (remote_timestamp_senders_[channel_index] != nullptr) {
+ aos::Sender<MessageHeader>::Builder builder =
+ remote_timestamp_senders_[channel_index]->MakeBuilder();
+
+ logger::MessageHeader::Builder message_header_builder =
+ builder.MakeBuilder<logger::MessageHeader>();
+
+ message_header_builder.add_channel_index(
+ factory_channel_index_[channel_index]);
+
+ // Swap the remote and sent metrics. They are from the sender's
+ // perspective, not the receiver's perspective.
+ message_header_builder.add_monotonic_sent_time(
+ sender->monotonic_sent_time().time_since_epoch().count());
+ message_header_builder.add_realtime_sent_time(
+ sender->realtime_sent_time().time_since_epoch().count());
+ message_header_builder.add_queue_index(sender->sent_queue_index());
+
+ message_header_builder.add_monotonic_remote_time(
+ delivery_timestamp.monotonic_remote_time.time_since_epoch().count());
+ message_header_builder.add_realtime_remote_time(
+ delivery_timestamp.realtime_remote_time.time_since_epoch().count());
+
+ message_header_builder.add_remote_queue_index(remote_queue_index);
+
+ builder.Send(message_header_builder.Finish());
+ }
+
+ return true;
+}
+
+aos::Sender<MessageHeader> *LogReader::State::RemoteTimestampSender(
+ const Node *delivered_node) {
+ auto sender = remote_timestamp_senders_map_.find(delivered_node);
+
+ if (sender == remote_timestamp_senders_map_.end()) {
+ sender = remote_timestamp_senders_map_
+ .emplace(std::make_pair(
+ delivered_node,
+ event_loop()->MakeSender<MessageHeader>(
+ absl::StrCat("/aos/remote_timestamps/",
+ delivered_node->name()->string_view()))))
+ .first;
+ }
+
+ return &(sender->second);
}
std::tuple<TimestampMerger::DeliveryTimestamp, int,
@@ -1648,6 +1974,7 @@
for (size_t i = 0; i < channels_.size(); ++i) {
channels_[i].reset();
}
+ remote_timestamp_senders_map_.clear();
event_loop_unique_ptr_.reset();
event_loop_ = nullptr;
timer_handler_ = nullptr;
diff --git a/aos/events/logging/logger.fbs b/aos/events/logging/logger.fbs
index 2905dfa..fecfd6e 100644
--- a/aos/events/logging/logger.fbs
+++ b/aos/events/logging/logger.fbs
@@ -17,24 +17,24 @@
// Time this log file started on the monotonic clock in nanoseconds.
// If this isn't known (the log file is being recorded from another node
// where we don't know the time offset), both timestamps will be min_time.
- monotonic_start_time:int64 = -9223372036854775808;
+ monotonic_start_time:int64 = -9223372036854775808 (id: 0);
// Time this log file started on the realtime clock in nanoseconds.
- realtime_start_time:int64 = -9223372036854775808;
+ realtime_start_time:int64 = -9223372036854775808 (id: 1);
// Messages are not written in order to disk. They will be out of order by
// at most this duration (in nanoseconds). If the log reader buffers until
// it finds messages this much newer than it's simulation time, it will never
// find a message out of order.
- max_out_of_order_duration:long;
+ max_out_of_order_duration:long (id: 2);
// The configuration of the channels.
- configuration:aos.Configuration;
+ configuration:aos.Configuration (id: 3);
// Name of the device which this log file is for.
- name:string;
+ name:string (id: 4);
// The current node, if known and running in a multi-node configuration.
- node:Node;
+ node:Node (id: 5);
// All UUIDs are uuid4.
@@ -45,10 +45,22 @@
// ---- basename_timestamps/pi1/aos/remote_timestamps/pi2/aos.logger.MessageHeader.part0.bfbs, etc.
// \-- basename_pi2_data/pi2/aos/aos.message_bridge.Timestamp.part0.bfbs, etc.
- // All log files and parts from a single run of a logger executable will have
+ // All log files and parts from a single logging event will have
// the same uuid. This should be all the files generated on a single node.
// Used to correlate files recorded together.
- logger_uuid:string;
+ log_event_uuid:string (id: 6);
+
+ // All log parts generated by a single Logger instance will have the same
+ // value here.
+ logger_instance_uuid: string (id: 10);
+
+ // All log parts generated on a single node while it is booted will have the
+ // same value here. It also matches with the one used by systemd.
+ boot_uuid: string (id: 11);
+
+ // All log events across all nodes produced by a single high-level start event
+ // will have the same value here.
+ log_start_uuid: string (id: 12);
// Part files which go together all have headers. When creating a log file
// with multiple parts, the logger should stop writing to part n-1 as soon
@@ -61,41 +73,44 @@
// will be created when the start time is known.
// All the parts which go together have the same uuid.
- parts_uuid:string;
+ parts_uuid:string (id: 7);
// And the parts_index corresponds to which part this is in the sequence. The
// index should start at 0.
- parts_index:int32;
+ parts_index:int32 (id: 8);
+
+ // The node the data was logged on, if known and running in a multi-node configuration.
+ logger_node:Node (id: 9);
}
// Table holding a message.
table MessageHeader {
// Index into the channel datastructure in the log file header. This
// provides the data type.
- channel_index:uint;
+ channel_index:uint (id: 0);
// Time this message was sent on the monotonic clock in nanoseconds on this
// node.
- monotonic_sent_time:long;
+ monotonic_sent_time:long (id: 1);
// Time this message was sent on the realtime clock in nanoseconds on this
// node.
- realtime_sent_time:long;
+ realtime_sent_time:long (id: 2);
// Index into the ipc queue of this message. This should start with 0 and
// always monotonically increment if no messages were ever lost. It will
// wrap at a multiple of the queue size.
- queue_index:uint;
+ queue_index:uint (id: 3);
// TODO(austin): Format? Compressed?
// The nested flatbuffer.
- data:[ubyte];
+ data:[ubyte] (id: 4);
// Time this message was sent on the monotonic clock of the remote node in
// nanoseconds.
- monotonic_remote_time:int64 = -9223372036854775808;
+ monotonic_remote_time:int64 = -9223372036854775808 (id: 5);
// Time this message was sent on the realtime clock of the remote node in
// nanoseconds.
- realtime_remote_time:int64 = -9223372036854775808;
+ realtime_remote_time:int64 = -9223372036854775808 (id: 6);
// Queue index of this message on the remote node.
- remote_queue_index:uint32 = 4294967295;
+ remote_queue_index:uint32 = 4294967295 (id: 7);
}
root_type MessageHeader;
diff --git a/aos/events/logging/logger.h b/aos/events/logging/logger.h
index a3189db..b37fea2 100644
--- a/aos/events/logging/logger.h
+++ b/aos/events/logging/logger.h
@@ -13,6 +13,7 @@
#include "aos/events/event_loop.h"
#include "aos/events/logging/eigen_mpq.h"
#include "aos/events/logging/log_namer.h"
+#include "aos/events/logging/logfile_sorting.h"
#include "aos/events/logging/logfile_utils.h"
#include "aos/events/logging/logger_generated.h"
#include "aos/events/logging/uuid.h"
@@ -32,66 +33,126 @@
class Logger {
public:
// Constructs a logger.
- // base_name/log_namer: Object used to write data to disk in one or more log
- // files. If a base_name is passed in, a LocalLogNamer is wrapped
- // around it.
// event_loop: The event loop used to read the messages.
- // polling_period: The period used to poll the data.
// configuration: When provided, this is the configuration to log, and the
// configuration to use for the channel list to log. If not provided,
// this becomes the configuration from the event loop.
- Logger(std::string_view base_name, EventLoop *event_loop,
- std::chrono::milliseconds polling_period =
- std::chrono::milliseconds(100));
- Logger(std::string_view base_name, EventLoop *event_loop,
- const Configuration *configuration,
- std::chrono::milliseconds polling_period =
- std::chrono::milliseconds(100));
- Logger(std::unique_ptr<LogNamer> log_namer, EventLoop *event_loop,
- std::chrono::milliseconds polling_period =
- std::chrono::milliseconds(100));
- Logger(std::unique_ptr<LogNamer> log_namer, EventLoop *event_loop,
- const Configuration *configuration,
- std::chrono::milliseconds polling_period =
- std::chrono::milliseconds(100));
+ // should_log: When provided, a filter for channels to log. If not provided,
+ // all available channels are logged.
+ Logger(EventLoop *event_loop)
+ : Logger(event_loop, event_loop->configuration()) {}
+ Logger(EventLoop *event_loop, const Configuration *configuration)
+ : Logger(event_loop, configuration,
+ [](const Channel *) { return true; }) {}
+ Logger(EventLoop *event_loop, const Configuration *configuration,
+ std::function<bool(const Channel *)> should_log);
~Logger();
// Overrides the name in the log file header.
void set_name(std::string_view name) { name_ = name; }
+ // Sets the callback to run after each period of data is logged. Defaults to
+ // doing nothing.
+ //
+ // This callback may safely do things like call Rotate().
+ void set_on_logged_period(std::function<void()> on_logged_period) {
+ on_logged_period_ = std::move(on_logged_period);
+ }
+
+ // Sets the period between polling the data. Defaults to 100ms.
+ //
+ // Changing this while a set of files is being written may result in
+ // unreadable files.
+ void set_polling_period(std::chrono::nanoseconds polling_period) {
+ polling_period_ = polling_period;
+ }
+
+ std::string_view log_start_uuid() const { return log_start_uuid_; }
+ UUID logger_instance_uuid() const { return logger_instance_uuid_; }
+
+ // The maximum time for a single fetch which returned a message, or 0 if none
+ // of those have happened.
+ std::chrono::nanoseconds max_message_fetch_time() const {
+ return max_message_fetch_time_;
+ }
+ // The channel for that longest fetch which returned a message, or -1 if none
+ // of those have happened.
+ int max_message_fetch_time_channel() const {
+ return max_message_fetch_time_channel_;
+ }
+ // The size of the message returned by that longest fetch, or -1 if none of
+ // those have happened.
+ int max_message_fetch_time_size() const {
+ return max_message_fetch_time_size_;
+ }
+ // The total time spent fetching messages.
+ std::chrono::nanoseconds total_message_fetch_time() const {
+ return total_message_fetch_time_;
+ }
+ // The total number of fetch calls which returned messages.
+ int total_message_fetch_count() const { return total_message_fetch_count_; }
+ // The total number of bytes fetched.
+ int64_t total_message_fetch_bytes() const {
+ return total_message_fetch_bytes_;
+ }
+
+ // The total time spent in fetches which did not return a message.
+ std::chrono::nanoseconds total_nop_fetch_time() const {
+ return total_nop_fetch_time_;
+ }
+ // The total number of fetches which did not return a message.
+ int total_nop_fetch_count() const { return total_nop_fetch_count_; }
+
+ // The maximum time for a single copy, or 0 if none of those have happened.
+ std::chrono::nanoseconds max_copy_time() const { return max_copy_time_; }
+ // The channel for that longest copy, or -1 if none of those have happened.
+ int max_copy_time_channel() const { return max_copy_time_channel_; }
+ // The size of the message for that longest copy, or -1 if none of those have
+ // happened.
+ int max_copy_time_size() const { return max_copy_time_size_; }
+ // The total time spent copying messages.
+ std::chrono::nanoseconds total_copy_time() const { return total_copy_time_; }
+ // The total number of messages copied.
+ int total_copy_count() const { return total_copy_count_; }
+ // The total number of bytes copied.
+ int64_t total_copy_bytes() const { return total_copy_bytes_; }
+
+ void ResetStatisics();
+
// Rotates the log file(s), triggering new part files to be written for each
// log file.
void Rotate();
+ // Starts logging to files with the given naming scheme.
+ //
+ // log_start_uuid may be used to tie this log event to other log events across
+ // multiple nodes. The default (empty string) indicates there isn't one
+ // available.
+ void StartLogging(std::unique_ptr<LogNamer> log_namer,
+ std::string_view log_start_uuid = "");
+
+ // Stops logging. Ensures any messages through end_time make it into the log.
+ //
+ // If you want to stop ASAP, pass min_time to avoid reading any more messages.
+ //
+ // Returns the LogNamer in case the caller wants to do anything else with it
+ // before destroying it.
+ std::unique_ptr<LogNamer> StopLogging(
+ aos::monotonic_clock::time_point end_time);
+
+ // Returns whether a log is currently being written.
+ bool is_started() const { return static_cast<bool>(log_namer_); }
+
+ // Shortcut to call StartLogging with a LocalLogNamer when event processing
+ // starts.
+ void StartLoggingLocalNamerOnRun(std::string base_name) {
+ event_loop_->OnRun([this, base_name]() {
+ StartLogging(
+ std::make_unique<LocalLogNamer>(base_name, event_loop_->node()));
+ });
+ }
+
private:
- void WriteHeader();
- aos::SizePrefixedFlatbufferDetachedBuffer<LogFileHeader> MakeHeader(
- const Node *node);
-
- bool MaybeUpdateTimestamp(
- const Node *node, int node_index,
- aos::monotonic_clock::time_point monotonic_start_time,
- aos::realtime_clock::time_point realtime_start_time);
-
- void DoLogData();
-
- void WriteMissingTimestamps();
-
- void StartLogging();
-
- // Fetches from each channel until all the data is logged.
- void LogUntil(monotonic_clock::time_point t);
-
- EventLoop *event_loop_;
- const UUID uuid_;
- std::unique_ptr<LogNamer> log_namer_;
-
- // The configuration to place at the top of the log file.
- const Configuration *configuration_;
-
- // Name to save in the log file. Defaults to hostname.
- std::string name_;
-
// Structure to track both a fetcher, and if the data fetched has been
// written. We may want to delay writing data to disk so that we don't let
// data get too far out of order when written to disk so we can avoid making
@@ -100,41 +161,30 @@
std::unique_ptr<RawFetcher> fetcher;
bool written = false;
+ // Channel index to log to.
int channel_index = -1;
+ const Channel *channel = nullptr;
+ const Node *timestamp_node = nullptr;
LogType log_type = LogType::kLogMessage;
+ // We fill out the metadata at construction, but the actual writers have to
+ // be updated each time we start logging. To avoid duplicating the complex
+ // logic determining whether each writer should be initialized, we just
+ // stash the answer in separate member variables.
+ bool wants_writer = false;
DetachedBufferWriter *writer = nullptr;
+ bool wants_timestamp_writer = false;
DetachedBufferWriter *timestamp_writer = nullptr;
+ bool wants_contents_writer = false;
DetachedBufferWriter *contents_writer = nullptr;
- const Node *writer_node = nullptr;
- const Node *timestamp_node = nullptr;
+
int node_index = 0;
};
- std::vector<FetcherStruct> fetchers_;
- TimerHandler *timer_handler_;
-
- // Period to poll the channels.
- const std::chrono::milliseconds polling_period_;
-
- // Last time that data was written for all channels to disk.
- monotonic_clock::time_point last_synchronized_time_;
-
- monotonic_clock::time_point monotonic_start_time_;
- realtime_clock::time_point realtime_start_time_;
-
- // Max size that the header has consumed. This much extra data will be
- // reserved in the builder to avoid reallocating.
- size_t max_header_size_ = 0;
-
- // Fetcher for all the statistics from all the nodes.
- aos::Fetcher<message_bridge::ServerStatistics> server_statistics_fetcher_;
-
- // Sets the start time for a specific node.
- void SetStartTime(size_t node_index,
- aos::monotonic_clock::time_point monotonic_start_time,
- aos::realtime_clock::time_point realtime_start_time);
+ // Vector mapping from the channel index from the event loop to the logged
+ // channel index.
+ std::vector<int> event_loop_to_logged_channel_index_;
struct NodeState {
aos::monotonic_clock::time_point monotonic_start_time =
@@ -145,12 +195,93 @@
aos::SizePrefixedFlatbufferDetachedBuffer<LogFileHeader> log_file_header =
aos::SizePrefixedFlatbufferDetachedBuffer<LogFileHeader>::Empty();
};
+
+ void WriteHeader();
+ aos::SizePrefixedFlatbufferDetachedBuffer<LogFileHeader> MakeHeader(
+ const Node *node);
+
+ bool MaybeUpdateTimestamp(
+ const Node *node, int node_index,
+ aos::monotonic_clock::time_point monotonic_start_time,
+ aos::realtime_clock::time_point realtime_start_time);
+
+ void DoLogData(const monotonic_clock::time_point end_time);
+
+ void WriteMissingTimestamps();
+
+ // Fetches from each channel until all the data is logged.
+ void LogUntil(monotonic_clock::time_point t);
+
+ void RecordFetchResult(aos::monotonic_clock::time_point start,
+ aos::monotonic_clock::time_point end, bool got_new,
+ FetcherStruct *fetcher);
+
+ void RecordCreateMessageTime(aos::monotonic_clock::time_point start,
+ aos::monotonic_clock::time_point end,
+ FetcherStruct *fetcher);
+
+ // Sets the start time for a specific node.
+ void SetStartTime(size_t node_index,
+ aos::monotonic_clock::time_point monotonic_start_time,
+ aos::realtime_clock::time_point realtime_start_time);
+
+ EventLoop *const event_loop_;
+ // The configuration to place at the top of the log file.
+ const Configuration *const configuration_;
+
+ UUID log_event_uuid_ = UUID::Zero();
+ const UUID logger_instance_uuid_ = UUID::Random();
+ std::unique_ptr<LogNamer> log_namer_;
+ // Empty indicates there isn't one.
+ std::string log_start_uuid_;
+ const std::string boot_uuid_;
+
+ // Name to save in the log file. Defaults to hostname.
+ std::string name_;
+
+ std::function<void()> on_logged_period_ = []() {};
+
+ std::chrono::nanoseconds max_message_fetch_time_ =
+ std::chrono::nanoseconds::zero();
+ int max_message_fetch_time_channel_ = -1;
+ int max_message_fetch_time_size_ = -1;
+ std::chrono::nanoseconds total_message_fetch_time_ =
+ std::chrono::nanoseconds::zero();
+ int total_message_fetch_count_ = 0;
+ int64_t total_message_fetch_bytes_ = 0;
+
+ std::chrono::nanoseconds total_nop_fetch_time_ =
+ std::chrono::nanoseconds::zero();
+ int total_nop_fetch_count_ = 0;
+
+ std::chrono::nanoseconds max_copy_time_ = std::chrono::nanoseconds::zero();
+ int max_copy_time_channel_ = -1;
+ int max_copy_time_size_ = -1;
+ std::chrono::nanoseconds total_copy_time_ = std::chrono::nanoseconds::zero();
+ int total_copy_count_ = 0;
+ int64_t total_copy_bytes_ = 0;
+
+ std::vector<FetcherStruct> fetchers_;
+ TimerHandler *timer_handler_;
+
+ // Period to poll the channels.
+ std::chrono::nanoseconds polling_period_ = std::chrono::milliseconds(100);
+
+ // Last time that data was written for all channels to disk.
+ monotonic_clock::time_point last_synchronized_time_;
+
+ // Max size that the header has consumed. This much extra data will be
+ // reserved in the builder to avoid reallocating.
+ size_t max_header_size_ = 0;
+
+ // Fetcher for all the statistics from all the nodes.
+ aos::Fetcher<message_bridge::ServerStatistics> server_statistics_fetcher_;
+
std::vector<NodeState> node_state_;
};
-// Takes a bunch of parts and sorts them based on part_uuid and part_index.
-std::vector<std::vector<std::string>> SortParts(
- const std::vector<std::string> &parts);
+std::vector<std::vector<std::string>> ToLogReaderVector(
+ const std::vector<LogFile> &log_files);
// We end up with one of the following 3 log file types.
//
@@ -205,6 +336,8 @@
const Configuration *replay_configuration = nullptr);
LogReader(const std::vector<std::vector<std::string>> &filenames,
const Configuration *replay_configuration = nullptr);
+ LogReader(const std::vector<LogFile> &log_files,
+ const Configuration *replay_configuration = nullptr);
~LogReader();
// Registers all the callbacks to send the log file data out on an event loop
@@ -233,7 +366,11 @@
// SimulatedEventLoopFactory, and then get the configuration from there for
// everything else.
const Configuration *logged_configuration() const;
- // Returns the configuration being used for replay.
+ // Returns the configuration being used for replay from the log file.
+ // Note that this may be different from the configuration actually used for
+ // handling events. You should generally only use this to create a
+ // SimulatedEventLoopFactory, and then get the configuration from there for
+ // everything else.
// The pointer is invalidated whenever RemapLoggedChannel is called.
const Configuration *configuration() const;
@@ -243,8 +380,10 @@
std::vector<const Node *> Nodes() const;
// Returns the starting timestamp for the log file.
- monotonic_clock::time_point monotonic_start_time(const Node *node = nullptr);
- realtime_clock::time_point realtime_start_time(const Node *node = nullptr);
+ monotonic_clock::time_point monotonic_start_time(
+ const Node *node = nullptr) const;
+ realtime_clock::time_point realtime_start_time(
+ const Node *node = nullptr) const;
// Causes the logger to publish the provided channel on a different name so
// that replayed applications can publish on the proper channel name without
@@ -258,11 +397,28 @@
RemapLoggedChannel(name, T::GetFullyQualifiedName(), add_prefix);
}
+ // Remaps the provided channel, though this respects node mappings, and
+ // preserves them too. This makes it so if /aos -> /pi1/aos on one node,
+ // /original/aos -> /original/pi1/aos on the same node after renaming, just
+ // like you would hope.
+ //
+ // TODO(austin): If you have 2 nodes remapping something to the same channel,
+ // this doesn't handle that. No use cases exist yet for that, so it isn't
+ // being done yet.
+ void RemapLoggedChannel(std::string_view name, std::string_view type,
+ const Node *node,
+ std::string_view add_prefix = "/original");
template <typename T>
- bool HasChannel(std::string_view name) {
+ void RemapLoggedChannel(std::string_view name, const Node *node,
+ std::string_view add_prefix = "/original") {
+ RemapLoggedChannel(name, T::GetFullyQualifiedName(), node, add_prefix);
+ }
+
+ template <typename T>
+ bool HasChannel(std::string_view name, const Node *node = nullptr) {
return configuration::GetChannel(log_file_header()->configuration(), name,
T::GetFullyQualifiedName(), "",
- nullptr) != nullptr;
+ node) != nullptr;
}
SimulatedEventLoopFactory *event_loop_factory() {
@@ -353,6 +509,11 @@
}
}
+ // Returns the MessageHeader sender to log delivery timestamps to for the
+ // provided remote node.
+ aos::Sender<MessageHeader> *RemoteTimestampSender(
+ const Node *delivered_node);
+
// Converts a timestamp from the monotonic clock on this node to the
// distributed clock.
distributed_clock::time_point ToDistributedClock(
@@ -375,17 +536,19 @@
// Returns the current time on the remote node which sends messages on
// channel_index.
monotonic_clock::time_point monotonic_remote_now(size_t channel_index) {
- return channel_target_event_loop_factory_[channel_index]->monotonic_now();
+ return channel_source_state_[channel_index]
+ ->node_event_loop_factory_->monotonic_now();
}
distributed_clock::time_point RemoteToDistributedClock(
size_t channel_index, monotonic_clock::time_point time) {
- return channel_target_event_loop_factory_[channel_index]
- ->ToDistributedClock(time);
+ return channel_source_state_[channel_index]
+ ->node_event_loop_factory_->ToDistributedClock(time);
}
const Node *remote_node(size_t channel_index) {
- return channel_target_event_loop_factory_[channel_index]->node();
+ return channel_source_state_[channel_index]
+ ->node_event_loop_factory_->node();
}
monotonic_clock::time_point monotonic_now() {
@@ -400,9 +563,11 @@
void SetChannelCount(size_t count);
// Sets the sender, filter, and target factory for a channel.
- void SetChannel(size_t channel, std::unique_ptr<RawSender> sender,
+ void SetChannel(size_t logged_channel_index, size_t factory_channel_index,
+ std::unique_ptr<RawSender> sender,
message_bridge::NoncausalOffsetEstimator *filter,
- NodeEventLoopFactory *channel_target_event_loop_factory);
+ aos::Sender<MessageHeader> *remote_timestamp_sender,
+ State *source_state);
// Returns if we have read all the messages from all the logs.
bool at_end() const { return channel_merger_->at_end(); }
@@ -422,13 +587,7 @@
// Sends a buffer on the provided channel index.
bool Send(size_t channel_index, const void *data, size_t size,
- aos::monotonic_clock::time_point monotonic_remote_time,
- aos::realtime_clock::time_point realtime_remote_time,
- uint32_t remote_queue_index) {
- return channels_[channel_index]->Send(data, size, monotonic_remote_time,
- realtime_remote_time,
- remote_queue_index);
- }
+ const TimestampMerger::DeliveryTimestamp &delivery_timestamp);
// Returns a debug string for the channel merger.
std::string DebugString() const {
@@ -461,6 +620,28 @@
// Senders.
std::vector<std::unique_ptr<RawSender>> channels_;
+ std::vector<aos::Sender<MessageHeader> *> remote_timestamp_senders_;
+ // The mapping from logged channel index to sent channel index. Needed for
+ // sending out MessageHeaders.
+ std::vector<int> factory_channel_index_;
+
+ struct SentTimestamp {
+ monotonic_clock::time_point monotonic_event_time =
+ monotonic_clock::min_time;
+ realtime_clock::time_point realtime_event_time = realtime_clock::min_time;
+ uint32_t queue_index = 0xffffffff;
+
+ // The queue index that this message *actually* was sent with.
+ uint32_t actual_queue_index = 0xffffffff;
+ };
+
+ // Stores all the timestamps that have been sent on this channel. This is
+ // only done for channels which are forwarded and on the node which
+ // initially sends the message.
+ //
+ // TODO(austin): This whole concept is a hack. We should be able to
+ // associate state with the message as it gets sorted and recover it.
+ std::vector<std::unique_ptr<std::vector<SentTimestamp>>> queue_index_map_;
// Factory (if we are in sim) that this loop was created on.
NodeEventLoopFactory *node_event_loop_factory_ = nullptr;
@@ -478,7 +659,10 @@
// List of NodeEventLoopFactorys (or nullptr if it isn't a forwarded
// channel) which correspond to the originating node.
- std::vector<NodeEventLoopFactory *> channel_target_event_loop_factory_;
+ std::vector<State *> channel_source_state_;
+
+ std::map<const Node *, aos::Sender<MessageHeader>>
+ remote_timestamp_senders_map_;
};
// Node index -> State.
@@ -583,6 +767,7 @@
// logged_configuration(), and the string key will be the name of the channel
// to send on instead of the logged channel name.
std::map<size_t, std::string> remapped_channels_;
+ std::vector<MapT> maps_;
// Number of nodes which still have data to send. This is used to figure out
// when to exit.
diff --git a/aos/events/logging/logger_main.cc b/aos/events/logging/logger_main.cc
index f95fa17..f6ec4d6 100644
--- a/aos/events/logging/logger_main.cc
+++ b/aos/events/logging/logger_main.cc
@@ -1,8 +1,11 @@
+#include <sys/resource.h>
+#include <sys/time.h>
+
#include "aos/configuration.h"
#include "aos/events/logging/logger.h"
#include "aos/events/shm_event_loop.h"
-#include "aos/logging/log_namer.h"
#include "aos/init.h"
+#include "aos/logging/log_namer.h"
#include "gflags/gflags.h"
#include "glog/logging.h"
@@ -22,19 +25,23 @@
aos::ShmEventLoop event_loop(&config.message());
- std::unique_ptr<aos::logger::DetachedBufferWriter> writer;
std::unique_ptr<aos::logger::LogNamer> log_namer;
if (event_loop.node() == nullptr) {
log_namer = std::make_unique<aos::logger::LocalLogNamer>(
aos::logging::GetLogName("fbs_log"), event_loop.node());
} else {
log_namer = std::make_unique<aos::logger::MultiNodeLogNamer>(
- aos::logging::GetLogName("fbs_log"), event_loop.configuration(),
- event_loop.node());
+ absl::StrCat(aos::logging::GetLogName("fbs_log"), "/"),
+ event_loop.configuration(), event_loop.node());
}
- aos::logger::Logger logger(std::move(log_namer), &event_loop,
- std::chrono::milliseconds(100));
+ aos::logger::Logger logger(&event_loop);
+ event_loop.OnRun([&log_namer, &logger]() {
+ errno = 0;
+ setpriority(PRIO_PROCESS, 0, -20);
+ PCHECK(errno == 0) << ": Renicing to -20 failed";
+ logger.StartLogging(std::move(log_namer));
+ });
event_loop.Run();
diff --git a/aos/events/logging/logger_math.cc b/aos/events/logging/logger_math.cc
index c333f55..ea360cc 100644
--- a/aos/events/logging/logger_math.cc
+++ b/aos/events/logging/logger_math.cc
@@ -167,13 +167,64 @@
return std::make_tuple(
Eigen::Matrix<double, Eigen::Dynamic, 1>::Ones(nodes_count()),
Eigen::Matrix<double, Eigen::Dynamic, 1>::Zero(nodes_count()));
- } else {
- // TODO(austin): Solve just the nodes we know about. This is harder and
- // there are no logs which require this yet to test on.
- CHECK_EQ(cached_valid_node_count_, nodes_count())
- << ": TODO(austin): Handle partial valid nodes";
-
+ } else if (cached_valid_node_count_ == nodes_count()) {
return Solve(mpq_map, mpq_offsets);
+ } else {
+ // Strip out any columns (nodes) which aren't relevant. Solve the
+ // simplified problem, then set any nodes which were missing back to slope
+ // 1, offset 0 (ie the distributed clock).
+ Eigen::Matrix<mpq_class, Eigen::Dynamic, Eigen::Dynamic>
+ valid_node_mpq_map =
+ Eigen::Matrix<mpq_class, Eigen::Dynamic, Eigen::Dynamic>::Zero(
+ nonzero_offset_count, cached_valid_node_count_);
+
+ {
+ // Only copy over the columns with valid nodes in them.
+ size_t column = 0;
+ for (size_t i = 0; i < valid_nodes.size(); ++i) {
+ if (valid_nodes[i]) {
+ valid_node_mpq_map.col(column) = mpq_map.col(i);
+
+ ++column;
+ }
+ }
+ // The 1/n needs to be based on the number of nodes being solved.
+ // Recreate it here.
+ for (int j = 0; j < valid_node_mpq_map.cols(); ++j) {
+ valid_node_mpq_map(0, j) = mpq_class(1, cached_valid_node_count_);
+ }
+ }
+
+ VLOG(1) << "Reduced node filtered map "
+ << ToDouble(valid_node_mpq_map).format(HeavyFmt);
+ VLOG(1) << "Reduced node filtered offsets "
+ << ToDouble(mpq_offsets).format(HeavyFmt);
+
+ // Solve the simplified problem now.
+ std::tuple<Eigen::Matrix<double, Eigen::Dynamic, 1>,
+ Eigen::Matrix<double, Eigen::Dynamic, 1>>
+ valid_result = Solve(valid_node_mpq_map, mpq_offsets);
+
+ // And expand the results back into a solution matrix.
+ std::tuple<Eigen::Matrix<double, Eigen::Dynamic, 1>,
+ Eigen::Matrix<double, Eigen::Dynamic, 1>>
+ result = std::make_tuple(
+ Eigen::Matrix<double, Eigen::Dynamic, 1>::Ones(nodes_count()),
+ Eigen::Matrix<double, Eigen::Dynamic, 1>::Zero(nodes_count()));
+
+ {
+ size_t column = 0;
+ for (size_t i = 0; i < valid_nodes.size(); ++i) {
+ if (valid_nodes[i]) {
+ std::get<0>(result)(i) = std::get<0>(valid_result)(column);
+ std::get<1>(result)(i) = std::get<1>(valid_result)(column);
+
+ ++column;
+ }
+ }
+ }
+
+ return result;
}
} else {
const Eigen::Matrix<mpq_class, Eigen::Dynamic, Eigen::Dynamic> mpq_map =
diff --git a/aos/events/logging/logger_test.cc b/aos/events/logging/logger_test.cc
index 47fe9e2..57d274b 100644
--- a/aos/events/logging/logger_test.cc
+++ b/aos/events/logging/logger_test.cc
@@ -1,9 +1,12 @@
#include "aos/events/logging/logger.h"
#include "aos/events/event_loop.h"
+#include "aos/events/message_counter.h"
#include "aos/events/ping_lib.h"
#include "aos/events/pong_lib.h"
#include "aos/events/simulated_event_loop.h"
+#include "aos/network/timestamp_generated.h"
+#include "aos/testing/tmpdir.h"
#include "aos/util/file.h"
#include "glog/logging.h"
#include "gmock/gmock.h"
@@ -14,6 +17,7 @@
namespace testing {
namespace chrono = std::chrono;
+using aos::testing::MessageCounter;
class LoggerTest : public ::testing::Test {
public:
@@ -39,10 +43,12 @@
Pong pong_;
};
+using LoggerDeathTest = LoggerTest;
+
// Tests that we can startup at all. This confirms that the channels are all in
// the config.
TEST_F(LoggerTest, Starts) {
- const ::std::string tmpdir(getenv("TEST_TMPDIR"));
+ const ::std::string tmpdir = aos::testing::TestTmpDir();
const ::std::string base_name = tmpdir + "/logfile";
const ::std::string logfile = base_name + ".part0.bfbs";
// Remove it.
@@ -56,8 +62,9 @@
event_loop_factory_.RunFor(chrono::milliseconds(95));
- Logger logger(base_name, logger_event_loop.get(),
- std::chrono::milliseconds(100));
+ Logger logger(logger_event_loop.get());
+ logger.set_polling_period(std::chrono::milliseconds(100));
+ logger.StartLoggingLocalNamerOnRun(base_name);
event_loop_factory_.RunFor(chrono::milliseconds(20000));
}
@@ -99,6 +106,130 @@
EXPECT_EQ(ping_count, 2010);
}
+// Tests calling StartLogging twice.
+TEST_F(LoggerDeathTest, ExtraStart) {
+ const ::std::string tmpdir(getenv("TEST_TMPDIR"));
+ const ::std::string base_name1 = tmpdir + "/logfile1";
+ const ::std::string logfile1 = base_name1 + ".part0.bfbs";
+ const ::std::string base_name2 = tmpdir + "/logfile2";
+ const ::std::string logfile2 = base_name2 + ".part0.bfbs";
+ unlink(logfile1.c_str());
+ unlink(logfile2.c_str());
+
+ LOG(INFO) << "Logging data to " << logfile1 << " then " << logfile2;
+
+ {
+ std::unique_ptr<EventLoop> logger_event_loop =
+ event_loop_factory_.MakeEventLoop("logger");
+
+ event_loop_factory_.RunFor(chrono::milliseconds(95));
+
+ Logger logger(logger_event_loop.get());
+ logger.set_polling_period(std::chrono::milliseconds(100));
+ logger_event_loop->OnRun(
+ [base_name1, base_name2, &logger_event_loop, &logger]() {
+ logger.StartLogging(std::make_unique<LocalLogNamer>(
+ base_name1, logger_event_loop->node()));
+ EXPECT_DEATH(logger.StartLogging(std::make_unique<LocalLogNamer>(
+ base_name2, logger_event_loop->node())),
+ "Already logging");
+ });
+ event_loop_factory_.RunFor(chrono::milliseconds(20000));
+ }
+}
+
+// Tests calling StopLogging twice.
+TEST_F(LoggerDeathTest, ExtraStop) {
+ const ::std::string tmpdir(getenv("TEST_TMPDIR"));
+ const ::std::string base_name = tmpdir + "/logfile";
+ const ::std::string logfile = base_name + ".part0.bfbs";
+ // Remove it.
+ unlink(logfile.c_str());
+
+ LOG(INFO) << "Logging data to " << logfile;
+
+ {
+ std::unique_ptr<EventLoop> logger_event_loop =
+ event_loop_factory_.MakeEventLoop("logger");
+
+ event_loop_factory_.RunFor(chrono::milliseconds(95));
+
+ Logger logger(logger_event_loop.get());
+ logger.set_polling_period(std::chrono::milliseconds(100));
+ logger_event_loop->OnRun([base_name, &logger_event_loop, &logger]() {
+ logger.StartLogging(std::make_unique<LocalLogNamer>(
+ base_name, logger_event_loop->node()));
+ logger.StopLogging(aos::monotonic_clock::min_time);
+ EXPECT_DEATH(logger.StopLogging(aos::monotonic_clock::min_time),
+ "Not logging right now");
+ });
+ event_loop_factory_.RunFor(chrono::milliseconds(20000));
+ }
+}
+
+// Tests that we can startup twice.
+TEST_F(LoggerTest, StartsTwice) {
+ const ::std::string tmpdir(getenv("TEST_TMPDIR"));
+ const ::std::string base_name1 = tmpdir + "/logfile1";
+ const ::std::string logfile1 = base_name1 + ".part0.bfbs";
+ const ::std::string base_name2 = tmpdir + "/logfile2";
+ const ::std::string logfile2 = base_name2 + ".part0.bfbs";
+ unlink(logfile1.c_str());
+ unlink(logfile2.c_str());
+
+ LOG(INFO) << "Logging data to " << logfile1 << " then " << logfile2;
+
+ {
+ std::unique_ptr<EventLoop> logger_event_loop =
+ event_loop_factory_.MakeEventLoop("logger");
+
+ event_loop_factory_.RunFor(chrono::milliseconds(95));
+
+ Logger logger(logger_event_loop.get());
+ logger.set_polling_period(std::chrono::milliseconds(100));
+ logger.StartLogging(
+ std::make_unique<LocalLogNamer>(base_name1, logger_event_loop->node()));
+ event_loop_factory_.RunFor(chrono::milliseconds(10000));
+ logger.StopLogging(logger_event_loop->monotonic_now());
+ event_loop_factory_.RunFor(chrono::milliseconds(10000));
+ logger.StartLogging(
+ std::make_unique<LocalLogNamer>(base_name2, logger_event_loop->node()));
+ event_loop_factory_.RunFor(chrono::milliseconds(10000));
+ }
+
+ for (const auto &logfile :
+ {std::make_tuple(logfile1, 10), std::make_tuple(logfile2, 2010)}) {
+ SCOPED_TRACE(std::get<0>(logfile));
+ LogReader reader(std::get<0>(logfile));
+ reader.Register();
+
+ EXPECT_THAT(reader.Nodes(), ::testing::ElementsAre(nullptr));
+
+ std::unique_ptr<EventLoop> test_event_loop =
+ reader.event_loop_factory()->MakeEventLoop("log_reader");
+
+ int ping_count = std::get<1>(logfile);
+ int pong_count = std::get<1>(logfile);
+
+ // Confirm that the ping and pong counts both match, and the value also
+ // matches.
+ test_event_loop->MakeWatcher("/test",
+ [&ping_count](const examples::Ping &ping) {
+ EXPECT_EQ(ping.value(), ping_count + 1);
+ ++ping_count;
+ });
+ test_event_loop->MakeWatcher(
+ "/test", [&pong_count, &ping_count](const examples::Pong &pong) {
+ EXPECT_EQ(pong.value(), pong_count + 1);
+ ++pong_count;
+ EXPECT_EQ(ping_count, pong_count);
+ });
+
+ reader.event_loop_factory()->RunFor(std::chrono::seconds(100));
+ EXPECT_EQ(ping_count, std::get<1>(logfile) + 1000);
+ }
+}
+
// Tests that we can read and write rotated log files.
TEST_F(LoggerTest, RotatedLogFile) {
const ::std::string tmpdir(getenv("TEST_TMPDIR"));
@@ -117,9 +248,9 @@
event_loop_factory_.RunFor(chrono::milliseconds(95));
- Logger logger(
- std::make_unique<LocalLogNamer>(base_name, logger_event_loop->node()),
- logger_event_loop.get(), std::chrono::milliseconds(100));
+ Logger logger(logger_event_loop.get());
+ logger.set_polling_period(std::chrono::milliseconds(100));
+ logger.StartLoggingLocalNamerOnRun(base_name);
event_loop_factory_.RunFor(chrono::milliseconds(10000));
logger.Rotate();
event_loop_factory_.RunFor(chrono::milliseconds(10000));
@@ -133,8 +264,8 @@
log_header.emplace_back(ReadHeader(f));
}
- EXPECT_EQ(log_header[0].message().logger_uuid()->string_view(),
- log_header[1].message().logger_uuid()->string_view());
+ EXPECT_EQ(log_header[0].message().log_event_uuid()->string_view(),
+ log_header[1].message().log_event_uuid()->string_view());
EXPECT_EQ(log_header[0].message().parts_uuid()->string_view(),
log_header[1].message().parts_uuid()->string_view());
@@ -216,8 +347,9 @@
chrono::microseconds(50));
});
- Logger logger(base_name, logger_event_loop.get(),
- std::chrono::milliseconds(100));
+ Logger logger(logger_event_loop.get());
+ logger.set_polling_period(std::chrono::milliseconds(100));
+ logger.StartLoggingLocalNamerOnRun(base_name);
event_loop_factory_.RunFor(chrono::milliseconds(1000));
}
@@ -282,16 +414,26 @@
std::unique_ptr<Logger> logger;
};
- LoggerState MakeLogger(const Node *node) {
- return {event_loop_factory_.MakeEventLoop("logger", node), {}};
+ LoggerState MakeLogger(const Node *node,
+ SimulatedEventLoopFactory *factory = nullptr) {
+ if (factory == nullptr) {
+ factory = &event_loop_factory_;
+ }
+ return {factory->MakeEventLoop("logger", node), {}};
}
- void StartLogger(LoggerState *logger) {
- logger->logger = std::make_unique<Logger>(
- std::make_unique<MultiNodeLogNamer>(logfile_base_,
- logger->event_loop->configuration(),
- logger->event_loop->node()),
- logger->event_loop.get(), chrono::milliseconds(100));
+ void StartLogger(LoggerState *logger, std::string logfile_base = "") {
+ if (logfile_base.empty()) {
+ logfile_base = logfile_base_;
+ }
+
+ logger->logger = std::make_unique<Logger>(logger->event_loop.get());
+ logger->logger->set_polling_period(std::chrono::milliseconds(100));
+ logger->event_loop->OnRun([logger, logfile_base]() {
+ logger->logger->StartLogging(std::make_unique<MultiNodeLogNamer>(
+ logfile_base, logger->event_loop->configuration(),
+ logger->event_loop->node()));
+ });
}
// Config and factory.
@@ -399,7 +541,7 @@
std::vector<FlatbufferVector<LogFileHeader>> log_header;
for (std::string_view f : logfiles_) {
log_header.emplace_back(ReadHeader(f));
- logfile_uuids.insert(log_header.back().message().logger_uuid()->str());
+ logfile_uuids.insert(log_header.back().message().log_event_uuid()->str());
parts_uuids.insert(log_header.back().message().parts_uuid()->str());
}
@@ -524,7 +666,7 @@
LogReader reader(structured_logfiles_);
- SimulatedEventLoopFactory log_reader_factory(reader.logged_configuration());
+ SimulatedEventLoopFactory log_reader_factory(reader.configuration());
log_reader_factory.set_send_delay(chrono::microseconds(0));
// This sends out the fetched messages and advances time to the start of the
@@ -599,7 +741,7 @@
++pi2_ping_count;
});
- constexpr ssize_t kQueueIndexOffset = 0;
+ constexpr ssize_t kQueueIndexOffset = -9;
// Confirm that the ping and pong counts both match, and the value also
// matches.
pi1_event_loop->MakeWatcher(
@@ -639,7 +781,7 @@
<< pi2_event_loop->context().monotonic_event_time;
EXPECT_EQ(pi2_event_loop->context().remote_queue_index,
- pi2_pong_count + kQueueIndexOffset - 9);
+ pi2_pong_count + kQueueIndexOffset);
EXPECT_EQ(pi2_event_loop->context().monotonic_remote_time,
chrono::microseconds(200) +
@@ -722,7 +864,7 @@
LogReader reader(structured_logfiles_);
- SimulatedEventLoopFactory log_reader_factory(reader.logged_configuration());
+ SimulatedEventLoopFactory log_reader_factory(reader.configuration());
log_reader_factory.set_send_delay(chrono::microseconds(0));
// This sends out the fetched messages and advances time to the start of the
@@ -862,7 +1004,7 @@
LogReader reader(structured_logfiles_);
- SimulatedEventLoopFactory log_reader_factory(reader.logged_configuration());
+ SimulatedEventLoopFactory log_reader_factory(reader.configuration());
log_reader_factory.set_send_delay(chrono::microseconds(0));
const Node *pi1 =
@@ -973,14 +1115,334 @@
event_loop_factory_.RunFor(chrono::milliseconds(2000));
}
- const std::vector<std::vector<std::string>> sorted_parts =
- SortParts(logfiles_);
+ const std::vector<LogFile> sorted_parts = SortParts(logfiles_);
+
+ EXPECT_EQ(sorted_parts.size(), 2u);
+
+ // Count up the number of UUIDs and make sure they are what we expect as a
+ // sanity check.
+ std::set<std::string> log_event_uuids;
+ std::set<std::string> parts_uuids;
+ std::set<std::string> both_uuids;
+
+ size_t missing_rt_count = 0;
+
+ std::vector<std::string> logger_nodes;
+ for (const LogFile &log_file : sorted_parts) {
+ EXPECT_FALSE(log_file.log_event_uuid.empty());
+ log_event_uuids.insert(log_file.log_event_uuid);
+ logger_nodes.emplace_back(log_file.logger_node);
+ both_uuids.insert(log_file.log_event_uuid);
+
+ for (const LogParts &part : log_file.parts) {
+ EXPECT_NE(part.monotonic_start_time, aos::monotonic_clock::min_time)
+ << ": " << part;
+ missing_rt_count +=
+ part.realtime_start_time == aos::realtime_clock::min_time;
+
+ EXPECT_TRUE(log_event_uuids.find(part.log_event_uuid) !=
+ log_event_uuids.end());
+ EXPECT_NE(part.node, "");
+ parts_uuids.insert(part.parts_uuid);
+ both_uuids.insert(part.parts_uuid);
+ }
+ }
+
+ // We won't have RT timestamps for 5 log files. We don't log the RT start
+ // time on remote nodes because we don't know it and would be guessing. And
+ // the log reader can actually do a better job.
+ EXPECT_EQ(missing_rt_count, 5u);
+
+ EXPECT_EQ(log_event_uuids.size(), 2u);
+ EXPECT_EQ(parts_uuids.size(), ToLogReaderVector(sorted_parts).size());
+ EXPECT_EQ(log_event_uuids.size() + parts_uuids.size(), both_uuids.size());
// Test that each list of parts is in order. Don't worry about the ordering
// between part file lists though.
// (inner vectors all need to be in order, but outer one doesn't matter).
- EXPECT_THAT(sorted_parts,
+ EXPECT_THAT(ToLogReaderVector(sorted_parts),
::testing::UnorderedElementsAreArray(structured_logfiles_));
+
+ EXPECT_THAT(logger_nodes, ::testing::UnorderedElementsAre("pi1", "pi2"));
+
+ EXPECT_NE(sorted_parts[0].realtime_start_time, aos::realtime_clock::min_time);
+ EXPECT_NE(sorted_parts[1].realtime_start_time, aos::realtime_clock::min_time);
+
+ EXPECT_NE(sorted_parts[0].monotonic_start_time,
+ aos::monotonic_clock::min_time);
+ EXPECT_NE(sorted_parts[1].monotonic_start_time,
+ aos::monotonic_clock::min_time);
+}
+
+// Tests that if we remap a remapped channel, it shows up correctly.
+TEST_F(MultinodeLoggerTest, RemapLoggedChannel) {
+ {
+ LoggerState pi1_logger = MakeLogger(pi1_);
+ LoggerState pi2_logger = MakeLogger(pi2_);
+
+ event_loop_factory_.RunFor(chrono::milliseconds(95));
+
+ StartLogger(&pi1_logger);
+ StartLogger(&pi2_logger);
+
+ event_loop_factory_.RunFor(chrono::milliseconds(20000));
+ }
+
+ LogReader reader(structured_logfiles_);
+
+ // Remap just on pi1.
+ reader.RemapLoggedChannel<aos::timing::Report>(
+ "/aos", configuration::GetNode(reader.configuration(), "pi1"));
+
+ SimulatedEventLoopFactory log_reader_factory(reader.configuration());
+ log_reader_factory.set_send_delay(chrono::microseconds(0));
+
+ reader.Register(&log_reader_factory);
+
+ const Node *pi1 =
+ configuration::GetNode(log_reader_factory.configuration(), "pi1");
+ const Node *pi2 =
+ configuration::GetNode(log_reader_factory.configuration(), "pi2");
+
+ // Confirm we can read the data on the remapped channel, just for pi1. Nothing
+ // else should have moved.
+ std::unique_ptr<EventLoop> pi1_event_loop =
+ log_reader_factory.MakeEventLoop("test", pi1);
+ pi1_event_loop->SkipTimingReport();
+ std::unique_ptr<EventLoop> full_pi1_event_loop =
+ log_reader_factory.MakeEventLoop("test", pi1);
+ full_pi1_event_loop->SkipTimingReport();
+ std::unique_ptr<EventLoop> pi2_event_loop =
+ log_reader_factory.MakeEventLoop("test", pi2);
+ pi2_event_loop->SkipTimingReport();
+
+ MessageCounter<aos::timing::Report> pi1_timing_report(pi1_event_loop.get(),
+ "/aos");
+ MessageCounter<aos::timing::Report> full_pi1_timing_report(
+ full_pi1_event_loop.get(), "/pi1/aos");
+ MessageCounter<aos::timing::Report> pi1_original_timing_report(
+ pi1_event_loop.get(), "/original/aos");
+ MessageCounter<aos::timing::Report> full_pi1_original_timing_report(
+ full_pi1_event_loop.get(), "/original/pi1/aos");
+ MessageCounter<aos::timing::Report> pi2_timing_report(pi2_event_loop.get(),
+ "/aos");
+
+ log_reader_factory.Run();
+
+ EXPECT_EQ(pi1_timing_report.count(), 0u);
+ EXPECT_EQ(full_pi1_timing_report.count(), 0u);
+ EXPECT_NE(pi1_original_timing_report.count(), 0u);
+ EXPECT_NE(full_pi1_original_timing_report.count(), 0u);
+ EXPECT_NE(pi2_timing_report.count(), 0u);
+
+ reader.Deregister();
+}
+
+// Tests that we properly recreate forwarded timestamps when replaying a log.
+// This should be enough that we can then re-run the logger and get a valid log
+// back.
+TEST_F(MultinodeLoggerTest, MessageHeader) {
+ {
+ LoggerState pi1_logger = MakeLogger(pi1_);
+ LoggerState pi2_logger = MakeLogger(pi2_);
+
+ event_loop_factory_.RunFor(chrono::milliseconds(95));
+
+ StartLogger(&pi1_logger);
+ StartLogger(&pi2_logger);
+
+ event_loop_factory_.RunFor(chrono::milliseconds(20000));
+ }
+
+ LogReader reader(structured_logfiles_);
+
+ SimulatedEventLoopFactory log_reader_factory(reader.configuration());
+ log_reader_factory.set_send_delay(chrono::microseconds(0));
+
+ // This sends out the fetched messages and advances time to the start of the
+ // log file.
+ reader.Register(&log_reader_factory);
+
+ const Node *pi1 =
+ configuration::GetNode(log_reader_factory.configuration(), "pi1");
+ const Node *pi2 =
+ configuration::GetNode(log_reader_factory.configuration(), "pi2");
+
+ LOG(INFO) << "Start time " << reader.monotonic_start_time(pi1) << " pi1";
+ LOG(INFO) << "Start time " << reader.monotonic_start_time(pi2) << " pi2";
+ LOG(INFO) << "now pi1 "
+ << log_reader_factory.GetNodeEventLoopFactory(pi1)->monotonic_now();
+ LOG(INFO) << "now pi2 "
+ << log_reader_factory.GetNodeEventLoopFactory(pi2)->monotonic_now();
+
+ EXPECT_THAT(reader.Nodes(), ::testing::ElementsAre(pi1, pi2));
+
+ reader.event_loop_factory()->set_send_delay(chrono::microseconds(0));
+
+ std::unique_ptr<EventLoop> pi1_event_loop =
+ log_reader_factory.MakeEventLoop("test", pi1);
+ std::unique_ptr<EventLoop> pi2_event_loop =
+ log_reader_factory.MakeEventLoop("test", pi2);
+
+ MessageCounter<MessageHeader> pi1_original_message_header_counter(
+ pi1_event_loop.get(), "/original/aos/remote_timestamps/pi2");
+ MessageCounter<MessageHeader> pi2_original_message_header_counter(
+ pi2_event_loop.get(), "/original/aos/remote_timestamps/pi1");
+
+ aos::Fetcher<message_bridge::Timestamp> pi1_timestamp_on_pi1_fetcher =
+ pi1_event_loop->MakeFetcher<message_bridge::Timestamp>("/pi1/aos");
+ aos::Fetcher<message_bridge::Timestamp> pi1_timestamp_on_pi2_fetcher =
+ pi2_event_loop->MakeFetcher<message_bridge::Timestamp>("/pi1/aos");
+
+ aos::Fetcher<examples::Ping> ping_on_pi1_fetcher =
+ pi1_event_loop->MakeFetcher<examples::Ping>("/test");
+ aos::Fetcher<examples::Ping> ping_on_pi2_fetcher =
+ pi2_event_loop->MakeFetcher<examples::Ping>("/test");
+
+ aos::Fetcher<message_bridge::Timestamp> pi2_timestamp_on_pi2_fetcher =
+ pi2_event_loop->MakeFetcher<message_bridge::Timestamp>("/pi2/aos");
+ aos::Fetcher<message_bridge::Timestamp> pi2_timestamp_on_pi1_fetcher =
+ pi1_event_loop->MakeFetcher<message_bridge::Timestamp>("/pi2/aos");
+
+ aos::Fetcher<examples::Pong> pong_on_pi2_fetcher =
+ pi2_event_loop->MakeFetcher<examples::Pong>("/test");
+ aos::Fetcher<examples::Pong> pong_on_pi1_fetcher =
+ pi1_event_loop->MakeFetcher<examples::Pong>("/test");
+
+ const size_t pi1_timestamp_channel = configuration::ChannelIndex(
+ pi1_event_loop->configuration(), pi1_timestamp_on_pi1_fetcher.channel());
+ const size_t ping_timestamp_channel = configuration::ChannelIndex(
+ pi2_event_loop->configuration(), ping_on_pi2_fetcher.channel());
+
+ const size_t pi2_timestamp_channel = configuration::ChannelIndex(
+ pi2_event_loop->configuration(), pi2_timestamp_on_pi2_fetcher.channel());
+ const size_t pong_timestamp_channel = configuration::ChannelIndex(
+ pi1_event_loop->configuration(), pong_on_pi1_fetcher.channel());
+
+ pi1_event_loop->MakeWatcher(
+ "/aos/remote_timestamps/pi2",
+ [&pi1_event_loop, pi1_timestamp_channel, ping_timestamp_channel,
+ &pi1_timestamp_on_pi1_fetcher, &pi1_timestamp_on_pi2_fetcher,
+ &ping_on_pi1_fetcher,
+ &ping_on_pi2_fetcher](const MessageHeader &header) {
+ const aos::monotonic_clock::time_point header_monotonic_sent_time(
+ chrono::nanoseconds(header.monotonic_sent_time()));
+ const aos::realtime_clock::time_point header_realtime_sent_time(
+ chrono::nanoseconds(header.realtime_sent_time()));
+ const aos::monotonic_clock::time_point header_monotonic_remote_time(
+ chrono::nanoseconds(header.monotonic_remote_time()));
+ const aos::realtime_clock::time_point header_realtime_remote_time(
+ chrono::nanoseconds(header.realtime_remote_time()));
+
+ const Context *pi1_context = nullptr;
+ const Context *pi2_context = nullptr;
+
+ if (header.channel_index() == pi1_timestamp_channel) {
+ ASSERT_TRUE(pi1_timestamp_on_pi1_fetcher.FetchNext());
+ ASSERT_TRUE(pi1_timestamp_on_pi2_fetcher.FetchNext());
+ pi1_context = &pi1_timestamp_on_pi1_fetcher.context();
+ pi2_context = &pi1_timestamp_on_pi2_fetcher.context();
+ } else if (header.channel_index() == ping_timestamp_channel) {
+ ASSERT_TRUE(ping_on_pi1_fetcher.FetchNext());
+ ASSERT_TRUE(ping_on_pi2_fetcher.FetchNext());
+ pi1_context = &ping_on_pi1_fetcher.context();
+ pi2_context = &ping_on_pi2_fetcher.context();
+ } else {
+ LOG(FATAL) << "Unknown channel " << FlatbufferToJson(&header) << " "
+ << configuration::CleanedChannelToString(
+ pi1_event_loop->configuration()->channels()->Get(
+ header.channel_index()));
+ }
+
+ EXPECT_EQ(pi1_context->queue_index, header.remote_queue_index());
+ EXPECT_EQ(pi2_context->remote_queue_index, header.remote_queue_index());
+ EXPECT_EQ(pi2_context->queue_index, header.queue_index());
+
+ EXPECT_EQ(pi2_context->monotonic_event_time,
+ header_monotonic_sent_time);
+ EXPECT_EQ(pi2_context->realtime_event_time, header_realtime_sent_time);
+ EXPECT_EQ(pi2_context->realtime_remote_time,
+ header_realtime_remote_time);
+ EXPECT_EQ(pi2_context->monotonic_remote_time,
+ header_monotonic_remote_time);
+
+ EXPECT_EQ(pi1_context->realtime_event_time,
+ header_realtime_remote_time);
+ EXPECT_EQ(pi1_context->monotonic_event_time,
+ header_monotonic_remote_time);
+ });
+ pi2_event_loop->MakeWatcher(
+ "/aos/remote_timestamps/pi1",
+ [&pi2_event_loop, pi2_timestamp_channel, pong_timestamp_channel,
+ &pi2_timestamp_on_pi2_fetcher, &pi2_timestamp_on_pi1_fetcher,
+ &pong_on_pi2_fetcher,
+ &pong_on_pi1_fetcher](const MessageHeader &header) {
+ const aos::monotonic_clock::time_point header_monotonic_sent_time(
+ chrono::nanoseconds(header.monotonic_sent_time()));
+ const aos::realtime_clock::time_point header_realtime_sent_time(
+ chrono::nanoseconds(header.realtime_sent_time()));
+ const aos::monotonic_clock::time_point header_monotonic_remote_time(
+ chrono::nanoseconds(header.monotonic_remote_time()));
+ const aos::realtime_clock::time_point header_realtime_remote_time(
+ chrono::nanoseconds(header.realtime_remote_time()));
+
+ const Context *pi2_context = nullptr;
+ const Context *pi1_context = nullptr;
+
+ if (header.channel_index() == pi2_timestamp_channel) {
+ ASSERT_TRUE(pi2_timestamp_on_pi2_fetcher.FetchNext());
+ ASSERT_TRUE(pi2_timestamp_on_pi1_fetcher.FetchNext());
+ pi2_context = &pi2_timestamp_on_pi2_fetcher.context();
+ pi1_context = &pi2_timestamp_on_pi1_fetcher.context();
+ } else if (header.channel_index() == pong_timestamp_channel) {
+ ASSERT_TRUE(pong_on_pi2_fetcher.FetchNext());
+ ASSERT_TRUE(pong_on_pi1_fetcher.FetchNext());
+ pi2_context = &pong_on_pi2_fetcher.context();
+ pi1_context = &pong_on_pi1_fetcher.context();
+ } else {
+ LOG(FATAL) << "Unknown channel " << FlatbufferToJson(&header) << " "
+ << configuration::CleanedChannelToString(
+ pi2_event_loop->configuration()->channels()->Get(
+ header.channel_index()));
+ }
+
+ EXPECT_EQ(pi2_context->queue_index, header.remote_queue_index());
+ EXPECT_EQ(pi1_context->remote_queue_index, header.remote_queue_index());
+ EXPECT_EQ(pi1_context->queue_index, header.queue_index());
+
+ EXPECT_EQ(pi1_context->monotonic_event_time,
+ header_monotonic_sent_time);
+ EXPECT_EQ(pi1_context->realtime_event_time, header_realtime_sent_time);
+ EXPECT_EQ(pi1_context->realtime_remote_time,
+ header_realtime_remote_time);
+ EXPECT_EQ(pi1_context->monotonic_remote_time,
+ header_monotonic_remote_time);
+
+ EXPECT_EQ(pi2_context->realtime_event_time,
+ header_realtime_remote_time);
+ EXPECT_EQ(pi2_context->monotonic_event_time,
+ header_monotonic_remote_time);
+ });
+
+ // And confirm we can re-create a log again, while checking the contents.
+ {
+ LoggerState pi1_logger = MakeLogger(
+ configuration::GetNode(log_reader_factory.configuration(), pi1_),
+ &log_reader_factory);
+ LoggerState pi2_logger = MakeLogger(
+ configuration::GetNode(log_reader_factory.configuration(), pi2_),
+ &log_reader_factory);
+
+ StartLogger(&pi1_logger, "relogged");
+ StartLogger(&pi2_logger, "relogged");
+
+ log_reader_factory.Run();
+ }
+
+ EXPECT_EQ(pi2_original_message_header_counter.count(), 0u);
+ EXPECT_EQ(pi1_original_message_header_counter.count(), 0u);
+
+ reader.Deregister();
}
// TODO(austin): We can write a test which recreates a logfile and confirms that
diff --git a/aos/events/logging/lzma_encoder.cc b/aos/events/logging/lzma_encoder.cc
new file mode 100644
index 0000000..a8cafda
--- /dev/null
+++ b/aos/events/logging/lzma_encoder.cc
@@ -0,0 +1,194 @@
+#include "aos/events/logging/lzma_encoder.h"
+
+#include "glog/logging.h"
+
+namespace aos::logger {
+namespace {
+
+// Returns if `status` is not an error code, otherwise logs the appropriate
+// error message and crashes.
+void CheckLzmaCodeIsOk(lzma_ret status) {
+ switch (status) {
+ case LZMA_OK:
+ case LZMA_STREAM_END:
+ return;
+ case LZMA_MEM_ERROR:
+ LOG(FATAL) << "Memory allocation failed:" << status;
+ case LZMA_OPTIONS_ERROR:
+ LOG(FATAL) << "The given compression preset or decompression options are "
+ "not supported: "
+ << status;
+ case LZMA_UNSUPPORTED_CHECK:
+ LOG(FATAL) << "The given check type is not supported: " << status;
+ case LZMA_PROG_ERROR:
+ LOG(FATAL) << "One or more of the parameters have values that will never "
+ "be valid: "
+ << status;
+ case LZMA_MEMLIMIT_ERROR:
+ LOG(FATAL) << "Decoder needs more memory than allowed by the specified "
+ "memory usage limit: "
+ << status;
+ case LZMA_FORMAT_ERROR:
+ LOG(FATAL) << "File format not recognized: " << status;
+ case LZMA_DATA_ERROR:
+ LOG(FATAL) << "Compressed file is corrupt: " << status;
+ case LZMA_BUF_ERROR:
+ LOG(FATAL) << "Compressed file is truncated or corrupt: " << status;
+ default:
+ LOG(FATAL) << "Unexpected return value: " << status;
+ }
+}
+
+} // namespace
+
+LzmaEncoder::LzmaEncoder(const uint32_t compression_preset)
+ : stream_(LZMA_STREAM_INIT), compression_preset_(compression_preset) {
+ CHECK_GE(compression_preset_, 0u)
+ << ": Compression preset must be in the range [0, 9].";
+ CHECK_LE(compression_preset_, 9u)
+ << ": Compression preset must be in the range [0, 9].";
+
+ lzma_ret status =
+ lzma_easy_encoder(&stream_, compression_preset_, LZMA_CHECK_CRC64);
+ CheckLzmaCodeIsOk(status);
+ stream_.avail_out = 0;
+ VLOG(2) << "LzmaEncoder: Initialization succeeded.";
+}
+
+LzmaEncoder::~LzmaEncoder() { lzma_end(&stream_); }
+
+void LzmaEncoder::Encode(flatbuffers::DetachedBuffer &&in) {
+ CHECK(in.data()) << ": Encode called with nullptr.";
+
+ stream_.next_in = in.data();
+ stream_.avail_in = in.size();
+
+ RunLzmaCode(LZMA_RUN);
+}
+
+void LzmaEncoder::Finish() { RunLzmaCode(LZMA_FINISH); }
+
+void LzmaEncoder::Clear(const int n) {
+ CHECK_GE(n, 0);
+ CHECK_LE(static_cast<size_t>(n), queue_size());
+ queue_.erase(queue_.begin(), queue_.begin() + n);
+ if (queue_.empty()) {
+ stream_.next_out = nullptr;
+ stream_.avail_out = 0;
+ }
+}
+
+std::vector<absl::Span<const uint8_t>> LzmaEncoder::queue() const {
+ std::vector<absl::Span<const uint8_t>> queue;
+ if (queue_.empty()) {
+ return queue;
+ }
+ for (size_t i = 0; i < queue_.size() - 1; ++i) {
+ queue.emplace_back(
+ absl::MakeConstSpan(queue_.at(i).data(), queue_.at(i).size()));
+ }
+ // For the last buffer in the queue, we must account for the possibility that
+ // the buffer isn't full yet.
+ queue.emplace_back(absl::MakeConstSpan(
+ queue_.back().data(), queue_.back().size() - stream_.avail_out));
+ return queue;
+}
+
+size_t LzmaEncoder::queued_bytes() const {
+ size_t bytes = queue_size() * kEncodedBufferSizeBytes;
+ // Subtract the bytes that the encoder hasn't filled yet.
+ bytes -= stream_.avail_out;
+ return bytes;
+}
+
+void LzmaEncoder::RunLzmaCode(lzma_action action) {
+ CHECK(!finished_);
+
+ // This is to keep track of how many bytes resulted from encoding this input
+ // buffer.
+ size_t last_avail_out = stream_.avail_out;
+
+ while (stream_.avail_in > 0 || action == LZMA_FINISH) {
+ // If output buffer is full, create a new one, queue it up, and resume
+ // encoding. This could happen in the first call to Encode after
+ // construction or a Reset, or when an input buffer is large enough to fill
+ // more than one output buffer.
+ if (stream_.avail_out == 0) {
+ queue_.emplace_back();
+ queue_.back().resize(kEncodedBufferSizeBytes);
+ stream_.next_out = queue_.back().data();
+ stream_.avail_out = kEncodedBufferSizeBytes;
+ // Update the byte count.
+ total_bytes_ += last_avail_out;
+ last_avail_out = stream_.avail_out;
+ }
+
+ // Encode the data.
+ lzma_ret status = lzma_code(&stream_, action);
+ CheckLzmaCodeIsOk(status);
+ if (action == LZMA_FINISH) {
+ if (status == LZMA_STREAM_END) {
+ // This is returned when lzma_code is all done.
+ finished_ = true;
+ break;
+ }
+ } else {
+ CHECK(status != LZMA_STREAM_END);
+ }
+ VLOG(2) << "LzmaEncoder: Encoded chunk.";
+ }
+
+ // Update the number of resulting encoded bytes.
+ total_bytes_ += last_avail_out - stream_.avail_out;
+}
+
+LzmaDecoder::LzmaDecoder(std::string_view filename)
+ : dummy_decoder_(filename), stream_(LZMA_STREAM_INIT) {
+ compressed_data_.resize(kBufSize);
+
+ lzma_ret status =
+ lzma_stream_decoder(&stream_, UINT64_MAX, LZMA_CONCATENATED);
+ CheckLzmaCodeIsOk(status);
+ stream_.avail_out = 0;
+ VLOG(2) << "LzmaDecoder: Initialization succeeded.";
+}
+
+LzmaDecoder::~LzmaDecoder() { lzma_end(&stream_); }
+
+size_t LzmaDecoder::Read(uint8_t *begin, uint8_t *end) {
+ if (finished_) {
+ return 0;
+ }
+
+ // Write into the given range.
+ stream_.next_out = begin;
+ stream_.avail_out = end - begin;
+ // Keep decompressing until we run out of buffer space.
+ while (stream_.avail_out > 0) {
+ if (action_ == LZMA_RUN && stream_.avail_in == 0) {
+ // Read more bytes from the file if we're all out.
+ const size_t count =
+ dummy_decoder_.Read(compressed_data_.begin(), compressed_data_.end());
+ if (count == 0) {
+ // No more data to read in the file, begin the finishing operation.
+ action_ = LZMA_FINISH;
+ } else {
+ stream_.next_in = compressed_data_.data();
+ stream_.avail_in = count;
+ }
+ }
+ // Decompress the data.
+ const lzma_ret status = lzma_code(&stream_, action_);
+ // Return if we're done.
+ if (status == LZMA_STREAM_END) {
+ CHECK_EQ(action_, LZMA_FINISH)
+ << ": Got LZMA_STREAM_END when action wasn't LZMA_FINISH";
+ finished_ = true;
+ return (end - begin) - stream_.avail_out;
+ }
+ CheckLzmaCodeIsOk(status);
+ }
+ return end - begin;
+}
+
+} // namespace aos::logger
diff --git a/aos/events/logging/lzma_encoder.h b/aos/events/logging/lzma_encoder.h
new file mode 100644
index 0000000..d7080a9
--- /dev/null
+++ b/aos/events/logging/lzma_encoder.h
@@ -0,0 +1,72 @@
+#ifndef AOS_EVENTS_LOGGING_LZMA_ENCODER_H_
+#define AOS_EVENTS_LOGGING_LZMA_ENCODER_H_
+
+#include "absl/types/span.h"
+#include "flatbuffers/flatbuffers.h"
+#include "lzma.h"
+
+#include "aos/containers/resizeable_buffer.h"
+#include "aos/events/logging/buffer_encoder.h"
+#include "aos/events/logging/logger_generated.h"
+
+namespace aos::logger {
+
+// Encodes buffers using liblzma.
+class LzmaEncoder final : public DetachedBufferEncoder {
+ public:
+ // Initializes the LZMA stream and encoder.
+ explicit LzmaEncoder(uint32_t compression_preset);
+ // Gracefully shuts down the encoder.
+ ~LzmaEncoder() final;
+
+ void Encode(flatbuffers::DetachedBuffer &&in) final;
+ void Finish() final;
+ void Clear(int n) final;
+ std::vector<absl::Span<const uint8_t>> queue() const final;
+ size_t queued_bytes() const final;
+ size_t total_bytes() const final { return total_bytes_; }
+ size_t queue_size() const final { return queue_.size(); }
+
+ private:
+ static constexpr size_t kEncodedBufferSizeBytes{1024};
+
+ void RunLzmaCode(lzma_action action);
+
+ lzma_stream stream_;
+ uint32_t compression_preset_;
+ std::vector<ResizeableBuffer> queue_;
+ bool finished_ = false;
+ // Total bytes that resulted from encoding raw data since the last call to
+ // Reset.
+ size_t total_bytes_ = 0;
+};
+
+// Decompresses data with liblzma.
+class LzmaDecoder final : public DataDecoder {
+ public:
+ explicit LzmaDecoder(std::string_view filename);
+ ~LzmaDecoder();
+
+ size_t Read(uint8_t *begin, uint8_t *end) final;
+
+ private:
+ // Size of temporary buffer to use.
+ static constexpr size_t kBufSize{256 * 1024};
+
+ // Temporary buffer for storing compressed data.
+ ResizeableBuffer compressed_data_;
+ // Used for reading data from the file.
+ DummyDecoder dummy_decoder_;
+ // Stream for decompression.
+ lzma_stream stream_;
+ // The current action. This is LZMA_RUN until we've run out of data to read
+ // from the file.
+ lzma_action action_ = LZMA_RUN;
+ // Flag that represents whether or not all the data from the file has been
+ // successfully decoded.
+ bool finished_ = false;
+};
+
+} // namespace aos::logger
+
+#endif // AOS_EVENTS_LOGGING_LZMA_ENCODER_H_
diff --git a/aos/events/logging/lzma_encoder_test.cc b/aos/events/logging/lzma_encoder_test.cc
new file mode 100644
index 0000000..2d619c4
--- /dev/null
+++ b/aos/events/logging/lzma_encoder_test.cc
@@ -0,0 +1,18 @@
+#include "aos/events/logging/lzma_encoder.h"
+
+#include "aos/events/logging/buffer_encoder_param_test.h"
+#include "gtest/gtest.h"
+
+namespace aos::logger::testing {
+
+INSTANTIATE_TEST_CASE_P(
+ Lzma, BufferEncoderTest,
+ ::testing::Combine(::testing::Values([]() {
+ return std::make_unique<LzmaEncoder>(2);
+ }),
+ ::testing::Values([](std::string_view filename) {
+ return std::make_unique<LzmaDecoder>(filename);
+ }),
+ ::testing::Range(0, 100)));
+
+} // namespace aos::logger::testing
diff --git a/aos/events/logging/test_message.fbs b/aos/events/logging/test_message.fbs
new file mode 100644
index 0000000..ef876aa
--- /dev/null
+++ b/aos/events/logging/test_message.fbs
@@ -0,0 +1,7 @@
+namespace aos.logger.testing;
+
+table TestMessage {
+ value:int;
+}
+
+root_type TestMessage;
diff --git a/aos/events/logging/uuid.cc b/aos/events/logging/uuid.cc
index 9298c8b..376fa95 100644
--- a/aos/events/logging/uuid.cc
+++ b/aos/events/logging/uuid.cc
@@ -57,4 +57,10 @@
return result;
}
+UUID UUID::Zero() {
+ UUID result;
+ result.data_.fill(0);
+ return result;
+}
+
} // namespace aos
diff --git a/aos/events/logging/uuid.h b/aos/events/logging/uuid.h
index b81b811..0387a4b 100644
--- a/aos/events/logging/uuid.h
+++ b/aos/events/logging/uuid.h
@@ -13,6 +13,8 @@
// Returns a randomly generated UUID. This is known as a UUID4.
static UUID Random();
+ static UUID Zero();
+
std::string_view string_view() const {
return std::string_view(data_.data(), data_.size());
}
diff --git a/aos/events/logging/uuid_test.cc b/aos/events/logging/uuid_test.cc
index d0320de..4ea351c 100644
--- a/aos/events/logging/uuid_test.cc
+++ b/aos/events/logging/uuid_test.cc
@@ -12,6 +12,8 @@
LOG(INFO) << UUID::Random().string_view();
EXPECT_NE(UUID::Random(), UUID::Random());
+ EXPECT_NE(UUID::Random(), UUID::Zero());
+ EXPECT_EQ(UUID::Zero(), UUID::Zero());
}
} // namespace testing
diff --git a/aos/events/message_counter.h b/aos/events/message_counter.h
new file mode 100644
index 0000000..418c1eb
--- /dev/null
+++ b/aos/events/message_counter.h
@@ -0,0 +1,28 @@
+#ifndef AOS_EVENTS_MESSAGE_COUNTER_H_
+#define AOS_EVENTS_MESSAGE_COUNTER_H_
+
+#include "aos/events/event_loop.h"
+
+namespace aos {
+namespace testing {
+
+// Simple class to count messages on a channel easily. This only counts
+// messages published while running.
+template <typename T>
+class MessageCounter {
+ public:
+ MessageCounter(aos::EventLoop *event_loop, std::string_view name) {
+ event_loop->MakeNoArgWatcher<T>(name, [this]() { ++count_; });
+ }
+
+ // Returns the number of messages seen so far.
+ size_t count() const { return count_; }
+
+ private:
+ size_t count_ = 0;
+};
+
+} // namespace testing
+} // namespace aos
+
+#endif // AOS_EVENTS_MESSAGE_COUNTER_H_
diff --git a/aos/events/shm_event_loop.cc b/aos/events/shm_event_loop.cc
index 406325d..6e152cc 100644
--- a/aos/events/shm_event_loop.cc
+++ b/aos/events/shm_event_loop.cc
@@ -537,9 +537,12 @@
CHECK_LE(length, static_cast<size_t>(channel()->max_size()))
<< ": Sent too big a message on "
<< configuration::CleanedChannelToString(channel());
- lockless_queue_sender_.Send(
+ CHECK(lockless_queue_sender_.Send(
length, monotonic_remote_time, realtime_remote_time, remote_queue_index,
- &monotonic_sent_time_, &realtime_sent_time_, &sent_queue_index_);
+ &monotonic_sent_time_, &realtime_sent_time_, &sent_queue_index_))
+ << ": Somebody wrote outside the buffer of their message on channel "
+ << configuration::CleanedChannelToString(channel());
+
wake_upper_.Wakeup(event_loop()->priority());
return true;
}
@@ -551,10 +554,12 @@
CHECK_LE(length, static_cast<size_t>(channel()->max_size()))
<< ": Sent too big a message on "
<< configuration::CleanedChannelToString(channel());
- lockless_queue_sender_.Send(reinterpret_cast<const char *>(msg), length,
- monotonic_remote_time, realtime_remote_time,
- remote_queue_index, &monotonic_sent_time_,
- &realtime_sent_time_, &sent_queue_index_);
+ CHECK(lockless_queue_sender_.Send(
+ reinterpret_cast<const char *>(msg), length, monotonic_remote_time,
+ realtime_remote_time, remote_queue_index, &monotonic_sent_time_,
+ &realtime_sent_time_, &sent_queue_index_))
+ << ": Somebody wrote outside the buffer of their message on channel "
+ << configuration::CleanedChannelToString(channel());
wake_upper_.Wakeup(event_loop()->priority());
// TODO(austin): Return an error if we send too fast.
return true;
@@ -662,12 +667,18 @@
void HandleEvent() {
CHECK(!event_.valid());
+ disabled_ = false;
const auto monotonic_now = Call(monotonic_clock::now, base_);
if (event_.valid()) {
// If someone called Setup inside Call, rescheduling is already taken care
// of. Bail.
return;
}
+ if (disabled_) {
+ // Somebody called Disable inside Call, so we don't want to reschedule.
+ // Bail.
+ return;
+ }
if (repeat_offset_ == chrono::seconds(0)) {
timerfd_.Disable();
@@ -703,6 +714,7 @@
void Disable() override {
shm_event_loop_->RemoveEvent(&event_);
timerfd_.Disable();
+ disabled_ = true;
}
private:
@@ -713,6 +725,10 @@
monotonic_clock::time_point base_;
monotonic_clock::duration repeat_offset_;
+
+ // Used to track if Disable() was called during the callback, so we know not
+ // to reschedule.
+ bool disabled_ = false;
};
// Adapter class to the timerfd and PhasedLoop.
diff --git a/aos/events/simulated_event_loop.cc b/aos/events/simulated_event_loop.cc
index c429eef..d86d86c 100644
--- a/aos/events/simulated_event_loop.cc
+++ b/aos/events/simulated_event_loop.cc
@@ -9,6 +9,7 @@
#include "aos/events/aos_logging.h"
#include "aos/events/simulated_network_bridge.h"
#include "aos/json_to_flatbuffer.h"
+#include "aos/realtime.h"
#include "aos/util/phased_loop.h"
namespace aos {
@@ -19,6 +20,16 @@
namespace {
+class ScopedMarkRealtimeRestorer {
+ public:
+ ScopedMarkRealtimeRestorer(bool rt) : rt_(rt), prior_(MarkRealtime(rt)) {}
+ ~ScopedMarkRealtimeRestorer() { CHECK_EQ(rt_, MarkRealtime(prior_)); }
+
+ private:
+ const bool rt_;
+ const bool prior_;
+};
+
// Container for both a message, and the context for it for simulation. This
// makes tracking the timestamps associated with the data easy.
struct SimulatedMessage final {
@@ -346,7 +357,9 @@
return std::make_pair(false, monotonic_clock::min_time);
}
- CHECK(!fell_behind_) << ": Got behind";
+ CHECK(!fell_behind_) << ": Got behind on "
+ << configuration::StrippedChannelToString(
+ simulated_channel_->channel());
SetMsg(msgs_.front());
msgs_.pop_front();
@@ -544,7 +557,10 @@
}
void OnRun(::std::function<void()> on_run) override {
- scheduler_->ScheduleOnRun(on_run);
+ scheduler_->ScheduleOnRun([this, on_run = std::move(on_run)]() {
+ ScopedMarkRealtimeRestorer rt(priority() > 0);
+ on_run();
+ });
}
const Node *node() const override { return node_; }
@@ -737,7 +753,10 @@
context.realtime_remote_time = context.realtime_event_time;
}
- DoCallCallback([monotonic_now]() { return monotonic_now; }, context);
+ {
+ ScopedMarkRealtimeRestorer rt(simulated_event_loop_->priority() > 0);
+ DoCallCallback([monotonic_now]() { return monotonic_now; }, context);
+ }
msgs_.pop_front();
if (token_ != scheduler_->InvalidToken()) {
@@ -855,7 +874,10 @@
simulated_event_loop_->AddEvent(&event_);
}
- Call([monotonic_now]() { return monotonic_now; }, monotonic_now);
+ {
+ ScopedMarkRealtimeRestorer rt(simulated_event_loop_->priority() > 0);
+ Call([monotonic_now]() { return monotonic_now; }, monotonic_now);
+ }
}
void SimulatedTimerHandler::Disable() {
@@ -891,9 +913,14 @@
if (simulated_event_loop_->log_impl_) {
prev_logger.Swap(simulated_event_loop_->log_impl_);
}
- Call(
- [monotonic_now]() { return monotonic_now; },
- [this](monotonic_clock::time_point sleep_time) { Schedule(sleep_time); });
+
+ {
+ ScopedMarkRealtimeRestorer rt(simulated_event_loop_->priority() > 0);
+ Call([monotonic_now]() { return monotonic_now; },
+ [this](monotonic_clock::time_point sleep_time) {
+ Schedule(sleep_time);
+ });
+ }
}
void SimulatedPhasedLoopHandler::Schedule(
diff --git a/aos/events/simulated_event_loop_test.cc b/aos/events/simulated_event_loop_test.cc
index 7de0540..3923297 100644
--- a/aos/events/simulated_event_loop_test.cc
+++ b/aos/events/simulated_event_loop_test.cc
@@ -4,6 +4,7 @@
#include "aos/events/event_loop_param_test.h"
#include "aos/events/logging/logger_generated.h"
+#include "aos/events/message_counter.h"
#include "aos/events/ping_lib.h"
#include "aos/events/pong_lib.h"
#include "aos/events/test_message_generated.h"
@@ -14,6 +15,11 @@
namespace aos {
namespace testing {
+namespace {
+
+std::string ConfigPrefix() { return "aos/"; }
+
+} // namespace
namespace chrono = ::std::chrono;
@@ -254,25 +260,12 @@
0.0);
}
-template <typename T>
-class MessageCounter {
- public:
- MessageCounter(aos::EventLoop *event_loop, std::string_view name) {
- event_loop->MakeNoArgWatcher<T>(name, [this]() { ++count_; });
- }
-
- size_t count() const { return count_; }
-
- private:
- size_t count_ = 0;
-};
-
// Tests that ping and pong work when on 2 different nodes, and the message
// gateway messages are sent out as expected.
TEST(SimulatedEventLoopTest, MultinodePingPong) {
aos::FlatbufferDetachedBuffer<aos::Configuration> config =
- aos::configuration::ReadConfig(
- "aos/events/multinode_pingpong_config.json");
+ aos::configuration::ReadConfig(ConfigPrefix() +
+ "events/multinode_pingpong_config.json");
const Node *pi1 = configuration::GetNode(&config.message(), "pi1");
const Node *pi2 = configuration::GetNode(&config.message(), "pi2");
const Node *pi3 = configuration::GetNode(&config.message(), "pi3");
@@ -525,6 +518,7 @@
// Confirm the forwarded message has matching timestamps to the
// timestamps we got back.
EXPECT_EQ(pi2_context->queue_index, header.queue_index());
+ EXPECT_EQ(pi2_context->remote_queue_index, header.remote_queue_index());
EXPECT_EQ(pi2_context->monotonic_event_time,
header_monotonic_sent_time);
EXPECT_EQ(pi2_context->realtime_event_time, header_realtime_sent_time);
@@ -534,7 +528,7 @@
header_monotonic_remote_time);
// Confirm the forwarded message also matches the source message.
- EXPECT_EQ(pi1_context->queue_index, header.queue_index());
+ EXPECT_EQ(pi1_context->queue_index, header.remote_queue_index());
EXPECT_EQ(pi1_context->monotonic_event_time,
header_monotonic_remote_time);
EXPECT_EQ(pi1_context->realtime_event_time,
@@ -573,8 +567,8 @@
// ServerStatistics correctly.
TEST(SimulatedEventLoopTest, MultinodePingPongWithOffset) {
aos::FlatbufferDetachedBuffer<aos::Configuration> config =
- aos::configuration::ReadConfig(
- "aos/events/multinode_pingpong_config.json");
+ aos::configuration::ReadConfig(ConfigPrefix() +
+ "events/multinode_pingpong_config.json");
const Node *pi1 = configuration::GetNode(&config.message(), "pi1");
const Node *pi2 = configuration::GetNode(&config.message(), "pi2");
const Node *pi3 = configuration::GetNode(&config.message(), "pi3");
@@ -673,8 +667,8 @@
// Test that disabling statistics actually disables them.
TEST(SimulatedEventLoopTest, MultinodeWithoutStatistics) {
aos::FlatbufferDetachedBuffer<aos::Configuration> config =
- aos::configuration::ReadConfig(
- "aos/events/multinode_pingpong_config.json");
+ aos::configuration::ReadConfig(ConfigPrefix() +
+ "events/multinode_pingpong_config.json");
const Node *pi1 = configuration::GetNode(&config.message(), "pi1");
const Node *pi2 = configuration::GetNode(&config.message(), "pi2");
const Node *pi3 = configuration::GetNode(&config.message(), "pi3");
@@ -774,5 +768,104 @@
EXPECT_EQ(remote_timestamps_pi1_on_pi2.count(), 1001);
}
+// Tests that the time offset having a slope doesn't break the world.
+// SimulatedMessageBridge has enough self consistency CHECK statements to
+// confirm, and we can can also check a message in each direction to make sure
+// it gets delivered as expected.
+TEST(SimulatedEventLoopTest, MultinodePingPongWithOffsetAndSlope) {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(ConfigPrefix() +
+ "events/multinode_pingpong_config.json");
+ const Node *pi1 = configuration::GetNode(&config.message(), "pi1");
+ const Node *pi2 = configuration::GetNode(&config.message(), "pi2");
+
+ SimulatedEventLoopFactory simulated_event_loop_factory(&config.message());
+ NodeEventLoopFactory *pi2_factory =
+ simulated_event_loop_factory.GetNodeEventLoopFactory(pi2);
+
+ // Move the pi far into the future so the slope is significant. And set it to
+ // something reasonable.
+ constexpr chrono::milliseconds kOffset{150100};
+ pi2_factory->SetDistributedOffset(kOffset, 1.0001);
+
+ std::unique_ptr<EventLoop> ping_event_loop =
+ simulated_event_loop_factory.MakeEventLoop("ping", pi1);
+ Ping ping(ping_event_loop.get());
+
+ std::unique_ptr<EventLoop> pong_event_loop =
+ simulated_event_loop_factory.MakeEventLoop("pong", pi2);
+ Pong pong(pong_event_loop.get());
+
+ std::unique_ptr<EventLoop> pi1_counter_event_loop =
+ simulated_event_loop_factory.MakeEventLoop("pi1_counter", pi1);
+ std::unique_ptr<EventLoop> pi2_counter_event_loop =
+ simulated_event_loop_factory.MakeEventLoop("pi2_counter", pi2);
+
+ aos::Fetcher<examples::Ping> ping_on_pi1_fetcher =
+ pi1_counter_event_loop->MakeFetcher<examples::Ping>("/test");
+ aos::Fetcher<examples::Ping> ping_on_pi2_fetcher =
+ pi2_counter_event_loop->MakeFetcher<examples::Ping>("/test");
+
+ aos::Fetcher<examples::Pong> pong_on_pi2_fetcher =
+ pi2_counter_event_loop->MakeFetcher<examples::Pong>("/test");
+ aos::Fetcher<examples::Pong> pong_on_pi1_fetcher =
+ pi1_counter_event_loop->MakeFetcher<examples::Pong>("/test");
+
+ // End after a pong message comes back. This will leave the latest messages
+ // on all channels so we can look at timestamps easily and check they make
+ // sense.
+ std::unique_ptr<EventLoop> pi1_pong_ender =
+ simulated_event_loop_factory.MakeEventLoop("pi2_counter", pi1);
+ int count = 0;
+ pi1_pong_ender->MakeWatcher(
+ "/test", [&simulated_event_loop_factory, &count](const examples::Pong &) {
+ if (++count == 100) {
+ simulated_event_loop_factory.Exit();
+ }
+ });
+
+ // Run enough that messages should be delivered.
+ simulated_event_loop_factory.Run();
+
+ // Grab the latest messages.
+ EXPECT_TRUE(ping_on_pi1_fetcher.Fetch());
+ EXPECT_TRUE(ping_on_pi2_fetcher.Fetch());
+ EXPECT_TRUE(pong_on_pi1_fetcher.Fetch());
+ EXPECT_TRUE(pong_on_pi2_fetcher.Fetch());
+
+ // Compute their time on the global distributed clock so we can compute
+ // distance betwen them.
+ const distributed_clock::time_point pi1_ping_time =
+ simulated_event_loop_factory.GetNodeEventLoopFactory(pi1)
+ ->ToDistributedClock(
+ ping_on_pi1_fetcher.context().monotonic_event_time);
+ const distributed_clock::time_point pi2_ping_time =
+ simulated_event_loop_factory.GetNodeEventLoopFactory(pi2)
+ ->ToDistributedClock(
+ ping_on_pi2_fetcher.context().monotonic_event_time);
+ const distributed_clock::time_point pi1_pong_time =
+ simulated_event_loop_factory.GetNodeEventLoopFactory(pi1)
+ ->ToDistributedClock(
+ pong_on_pi1_fetcher.context().monotonic_event_time);
+ const distributed_clock::time_point pi2_pong_time =
+ simulated_event_loop_factory.GetNodeEventLoopFactory(pi2)
+ ->ToDistributedClock(
+ pong_on_pi2_fetcher.context().monotonic_event_time);
+
+ // And confirm the delivery delay is just about exactly 150 uS for both
+ // directions like expected. There will be a couple ns of rounding errors in
+ // the conversion functions that aren't worth accounting for right now. This
+ // will either be really close, or really far.
+ EXPECT_GE(pi2_ping_time, chrono::microseconds(150) - chrono::nanoseconds(10) +
+ pi1_ping_time);
+ EXPECT_LE(pi2_ping_time, chrono::microseconds(150) + chrono::nanoseconds(10) +
+ pi1_ping_time);
+
+ EXPECT_GE(pi1_pong_time, chrono::microseconds(150) - chrono::nanoseconds(10) +
+ pi2_pong_time);
+ EXPECT_LE(pi1_pong_time, chrono::microseconds(150) + chrono::nanoseconds(10) +
+ pi2_pong_time);
+}
+
} // namespace testing
} // namespace aos
diff --git a/aos/events/simulated_network_bridge.cc b/aos/events/simulated_network_bridge.cc
index 825c830..5660331 100644
--- a/aos/events/simulated_network_bridge.cc
+++ b/aos/events/simulated_network_bridge.cc
@@ -82,7 +82,11 @@
DeliveredTime(fetcher_->context());
CHECK_GE(monotonic_delivered_time, send_node_factory_->monotonic_now())
- << ": Trying to deliver message in the past...";
+ << ": Trying to deliver message in the past on channel "
+ << configuration::StrippedChannelToString(fetcher_->channel())
+ << " to node " << send_event_loop_->node()->name()->string_view()
+ << " sent from " << fetcher_->channel()->source_node()->string_view()
+ << " at " << fetch_node_factory_->monotonic_now();
server_connection_->mutate_sent_packets(server_connection_->sent_packets() +
1);
@@ -142,11 +146,9 @@
const distributed_clock::time_point distributed_sent_time =
fetch_node_factory_->ToDistributedClock(context.monotonic_event_time);
- return aos::monotonic_clock::epoch() +
- (distributed_sent_time - send_node_factory_->ToDistributedClock(
- aos::monotonic_clock::epoch())) +
- send_node_factory_->network_delay() +
- send_node_factory_->send_delay();
+ return send_node_factory_->FromDistributedClock(
+ distributed_sent_time + send_node_factory_->network_delay() +
+ send_node_factory_->send_delay());
}
// Factories used for time conversion.
diff --git a/aos/flatbuffer_introspection.cc b/aos/flatbuffer_introspection.cc
index 0fc37a7..5f6ca45 100644
--- a/aos/flatbuffer_introspection.cc
+++ b/aos/flatbuffer_introspection.cc
@@ -76,12 +76,8 @@
int64_t enum_value,
const flatbuffers::Vector<flatbuffers::Offset<reflection::EnumVal>>
*values) {
- auto result = std::find_if(values->begin(), values->end(),
- [enum_value](const reflection::EnumVal *a) {
- return a->value() == enum_value;
- });
- return result != values->end() ? result->name()->string_view()
- : std::string_view();
+ auto search = values->LookupByKey(enum_value);
+ return search != nullptr ? search->name()->string_view() : std::string_view();
}
// Convert integer to string, checking if it is an enum.
diff --git a/aos/flatbuffers.h b/aos/flatbuffers.h
index bab006a..b619452 100644
--- a/aos/flatbuffers.h
+++ b/aos/flatbuffers.h
@@ -5,8 +5,9 @@
#include <string_view>
#include "absl/types/span.h"
+#include "aos/containers/resizeable_buffer.h"
#include "aos/macros.h"
-#include "flatbuffers/flatbuffers.h"
+#include "flatbuffers/flatbuffers.h" // IWYU pragma: export
#include "glog/logging.h"
namespace aos {
@@ -164,15 +165,16 @@
class FlatbufferVector : public Flatbuffer<T> {
public:
// Builds a Flatbuffer around a vector.
- FlatbufferVector(std::vector<uint8_t> &&data) : data_(std::move(data)) {}
+ FlatbufferVector(ResizeableBuffer &&data) : data_(std::move(data)) {}
// Builds a Flatbuffer by copying the data from the other flatbuffer.
- FlatbufferVector(const Flatbuffer<T> &other)
- : data_(other.data(), other.data() + other.size()) {}
+ FlatbufferVector(const Flatbuffer<T> &other) {
+ data_.resize(other.size());
+ memcpy(data_.data(), other.data(), data_.size());
+ }
// Copy constructor.
- FlatbufferVector(const FlatbufferVector<T> &other)
- : data_(other.data(), other.data() + other.size()) {}
+ FlatbufferVector(const FlatbufferVector<T> &other) : data_(other.data_) {}
// Move constructor.
FlatbufferVector(FlatbufferVector<T> &&other)
@@ -180,7 +182,7 @@
// Copies the data from the other flatbuffer.
FlatbufferVector &operator=(const FlatbufferVector<T> &other) {
- data_ = std::vector<uint8_t>(other.data(), other.data() + other.size());
+ data_ = other.data_;
return *this;
}
FlatbufferVector &operator=(FlatbufferVector<T> &&other) {
@@ -190,7 +192,7 @@
// Constructs an empty flatbuffer of type T.
static FlatbufferVector<T> Empty() {
- return FlatbufferVector<T>(std::vector<uint8_t>{});
+ return FlatbufferVector<T>(ResizeableBuffer());
}
virtual ~FlatbufferVector() override {}
@@ -200,7 +202,7 @@
size_t size() const override { return data_.size(); }
private:
- std::vector<uint8_t> data_;
+ ResizeableBuffer data_;
};
// This object associates the message type with the memory storing the
@@ -262,9 +264,12 @@
}
void Reset() {
- CHECK(!allocator_.is_allocated()) << ": May not reset while building";
+ CHECK(!allocator_.is_allocated() || data_ != nullptr)
+ << ": May not reset while building";
fbb_ = flatbuffers::FlatBufferBuilder(Size, &allocator_);
fbb_.ForceDefaults(true);
+ data_ = nullptr;
+ size_ = 0;
}
flatbuffers::FlatBufferBuilder *fbb() {
@@ -359,6 +364,16 @@
flatbuffers::DetachedBuffer buffer_;
};
+inline flatbuffers::DetachedBuffer CopySpanAsDetachedBuffer(
+ absl::Span<const uint8_t> span) {
+ // Copy the data from the span.
+ uint8_t *buf = flatbuffers::DefaultAllocator().allocate(span.size());
+ memcpy(buf, span.data(), span.size());
+ // Then give it to a DetachedBuffer to manage.
+ return flatbuffers::DetachedBuffer(nullptr, false, buf, span.size(), buf,
+ span.size());
+}
+
} // namespace aos
#endif // AOS_FLATBUFFERS_H_
diff --git a/aos/ipc_lib/BUILD b/aos/ipc_lib/BUILD
index d5add53..9532e82 100644
--- a/aos/ipc_lib/BUILD
+++ b/aos/ipc_lib/BUILD
@@ -12,6 +12,7 @@
visibility = ["//visibility:public"],
deps = [
"//aos:macros",
+ "//aos:thread_local",
"//aos/util:compiler_memory_barrier",
"@com_github_google_glog//:glog",
"@com_google_absl//absl/base",
@@ -91,6 +92,7 @@
srcs = [
"ipc_comparison.cc",
],
+ linkopts = ["-lrt"],
deps = [
"//aos:condition",
"//aos:event",
@@ -165,6 +167,7 @@
"//aos/time",
"//aos/util:compiler_memory_barrier",
"@com_github_google_glog//:glog",
+ "@com_google_absl//absl/strings",
"@com_google_absl//absl/types:span",
],
)
@@ -295,10 +298,10 @@
cc_binary(
name = "print_lockless_queue_memory",
- visibility = ["//visibility:public"],
srcs = [
"print_lockless_queue_memory.cc",
],
+ visibility = ["//visibility:public"],
deps = [
":lockless_queue",
],
diff --git a/aos/ipc_lib/aos_sync.cc b/aos/ipc_lib/aos_sync.cc
index d5c3d4e..34d9489 100644
--- a/aos/ipc_lib/aos_sync.cc
+++ b/aos/ipc_lib/aos_sync.cc
@@ -27,10 +27,12 @@
#include <type_traits>
#include "absl/base/call_once.h"
-#include "aos/macros.h"
-#include "aos/util/compiler_memory_barrier.h"
#include "glog/logging.h"
+#include "aos/macros.h"
+#include "aos/thread_local.h"
+#include "aos/util/compiler_memory_barrier.h"
+
using ::aos::linux_code::ipc_lib::FutexAccessorObserver;
// This code was originally based on <https://www.akkadia.org/drepper/futex.pdf>,
@@ -369,7 +371,7 @@
// Starts off at 0 in each new thread (because that's what it gets initialized
// to in most of them or it gets to reset to 0 after a fork by atfork_child()).
-thread_local pid_t my_tid = 0;
+AOS_THREAD_LOCAL pid_t my_tid = 0;
// Gets called before the fork(2) wrapper function returns in the child.
void atfork_child() {
@@ -428,7 +430,7 @@
static_assert(sizeof(aos_robust_list_head) == sizeof(robust_list_head),
"Our aos_robust_list_head doesn't match the kernel's");
-thread_local aos_robust_list_head robust_head;
+AOS_THREAD_LOCAL aos_robust_list_head robust_head;
// Extra offset between mutex values and where we point to for their robust list
// entries (from SetRobustListOffset).
diff --git a/aos/ipc_lib/lockless_queue.cc b/aos/ipc_lib/lockless_queue.cc
index 6774489..2b704ef 100644
--- a/aos/ipc_lib/lockless_queue.cc
+++ b/aos/ipc_lib/lockless_queue.cc
@@ -9,6 +9,7 @@
#include <iostream>
#include <sstream>
+#include "absl/strings/escaping.h"
#include "aos/ipc_lib/lockless_queue_memory.h"
#include "aos/realtime.h"
#include "aos/util/compiler_memory_barrier.h"
@@ -903,7 +904,7 @@
return message->data(memory_->message_data_size());
}
-void LocklessQueueSender::Send(
+bool LocklessQueueSender::Send(
const char *data, size_t length,
aos::monotonic_clock::time_point monotonic_remote_time,
aos::realtime_clock::time_point realtime_remote_time,
@@ -916,11 +917,12 @@
// going to write an explicit chunk of memory into the buffer, we need to
// adhere to this convention and place it at the end.
memcpy((reinterpret_cast<char *>(Data()) + size() - length), data, length);
- Send(length, monotonic_remote_time, realtime_remote_time, remote_queue_index,
- monotonic_sent_time, realtime_sent_time, queue_index);
+ return Send(length, monotonic_remote_time, realtime_remote_time,
+ remote_queue_index, monotonic_sent_time, realtime_sent_time,
+ queue_index);
}
-void LocklessQueueSender::Send(
+bool LocklessQueueSender::Send(
size_t length, aos::monotonic_clock::time_point monotonic_remote_time,
aos::realtime_clock::time_point realtime_remote_time,
uint32_t remote_queue_index,
@@ -935,8 +937,9 @@
// modifying it right now.
const Index scratch_index = sender->scratch_index.RelaxedLoad();
Message *const message = memory_->GetMessage(scratch_index);
- CHECK(!CheckBothRedzones(memory_, message))
- << ": Somebody wrote outside the buffer of their message";
+ if (CheckBothRedzones(memory_, message)) {
+ return false;
+ }
// We should have invalidated this when we first got the buffer. Verify that
// in debug mode.
@@ -1083,6 +1086,7 @@
// If anybody is looking at this message (they shouldn't be), then try telling
// them about it (best-effort).
memory_->GetMessage(new_scratch)->header.queue_index.RelaxedInvalidate();
+ return true;
}
int LocklessQueueSender::buffer_index() const {
@@ -1421,27 +1425,29 @@
::std::cout << " size_t length = " << m->header.length
<< ::std::endl;
::std::cout << " }" << ::std::endl;
- if (!CheckBothRedzones(memory, m)) {
+ const bool corrupt = CheckBothRedzones(memory, m);
+ if (corrupt) {
+ absl::Span<char> pre_redzone = m->PreRedzone(memory->message_data_size());
+ absl::Span<char> post_redzone =
+ m->PostRedzone(memory->message_data_size(), memory->message_size());
+
+ ::std::cout << " pre-redzone: \""
+ << absl::BytesToHexString(std::string_view(
+ pre_redzone.data(), pre_redzone.size()))
+ << std::endl;
::std::cout << " // *** DATA REDZONES ARE CORRUPTED ***"
<< ::std::endl;
+ ::std::cout << " post-redzone: \""
+ << absl::BytesToHexString(std::string_view(
+ post_redzone.data(), post_redzone.size()))
+ << std::endl;
}
::std::cout << " data: {";
if (FLAGS_dump_lockless_queue_data) {
const char *const m_data = m->data(memory->message_data_size());
- for (size_t j = 0; j < m->header.length; ++j) {
- char data = m_data[j];
- if (j != 0) {
- ::std::cout << " ";
- }
- if (::std::isprint(data)) {
- ::std::cout << ::std::setfill(' ') << ::std::setw(2) << ::std::hex
- << data;
- } else {
- ::std::cout << "0x" << ::std::setfill('0') << ::std::setw(2)
- << ::std::hex << (static_cast<unsigned>(data) & 0xff);
- }
- }
+ std::cout << absl::BytesToHexString(std::string_view(
+ m_data, corrupt ? memory->message_data_size() : m->header.length));
}
::std::cout << ::std::setfill(' ') << ::std::dec << "}" << ::std::endl;
::std::cout << " }," << ::std::endl;
diff --git a/aos/ipc_lib/lockless_queue.h b/aos/ipc_lib/lockless_queue.h
index b71bd7f..a5de6cc 100644
--- a/aos/ipc_lib/lockless_queue.h
+++ b/aos/ipc_lib/lockless_queue.h
@@ -303,7 +303,7 @@
// Note: calls to Data() are expensive enough that you should cache it.
size_t size() const;
void *Data();
- void Send(size_t length,
+ bool Send(size_t length,
aos::monotonic_clock::time_point monotonic_remote_time =
aos::monotonic_clock::min_time,
aos::realtime_clock::time_point realtime_remote_time =
@@ -314,7 +314,7 @@
uint32_t *queue_index = nullptr);
// Sends up to length data. Does not wakeup the target.
- void Send(const char *data, size_t length,
+ bool Send(const char *data, size_t length,
aos::monotonic_clock::time_point monotonic_remote_time =
aos::monotonic_clock::min_time,
aos::realtime_clock::time_point realtime_remote_time =
diff --git a/aos/json_to_flatbuffer.h b/aos/json_to_flatbuffer.h
index 381621e..c997f03 100644
--- a/aos/json_to_flatbuffer.h
+++ b/aos/json_to_flatbuffer.h
@@ -122,8 +122,12 @@
template <typename T>
inline FlatbufferVector<T> FileToFlatbuffer(const std::string_view path) {
std::ifstream instream(std::string(path), std::ios::in | std::ios::binary);
- std::vector<uint8_t> data((std::istreambuf_iterator<char>(instream)),
- std::istreambuf_iterator<char>());
+ ResizeableBuffer data;
+ std::istreambuf_iterator<char> it(instream);
+ while (it != std::istreambuf_iterator<char>()) {
+ data.push_back(*it);
+ ++it;
+ }
return FlatbufferVector<T>(std::move(data));
}
diff --git a/aos/libc/BUILD b/aos/libc/BUILD
index 234c29a..35b6629 100644
--- a/aos/libc/BUILD
+++ b/aos/libc/BUILD
@@ -9,6 +9,7 @@
"aos_strsignal.h",
],
deps = [
+ "//aos:thread_local",
"@com_github_google_glog//:glog",
],
)
@@ -53,6 +54,9 @@
hdrs = [
"aos_strerror.h",
],
+ deps = [
+ "//aos:thread_local",
+ ],
)
cc_test(
diff --git a/aos/libc/aos_strerror.cc b/aos/libc/aos_strerror.cc
index 1353242..b045f24 100644
--- a/aos/libc/aos_strerror.cc
+++ b/aos/libc/aos_strerror.cc
@@ -1,9 +1,11 @@
#include "aos/libc/aos_strerror.h"
#include <assert.h>
-#include <sys/types.h>
-#include <string.h>
#include <stdio.h>
+#include <string.h>
+#include <sys/types.h>
+
+#include "aos/thread_local.h"
// This code uses an overloaded function to handle the result from either
// version of strerror_r correctly without needing a way to get the choice out
@@ -15,14 +17,15 @@
// Handle the result from the GNU version of strerror_r. It never fails, so
// that's pretty easy...
-__attribute__((unused))
-char *aos_strerror_handle_result(int /*error*/, char *ret, char * /*buffer*/) {
+__attribute__((unused)) char *aos_strerror_handle_result(int /*error*/,
+ char *ret,
+ char * /*buffer*/) {
return ret;
}
// Handle the result from the POSIX version of strerror_r.
-__attribute__((unused))
-char *aos_strerror_handle_result(int error, int ret, char *buffer) {
+__attribute__((unused)) char *aos_strerror_handle_result(int error, int ret,
+ char *buffer) {
if (ret != 0) {
#ifndef NDEBUG
// assert doesn't use the return value when building optimized.
@@ -37,7 +40,7 @@
} // namespace
const char *aos_strerror(int error) {
- static thread_local char buffer[kBufferSize];
+ AOS_THREAD_LOCAL char buffer[kBufferSize];
// Call the overload for whichever version we're using.
return aos_strerror_handle_result(
diff --git a/aos/libc/aos_strsignal.cc b/aos/libc/aos_strsignal.cc
index 9262ac6..91c2211 100644
--- a/aos/libc/aos_strsignal.cc
+++ b/aos/libc/aos_strsignal.cc
@@ -4,8 +4,10 @@
#include "glog/logging.h"
+#include "aos/thread_local.h"
+
const char *aos_strsignal(int signal) {
- static thread_local char buffer[512];
+ AOS_THREAD_LOCAL char buffer[512];
if (signal >= SIGRTMIN && signal <= SIGRTMAX) {
CHECK_GT(snprintf(buffer, sizeof(buffer), "Real-time signal %d",
diff --git a/aos/logging/BUILD b/aos/logging/BUILD
index 6686a3c..45a9921 100644
--- a/aos/logging/BUILD
+++ b/aos/logging/BUILD
@@ -21,6 +21,7 @@
"//aos:complex_thread_local",
"//aos:die",
"//aos:macros",
+ "//aos:thread_local",
"//aos/libc:aos_strerror",
"//aos/mutex",
"//aos/stl_mutex",
diff --git a/aos/logging/context.cc b/aos/logging/context.cc
index 2c689ed..c6f1063 100644
--- a/aos/logging/context.cc
+++ b/aos/logging/context.cc
@@ -22,6 +22,7 @@
#include "aos/complex_thread_local.h"
#include "aos/die.h"
#include "aos/logging/implementations.h"
+#include "aos/thread_local.h"
namespace aos {
namespace logging {
@@ -68,7 +69,7 @@
// reason for doing this instead of just deleting them is that tsan (at least)
// doesn't like it when pthread_atfork handlers do complicated stuff and it's
// not a great idea anyways.
-thread_local bool delete_current_context(false);
+AOS_THREAD_LOCAL bool delete_current_context(false);
} // namespace
diff --git a/aos/logging/log_namer.cc b/aos/logging/log_namer.cc
index f025054..c1dd30a 100644
--- a/aos/logging/log_namer.cc
+++ b/aos/logging/log_namer.cc
@@ -146,11 +146,11 @@
AllocateLogName(&tmp, folder, basename);
std::string log_base_name = tmp;
- std::string log_roborio_name = log_base_name + "_roborio_data.bfbs";
+ std::string log_roborio_name = log_base_name + "/";
free(tmp);
char *tmp2;
- if (asprintf(&tmp2, "%s/%s-current.bfbs", folder, basename) == -1) {
+ if (asprintf(&tmp2, "%s/%s-current", folder, basename) == -1) {
PLOG(WARNING) << "couldn't create current symlink name";
} else {
if (unlink(tmp2) == -1 && (errno != EROFS && errno != ENOENT)) {
diff --git a/aos/network/message_bridge_server_lib.cc b/aos/network/message_bridge_server_lib.cc
index de836ae..b80547d 100644
--- a/aos/network/message_bridge_server_lib.cc
+++ b/aos/network/message_bridge_server_lib.cc
@@ -248,6 +248,12 @@
const Channel *const timestamp_channel = configuration::GetChannel(
event_loop_->configuration(), "/aos", Timestamp::GetFullyQualifiedName(),
event_loop_->name(), event_loop_->node());
+ CHECK(timestamp_channel != nullptr)
+ << ": Failed to find timestamp channel {\"name\": \"/aos\", \"type\": \""
+ << Timestamp::GetFullyQualifiedName() << "\"}";
+ CHECK(configuration::ChannelIsSendableOnNode(timestamp_channel,
+ event_loop_->node()))
+ << ": Timestamp channel is not sendable on this node.";
for (const Channel *channel : *event_loop_->configuration()->channels()) {
CHECK(channel->has_source_node());
@@ -302,11 +308,17 @@
timestamp_state_ = state.get();
}
channels_.emplace_back(std::move(state));
+ } else if (channel == timestamp_channel) {
+ std::unique_ptr<ChannelState> state(
+ new ChannelState{channel, channel_index});
+ timestamp_state_ = state.get();
+ channels_.emplace_back(std::move(state));
} else {
channels_.emplace_back(nullptr);
}
++channel_index;
}
+ CHECK(timestamp_state_ != nullptr);
// Buffer up the max size a bit so everything fits nicely.
LOG(INFO) << "Max message size for all clients is " << max_size;
@@ -422,9 +434,12 @@
const logger::MessageHeader *message_header =
flatbuffers::GetRoot<logger::MessageHeader>(message->data());
- channels_[message_header->channel_index()]->HandleDelivery(
- message->header.rcvinfo.rcv_assoc_id, message->header.rcvinfo.rcv_ssn,
- absl::Span<const uint8_t>(message->data(), message->size));
+ CHECK_LT(message_header->channel_index(), channels_.size());
+ CHECK_NOTNULL(channels_[message_header->channel_index()])
+ ->HandleDelivery(
+ message->header.rcvinfo.rcv_assoc_id,
+ message->header.rcvinfo.rcv_ssn,
+ absl::Span<const uint8_t>(message->data(), message->size));
}
if (VLOG_IS_ON(1)) {
diff --git a/aos/network/www/test_config_file.json b/aos/network/www/test_config_file.json
index 09b22dd..39b8ef1 100644
--- a/aos/network/www/test_config_file.json
+++ b/aos/network/www/test_config_file.json
@@ -10,7 +10,7 @@
"name": "/test",
"type": "aos.Configuration",
"source_node": "roborio",
- "max_size": 1678,
+ "max_size": 2056,
"frequency": 200
}
],
diff --git a/aos/realtime.cc b/aos/realtime.cc
index a0af97a..9df7aca 100644
--- a/aos/realtime.cc
+++ b/aos/realtime.cc
@@ -1,18 +1,19 @@
#include "aos/realtime.h"
+#include <errno.h>
+#include <malloc.h>
+#include <sched.h>
+#include <stdint.h>
#include <stdio.h>
+#include <stdlib.h>
#include <string.h>
#include <sys/mman.h>
-#include <errno.h>
-#include <sched.h>
+#include <sys/prctl.h>
#include <sys/resource.h>
#include <sys/types.h>
#include <unistd.h>
-#include <stdlib.h>
-#include <stdint.h>
-#include <sys/prctl.h>
-#include <malloc.h>
+#include "aos/thread_local.h"
#include "glog/logging.h"
namespace FLAG__namespace_do_not_use_directly_use_DECLARE_double_instead {
@@ -68,13 +69,14 @@
} // namespace
void LockAllMemory() {
+ CheckNotRealtime();
// Allow locking as much as we want into RAM.
SetSoftRLimit(RLIMIT_MEMLOCK, RLIM_INFINITY, SetLimitForRoot::kNo);
WriteCoreDumps();
PCHECK(mlockall(MCL_CURRENT | MCL_FUTURE) == 0);
-#if !__has_feature(address_sanitizer)
+#if !__has_feature(address_sanitizer) && !__has_feature(memory_sanitizer)
// Don't give freed memory back to the OS.
CHECK_EQ(1, mallopt(M_TRIM_THRESHOLD, -1));
// Don't use mmap for large malloc chunks.
@@ -101,6 +103,7 @@
}
void InitRT() {
+ CheckNotRealtime();
LockAllMemory();
// Only let rt processes run for 3 seconds straight.
@@ -114,6 +117,7 @@
struct sched_param param;
param.sched_priority = 0;
PCHECK(sched_setscheduler(0, SCHED_OTHER, ¶m) == 0);
+ MarkRealtime(false);
}
void SetCurrentThreadAffinity(const cpu_set_t &cpuset) {
@@ -141,6 +145,7 @@
struct sched_param param;
param.sched_priority = priority;
+ MarkRealtime(true);
PCHECK(sched_setscheduler(0, SCHED_FIFO, ¶m) == 0)
<< ": changing to SCHED_FIFO with " << priority;
}
@@ -155,4 +160,20 @@
AllowSoftLimitDecrease::kNo);
}
+namespace {
+AOS_THREAD_LOCAL bool is_realtime = false;
+}
+
+bool MarkRealtime(bool realtime) {
+ const bool prior = is_realtime;
+ is_realtime = realtime;
+ return prior;
+}
+
+void CheckRealtime() { CHECK(is_realtime); }
+
+void CheckNotRealtime() { CHECK(!is_realtime); }
+
+ScopedRealtimeRestorer::ScopedRealtimeRestorer() : prior_(is_realtime) {}
+
} // namespace aos
diff --git a/aos/realtime.h b/aos/realtime.h
index 6e0a472..52db4a8 100644
--- a/aos/realtime.h
+++ b/aos/realtime.h
@@ -4,6 +4,8 @@
#include <sched.h>
#include <string_view>
+#include "glog/logging.h"
+
namespace aos {
// Locks everything into memory and sets the limits. This plus InitNRT are
@@ -34,6 +36,55 @@
void ExpandStackSize();
+// CHECKs that we are (or are not) running on the RT scheduler. Useful for
+// enforcing that operations which are or are not bounded shouldn't be run. This
+// works both in simulation and when running against the real target.
+void CheckRealtime();
+void CheckNotRealtime();
+
+// Marks that we are or are not running on the realtime scheduler. Returns the
+// previous state.
+//
+// Note: this shouldn't be used directly. The event loop primitives should be
+// used instead.
+bool MarkRealtime(bool realtime);
+
+// Class which restores the current RT state when destructed.
+class ScopedRealtimeRestorer {
+ public:
+ ScopedRealtimeRestorer();
+ ~ScopedRealtimeRestorer() { MarkRealtime(prior_); }
+
+ private:
+ const bool prior_;
+};
+
+// Class which marks us as on the RT scheduler until it goes out of scope.
+// Note: this shouldn't be needed for most applications.
+class ScopedRealtime {
+ public:
+ ScopedRealtime() : prior_(MarkRealtime(true)) {}
+ ~ScopedRealtime() {
+ CHECK(MarkRealtime(prior_)) << ": Priority was modified";
+ }
+
+ private:
+ const bool prior_;
+};
+
+// Class which marks us as not on the RT scheduler until it goes out of scope.
+// Note: this shouldn't be needed for most applications.
+class ScopedNotRealtime {
+ public:
+ ScopedNotRealtime() : prior_(MarkRealtime(false)) {}
+ ~ScopedNotRealtime() {
+ CHECK(!MarkRealtime(prior_)) << ": Priority was modified";
+ }
+
+ private:
+ const bool prior_;
+};
+
} // namespace aos
#endif // AOS_REALTIME_H_
diff --git a/aos/realtime_test.cc b/aos/realtime_test.cc
new file mode 100644
index 0000000..e77f140
--- /dev/null
+++ b/aos/realtime_test.cc
@@ -0,0 +1,76 @@
+#include "aos/realtime.h"
+
+#include "gtest/gtest.h"
+
+namespace aos {
+namespace testing {
+
+// Tests that ScopedRealtime handles the simple case.
+TEST(RealtimeTest, ScopedRealtime) {
+ CheckNotRealtime();
+ {
+ ScopedRealtime rt;
+ CheckRealtime();
+ }
+ CheckNotRealtime();
+}
+
+// Tests that ScopedRealtime handles nesting.
+TEST(RealtimeTest, DoubleScopedRealtime) {
+ CheckNotRealtime();
+ {
+ ScopedRealtime rt;
+ CheckRealtime();
+ {
+ ScopedRealtime rt2;
+ CheckRealtime();
+ }
+ CheckRealtime();
+ }
+ CheckNotRealtime();
+}
+
+// Tests that ScopedRealtime handles nesting with ScopedNotRealtime.
+TEST(RealtimeTest, ScopedNotRealtime) {
+ CheckNotRealtime();
+ {
+ ScopedRealtime rt;
+ CheckRealtime();
+ {
+ ScopedNotRealtime nrt;
+ CheckNotRealtime();
+ }
+ CheckRealtime();
+ }
+ CheckNotRealtime();
+}
+
+// Tests that ScopedRealtimeRestorer works both when starting RT and nonrt.
+TEST(RealtimeTest, ScopedRealtimeRestorer) {
+ CheckNotRealtime();
+ {
+ ScopedRealtime rt;
+ CheckRealtime();
+ {
+ ScopedRealtimeRestorer restore;
+ CheckRealtime();
+
+ MarkRealtime(false);
+ CheckNotRealtime();
+ }
+ CheckRealtime();
+ }
+ CheckNotRealtime();
+
+ {
+ ScopedRealtimeRestorer restore;
+ CheckNotRealtime();
+
+ MarkRealtime(true);
+ CheckRealtime();
+ }
+ CheckNotRealtime();
+}
+
+} // namespace testing
+} // namespace aos
diff --git a/aos/starter/starter.sh b/aos/starter/starter.sh
index e140351..87f58a4 100755
--- a/aos/starter/starter.sh
+++ b/aos/starter/starter.sh
@@ -4,6 +4,11 @@
if [[ "$(hostname)" == "roboRIO"* ]]; then
ROBOT_CODE="/home/admin/robot_code"
+ # Configure throttling so we reserve 5% of the CPU for non-rt work.
+ # This makes things significantly more stable.
+ echo 950000 > /proc/sys/kernel/sched_rt_runtime_us
+ echo 1000000 > /proc/sys/kernel/sched_rt_period_us
+
ln -s /var/local/natinst/log/FRC_UserProgram.log /tmp/FRC_UserProgram.log
ln -s /var/local/natinst/log/FRC_UserProgram.log "${ROBOT_CODE}/FRC_UserProgram.log"
elif [[ "$(hostname)" == "pi-"* ]]; then
diff --git a/aos/testing/BUILD b/aos/testing/BUILD
index 052cabe..e70b5a4 100644
--- a/aos/testing/BUILD
+++ b/aos/testing/BUILD
@@ -24,6 +24,7 @@
visibility = ["//visibility:public"],
deps = [
":googletest",
+ "//aos:thread_local",
"//aos/logging:implementations",
"//aos/mutex",
"@com_google_absl//absl/base",
@@ -85,3 +86,11 @@
],
visibility = ["//visibility:public"],
)
+
+cc_library(
+ name = "tmpdir",
+ testonly = True,
+ srcs = ["tmpdir.cc"],
+ hdrs = ["tmpdir.h"],
+ visibility = ["//visibility:public"],
+)
diff --git a/aos/testing/test_logging.cc b/aos/testing/test_logging.cc
index db6246c..77bc4e9 100644
--- a/aos/testing/test_logging.cc
+++ b/aos/testing/test_logging.cc
@@ -7,8 +7,10 @@
#include "gtest/gtest.h"
#include "absl/base/call_once.h"
+
#include "aos/logging/implementations.h"
#include "aos/mutex/mutex.h"
+#include "aos/thread_local.h"
using ::aos::logging::LogMessage;
@@ -97,12 +99,12 @@
// Thread local storage for mock time. This is thread local because if
// someone spawns a thread and goes to town in parallel with a simulated event
// loop, we want to just print the actual monotonic clock out.
- static thread_local bool mock_time_;
- static thread_local ::aos::monotonic_clock::time_point monotonic_now_;
+ static AOS_THREAD_LOCAL bool mock_time_;
+ static AOS_THREAD_LOCAL ::aos::monotonic_clock::time_point monotonic_now_;
};
-thread_local bool TestLogImplementation::mock_time_ = false;
-thread_local ::aos::monotonic_clock::time_point
+AOS_THREAD_LOCAL bool TestLogImplementation::mock_time_ = false;
+AOS_THREAD_LOCAL ::aos::monotonic_clock::time_point
TestLogImplementation::monotonic_now_ = ::aos::monotonic_clock::min_time;
class MyTestEventListener : public ::testing::EmptyTestEventListener {
diff --git a/aos/testing/tmpdir.cc b/aos/testing/tmpdir.cc
new file mode 100644
index 0000000..15e3c13
--- /dev/null
+++ b/aos/testing/tmpdir.cc
@@ -0,0 +1,18 @@
+#include "aos/testing/tmpdir.h"
+
+#include <cstdlib>
+#include <string>
+
+namespace aos {
+namespace testing {
+
+std::string TestTmpDir() {
+ const char *tmp_dir = std::getenv("TEST_TMPDIR");
+ if (tmp_dir != nullptr) {
+ return tmp_dir;
+ }
+ return "/tmp";
+}
+
+} // namespace testing
+} // namespace aos
diff --git a/aos/testing/tmpdir.h b/aos/testing/tmpdir.h
new file mode 100644
index 0000000..7e64342
--- /dev/null
+++ b/aos/testing/tmpdir.h
@@ -0,0 +1,15 @@
+#ifndef AOS_TESTING_TMPDIR_H_
+#define AOS_TESTING_TMPDIR_H_
+
+#include <string>
+
+namespace aos {
+namespace testing {
+
+// Returns a usable temporary directory.
+std::string TestTmpDir();
+
+} // namespace testing
+} // namespace aos
+
+#endif // AOS_TESTING_TMPDIR_H_
diff --git a/aos/thread_local.h b/aos/thread_local.h
new file mode 100644
index 0000000..e8c8854
--- /dev/null
+++ b/aos/thread_local.h
@@ -0,0 +1,36 @@
+#ifndef AOS_THREAD_LOCAL_H_
+#define AOS_THREAD_LOCAL_H_
+
+// Use AOS_THREAD_LOCAL instead of thread_local to pick up specifics for various
+// compilers/platforms.
+
+#ifdef __aarch64__
+// Workaround for https://bugs.llvm.org/show_bug.cgi?id=41527.
+// TODO(Brian): Remove this once we upgrade past LLVM 9.0.0.
+// 9.0.1 might have the fix, but I can't find prebuilt binaries for it, for some
+// reason. Going by release dates, 10.0.0 should definitely have the fix.
+//
+// https://reviews.llvm.org/D53906 broke it, https://reviews.llvm.org/D62055
+// reverted it, https://reviews.llvm.org/D61825 re-enabled it for only Android.
+//
+// Basically, LLD hacks the program header, but fails to change enough of the
+// values to be self-consistent. The resulting values cause glibc's dynamic
+// linker to do something different than lld is expecting, so then things
+// overlap at runtime and break horribly.
+//
+// This workaround ensures that the program header has the alignment lld wants
+// already, which ensures it's set there early enough in lld's processing that
+// it actually gets the proper alignment. This makes the hack a NOP so
+// everything works correctly.
+//
+// To check for the problem, build a binary (a complete binary, not a test
+// binary which references shared objects for all the code) and run `readelf
+// -aW` on it. Look for the TLS program header. If its alignment is 0x40, this
+// workaround is probably needed. Verify its address is aligned mod 0x40 to
+// verify the workaround is effective.
+#define AOS_THREAD_LOCAL __attribute__((aligned(0x40))) thread_local
+#else
+#define AOS_THREAD_LOCAL thread_local
+#endif
+
+#endif // AOS_THREAD_LOCAL_H_
diff --git a/aos/util/file.cc b/aos/util/file.cc
index 089efbc..afefa86 100644
--- a/aos/util/file.cc
+++ b/aos/util/file.cc
@@ -8,7 +8,6 @@
#include <string_view>
#include "aos/scoped/scoped_fd.h"
-#include "glog/logging.h"
namespace aos {
namespace util {
@@ -48,22 +47,30 @@
}
}
-void MkdirP(std::string_view path, mode_t mode) {
+bool MkdirPIfSpace(std::string_view path, mode_t mode) {
auto last_slash_pos = path.find_last_of("/");
std::string folder(last_slash_pos == std::string_view::npos
? std::string_view("")
: path.substr(0, last_slash_pos));
- if (folder.empty()) return;
- MkdirP(folder, mode);
+ if (folder.empty()) {
+ return true;
+ }
+ if (!MkdirPIfSpace(folder, mode)) {
+ return false;
+ }
const int result = mkdir(folder.c_str(), mode);
if (result == -1 && errno == EEXIST) {
VLOG(2) << folder << " already exists";
- return;
+ return true;
+ } else if (result == -1 && errno == ENOSPC) {
+ VLOG(2) << "Out of space";
+ return false;
} else {
VLOG(1) << "Created " << folder;
}
PCHECK(result == 0) << ": Error creating " << folder;
+ return true;
}
bool PathExists(std::string_view path) {
diff --git a/aos/util/file.h b/aos/util/file.h
index 9ee2fb4..bf216bb 100644
--- a/aos/util/file.h
+++ b/aos/util/file.h
@@ -4,6 +4,8 @@
#include <string>
#include <string_view>
+#include "glog/logging.h"
+
namespace aos {
namespace util {
@@ -15,7 +17,12 @@
void WriteStringToFileOrDie(const std::string_view filename,
const std::string_view contents);
-void MkdirP(std::string_view path, mode_t mode);
+// Returns true if it succeeds or false if the filesystem is full.
+bool MkdirPIfSpace(std::string_view path, mode_t mode);
+
+inline void MkdirP(std::string_view path, mode_t mode) {
+ CHECK(MkdirPIfSpace(path, mode));
+}
bool PathExists(std::string_view path);
diff --git a/debian/BUILD b/debian/BUILD
index f498dff..6f955ae 100644
--- a/debian/BUILD
+++ b/debian/BUILD
@@ -65,11 +65,18 @@
gstreamer_armhf_debs = "files",
)
load(
- "//debian:m4.bzl",
+ ":m4.bzl",
m4_debs = "files",
)
-
-load("//debian:packages.bzl", "download_packages", "generate_deb_tarball")
+load(
+ ":lzma_amd64.bzl",
+ lzma_amd64_debs = "files",
+)
+load(
+ ":lzma_arm64.bzl",
+ lzma_arm64_debs = "files",
+)
+load(":packages.bzl", "download_packages", "generate_deb_tarball")
filegroup(
name = "matplotlib_patches",
@@ -341,6 +348,23 @@
files = m4_debs,
)
+download_packages(
+ name = "download_lzma",
+ packages = [
+ "liblzma-dev",
+ ],
+)
+
+generate_deb_tarball(
+ name = "lzma_amd64",
+ files = lzma_amd64_debs,
+)
+
+generate_deb_tarball(
+ name = "lzma_arm64",
+ files = lzma_arm64_debs,
+)
+
exports_files([
"ssh_wrapper.sh",
])
diff --git a/debian/lzma_amd64.bzl b/debian/lzma_amd64.bzl
new file mode 100644
index 0000000..1dbfa38
--- /dev/null
+++ b/debian/lzma_amd64.bzl
@@ -0,0 +1,3 @@
+files = {
+ "liblzma-dev_5.2.4-1_amd64.deb": "8373145ed28563aff33b116c434399347ab1fa77ba672d8d94151dd8f68c8ece",
+}
diff --git a/debian/lzma_arm64.bzl b/debian/lzma_arm64.bzl
new file mode 100644
index 0000000..a38d900
--- /dev/null
+++ b/debian/lzma_arm64.bzl
@@ -0,0 +1,3 @@
+files = {
+ "liblzma-dev_5.2.2-1.3_arm64.deb": "eb730c11f65c9bd10ae6c39b5a461e16294c21a423c9a8bc4011390798387e82",
+}
diff --git a/debian/matplotlib.BUILD b/debian/matplotlib.BUILD
index e7e9b6f..881521c 100644
--- a/debian/matplotlib.BUILD
+++ b/debian/matplotlib.BUILD
@@ -1,6 +1,9 @@
load("@//debian:matplotlib.bzl", "build_matplotlib")
-build_matplotlib("3", tkinter_py_version="3.5")
+build_matplotlib(
+ "3",
+ tkinter_py_version = "3.5",
+)
build_matplotlib(
"2.7",
diff --git a/frc971/codelab/README.md b/frc971/codelab/README.md
index e274f24..2fdf08c 100644
--- a/frc971/codelab/README.md
+++ b/frc971/codelab/README.md
@@ -1,5 +1,12 @@
# FRC971 "Codelab"
+## Flatbuffers tutorial
+
+Flatbuffers has pretty good [tutorials](https://google.github.io/flatbuffers/flatbuffers_guide_tutorial.html) for how to use them. We recommend
+going through them for more information.
+
+## Basic control loop
+
Welcome! This folder contains a "codelab" where you can go through the process
of fleshing out a basic control-loop using the same infrastructure as we do for
the control loops that normally run on our robots. Currently, this just consists
diff --git a/frc971/codelab/basic.cc b/frc971/codelab/basic.cc
index 36e64c5..6144fed 100644
--- a/frc971/codelab/basic.cc
+++ b/frc971/codelab/basic.cc
@@ -30,7 +30,8 @@
Status::Builder builder = status->MakeBuilder<Status>();
// TODO(you): Fill out the Status message! In order to populate fields, use
// the add_field_name() method on the builder, just like we do with the
- // Output message above.
+ // Output message above. Look at the definition of Status in
+ // basic_status.fbs and populate the field according to the comments there.
status->Send(builder.Finish());
}
diff --git a/frc971/codelab/basic.h b/frc971/codelab/basic.h
index 944048f..b75a87f 100644
--- a/frc971/codelab/basic.h
+++ b/frc971/codelab/basic.h
@@ -15,6 +15,9 @@
// This codelab helps build basic knowledge of how to use 971 control loop
// primatives.
//
+// For flatbuffers background, we recommend checking out
+// https://google.github.io/flatbuffers/flatbuffers_guide_tutorial.html
+//
// The meat of the task is to make the tests pass.
//
// Run the tests with:
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index a822ac4..f517e83 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -107,6 +107,19 @@
has_been_enabled_ = true;
}
+ if (WasReset()) {
+ // If all the sensors got reset (e.g., due to wpilib_interface restarting),
+ // reset the localizer and down estimator to avoid weird jumps in the
+ // filters.
+ down_estimator_.Reset();
+ // Just reset the localizer to the current state, except for the encoders.
+ LocalizerInterface::Ekf::State X_hat = localizer_->Xhat();
+ X_hat(LocalizerInterface::StateIdx::kLeftEncoder) = position->left_encoder();
+ X_hat(LocalizerInterface::StateIdx::kRightEncoder) =
+ position->right_encoder();
+ localizer_->Reset(monotonic_now, X_hat);
+ }
+
// TODO(austin): Put gear detection logic here.
switch (dt_config_.shifter_type) {
case ShifterType::SIMPLE_SHIFTER:
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 1f362f3..9ce3a48 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -64,8 +64,8 @@
if (!FLAGS_output_file.empty()) {
unlink(FLAGS_output_file.c_str());
logger_event_loop_ = MakeEventLoop("logger");
- logger_ = std::make_unique<aos::logger::Logger>(FLAGS_output_file,
- logger_event_loop_.get());
+ logger_ = std::make_unique<aos::logger::Logger>(logger_event_loop_.get());
+ logger_->StartLoggingLocalNamerOnRun(FLAGS_output_file);
}
// Run for enough time to allow the gyro/imu zeroing code to run.
@@ -125,11 +125,9 @@
// TODO(james): Handle Euler angle singularities...
const double down_estimator_yaw =
CHECK_NOTNULL(drivetrain_status_fetcher_->down_estimator())->yaw();
- const double localizer_yaw =
- drivetrain_status_fetcher_->theta();
- EXPECT_LT(
- std::abs(aos::math::DiffAngle(down_estimator_yaw, localizer_yaw)),
- 1e-2);
+ const double localizer_yaw = drivetrain_status_fetcher_->theta();
+ EXPECT_LT(std::abs(aos::math::DiffAngle(down_estimator_yaw, localizer_yaw)),
+ 1e-2);
const double true_yaw = (drivetrain_plant_.GetRightPosition() -
drivetrain_plant_.GetLeftPosition()) /
(dt_config_.robot_radius * 2.0);
@@ -558,9 +556,11 @@
auto builder = drivetrain_goal_sender_.MakeBuilder();
flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
- builder.fbb()->CreateVector<float>({0.0, -0.25, -0.5, -0.5, -0.75, -1.0});
+ builder.fbb()->CreateVector<float>(
+ {0.0, -0.25, -0.5, -0.5, -0.75, -1.0});
flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
- builder.fbb()->CreateVector<float>({0.0, 0.0, -0.25, -0.75, -1.0, -1.0});
+ builder.fbb()->CreateVector<float>(
+ {0.0, 0.0, -0.25, -0.75, -1.0, -1.0});
MultiSpline::Builder multispline_builder =
builder.MakeBuilder<MultiSpline>();
@@ -1463,6 +1463,47 @@
EXPECT_EQ(1.0, localizer_.theta());
}
+// Tests that if wpilib_interface restarts, the drivetrain handles it.
+TEST_F(DrivetrainTest, ResetDrivetrain) {
+ SetEnabled(true);
+ EXPECT_EQ(0.0, localizer_.x());
+ EXPECT_EQ(0.0, localizer_.y());
+ EXPECT_EQ(0.0, localizer_.theta());
+
+ {
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType::MOTION_PROFILE);
+ goal_builder.add_left_goal(4.0);
+ goal_builder.add_right_goal(4.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ }
+
+ RunFor(chrono::seconds(2));
+
+ const double x_pos = localizer_.x();
+ // The dead-reckoned X position in these sorts of situations tends to be
+ // relatively poor with the current localizer, since it ignores voltage inputs
+ // and models the robot's speed as always decaying towards zero, hence the
+ // large tolerances on the x position.
+ EXPECT_NEAR(4.0, x_pos, 2.0);
+ EXPECT_NEAR(0.0, localizer_.y(), 1e-5);
+ EXPECT_NEAR(0.0, localizer_.theta(), 1e-5);
+ EXPECT_NEAR(4.0, localizer_.left_encoder(), 1e-3);
+ EXPECT_NEAR(4.0, localizer_.right_encoder(), 1e-3);
+
+ SimulateSensorReset();
+ drivetrain_plant_.Reinitialize();
+
+ RunFor(dt());
+
+ EXPECT_EQ(x_pos, localizer_.x());
+ EXPECT_NEAR(0.0, localizer_.y(), 1e-5);
+ EXPECT_NEAR(0.0, localizer_.theta(), 1e-5);
+ EXPECT_NEAR(0.0, localizer_.left_encoder(), 1e-5);
+ EXPECT_NEAR(0.0, localizer_.right_encoder(), 1e-5);
+}
+
// TODO(austin): Make sure the profile reset code when we disable works.
} // namespace testing
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
index 3ed8d00..c7ef9fd 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -174,7 +174,7 @@
const double left_encoder = GetLeftPosition();
const double right_encoder = GetRightPosition();
- {
+ if (send_messages_) {
::aos::Sender<::frc971::control_loops::drivetrain::Position>::Builder
builder = drivetrain_position_sender_.MakeBuilder();
frc971::control_loops::drivetrain::Position::Builder position_builder =
@@ -188,6 +188,9 @@
}
void DrivetrainSimulation::SendImuMessage() {
+ if (!send_messages_) {
+ return;
+ }
auto builder = imu_sender_.MakeBuilder();
frc971::IMUValues::Builder imu_builder =
builder.MakeBuilder<frc971::IMUValues>();
@@ -228,10 +231,16 @@
void DrivetrainSimulation::Simulate() {
last_left_position_ = drivetrain_plant_.Y(0, 0);
last_right_position_ = drivetrain_plant_.Y(1, 0);
- EXPECT_TRUE(drivetrain_output_fetcher_.Fetch());
::Eigen::Matrix<double, 2, 1> U = last_U_;
- last_U_ << drivetrain_output_fetcher_->left_voltage(),
- drivetrain_output_fetcher_->right_voltage();
+ if (send_messages_) {
+ EXPECT_TRUE(drivetrain_output_fetcher_.Fetch());
+ last_U_ << drivetrain_output_fetcher_->left_voltage(),
+ drivetrain_output_fetcher_->right_voltage();
+ left_gear_high_ = drivetrain_output_fetcher_->left_high();
+ right_gear_high_ = drivetrain_output_fetcher_->right_high();
+ } else {
+ U = U.Zero();
+ }
{
robot_state_fetcher_.Fetch();
const double scalar = robot_state_fetcher_.get()
@@ -239,8 +248,6 @@
: 1.0;
last_U_ *= scalar;
}
- left_gear_high_ = drivetrain_output_fetcher_->left_high();
- right_gear_high_ = drivetrain_output_fetcher_->right_high();
if (left_gear_high_) {
if (right_gear_high_) {
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.h b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
index f749626..800b758 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.h
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
@@ -73,6 +73,12 @@
void MaybePlot();
+ // Set whether we should send out the drivetrain Position and IMU messages
+ // (this will keep sending the "truth" message).
+ void set_send_messages(const bool send_messages) {
+ send_messages_ = send_messages;
+ }
+
private:
// Sends out the position queue messages.
void SendPositionMessage();
@@ -123,6 +129,8 @@
::std::vector<double> actual_y_;
::std::vector<double> trajectory_x_;
::std::vector<double> trajectory_y_;
+
+ bool send_messages_ = true;
};
} // namespace testing
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index bd358ca..61408aa 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -160,6 +160,11 @@
// offsets decay, in seconds.
static constexpr double kVelocityOffsetTimeConstant = 1.0;
static constexpr double kLateralVelocityTimeConstant = 1.0;
+ // The maximum allowable timestep--we use this to check for situations where
+ // measurement updates come in too infrequently and this might cause the
+ // integrator and discretization in the prediction step to be overly
+ // aggressive.
+ static constexpr std::chrono::milliseconds kMaxTimestep{20};
// Inputs are [left_volts, right_volts]
typedef Eigen::Matrix<Scalar, kNInputs, 1> Input;
// Outputs are either:
@@ -559,7 +564,7 @@
State *state, StateSquare *P) {
*state = obs->X_hat;
*P = obs->P;
- if (dt.count() != 0) {
+ if (dt.count() != 0 && dt < kMaxTimestep) {
PredictImpl(obs, dt, state, P);
}
if (!(obs->h && obs->dhdx)) {
@@ -698,7 +703,7 @@
// Pull velocity derivatives from velocity matrices.
// Note that this looks really awkward (doesn't use
// Eigen blocks) because someone decided that the full
- // drivetrain Kalman Filter should half a weird convention.
+ // drivetrain Kalman Filter should have a weird convention.
// TODO(james): Support shifting drivetrains with changing A_continuous
const auto &vel_coefs = velocity_drivetrain_coefficients_;
A_continuous_(kLeftVelocity, kLeftVelocity) = vel_coefs.A_continuous(0, 0);
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
index bdfdd4e..48b8b5f 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
+++ b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
@@ -406,6 +406,19 @@
}
}
+// Tests that if we have an unusually large gap between measurement updates that
+// nothing crazy happens (a previous implementation had a bug where large
+// time-steps would mess up the prediction step due to how we approximated
+// things).
+TEST_F(HybridEkfTest, ExtraLongUpdateTime) {
+ Input U;
+ U << 0.0, 0.0, 0.1, 0.1;
+ ekf_.RawUpdateEncodersAndGyro(0.0, 0.0, 0.0, U, t0_ + dt_config_.dt);
+ ekf_.RawUpdateEncodersAndGyro(0.0, 0.0, 0.0, U,
+ t0_ + std::chrono::seconds(1000));
+ EXPECT_LT(ekf_.X_hat().norm(), 10.0) << ekf_.X_hat();
+}
+
// Tests encoder/gyro updates when we have some errors in our estimate.
TEST_F(HybridEkfTest, PerfectEncoderUpdateConverges) {
// In order to simulate modelling errors, we add an angular_error and start
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator.cc b/frc971/control_loops/drivetrain/improved_down_estimator.cc
index 665bf32..8d2592b 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator.cc
+++ b/frc971/control_loops/drivetrain/improved_down_estimator.cc
@@ -9,6 +9,30 @@
namespace control_loops {
namespace drivetrain {
+void QuaternionUkf::Reset() {
+ // The various noise matrices are tuned to provide values that seem
+ // reasonable given our current setup (using the ADIS16470 for
+ // measurements).
+ R_.setIdentity();
+ R_ /= std::pow(1000.0, 2);
+
+ Q_.setIdentity();
+ Q_ /= std::pow(2000.0 * 500.0, 2);
+
+ // Assume that the robot starts flat on the ground pointed straight forward.
+ X_hat_ = Eigen::Quaternion<double>(1.0, 0.0, 0.0, 0.0);
+
+ P_.setIdentity();
+ P_ /= 1000.0;
+
+ pos_vel_.setZero();
+ for (auto &last_accel : last_accels_) {
+ last_accel.setZero();
+ }
+
+ last_yaw_rates_.fill(0);
+}
+
void QuaternionUkf::Predict(const Eigen::Matrix<double, 3, 1> &U,
const Eigen::Matrix<double, 3, 1> &measurement,
const aos::monotonic_clock::duration dt) {
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator.h b/frc971/control_loops/drivetrain/improved_down_estimator.h
index 457bbef..79e9b7f 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator.h
+++ b/frc971/control_loops/drivetrain/improved_down_estimator.h
@@ -59,29 +59,11 @@
QuaternionUkf(const Eigen::Matrix<double, 3, 3> &imu_transform =
Eigen::Matrix<double, 3, 3>::Identity())
: imu_transform_(imu_transform) {
- // The various noise matrices are tuned to provide values that seem
- // reasonable given our current setup (using the ADIS16470 for
- // measurements).
- R_.setIdentity();
- R_ /= std::pow(1000.0, 2);
-
- Q_.setIdentity();
- Q_ /= std::pow(2000.0 * 500.0, 2);
-
- // Assume that the robot starts flat on the ground pointed straight forward.
- X_hat_ = Eigen::Quaternion<double>(1.0, 0.0, 0.0, 0.0);
-
- P_.setIdentity();
- P_ /= 1000.0;
-
- pos_vel_.setZero();
- for (auto &last_accel : last_accels_) {
- last_accel.setZero();
- }
-
- last_yaw_rates_.fill(0);
+ Reset();
}
+ void Reset();
+
// Handles updating the state of the UKF, given the gyro and accelerometer
// measurements. Given the design of the filter, U is the x/y/z gyro
// measurements and measurement is the accelerometer x/y/z measurements.
diff --git a/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
index 6677145..67d681e 100644
--- a/frc971/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -55,6 +55,8 @@
::aos::monotonic_clock::time_point now,
double left_encoder, double right_encoder,
double gyro_rate, const Eigen::Vector3d &accel) = 0;
+ virtual void Reset(aos::monotonic_clock::time_point t,
+ const Ekf::State &state) = 0;
// Reset the absolute position of the estimator.
virtual void ResetPosition(::aos::monotonic_clock::time_point t, double x,
double y, double theta, double theta_uncertainty,
@@ -148,6 +150,11 @@
now);
}
+ void Reset(aos::monotonic_clock::time_point t,
+ const HybridEkf<double>::State &state) override {
+ ekf_.ResetInitialState(t, state, ekf_.P());
+ }
+
void ResetPosition(::aos::monotonic_clock::time_point t, double x, double y,
double theta, double /*theta_override*/,
bool /*reset_theta*/) override {
diff --git a/frc971/control_loops/position_sensor_sim.cc b/frc971/control_loops/position_sensor_sim.cc
index b874b1d..b605196 100644
--- a/frc971/control_loops/position_sensor_sim.cc
+++ b/frc971/control_loops/position_sensor_sim.cc
@@ -73,8 +73,10 @@
known_absolute_encoder_ = known_absolute_encoder_pos;
- lower_index_edge_.mutable_pot_noise()->set_standard_deviation(pot_noise_stddev);
- upper_index_edge_.mutable_pot_noise()->set_standard_deviation(pot_noise_stddev);
+ lower_index_edge_.mutable_pot_noise()->set_standard_deviation(
+ pot_noise_stddev);
+ upper_index_edge_.mutable_pot_noise()->set_standard_deviation(
+ pot_noise_stddev);
}
void PositionSensorSimulator::InitializeHallEffectAndPosition(
@@ -230,6 +232,5 @@
return builder->Finish();
}
-
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/position_sensor_sim_test.cc b/frc971/control_loops/position_sensor_sim_test.cc
index 93a648a..424a3af 100644
--- a/frc971/control_loops/position_sensor_sim_test.cc
+++ b/frc971/control_loops/position_sensor_sim_test.cc
@@ -4,10 +4,10 @@
#include <random>
-#include "gtest/gtest.h"
+#include "aos/die.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/position_sensor_sim.h"
-#include "aos/die.h"
+#include "gtest/gtest.h"
#include "flatbuffers/flatbuffers.h"
@@ -33,7 +33,8 @@
// Make sure that we don't accidentally hit an index pulse.
for (int i = 0; i < 30; i++) {
sim.MoveTo(3.6 * index_diff);
- const IndexPosition *index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ const IndexPosition *index_position =
+ sim.FillSensorValues<IndexPosition>(&fbb);
const PotAndIndexPosition *pot_and_index_position =
sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
ASSERT_DOUBLE_EQ(3.6 * index_diff, pot_and_index_position->pot());
@@ -44,7 +45,8 @@
for (int i = 0; i < 30; i++) {
sim.MoveTo(3.0 * index_diff);
- const IndexPosition *index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ const IndexPosition *index_position =
+ sim.FillSensorValues<IndexPosition>(&fbb);
const PotAndIndexPosition *pot_and_index_position =
sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
ASSERT_DOUBLE_EQ(3.0 * index_diff, pot_and_index_position->pot());
@@ -54,7 +56,8 @@
for (int i = 0; i < 30; i++) {
sim.MoveTo(3.99 * index_diff);
- const IndexPosition *index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ const IndexPosition *index_position =
+ sim.FillSensorValues<IndexPosition>(&fbb);
const PotAndIndexPosition *pot_and_index_position =
sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
ASSERT_DOUBLE_EQ(3.99 * index_diff, pot_and_index_position->pot());
@@ -64,7 +67,8 @@
for (int i = 0; i < 30; i++) {
sim.MoveTo(3.0 * index_diff);
- const IndexPosition *index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ const IndexPosition *index_position =
+ sim.FillSensorValues<IndexPosition>(&fbb);
const PotAndIndexPosition *pot_and_index_position =
sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
ASSERT_DOUBLE_EQ(3.0 * index_diff, pot_and_index_position->pot());
@@ -85,7 +89,8 @@
sim.Initialize(4.6 * index_diff, 0);
// Make sure that we get an index pulse on every transition.
- const IndexPosition *index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ const IndexPosition *index_position =
+ sim.FillSensorValues<IndexPosition>(&fbb);
const PotAndIndexPosition *pot_and_index_position =
sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
ASSERT_EQ(0u, index_position->index_pulses());
@@ -144,14 +149,16 @@
flatbuffers::FlatBufferBuilder pot_fbb;
sim.MoveTo(0.75 * index_diff);
- const IndexPosition *index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ const IndexPosition *index_position =
+ sim.FillSensorValues<IndexPosition>(&fbb);
const PotAndIndexPosition *pot_and_index_position =
sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
EXPECT_EQ(1u, index_position->index_pulses());
EXPECT_EQ(1u, pot_and_index_position->index_pulses());
EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position->latched_pot());
EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position->latched_encoder());
- EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position->latched_encoder());
+ EXPECT_DOUBLE_EQ(index_diff * 0.25,
+ pot_and_index_position->latched_encoder());
sim.MoveTo(index_diff);
index_position = sim.FillSensorValues<IndexPosition>(&fbb);
@@ -160,7 +167,8 @@
EXPECT_EQ(1u, pot_and_index_position->index_pulses());
EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position->latched_pot());
EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position->latched_encoder());
- EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position->latched_encoder());
+ EXPECT_DOUBLE_EQ(index_diff * 0.25,
+ pot_and_index_position->latched_encoder());
sim.MoveTo(1.75 * index_diff);
index_position = sim.FillSensorValues<IndexPosition>(&fbb);
@@ -169,7 +177,8 @@
EXPECT_EQ(2u, pot_and_index_position->index_pulses());
EXPECT_DOUBLE_EQ(index_diff * 1.5, pot_and_index_position->latched_pot());
EXPECT_DOUBLE_EQ(index_diff * 1.25, index_position->latched_encoder());
- EXPECT_DOUBLE_EQ(index_diff * 1.25, pot_and_index_position->latched_encoder());
+ EXPECT_DOUBLE_EQ(index_diff * 1.25,
+ pot_and_index_position->latched_encoder());
// Try it with our known index pulse not being our first one.
sim.Initialize(index_diff * 0.25, 0.0, index_diff * 1.5);
@@ -181,7 +190,8 @@
EXPECT_EQ(1u, pot_and_index_position->index_pulses());
EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position->latched_pot());
EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position->latched_encoder());
- EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position->latched_encoder());
+ EXPECT_DOUBLE_EQ(index_diff * 0.25,
+ pot_and_index_position->latched_encoder());
sim.MoveTo(index_diff);
index_position = sim.FillSensorValues<IndexPosition>(&fbb);
@@ -190,7 +200,8 @@
EXPECT_EQ(1u, pot_and_index_position->index_pulses());
EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position->latched_pot());
EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position->latched_encoder());
- EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position->latched_encoder());
+ EXPECT_DOUBLE_EQ(index_diff * 0.25,
+ pot_and_index_position->latched_encoder());
sim.MoveTo(1.75 * index_diff);
index_position = sim.FillSensorValues<IndexPosition>(&fbb);
@@ -199,7 +210,8 @@
EXPECT_EQ(2u, pot_and_index_position->index_pulses());
EXPECT_DOUBLE_EQ(index_diff * 1.5, pot_and_index_position->latched_pot());
EXPECT_DOUBLE_EQ(index_diff * 1.25, index_position->latched_encoder());
- EXPECT_DOUBLE_EQ(index_diff * 1.25, pot_and_index_position->latched_encoder());
+ EXPECT_DOUBLE_EQ(index_diff * 1.25,
+ pot_and_index_position->latched_encoder());
}
// Tests that the latched values update correctly.
@@ -320,7 +332,7 @@
const double index_diff = 1.0;
PositionSensorSimulator sim(index_diff);
sim.InitializeHallEffectAndPosition(-0.25, 0.0, 0.5);
- //HallEffectAndPosition position;
+ // HallEffectAndPosition position;
flatbuffers::FlatBufferBuilder fbb;
// Go over only the lower edge rising.
@@ -397,6 +409,5 @@
}
}
-
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/downloader/downloader.py b/frc971/downloader/downloader.py
index 42d4a7c..fa618fe 100644
--- a/frc971/downloader/downloader.py
+++ b/frc971/downloader/downloader.py
@@ -72,9 +72,9 @@
if user is None:
if args.type == "pi":
- user = "pi"
+ user = "pi"
elif args.type == "roborio":
- user = "admin"
+ user = "admin"
target_dir = "/home/" + user + "/robot_code"
ssh_target = "%s@%s" % (user, hostname)
@@ -86,7 +86,7 @@
rsync_cmd = ([
"external/rsync/usr/bin/rsync", "-e", ssh_path, "-c", "-v", "-z",
- "--copy-links"
+ "--perms", "--copy-links"
] + srcs + ["%s:%s/%s" % (ssh_target, target_dir, relative_dir)])
try:
subprocess.check_call(rsync_cmd)
diff --git a/frc971/wpilib/ADIS16470.cc b/frc971/wpilib/ADIS16470.cc
index fa61d9b..ec2ee9e 100644
--- a/frc971/wpilib/ADIS16470.cc
+++ b/frc971/wpilib/ADIS16470.cc
@@ -296,8 +296,9 @@
(0 << 2) /* internal clock mode */ |
(0 << 1) /* sync polarity, doesn't matter */ |
(1 << 0) /* data ready is active high */);
+ // Rate of the output will be 2000 / (DEC_RATE + 1) Hz.
WriteRegister(registers::DEC_RATE,
- 0 /* no internal decimation (averaging) */);
+ 1 /* Average every pair of values. */);
// Start a sensor self test.
WriteRegister(registers::GLOB_CMD, 1 << 2);
diff --git a/third_party/BUILD b/third_party/BUILD
index bcba978..1ef7169 100644
--- a/third_party/BUILD
+++ b/third_party/BUILD
@@ -113,3 +113,15 @@
"roborio": ["@webrtc_rio//:webrtc"],
}),
)
+
+# TODO(Brian): Properly restrict this to specific platforms and remove the
+# default deps once we have support for that which interacts with select properly.
+cc_library(
+ name = "lzma",
+ visibility = ["//visibility:public"],
+ deps = select({
+ "//tools:cpu_k8": ["@lzma_amd64//:lib"],
+ "//tools:cpu_aarch64": ["@lzma_arm64//:lib"],
+ "//conditions:default": [],
+ }),
+)
diff --git a/third_party/gmp/BUILD b/third_party/gmp/BUILD
index b60c349..0ac8edc 100644
--- a/third_party/gmp/BUILD
+++ b/third_party/gmp/BUILD
@@ -47,6 +47,9 @@
],
}
+# gmp's tools leak memory on purpose. Just skip asan for them.
+tool_features = ["-asan"]
+
genrule(
name = "gmp_h_copy",
srcs = file_from_architecture(architecture_paths, "gmp.h"),
@@ -112,6 +115,7 @@
name = "gen-fac",
srcs = ["gen-fac.c"],
copts = copts,
+ features = tool_features,
deps = [":bootstrap"],
)
@@ -133,6 +137,7 @@
name = "gen-fib",
srcs = ["gen-fib.c"],
copts = copts,
+ features = tool_features,
deps = [":bootstrap"],
)
@@ -154,6 +159,7 @@
name = "gen-bases",
srcs = ["gen-bases.c"],
copts = copts,
+ features = tool_features,
deps = [":bootstrap"],
)
@@ -175,6 +181,7 @@
name = "gen-trialdivtab",
srcs = ["gen-trialdivtab.c"],
copts = copts,
+ features = tool_features,
deps = [":bootstrap"],
)
diff --git a/third_party/google-glog/src/logging.cc b/third_party/google-glog/src/logging.cc
index 2bfce3d..db22cd5 100644
--- a/third_party/google-glog/src/logging.cc
+++ b/third_party/google-glog/src/logging.cc
@@ -44,6 +44,7 @@
#ifdef HAVE_SYS_UTSNAME_H
# include <sys/utsname.h> // For uname.
#endif
+#include <dirent.h>
#include <fcntl.h>
#include <cstdio>
#include <iostream>
@@ -1461,9 +1462,26 @@
if (data_->first_fatal_) {
{
// Put this back on SCHED_OTHER by default.
- struct sched_param param;
- param.sched_priority = 0;
- sched_setscheduler(0, SCHED_OTHER, ¶m);
+ DIR *dirp = opendir("/proc/self/task");
+ if (dirp) {
+ struct dirent *directory_entry;
+ while ((directory_entry = readdir(dirp)) != NULL) {
+ int thread_id = std::atoi(directory_entry->d_name);
+
+ // ignore . and .. which are zeroes for some reason
+ if (thread_id != 0) {
+ struct sched_param param;
+ param.sched_priority = 20;
+ sched_setscheduler(thread_id, SCHED_OTHER, ¶m);
+ }
+ }
+ closedir(dirp);
+ } else {
+ // Can't get other threads; just lower own priority.
+ struct sched_param param;
+ param.sched_priority = 20;
+ sched_setscheduler(0, SCHED_OTHER, ¶m);
+ }
}
// Store crash information so that it is accessible from within signal
diff --git a/third_party/google-glog/src/signalhandler.cc b/third_party/google-glog/src/signalhandler.cc
index 049efa5..87ed94f 100644
--- a/third_party/google-glog/src/signalhandler.cc
+++ b/third_party/google-glog/src/signalhandler.cc
@@ -44,6 +44,7 @@
#ifdef HAVE_SYS_UCONTEXT_H
# include <sys/ucontext.h>
#endif
+#include <dirent.h>
#include <algorithm>
_START_GOOGLE_NAMESPACE_
@@ -311,9 +312,26 @@
{
// Put this back on SCHED_OTHER by default.
- struct sched_param param;
- param.sched_priority = 0;
- sched_setscheduler(0, SCHED_OTHER, ¶m);
+ DIR *dirp = opendir("/proc/self/task");
+ if (dirp) {
+ struct dirent *directory_entry;
+ while ((directory_entry = readdir(dirp)) != NULL) {
+ int thread_id = std::atoi(directory_entry->d_name);
+
+ // ignore . and .. which are zeroes for some reason
+ if (thread_id != 0) {
+ struct sched_param param;
+ param.sched_priority = 20;
+ sched_setscheduler(thread_id, SCHED_OTHER, ¶m);
+ }
+ }
+ closedir(dirp);
+ } else {
+ // Can't get other threads; just lower own priority.
+ struct sched_param param;
+ param.sched_priority = 20;
+ sched_setscheduler(0, SCHED_OTHER, ¶m);
+ }
}
// This is the first time we enter the signal handler. We are going to
diff --git a/tools/BUILD b/tools/BUILD
index ac76ee9..9bf2931 100644
--- a/tools/BUILD
+++ b/tools/BUILD
@@ -49,6 +49,11 @@
)
config_setting(
+ name = "cpu_aarch64",
+ values = {"cpu": "aarch64"},
+)
+
+config_setting(
name = "has_asan",
values = {"define": "have_asan=true"},
)
@@ -75,11 +80,14 @@
environment(name = "cortex-m4f-k22")
+environment(name = "aarch64")
+
environment_group(
name = "cpus",
defaults = [
":k8",
":roborio",
+ ":aarch64",
":armhf-debian",
],
environments = [
@@ -89,5 +97,6 @@
":armhf-debian",
":cortex-m4f",
":cortex-m4f-k22",
+ ":aarch64",
],
)
diff --git a/y2019/control_loops/drivetrain/drivetrain_replay.cc b/y2019/control_loops/drivetrain/drivetrain_replay.cc
index f7b69e4..98f3eeb 100644
--- a/y2019/control_loops/drivetrain/drivetrain_replay.cc
+++ b/y2019/control_loops/drivetrain/drivetrain_replay.cc
@@ -42,7 +42,8 @@
log_writer_event_loop->SkipTimingReport();
log_writer_event_loop->SkipAosLog();
CHECK(nullptr == log_writer_event_loop->node());
- aos::logger::Logger writer(FLAGS_output_file, log_writer_event_loop.get());
+ aos::logger::Logger writer(log_writer_event_loop.get());
+ writer.StartLoggingLocalNamerOnRun(FLAGS_output_file);
std::unique_ptr<aos::EventLoop> drivetrain_event_loop =
reader.event_loop_factory()->MakeEventLoop("drivetrain");
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.cc b/y2019/control_loops/drivetrain/event_loop_localizer.cc
index 16cd723..634cedb 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.cc
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.cc
@@ -49,6 +49,11 @@
}
void EventLoopLocalizer::Reset(::aos::monotonic_clock::time_point now,
+ const Localizer::State &state) {
+ localizer_.ResetInitialState(now, state, localizer_.P());
+}
+
+void EventLoopLocalizer::Reset(::aos::monotonic_clock::time_point now,
const Localizer::State &state,
double theta_uncertainty) {
Localizer::StateSquare newP = localizer_.P();
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.h b/y2019/control_loops/drivetrain/event_loop_localizer.h
index 5305b8d..a78f3a2 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.h
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.h
@@ -34,6 +34,8 @@
&dt_config);
void Reset(::aos::monotonic_clock::time_point t,
+ const Localizer::State &state) override;
+ void Reset(::aos::monotonic_clock::time_point t,
const Localizer::State &state, double theta_uncertainty);
void ResetPosition(::aos::monotonic_clock::time_point t, double x, double y,
double theta, double theta_uncertainty,
diff --git a/y2020/control_loops/drivetrain/drivetrain_replay.cc b/y2020/control_loops/drivetrain/drivetrain_replay.cc
index 0dd5a1a..0ed57c5 100644
--- a/y2020/control_loops/drivetrain/drivetrain_replay.cc
+++ b/y2020/control_loops/drivetrain/drivetrain_replay.cc
@@ -49,7 +49,8 @@
std::unique_ptr<aos::EventLoop> log_writer_event_loop =
reader.event_loop_factory()->MakeEventLoop("log_writer", node);
log_writer_event_loop->SkipTimingReport();
- aos::logger::Logger writer(FLAGS_output_file, log_writer_event_loop.get());
+ aos::logger::Logger writer(log_writer_event_loop.get());
+ writer.StartLoggingLocalNamerOnRun(FLAGS_output_file);
std::unique_ptr<aos::EventLoop> drivetrain_event_loop =
reader.event_loop_factory()->MakeEventLoop("drivetrain", node);
diff --git a/y2020/control_loops/drivetrain/localizer.cc b/y2020/control_loops/drivetrain/localizer.cc
index 7d7c83f..79035df 100644
--- a/y2020/control_loops/drivetrain/localizer.cc
+++ b/y2020/control_loops/drivetrain/localizer.cc
@@ -95,6 +95,16 @@
target_selector_.set_has_target(false);
}
+void Localizer::Reset(
+ aos::monotonic_clock::time_point t,
+ const frc971::control_loops::drivetrain::HybridEkf<double>::State &state) {
+ // Go through and clear out all of the fetchers so that we don't get behind.
+ for (auto &fetcher : image_fetchers_) {
+ fetcher.Fetch();
+ }
+ ekf_.ResetInitialState(t, state.cast<float>(), ekf_.P());
+}
+
void Localizer::HandleSuperstructureStatus(
const y2020::control_loops::superstructure::Status &status) {
CHECK(status.has_turret());
@@ -162,7 +172,10 @@
LOG(WARNING) << "Got camera frame from the future.";
return;
}
- CHECK(result.has_camera_calibration());
+ if (!result.has_camera_calibration()) {
+ LOG(WARNING) << "Got camera frame without calibration data.";
+ return;
+ }
// Per the ImageMatchResult specification, we can actually determine whether
// the camera is the turret camera just from the presence of the
// turret_extrinsics member.
diff --git a/y2020/control_loops/drivetrain/localizer.h b/y2020/control_loops/drivetrain/localizer.h
index 341f304..5dbad02 100644
--- a/y2020/control_loops/drivetrain/localizer.h
+++ b/y2020/control_loops/drivetrain/localizer.h
@@ -51,9 +51,9 @@
double right_encoder, double gyro_rate,
const Eigen::Vector3d &accel) override;
- void Reset(aos::monotonic_clock::time_point t, const State &state) {
- ekf_.ResetInitialState(t, state, ekf_.P());
- }
+ void Reset(aos::monotonic_clock::time_point t,
+ const frc971::control_loops::drivetrain::HybridEkf<double>::State
+ &state) override;
void ResetPosition(aos::monotonic_clock::time_point t, double x, double y,
double theta, double /*theta_override*/,
diff --git a/y2020/control_loops/drivetrain/localizer_test.cc b/y2020/control_loops/drivetrain/localizer_test.cc
index ae9f887..da3ef17 100644
--- a/y2020/control_loops/drivetrain/localizer_test.cc
+++ b/y2020/control_loops/drivetrain/localizer_test.cc
@@ -26,10 +26,10 @@
using frc971::control_loops::drivetrain::DrivetrainConfig;
using frc971::control_loops::drivetrain::Goal;
using frc971::control_loops::drivetrain::LocalizerControl;
+using frc971::vision::sift::CameraCalibrationT;
+using frc971::vision::sift::CameraPoseT;
using frc971::vision::sift::ImageMatchResult;
using frc971::vision::sift::ImageMatchResultT;
-using frc971::vision::sift::CameraPoseT;
-using frc971::vision::sift::CameraCalibrationT;
using frc971::vision::sift::TransformationMatrixT;
namespace {
@@ -90,9 +90,9 @@
} // namespace
namespace chrono = std::chrono;
-using frc971::control_loops::drivetrain::testing::DrivetrainSimulation;
-using frc971::control_loops::drivetrain::DrivetrainLoop;
using aos::monotonic_clock;
+using frc971::control_loops::drivetrain::DrivetrainLoop;
+using frc971::control_loops::drivetrain::testing::DrivetrainSimulation;
class LocalizedDrivetrainTest : public aos::testing::ControlLoopTest {
protected:
@@ -136,8 +136,8 @@
if (!FLAGS_output_file.empty()) {
logger_event_loop_ = MakeEventLoop("logger", roborio_);
- logger_ = std::make_unique<aos::logger::Logger>(FLAGS_output_file,
- logger_event_loop_.get());
+ logger_ = std::make_unique<aos::logger::Logger>(logger_event_loop_.get());
+ logger_->StartLoggingLocalNamerOnRun(FLAGS_output_file);
}
test_event_loop_->MakeWatcher(
@@ -152,7 +152,7 @@
last_frame_ = monotonic_now();
}
}
- });
+ });
test_event_loop_->AddPhasedLoop(
[this](int) {
@@ -206,9 +206,9 @@
void SetStartingPosition(const Eigen::Matrix<double, 3, 1> &xytheta) {
*drivetrain_plant_.mutable_state() << xytheta.x(), xytheta.y(),
xytheta(2, 0), 0.0, 0.0;
- Eigen::Matrix<float, Localizer::HybridEkf::kNStates, 1> localizer_state;
+ Eigen::Matrix<double, Localizer::HybridEkf::kNStates, 1> localizer_state;
localizer_state.setZero();
- localizer_state.block<3, 1>(0, 0) = xytheta.cast<float>();
+ localizer_state.block<3, 1>(0, 0) = xytheta;
localizer_.Reset(monotonic_now(), localizer_state);
}
@@ -337,8 +337,7 @@
aos::Sender<aos::message_bridge::ServerStatistics> server_statistics_sender_;
std::unique_ptr<aos::EventLoop> drivetrain_event_loop_;
- const frc971::control_loops::drivetrain::DrivetrainConfig<double>
- dt_config_;
+ const frc971::control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
std::unique_ptr<aos::EventLoop> pi1_event_loop_;
aos::Sender<ImageMatchResult> camera_sender_;
@@ -548,6 +547,33 @@
EXPECT_TRUE(VerifyEstimatorAccurate(1e-2));
}
+// Tests that we don't blow up if we stop getting updates for an extended period
+// of time and fall behind on fetching fron the cameras.
+TEST_F(LocalizedDrivetrainTest, FetchersHandleTimeGap) {
+ set_enable_cameras(true);
+ set_send_delay(std::chrono::seconds(0));
+ event_loop_factory()->set_network_delay(std::chrono::nanoseconds(1));
+ test_event_loop_
+ ->AddTimer([this]() { drivetrain_plant_.set_send_messages(false); })
+ ->Setup(test_event_loop_->monotonic_now());
+ test_event_loop_->AddPhasedLoop(
+ [this](int) {
+ auto builder = camera_sender_.MakeBuilder();
+ ImageMatchResultT image;
+ ASSERT_TRUE(
+ builder.Send(ImageMatchResult::Pack(*builder.fbb(), &image)));
+ },
+ std::chrono::milliseconds(20));
+ test_event_loop_
+ ->AddTimer([this]() {
+ drivetrain_plant_.set_send_messages(true);
+ SimulateSensorReset();
+ })
+ ->Setup(test_event_loop_->monotonic_now() + std::chrono::seconds(10));
+
+ RunFor(chrono::seconds(20));
+}
+
} // namespace testing
} // namespace drivetrain
} // namespace control_loops
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index ffa9539..ac1e4a0 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -440,8 +440,8 @@
if (!FLAGS_output_file.empty()) {
unlink(FLAGS_output_file.c_str());
logger_event_loop_ = MakeEventLoop("logger", roborio_);
- logger_ = std::make_unique<aos::logger::Logger>(FLAGS_output_file,
- logger_event_loop_.get());
+ logger_ = std::make_unique<aos::logger::Logger>(logger_event_loop_.get());
+ logger_->StartLoggingLocalNamerOnRun(FLAGS_output_file);
}
}
diff --git a/y2020/y2020.json b/y2020/y2020.json
index 808a796..59f3fc0 100644
--- a/y2020/y2020.json
+++ b/y2020/y2020.json
@@ -1,5 +1,5 @@
{
- "channel_storage_duration": 5000000000,
+ "channel_storage_duration": 2000000000,
"maps": [
{
"match": {
diff --git a/y2020/y2020_laptop.json b/y2020/y2020_laptop.json
index 0797110..37961f8 100644
--- a/y2020/y2020_laptop.json
+++ b/y2020/y2020_laptop.json
@@ -135,7 +135,7 @@
"source_node": "laptop",
"frequency": 10,
"num_senders": 2,
- "max_size": 300,
+ "max_size": 400,
"destination_nodes": [
{
"name": "pi1",
@@ -181,7 +181,7 @@
"logger": "NOT_LOGGED",
"frequency": 20,
"num_senders": 2,
- "max_size": 300
+ "max_size": 200
},
{
"name": "/laptop/aos/remote_timestamps/pi1",
@@ -190,7 +190,7 @@
"logger": "NOT_LOGGED",
"frequency": 20,
"num_senders": 2,
- "max_size": 300
+ "max_size": 200
},
{
"name": "/laptop/aos/remote_timestamps/pi2",
@@ -199,7 +199,7 @@
"logger": "NOT_LOGGED",
"frequency": 20,
"num_senders": 2,
- "max_size": 300
+ "max_size": 200
},
{
"name": "/laptop/aos/remote_timestamps/pi3",
@@ -208,7 +208,7 @@
"logger": "NOT_LOGGED",
"frequency": 20,
"num_senders": 2,
- "max_size": 300
+ "max_size": 200
},
{
"name": "/laptop/aos/remote_timestamps/pi4",
@@ -217,7 +217,7 @@
"logger": "NOT_LOGGED",
"frequency": 20,
"num_senders": 2,
- "max_size": 300
+ "max_size": 200
},
{
"name": "/pi1/camera",
diff --git a/y2020/y2020_roborio.json b/y2020/y2020_roborio.json
index b63982d..0ab9664 100644
--- a/y2020/y2020_roborio.json
+++ b/y2020/y2020_roborio.json
@@ -74,7 +74,7 @@
"source_node": "roborio",
"frequency": 10,
"num_senders": 2,
- "max_size": 300,
+ "max_size": 400,
"destination_nodes": [
{
"name": "pi1",