diff --git a/WORKSPACE b/WORKSPACE index 26e4a637..284ab8c2 100644 --- a/WORKSPACE +++ b/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=" +) diff --git a/common/BUILD b/common/BUILD index 9d2bf3da..ed0bddf6 100644 --- a/common/BUILD +++ b/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"], diff --git a/common/matplotlib.cc b/common/matplotlib.cc new file mode 100644 index 00000000..12696dd0 --- /dev/null +++ b/common/matplotlib.cc @@ -0,0 +1,49 @@ + +#include "common/matplotlib.hh" + +#include +#include + +#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::max()) { + return nullptr; + } + wchar_t *out = static_cast(malloc(num_chars * sizeof(wchar_t))); + + std::mbstowcs(out, str, num_chars); + + return out; +} +} // namespace + +void plot(const std::vector &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 diff --git a/common/matplotlib.hh b/common/matplotlib.hh new file mode 100644 index 00000000..b3590388 --- /dev/null +++ b/common/matplotlib.hh @@ -0,0 +1,17 @@ + +#pragma once + +#include +#include + +namespace robot { +struct PlotSignal { + std::vector x; + std::vector y; + std::string label = ""; + std::string marker = "-"; +}; + +void plot(const std::vector &signals, const bool block = true); + +} // namespace robot diff --git a/common/matplotlib_test.cc b/common/matplotlib_test.cc new file mode 100644 index 00000000..02493c49 --- /dev/null +++ b/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 diff --git a/common/openstreetmap/BUILD b/common/openstreetmap/BUILD new file mode 100644 index 00000000..43f068b5 --- /dev/null +++ b/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", + ] +) diff --git a/common/openstreetmap/openstreetmap_test.cc b/common/openstreetmap/openstreetmap_test.cc new file mode 100644 index 00000000..ab4127ca --- /dev/null +++ b/common/openstreetmap/openstreetmap_test.cc @@ -0,0 +1,44 @@ + +#include + +#include +#include + +#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 diff --git a/common/python/embedded_py.bzl b/common/python/embedded_py.bzl new file mode 100644 index 00000000..32f5d0a1 --- /dev/null +++ b/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 + ) diff --git a/experimental/overhead_matching/BUILD b/experimental/overhead_matching/BUILD index ad1b2cd0..a169f78c 100644 --- a/experimental/overhead_matching/BUILD +++ b/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", diff --git a/experimental/overhead_matching/spectacular_log_test.cc b/experimental/overhead_matching/spectacular_log_test.cc index e6ded070..d8b05422 100644 --- a/experimental/overhead_matching/spectacular_log_test.cc +++ b/experimental/overhead_matching/spectacular_log_test.cc @@ -5,6 +5,7 @@ #include #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 ts; + std::vector axs; + std::vector ays; + std::vector azs; + std::vector gxs; + std::vector gys; + std::vector 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(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; diff --git a/third_party/BUILD.libosmium b/third_party/BUILD.libosmium new file mode 100644 index 00000000..fcf30781 --- /dev/null +++ b/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", + ] +) diff --git a/third_party/BUILD.protozero b/third_party/BUILD.protozero new file mode 100644 index 00000000..e2621c9f --- /dev/null +++ b/third_party/BUILD.protozero @@ -0,0 +1,26 @@ + +cc_library( + name = "protozero", + visibility = ["//visibility:public"], + hdrs = [ + "include/protozero/basic_pbf_builder.hpp", + "include/protozero/basic_pbf_writer.hpp", + "include/protozero/buffer_fixed.hpp", + "include/protozero/buffer_string.hpp", + "include/protozero/buffer_tmpl.hpp", + "include/protozero/buffer_vector.hpp", + "include/protozero/byteswap.hpp", + "include/protozero/config.hpp", + "include/protozero/data_view.hpp", + "include/protozero/exception.hpp", + "include/protozero/iterators.hpp", + "include/protozero/pbf_builder.hpp", + "include/protozero/pbf_message.hpp", + "include/protozero/pbf_reader.hpp", + "include/protozero/pbf_writer.hpp", + "include/protozero/types.hpp", + "include/protozero/varint.hpp", + "include/protozero/version.hpp", + ], + strip_include_prefix = "include", +)