Merge branch 'main' into add_kimera_spectacular_data_provider

WORKSPACE

@@ -556,3 +556,26 @@ http_archive(
   build_file = "//third_party:BUILD.zip_file",
   integrity = "sha256-K+KFNzufwccL4vEJLErEtNKNVjnWStClXVF2mKpI6lI="
 )
+
+http_archive(
+  name = "libosmium",
+  urls = ["https://github.com/osmcode/libosmium/archive/refs/tags/v2.20.0.zip"],
+  build_file = "//third_party:BUILD.libosmium",
+  strip_prefix = "libosmium-2.20.0",
+  integrity = "sha256-cS7BoPRNtinhyvatqlM2kV9CPLD2ZJFC14I5i1p1x3k=",
+)
+
+http_archive(
+  name = "protozero",
+  urls = ["https://github.com/mapbox/protozero/archive/refs/tags/v1.7.1.zip"],
+  build_file = "//third_party:BUILD.protozero",
+  strip_prefix = "protozero-1.7.1",
+
+)
+
+http_archive(
+  name = "openstreetmap_snippet",
+  urls = ["https://www.dropbox.com/scl/fi/ku5dwktiul0wzx9ocelwc/us-virgin-islands-latest.osm.zip?rlkey=ggo7dnskexdqxvira6m6y32f3&dl=1"],
+  build_file = "//third_party:BUILD.zip_file",
+  integrity = "sha256-miPvL6co2035EGlkvbcksmTO6HwB/AknVLQK+/YDet0="
+)

common/BUILD

@@ -3,6 +3,8 @@ package(features=["warning_compile_flags"])
 
 load("@pip//:requirements.bzl", "requirement")
 load("@rules_python//python:defs.bzl", "py_library")
+load("@pybind11_bazel//:build_defs.bzl", "pybind_extension")
+load("@//common/python:embedded_py.bzl", "cc_py_library", "cc_py_test")
 
 cc_library(
   name="argument_wrapper",
@@ -63,6 +65,32 @@ cc_test(
   ]
 )
 
