Enable deploying code for the raspberry pis

Also, sets up a systemd service to start all the code.

Change-Id: If993426b36d0910497579ad86e330699f8a7d2e7
diff --git a/aos/starter/starter.sh b/aos/starter/starter.sh
index f5b14a5..e140351 100755
--- a/aos/starter/starter.sh
+++ b/aos/starter/starter.sh
@@ -6,6 +6,9 @@
 
   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
+  ROBOT_CODE="/home/pi/robot_code"
+
 else
   ROBOT_CODE="${HOME}/robot_code"
 fi
diff --git a/frc971/downloader.bzl b/frc971/downloader.bzl
index aff3420..11423e0 100644
--- a/frc971/downloader.bzl
+++ b/frc971/downloader.bzl
@@ -1,7 +1,15 @@
 load("//frc971/downloader:downloader.bzl", "aos_downloader")
 load("//tools/build_rules:label.bzl", "expand_label")
 
-def robot_downloader(start_binaries, binaries = [], data = [], dirs = None, default_target = None):
+
+def robot_downloader(start_binaries,
+                     name="download",
+                     binaries=[],
+                     data=[],
+                     dirs=None,
+                     default_target=None,
+                     restricted_to=["//tools:roborio"],
+                     target_type="roborio"):
     """Sets up the standard robot download targets.
 
     Attrs:
@@ -11,27 +19,30 @@
     """
 
     aos_downloader(
-        name = "download",
-        start_srcs = [
+        name=name,
+        start_srcs=([
             "//aos:prime_start_binaries",
-        ] + start_binaries,
-        srcs = [
+        ] if target_type == "roborio" else []) + start_binaries,
+        srcs=[
             "//aos:prime_binaries",
         ] + binaries + data,
-        dirs = dirs,
-        default_target = default_target,
-        restricted_to = ["//tools:roborio"],
+        dirs=dirs,
+        target_type=target_type,
+        default_target=default_target,
+        restricted_to=restricted_to,
     )
 
     aos_downloader(
-        name = "download_stripped",
-        start_srcs = [
+        name=name + "_stripped",
+        start_srcs=([
             "//aos:prime_start_binaries_stripped",
-        ] + [expand_label(binary) + ".stripped" for binary in start_binaries],
-        srcs = [
+        ] if target_type == "roborio" else []) +
+        [expand_label(binary) + ".stripped" for binary in start_binaries],
+        srcs=[
             "//aos:prime_binaries_stripped",
         ] + [expand_label(binary) + ".stripped" for binary in binaries] + data,
-        dirs = dirs,
-        default_target = default_target,
-        restricted_to = ["//tools:roborio"],
+        dirs=dirs,
+        target_type=target_type,
+        default_target=default_target,
+        restricted_to=restricted_to,
     )
diff --git a/frc971/downloader/downloader.bzl b/frc971/downloader/downloader.bzl
index a5d1bc1..75a1a9b 100644
--- a/frc971/downloader/downloader.bzl
+++ b/frc971/downloader/downloader.bzl
@@ -7,16 +7,16 @@
             "#!/bin/bash",
             "set -e",
             'cd "${BASH_SOURCE[0]}.runfiles/%s"' % ctx.workspace_name,
-        ] + ['%s %s --dirs %s -- %s "$@"' % (
+        ] + ['%s --dir %s --target "$@" --type %s %s' % (
             ctx.executable._downloader.short_path,
-            " ".join([src.short_path for src in d.downloader_srcs]),
             d.downloader_dir,
-            ctx.attr.default_target,
+            ctx.attr.target_type,
+            " ".join([src.short_path for src in d.downloader_srcs]),
         ) for d in ctx.attr.dirs] + [
-            'exec %s %s -- %s "$@"' % (
+            'exec %s --target "$@" --type %s %s' % (
                 ctx.executable._downloader.short_path,
+                ctx.attr.target_type,
                 " ".join([src.short_path for src in all_files]),
-                ctx.attr.default_target,
             ),
         ]),
     )
@@ -57,8 +57,6 @@
   srcs: The files to download. They currently all get shoved into one folder.
   dirs: A list of aos_downloader_dirs to download too.
   start_srcs: Like srcs, except they also get put into start_list.txt.
-  default_target: The default host to download to. If not specified, defaults to
-                  roboRIO-971.local.
 """
 
 aos_downloader = rule(
@@ -76,6 +74,9 @@
             mandatory = True,
             allow_files = True,
         ),
+        "target_type": attr.string(
+            default = "roborio",
+        ),
         "dirs": attr.label_list(
             mandatory = False,
             providers = [
@@ -83,9 +84,6 @@
                 "downloader_srcs",
             ],
         ),
-        "default_target": attr.string(
-            default = "roboRIO-971-frc.local",
-        ),
     },
     executable = True,
     outputs = {
diff --git a/frc971/downloader/downloader.py b/frc971/downloader/downloader.py
index e4826cf..5ebf417 100644
--- a/frc971/downloader/downloader.py
+++ b/frc971/downloader/downloader.py
@@ -4,6 +4,7 @@
 
 from __future__ import print_function
 
+import argparse
 import sys
 import subprocess
 import re
@@ -16,61 +17,74 @@
     PKG_URL = "http://download.ni.com/ni-linux-rt/feeds/2015/arm/ipk/cortexa9-vfpv3/" + pkg
     subprocess.check_call(["wget", PKG_URL, "-O", pkg])
     try:
-        subprocess.check_call([
-            scp_path, "-S", ssh_path, pkg,
-            ssh_target + ":/tmp/" + pkg
-        ])
-        subprocess.check_call([
-            ssh_path, ssh_target, "opkg", "install",
-            "/tmp/" + pkg
-        ])
         subprocess.check_call(
-            [ssh_path, ssh_target, "rm", "/tmp/" + pkg])
+            [scp_path, "-S", ssh_path, pkg, ssh_target + ":/tmp/" + pkg])
+        subprocess.check_call(
+            [ssh_path, ssh_target, "opkg", "install", "/tmp/" + pkg])
+        subprocess.check_call([ssh_path, ssh_target, "rm", "/tmp/" + pkg])
     finally:
         subprocess.check_call(["rm", pkg])
 
 
 def main(argv):
-    args = argv[argv.index("--") + 1:]
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--target",
+                        type=str,
+                        default="roborio-971-frc.local",
+                        help="Target to deploy code to.")
+    parser.add_argument("--type",
+                        type=str,
+                        choices=["roborio", "pi"],
+                        required=True,
+                        help="Target type for deployment")
+    parser.add_argument(
+        "--dir",
+        type=str,
+        help="Directory within robot_code to copy the files to.")
+    parser.add_argument("srcs",
+                        type=str,
+                        nargs='+',
+                        help="List of files to copy over")
+    args = parser.parse_args(argv[1:])
 
     relative_dir = ""
     recursive = False
 
-    if "--dirs" in argv:
-        dirs_index = argv.index("--dirs")
-        srcs = argv[1:dirs_index]
-        relative_dir = argv[dirs_index + 1]
+    srcs = args.srcs
+    if args.dir is not None:
+        relative_dir = args.dir
         recursive = True
-    else:
-        srcs = argv[1:argv.index("--")]
 
-    ROBORIO_TARGET_DIR = "/home/admin/robot_code"
-    ROBORIO_USER = "admin"
-
-    target_dir = ROBORIO_TARGET_DIR
-    user = ROBORIO_USER
-    destination = args[-1]
+    destination = args.target
 
     result = re.match("(?:([^:@]+)@)?([^:@]+)(?::([^:@]+))?", destination)
     if not result:
-        print(
-            "Not sure how to parse destination \"%s\"!" % destination,
-            file=sys.stderr)
+        print("Not sure how to parse destination \"%s\"!" % destination,
+              file=sys.stderr)
         return 1
+    user = None
     if result.group(1):
         user = result.group(1)
     hostname = result.group(2)
+
     if result.group(3):
         target_dir = result.group(3)
 
+    if user is None:
+        if args.type == "pi":
+          user = "pi"
+        elif args.type == "roborio":
+          user = "admin"
+    target_dir = "/home/" + user + "/robot_code"
+
     ssh_target = "%s@%s" % (user, hostname)
 
     ssh_path = "external/ssh/ssh"
     scp_path = "external/ssh/scp"
 
     rsync_cmd = ([
-        "external/rsync/usr/bin/rsync", "-e", ssh_path, "-c",
-        "-v", "-z", "--copy-links"
+        "external/rsync/usr/bin/rsync", "-e", ssh_path, "-c", "-v", "-z",
+        "--copy-links"
     ] + srcs + ["%s:%s/%s" % (ssh_target, target_dir, relative_dir)])
     try:
         subprocess.check_call(rsync_cmd)
@@ -88,12 +102,11 @@
             raise e
 
     if not recursive:
-        subprocess.check_call(
-            (ssh_path, ssh_target, "&&".join([
-                "chmod u+s %s/starter_exe" % target_dir,
-                "echo \'Done moving new executables into place\'",
-                "bash -c \'sync && sync && sync\'",
-            ])))
+        subprocess.check_call((ssh_path, ssh_target, "&&".join([
+            "chmod u+s %s/starter_exe" % target_dir,
+            "echo \'Done moving new executables into place\'",
+            "bash -c \'sync && sync && sync\'",
+        ])))
 
 
 if __name__ == "__main__":
diff --git a/y2020/BUILD b/y2020/BUILD
index 05b9147..f3ea4d6 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -8,6 +8,7 @@
         ":config.json",
     ],
     start_binaries = [
+        "//aos/events/logging:logger_main",
         ":joystick_reader",
         ":wpilib_interface",
         "//aos/network:message_bridge_client",
@@ -18,6 +19,27 @@
     ],
 )
 
+robot_downloader(
+    name = "pi_download",
+    data = [
+        ":config.json",
+        "//y2020/www:files",
+        "//y2020/www:flatbuffers",
+        "//y2020/www:main_bundle",
+    ],
+    dirs = [
+        "//y2020/www:www_files",
+    ],
+    restricted_to = ["//tools:armhf-debian"],
+    start_binaries = [
+        "//aos/network:message_bridge_client",
+        "//aos/network:message_bridge_server",
+        "//y2020/vision:camera_reader",
+        "//aos/network:web_proxy_main",
+    ],
+    target_type = "pi",
+)
+
 cc_library(
     name = "constants",
     srcs = [
diff --git a/y2020/control_loops/drivetrain/drivetrain_main.cc b/y2020/control_loops/drivetrain/drivetrain_main.cc
index 7159118..7804814 100644
--- a/y2020/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2020/control_loops/drivetrain/drivetrain_main.cc
@@ -6,7 +6,8 @@
 
 using ::frc971::control_loops::drivetrain::DrivetrainLoop;
 
-int main() {
+int main(int argc, char *argv[]) {
+  ::aos::InitGoogle(&argc, &argv);
   ::aos::InitNRT();
 
   aos::FlatbufferDetachedBuffer<aos::Configuration> config =
diff --git a/y2020/control_loops/drivetrain/localizer.cc b/y2020/control_loops/drivetrain/localizer.cc
index 927eec0..4b164b5 100644
--- a/y2020/control_loops/drivetrain/localizer.cc
+++ b/y2020/control_loops/drivetrain/localizer.cc
@@ -115,6 +115,14 @@
   aos::monotonic_clock::time_point capture_time(
       std::chrono::nanoseconds(result.image_monotonic_timestamp_ns()) -
       monotonic_offset);
+  VLOG(1) << "Got monotonic offset of "
+          << aos::time::DurationInSeconds(monotonic_offset)
+          << " when at time of " << event_loop_->monotonic_now()
+          << " and capture time estimate of " << capture_time;
+  if (capture_time > event_loop_->monotonic_now()) {
+    LOG(WARNING) << "Got camera frame from the future.";
+    return;
+  }
   CHECK(result.has_camera_calibration());
   // Per the ImageMatchResult specification, we can actually determine whether
   // the camera is the turret camera just from the presence of the
diff --git a/y2020/vision/rootfs/frc971.service b/y2020/vision/rootfs/frc971.service
new file mode 100644
index 0000000..bf42652
--- /dev/null
+++ b/y2020/vision/rootfs/frc971.service
@@ -0,0 +1,9 @@
+[Unit]
+Description=Start up 971 robot code
+
+[Service]
+Type=oneshot
+ExecStart=su pi -c /home/pi/robot_code/starter.sh
+
+[Install]
+WantedBy=multi-user.target
diff --git a/y2020/vision/rootfs/modify_rootfs.sh b/y2020/vision/rootfs/modify_rootfs.sh
index 239db0a..219590f 100755
--- a/y2020/vision/rootfs/modify_rootfs.sh
+++ b/y2020/vision/rootfs/modify_rootfs.sh
@@ -76,6 +76,7 @@
 sudo cp target_configure.sh "${PARTITION}/tmp/"
 sudo cp dhcpcd.conf "${PARTITION}/tmp/dhcpcd.conf"
 sudo cp change_hostname.sh "${PARTITION}/tmp/change_hostname.sh"
+sudo cp frc971.service "${PARTITION}/etc/systemd/system/frc971.service"
 
 target /bin/mkdir -p /home/pi/.ssh/
 cat ~/.ssh/id_rsa.pub | target tee /home/pi/.ssh/authorized_keys
diff --git a/y2020/vision/rootfs/target_configure.sh b/y2020/vision/rootfs/target_configure.sh
index 38d29b9..032764a 100755
--- a/y2020/vision/rootfs/target_configure.sh
+++ b/y2020/vision/rootfs/target_configure.sh
@@ -68,6 +68,7 @@
 rm -f /etc/profile.d/wifi-check.sh
 
 systemctl enable ssh.service
+systemctl enable frc971.service
 
 # Default us to pi-8971-1
 /root/bin/change_hostname.sh pi-8971-1
diff --git a/y2020/www/BUILD b/y2020/www/BUILD
index c280e91..292618b 100644
--- a/y2020/www/BUILD
+++ b/y2020/www/BUILD
@@ -1,5 +1,6 @@
 load("@build_bazel_rules_typescript//:defs.bzl", "ts_library")
 load("@build_bazel_rules_nodejs//:defs.bzl", "rollup_bundle")
+load("//frc971/downloader:downloader.bzl", "aos_downloader_dir")
 
 ts_library(
     name = "main",
@@ -44,3 +45,14 @@
     cmd = "cp $(location @com_github_google_flatbuffers//:flatjs) $@",
     visibility = ["//y2020:__subpackages__"],
 )
+
+aos_downloader_dir(
+    name = "www_files",
+    srcs = [
+        ":files",
+        ":flatbuffers",
+        ":main_bundle",
+    ],
+    dir = "www",
+    visibility = ["//visibility:public"],
+)
diff --git a/y2020/y2020.json b/y2020/y2020.json
index 95ee582..0e1a6ba 100644
--- a/y2020/y2020.json
+++ b/y2020/y2020.json
@@ -60,7 +60,7 @@
       "name": "/aos/roborio",
       "type": "aos.message_bridge.ClientStatistics",
       "source_node": "roborio",
-      "frequency": 2,
+      "frequency": 10,
       "num_senders": 2
     },
     {
@@ -114,7 +114,7 @@
       "name": "/aos/pi1",
       "type": "aos.message_bridge.ClientStatistics",
       "source_node": "pi1",
-      "frequency": 2,
+      "frequency": 10,
       "num_senders": 2
     },
     {
@@ -158,7 +158,7 @@
       "name": "/aos/pi2",
       "type": "aos.message_bridge.ClientStatistics",
       "source_node": "pi2",
-      "frequency": 2,
+      "frequency": 10,
       "num_senders": 2
     },
     {
@@ -202,7 +202,7 @@
       "name": "/aos/pi3",
       "type": "aos.message_bridge.ClientStatistics",
       "source_node": "pi3",
-      "frequency": 2,
+      "frequency": 10,
       "num_senders": 2
     },
     {