+cc_py_library(
+  name = "matplotlib",
+  hdrs = ["matplotlib.hh"],
+  srcs = ["matplotlib.cc"],
+  visibility= ["//visibility:public"],
+  py_deps = [
+    requirement("matplotlib"),
+    requirement("PyGObject"),
+  ],
+  deps = [
+    "@pybind11",
+    "@rules_python//python/cc:current_py_cc_headers",
+    "@rules_python//python/cc:current_py_cc_libs",
+  ],
+)
+
+cc_test(
+  name = "matplotlib_test",
+  srcs = ["matplotlib_test.cc"],
+  tags = ["manual"],
+  deps = [
+    ":matplotlib",
+    "@com_google_googletest//:gtest_main",
+  ],
+)
+
 py_library(
   name = "torch",
   srcs = ["torch.py"],

common/matplotlib.cc

@@ -0,0 +1,49 @@
+
+#include "common/matplotlib.hh"
+
+#include <cstdlib>
+#include <limits>
+
+#include "pybind11/embed.h"
+#include "pybind11/stl.h"
+
+namespace py = pybind11;
+using namespace pybind11::literals;
+
+namespace robot {
+namespace {
+wchar_t *to_wchar(const char *str) {
+    const size_t num_chars = std::mbstowcs(nullptr, str, 0) + 1;
+    if (num_chars == std::numeric_limits<size_t>::max()) {
+        return nullptr;
+    }
+    wchar_t *out = static_cast<wchar_t *>(malloc(num_chars * sizeof(wchar_t)));
+
+    std::mbstowcs(out, str, num_chars);
+
+    return out;
+}
+}  // namespace
+
+void plot(const std::vector<PlotSignal> &signals, const bool block) {
+    PyConfig config;
+    PyConfig_InitPythonConfig(&config);
+    config.home = to_wchar(CPP_PYTHON_HOME);
+    config.pathconfig_warnings = 1;
+    config.program_name = to_wchar(CPP_PYVENV_LAUNCHER);
+    config.pythonpath_env = to_wchar(CPP_PYTHON_PATH);
+    config.user_site_directory = 0;
+    py::scoped_interpreter guard{&config};
+
+    py::module_ mpl = py::module_::import("matplotlib");
+    mpl.attr("use")("GTK3Agg");
+    py::module_ plt = py::module_::import("matplotlib.pyplot");
+
+    plt.attr("figure")();
+    for (const auto &signal : signals) {
+        plt.attr("plot")(signal.x, signal.y, signal.marker, "label"_a = signal.label);
+    }
+    plt.attr("legend")();
+    plt.attr("show")("block"_a = block);
+}
+}  // namespace robot

common/matplotlib.hh

@@ -0,0 +1,17 @@
+
+#pragma once
+
+#include <string>
+#include <vector>
+
+namespace robot {
+struct PlotSignal {
+    std::vector<double> x;
+    std::vector<double> y;
+    std::string label = "";
+    std::string marker = "-";
+};
+
+void plot(const std::vector<PlotSignal> &signals, const bool block = true);
+
+}  // namespace robot

common/matplotlib_test.cc

@@ -0,0 +1,13 @@
+
+#include "common/matplotlib.hh"
+
+#include "gtest/gtest.h"
+
+namespace robot {
+
+TEST(MatplotlibTest, simple_plot) {
+    EXPECT_NO_THROW(plot(
+        {{.x = std::vector{0.0, 1.0, 2.0}, .y = std::vector{10.0, 20.0, 15.0}, .label = "label"}},
+        false));
+}
+}  // namespace robot

common/openstreetmap/BUILD

@@ -0,0 +1,11 @@
+
+cc_test(
+  name = "openstreetmap_test",
+  srcs = ["openstreetmap_test.cc"],
+  data = ["@openstreetmap_snippet//:files"],
+  deps = [
+    "@fmt",
+    "@libosmium",
+    "@com_google_googletest//:gtest_main",
+  ]
+)

common/openstreetmap/openstreetmap_test.cc

@@ -0,0 +1,44 @@
+
+#include <fmt/core.h>
+
+#include <filesystem>
+#include <limits>
+
+#include "fmt/format.h"
+#include "gtest/gtest.h"
+#include "osmium/handler.hpp"
+#include "osmium/handler/dump.hpp"
+#include "osmium/io/pbf_input.hpp"
+#include "osmium/visitor.hpp"
+
+namespace robot::openstreetmap {
+TEST(OpenstreetmapTest, can_open_pbf_file) {
+    // Setup
+    struct TestHandler : public osmium::handler::Handler {
+        int node_counter = 0;
+        int way_counter = 0;
+        int relation_counter = 0;
+
+        void node(const osmium::Node &node) { node_counter++; }
+
+        void way(const osmium::Way &way) { way_counter++; }
+
+        void relation(const osmium::Relation &relation) { relation_counter++; }
+    };
+    const std::filesystem::path osm_pbf_path(
+        "external/openstreetmap_snippet/us-virgin-islands-latest.osm.pbf");
+    osmium::io::Reader reader(osm_pbf_path, osmium::osm_entity_bits::node |
+                                                osmium::osm_entity_bits::way |
+                                                osmium::osm_entity_bits::relation);
+
+    // Action
+    auto handler = TestHandler();
+    osmium::apply(reader, handler);
+
+    // Verification
+    EXPECT_GT(handler.node_counter, 0);
+    EXPECT_GT(handler.way_counter, 0);
+    EXPECT_GT(handler.relation_counter, 0);
+}
+
+}  // namespace robot::openstreetmap

common/python/embedded_py.bzl

@@ -0,0 +1,100 @@
+def _cc_py_runtime_impl(ctx):
+    toolchain = ctx.toolchains["@bazel_tools//tools/python:toolchain_type"]
+    py3_runtime = toolchain.py3_runtime
+    imports = []
+    for dep in ctx.attr.deps:
+        imports.append(dep[PyInfo].imports)
+    python_path = ""
+    for path in depset(transitive = imports).to_list():
+        # print("Printing python path: " + str(path))
+        python_path += "external/" + path + ":"
+
+    py3_runfiles = ctx.runfiles(files = py3_runtime.files.to_list())
+    runfiles = [py3_runfiles]
+    for dep in ctx.attr.deps:
+        dep_runfiles = ctx.runfiles(files = dep[PyInfo].transitive_sources.to_list())
+        runfiles.append(dep_runfiles)
+        runfiles.append(dep[DefaultInfo].default_runfiles)
+
+    runfiles = ctx.runfiles().merge_all(runfiles)
+
+    # print("Printing interpreter path: " + str(py3_runtime.interpreter.path))
+    # print("Printing interpreter home: " + str(py3_runtime.interpreter.dirname.rstrip("bin")))
+
+    return [
+        DefaultInfo(runfiles = runfiles),
+        platform_common.TemplateVariableInfo({
+            "PYTHON3": str(py3_runtime.interpreter.path),
+            "PYTHONPATH": python_path,
+            "PYTHONHOME": str(py3_runtime.interpreter.dirname.rstrip("bin")),
+        }),
+    ]
+
+_cc_py_runtime = rule(
+    implementation = _cc_py_runtime_impl,
+    attrs = {
+        "deps": attr.label_list(providers = [PyInfo]),
+    },
+    toolchains = [
+        str(Label("@bazel_tools//tools/python:toolchain_type")),
+    ],
+)
+
+def cc_py_test(name, py_deps = [], **kwargs):
+    py_runtime_target = name + "_py_runtime"
+    _cc_py_runtime(
+        name = py_runtime_target,
+        deps = py_deps,
+    )
+
+    kwargs.update({
+        "data": kwargs.get("data", []) + [":" + py_runtime_target],
+        "env": {"__PYVENV_LAUNCHER__": "$(PYTHON3)", "PYTHONPATH": "$(PYTHONPATH)", "PYTHONHOME": "$(PYTHONHOME)", "PYTHONNOUSERSITE": "1"},
+        "toolchains": kwargs.get("toolchains", []) + [":" + py_runtime_target],
+    })
+
+    native.cc_test(
+        name = name,
+        **kwargs
+    )
+
+def cc_py_binary(name, py_deps = [], **kwargs):
+    py_runtime_target = name + "_py_runtime"
+    _cc_py_runtime(
+        name = py_runtime_target,
+        deps = py_deps,
+    )
+
+    kwargs.update({
+        "data": kwargs.get("data", []) + [":" + py_runtime_target],
+        "env": {"__PYVENV_LAUNCHER__": "$(PYTHON3)", "PYTHONPATH": "$(PYTHONPATH)", "PYTHONHOME": "$(PYTHONHOME)", "PYTHONNOUSERSITE": "1"},
+        "toolchains": kwargs.get("toolchains", []) + [":" + py_runtime_target],
+    })
+
+    native.cc_binary(
+        name = name,
+        **kwargs
+    )
+
+def cc_py_library(name, py_deps = [], **kwargs):
+    py_runtime_target = name + "_py_runtime"
+    _cc_py_runtime(
+        name = py_runtime_target,
+        deps = py_deps,
+    )
+
+    kwargs.update({
+        "data": kwargs.get("data", []) + [":" + py_runtime_target],
+        "defines": [
+            "CPP_PYVENV_LAUNCHER=\\\"$(PYTHON3)\\\"",
+            "CPP_PYTHON_PATH=\\\"$(PYTHONPATH)\\\"",
+            "CPP_PYTHON_HOME=\\\"$(PYTHONHOME)\\\"",
+            "PYTHONNOUSERSITE=\\\"1\\\"",
+        ],
+        "toolchains": kwargs.get("toolchains", []) + [":" + py_runtime_target],
+    })
+
+    native.cc_library(
+        name = name,
+        **kwargs
+    )

experimental/overhead_matching/BUILD

@@ -25,6 +25,7 @@ cc_test(
     srcs = ["spectacular_log_test.cc"],
     data = ["@spectacular_log_snippet//:files"],
     deps = [
+	"//common:matplotlib",
 	":spectacular_log",
 	"@com_google_googletest//:gtest_main",
 	"@fmt",

experimental/overhead_matching/spectacular_log_test.cc

@@ -5,6 +5,7 @@
 #include <sstream>
 
 #include "common/video.hh"
+#include "common/matplotlib.hh"
 #include "fmt/format.h"
 #include "gtest/gtest.h"
 #include "opencv2/opencv.hpp"
@@ -30,18 +31,46 @@ TEST(SpectacularLogTest, happy_case) {
               << " frame time: (" << log.min_frame_time() << ", " << log.max_frame_time() << ")"
               << std::endl;
 
+    std::vector<double> ts;
+    std::vector<double> axs;
+    std::vector<double> ays;
+    std::vector<double> azs;
+    std::vector<double> gxs;
+    std::vector<double> gys;
+    std::vector<double> gzs;
     std::cout << "IMU Samples" << std::endl;
     for (time::RobotTimestamp t = log.min_imu_time();
-         t < std::min(log.min_imu_time() + time::as_duration(5.0), log.max_imu_time());
+         t < std::min(log.min_imu_time() + time::as_duration(20.0), log.max_imu_time());
          t += time::as_duration(0.1)) {
         const auto sample = log.get_imu_sample(t);
         if (sample.has_value()) {
+            ts.push_back(std::chrono::duration<double>(t.time_since_epoch()).count());
+            axs.push_back(sample->accel_mpss.x());
+            ays.push_back(sample->accel_mpss.y());
+            azs.push_back(sample->accel_mpss.z());
+            gxs.push_back(sample->gyro_radps.x());
+            gys.push_back(sample->gyro_radps.y());
+            gzs.push_back(sample->gyro_radps.z());
             std::cout << "t: " << sample->time_of_validity
                       << " accel: " << sample->accel_mpss.transpose()
                       << " gyro: " << sample->gyro_radps.transpose() << std::endl;
         }
     }
 
+    if (false) {
+        const bool should_block = false;
+        plot(
+            {
+                {ts, axs, "ax"},
+                {ts, ays, "ay"},
+                {ts, azs, "az"},
+                {ts, gxs, "gx"},
+                {ts, gys, "gy"},
+                {ts, gzs, "gz"},
+            },
+            should_block);
+    }
+
     cv::VideoCapture video(log_path / "data.mov", cv::CAP_FFMPEG);
     constexpr int FRAME_SKIP = 50;
     cv::Mat expected_frame;

third_party/BUILD.libosmium

@@ -0,0 +1,19 @@
+
+cc_library(
+  name = "libosmium",
+  visibility = ["//visibility:public"],
+  hdrs = glob(["include/**/*.hpp"],
+    exclude = [
+      "include/io/any_input.hpp",
+      "include/io/any_output.hpp",
+      "include/io/xml_input.hpp",
+      "include/io/xml_output.hpp"
+    ]
+  ),
+  strip_include_prefix="include",
+  deps = [
+    "@protozero",
+    "@org_bzip_bzip2//:bz2lib",
+    "@zlib",
+  ]
+)