Merge changes I3c668776,I131be7da
* changes:
Add note about changes for the AOS foxglove extension
Add basic foxglove extension for analysis
diff --git a/aos/actions/action_test.cc b/aos/actions/action_test.cc
index c7c3c48..fca60a3 100644
--- a/aos/actions/action_test.cc
+++ b/aos/actions/action_test.cc
@@ -1,17 +1,27 @@
-#include <unistd.h>
+#include <stdint.h>
#include <chrono>
+#include <functional>
#include <memory>
-#include <thread>
+#include <ostream>
+#include <string>
+#include "flatbuffers/buffer.h"
+#include "glog/logging.h"
#include "gtest/gtest.h"
#include "aos/actions/actions.h"
#include "aos/actions/actions_generated.h"
#include "aos/actions/actor.h"
#include "aos/actions/test_action_generated.h"
+#include "aos/configuration.h"
+#include "aos/events/event_loop.h"
#include "aos/events/simulated_event_loop.h"
+#include "aos/flatbuffers.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/logging/implementations.h"
#include "aos/testing/path.h"
+#include "aos/time/time.h"
namespace aos::common::actions::testing {
diff --git a/aos/actions/actions.cc b/aos/actions/actions.cc
index 133101b..0e0e6fd 100644
--- a/aos/actions/actions.cc
+++ b/aos/actions/actions.cc
@@ -1,5 +1,7 @@
#include "aos/actions/actions.h"
+#include <utility>
+
namespace aos::common::actions {
void ActionQueue::EnqueueAction(::std::unique_ptr<Action> action) {
diff --git a/aos/actions/actions.h b/aos/actions/actions.h
index f60f20b..1aae62f 100644
--- a/aos/actions/actions.h
+++ b/aos/actions/actions.h
@@ -1,14 +1,17 @@
#ifndef AOS_ACTIONS_ACTIONS_H_
#define AOS_ACTIONS_ACTIONS_H_
-#include <sys/types.h>
#include <unistd.h>
#include <atomic>
#include <cinttypes>
#include <memory>
+#include <ostream>
+#include <string>
#include <type_traits>
+#include "glog/logging.h"
+
#include "aos/actions/actions_generated.h"
#include "aos/events/event_loop.h"
#include "aos/json_to_flatbuffer.h"
diff --git a/aos/actions/actor.h b/aos/actions/actor.h
index f7066fc..b0b6bd4 100644
--- a/aos/actions/actor.h
+++ b/aos/actions/actor.h
@@ -3,11 +3,19 @@
#include <chrono>
#include <cinttypes>
-#include <cstdio>
+#include <compare>
#include <functional>
+#include <ostream>
+#include <string>
+#include <type_traits>
+
+#include "flatbuffers/string.h"
+#include "glog/logging.h"
#include "aos/actions/actions_generated.h"
+#include "aos/configuration_generated.h"
#include "aos/events/event_loop.h"
+#include "aos/json_to_flatbuffer.h"
#include "aos/logging/logging.h"
#include "aos/time/time.h"
#include "aos/util/phased_loop.h"
diff --git a/aos/analysis/in_process_plotter.cc b/aos/analysis/in_process_plotter.cc
index 88591b3..dcb60b7 100644
--- a/aos/analysis/in_process_plotter.cc
+++ b/aos/analysis/in_process_plotter.cc
@@ -1,5 +1,12 @@
#include "aos/analysis/in_process_plotter.h"
+#include <algorithm>
+#include <ostream>
+
+#include "flatbuffers/flatbuffer_builder.h"
+#include "flatbuffers/vector.h"
+#include "glog/logging.h"
+
#include "aos/configuration.h"
namespace aos::analysis {
diff --git a/aos/analysis/in_process_plotter.h b/aos/analysis/in_process_plotter.h
index 0d00deb..8c00096 100644
--- a/aos/analysis/in_process_plotter.h
+++ b/aos/analysis/in_process_plotter.h
@@ -1,10 +1,21 @@
#ifndef AOS_ANALYSIS_IN_PROCESS_PLOTTER_H_
#define AOS_ANALYSIS_IN_PROCESS_PLOTTER_H_
+#include <stddef.h>
+
+#include <memory>
+#include <string>
+#include <string_view>
#include <vector>
+#include "flatbuffers/buffer.h"
+#include "flatbuffers/string.h"
+
#include "aos/analysis/plot_data_generated.h"
+#include "aos/configuration_generated.h"
+#include "aos/events/event_loop.h"
#include "aos/events/simulated_event_loop.h"
+#include "aos/flatbuffers.h"
#include "aos/network/web_proxy.h"
namespace aos::analysis {
diff --git a/aos/analysis/in_process_plotter_demo.cc b/aos/analysis/in_process_plotter_demo.cc
index 6141d6a..5b92ab7 100644
--- a/aos/analysis/in_process_plotter_demo.cc
+++ b/aos/analysis/in_process_plotter_demo.cc
@@ -1,3 +1,7 @@
+#include <algorithm>
+#include <cmath>
+#include <vector>
+
#include "aos/analysis/in_process_plotter.h"
#include "aos/init.h"
diff --git a/aos/analysis/local_foxglove.cc b/aos/analysis/local_foxglove.cc
index ade56a5..b07a628 100644
--- a/aos/analysis/local_foxglove.cc
+++ b/aos/analysis/local_foxglove.cc
@@ -1,8 +1,11 @@
-#include "glog/logging.h"
+#include <memory>
+
+#include "gflags/gflags.h"
#include "aos/init.h"
#include "aos/seasocks/seasocks_logger.h"
#include "internal/Embedded.h"
+#include "seasocks/Logger.h"
#include "seasocks/Server.h"
DEFINE_string(data_path, "external/foxglove_studio",
diff --git a/aos/analysis/py_log_reader.cc b/aos/analysis/py_log_reader.cc
index 4a97a0f..340140c 100644
--- a/aos/analysis/py_log_reader.cc
+++ b/aos/analysis/py_log_reader.cc
@@ -16,16 +16,29 @@
#define PY_SSIZE_T_CLEAN
// Note that Python.h needs to be included before anything else.
#include <Python.h>
+#include <stddef.h>
+#include <stdint.h>
+#include <algorithm>
#include <cerrno>
+#include <chrono>
#include <memory>
+#include <string>
+#include <vector>
+
+#include "absl/types/span.h"
+#include "glog/logging.h"
#include "aos/configuration.h"
+#include "aos/events/context.h"
+#include "aos/events/event_loop.h"
#include "aos/events/logging/log_reader.h"
#include "aos/events/simulated_event_loop.h"
#include "aos/flatbuffer_merge.h"
+#include "aos/flatbuffers.h"
#include "aos/init.h"
#include "aos/json_to_flatbuffer.h"
+#include "aos/time/time.h"
namespace aos::analysis {
namespace {
diff --git a/aos/containers/error_list.h b/aos/containers/error_list.h
index 42400fb..7ceb1fe 100644
--- a/aos/containers/error_list.h
+++ b/aos/containers/error_list.h
@@ -1,9 +1,13 @@
#ifndef AOS_CONTAINERS_ERROR_LIST_H_
#define AOS_CONTAINERS_ERROR_LIST_H_
-#include <iostream>
+#include <stddef.h>
-#include "flatbuffers/flatbuffers.h"
+#include <algorithm>
+
+#include "flatbuffers/buffer.h"
+#include "flatbuffers/flatbuffer_builder.h"
+#include "flatbuffers/vector.h"
#include "aos/containers/sized_array.h"
diff --git a/aos/containers/error_list_test.cc b/aos/containers/error_list_test.cc
index 94eabf9..98d7257 100644
--- a/aos/containers/error_list_test.cc
+++ b/aos/containers/error_list_test.cc
@@ -1,5 +1,9 @@
#include "aos/containers/error_list.h"
+#include <stdint.h>
+
+#include <memory>
+
#include "gtest/gtest.h"
#include "aos/json_to_flatbuffer_generated.h"
diff --git a/aos/containers/inlined_vector.h b/aos/containers/inlined_vector.h
index abd375d..5cf33a4 100644
--- a/aos/containers/inlined_vector.h
+++ b/aos/containers/inlined_vector.h
@@ -1,6 +1,8 @@
#ifndef AOS_CONTAINERS_INLINED_VECTOR_H_
#define AOS_CONTAINERS_INLINED_VECTOR_H_
+#include <stddef.h>
+
#include <vector>
#include "absl/container/inlined_vector.h"
diff --git a/aos/containers/inlined_vector_test.cc b/aos/containers/inlined_vector_test.cc
index c9f2dd2..b8aab6b 100644
--- a/aos/containers/inlined_vector_test.cc
+++ b/aos/containers/inlined_vector_test.cc
@@ -1,5 +1,6 @@
#include "aos/containers/inlined_vector.h"
+#include "gflags/gflags.h"
#include "gtest/gtest.h"
#include "aos/realtime.h"
diff --git a/aos/containers/priority_queue.h b/aos/containers/priority_queue.h
index d4d0196..90861dc 100644
--- a/aos/containers/priority_queue.h
+++ b/aos/containers/priority_queue.h
@@ -1,8 +1,9 @@
#ifndef AOS_CONTAINERS_PRIORITY_QUEUE_H_
#define AOS_CONTAINERS_PRIORITY_QUEUE_H_
+#include <stddef.h>
+
#include <array>
-#include <iterator>
#include <optional>
namespace aos {
diff --git a/aos/containers/priority_queue_test.cc b/aos/containers/priority_queue_test.cc
index f27a206..5e75d1e 100644
--- a/aos/containers/priority_queue_test.cc
+++ b/aos/containers/priority_queue_test.cc
@@ -1,5 +1,8 @@
#include "aos/containers/priority_queue.h"
+#include <functional>
+#include <vector>
+
#include "gtest/gtest.h"
namespace aos::testing {
diff --git a/aos/containers/resizeable_buffer.h b/aos/containers/resizeable_buffer.h
index 9f9967f..484d93e 100644
--- a/aos/containers/resizeable_buffer.h
+++ b/aos/containers/resizeable_buffer.h
@@ -1,6 +1,10 @@
#ifndef AOS_CONTAINERS_RESIZEABLE_BUFFER_H_
#define AOS_CONTAINERS_RESIZEABLE_BUFFER_H_
+#include <stdint.h>
+#include <string.h>
+
+#include <algorithm>
#include <cstdlib>
#include <memory>
diff --git a/aos/containers/resizeable_buffer_test.cc b/aos/containers/resizeable_buffer_test.cc
index 2665ba8..6e3beb8 100644
--- a/aos/containers/resizeable_buffer_test.cc
+++ b/aos/containers/resizeable_buffer_test.cc
@@ -1,6 +1,7 @@
#include "aos/containers/resizeable_buffer.h"
#include <numeric>
+#include <vector>
#include "absl/types/span.h"
#include "gmock/gmock.h"
diff --git a/aos/containers/ring_buffer.h b/aos/containers/ring_buffer.h
index 7d0bb7e..083d7cf 100644
--- a/aos/containers/ring_buffer.h
+++ b/aos/containers/ring_buffer.h
@@ -3,6 +3,9 @@
#include <array>
#include <cstddef>
+#include <iterator>
+#include <new>
+#include <type_traits>
namespace aos {
diff --git a/aos/containers/ring_buffer_test.cc b/aos/containers/ring_buffer_test.cc
index c06ef0e..b7b0f17 100644
--- a/aos/containers/ring_buffer_test.cc
+++ b/aos/containers/ring_buffer_test.cc
@@ -1,5 +1,8 @@
#include "aos/containers/ring_buffer.h"
+#include <memory>
+#include <ostream>
+
#include "glog/logging.h"
#include "gtest/gtest.h"
diff --git a/aos/containers/sized_array.h b/aos/containers/sized_array.h
index 8a8d2de..4febf55 100644
--- a/aos/containers/sized_array.h
+++ b/aos/containers/sized_array.h
@@ -1,6 +1,8 @@
#ifndef AOS_CONTAINERS_SIZED_ARRAY_H_
#define AOS_CONTAINERS_SIZED_ARRAY_H_
+#include <cstddef>
+
#include "absl/container/inlined_vector.h"
namespace aos {
diff --git a/aos/containers/sized_array_test.cc b/aos/containers/sized_array_test.cc
index 645d669..5fc4b38 100644
--- a/aos/containers/sized_array_test.cc
+++ b/aos/containers/sized_array_test.cc
@@ -1,5 +1,8 @@
#include "aos/containers/sized_array.h"
+#include <iterator>
+#include <memory>
+
#include "gtest/gtest.h"
namespace aos::testing {
diff --git a/frc971/control_loops/drivetrain/swerve/BUILD b/frc971/control_loops/drivetrain/swerve/BUILD
deleted file mode 100644
index 5b94fa8..0000000
--- a/frc971/control_loops/drivetrain/swerve/BUILD
+++ /dev/null
@@ -1,13 +0,0 @@
-load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
-
-package(default_visibility = ["//visibility:public"])
-
-static_flatbuffer(
- name = "swerve_drivetrain_output_fbs",
- srcs = ["swerve_drivetrain_output.fbs"],
-)
-
-static_flatbuffer(
- name = "swerve_drivetrain_position_fbs",
- srcs = ["swerve_drivetrain_position.fbs"],
-)
diff --git a/frc971/control_loops/swerve/BUILD b/frc971/control_loops/swerve/BUILD
index e411fc9..4e06932 100644
--- a/frc971/control_loops/swerve/BUILD
+++ b/frc971/control_loops/swerve/BUILD
@@ -1,3 +1,19 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
+
+static_flatbuffer(
+ name = "swerve_drivetrain_output_fbs",
+ srcs = ["swerve_drivetrain_output.fbs"],
+ visibility = ["//visibility:public"],
+ deps = ["//frc971/control_loops:can_talonfx_fbs"],
+)
+
+static_flatbuffer(
+ name = "swerve_drivetrain_position_fbs",
+ srcs = ["swerve_drivetrain_position.fbs"],
+ visibility = ["//visibility:public"],
+ deps = ["//frc971/control_loops:control_loops_fbs"],
+)
+
py_binary(
name = "simulation",
srcs = [
diff --git a/frc971/control_loops/drivetrain/swerve/swerve_drivetrain_output.fbs b/frc971/control_loops/swerve/swerve_drivetrain_output.fbs
similarity index 100%
rename from frc971/control_loops/drivetrain/swerve/swerve_drivetrain_output.fbs
rename to frc971/control_loops/swerve/swerve_drivetrain_output.fbs
diff --git a/frc971/control_loops/drivetrain/swerve/swerve_drivetrain_position.fbs b/frc971/control_loops/swerve/swerve_drivetrain_position.fbs
similarity index 80%
rename from frc971/control_loops/drivetrain/swerve/swerve_drivetrain_position.fbs
rename to frc971/control_loops/swerve/swerve_drivetrain_position.fbs
index e0f5571..bad8af1 100644
--- a/frc971/control_loops/drivetrain/swerve/swerve_drivetrain_position.fbs
+++ b/frc971/control_loops/swerve/swerve_drivetrain_position.fbs
@@ -5,11 +5,6 @@
rotation_encoder:double (id: 0);
// Translation in meters
translation_encoder:double (id: 1);
-
- // Speed in radians/s
- rotation_speed:double (id: 2);
- // Speed in m/s
- translation_speed:double (id: 3);
}
table Position {
diff --git a/frc971/vision/BUILD b/frc971/vision/BUILD
index 01a6d1a..14e3d41 100644
--- a/frc971/vision/BUILD
+++ b/frc971/vision/BUILD
@@ -3,6 +3,72 @@
load("//aos:config.bzl", "aos_config")
load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
+cc_binary(
+ name = "modify_extrinsics",
+ srcs = [
+ "modify_extrinsics.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos:configuration",
+ "//aos:init",
+ "//aos/events:event_loop",
+ "//frc971/vision:calibration_fbs",
+ "//frc971/vision:vision_util_lib",
+ "@com_google_absl//absl/strings:str_format",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_binary(
+ name = "image_replay",
+ srcs = [
+ "image_replay.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos:configuration",
+ "//aos:init",
+ "//aos/events:simulated_event_loop",
+ "//aos/events/logging:log_reader",
+ "//frc971/vision:vision_fbs",
+ "//third_party:opencv",
+ ],
+)
+
+cc_binary(
+ name = "image_logger",
+ srcs = [
+ "image_logger.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos:configuration",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//aos/events/logging:log_writer",
+ "//aos/logging:log_namer",
+ "//aos/util:filesystem_fbs",
+ "//frc971/input:joystick_state_fbs",
+ "@com_github_gflags_gflags//:gflags",
+ "@com_github_google_glog//:glog",
+ ],
+)
+
+cc_binary(
+ name = "foxglove_image_converter",
+ srcs = ["foxglove_image_converter.cc"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//frc971/vision:foxglove_image_converter_lib",
+ ],
+)
+
static_flatbuffer(
name = "vision_fbs",
srcs = ["vision.fbs"],
@@ -343,12 +409,7 @@
"intrinsics_calibration.cc",
],
target_compatible_with = ["@platforms//os:linux"],
- visibility = [
- "//y2020:__subpackages__",
- "//y2022:__subpackages__",
- "//y2023:__subpackages__",
- "//y2024:__subpackages__",
- ],
+ visibility = ["//visibility:public"],
deps = [
":intrinsics_calibration_lib",
"//aos:init",
diff --git a/y2024/vision/foxglove_image_converter.cc b/frc971/vision/foxglove_image_converter.cc
similarity index 100%
rename from y2024/vision/foxglove_image_converter.cc
rename to frc971/vision/foxglove_image_converter.cc
diff --git a/y2024/vision/image_logger.cc b/frc971/vision/image_logger.cc
similarity index 100%
rename from y2024/vision/image_logger.cc
rename to frc971/vision/image_logger.cc
diff --git a/y2024/vision/image_replay.cc b/frc971/vision/image_replay.cc
similarity index 97%
rename from y2024/vision/image_replay.cc
rename to frc971/vision/image_replay.cc
index f03bcf1..3b75c92 100644
--- a/y2024/vision/image_replay.cc
+++ b/frc971/vision/image_replay.cc
@@ -1,6 +1,6 @@
#include "gflags/gflags.h"
+#include "opencv2/highgui.hpp"
#include "opencv2/imgproc.hpp"
-#include <opencv2/highgui.hpp>
#include "aos/events/logging/log_reader.h"
#include "aos/events/logging/log_writer.h"
diff --git a/y2024/vision/modify_extrinsics.cc b/frc971/vision/modify_extrinsics.cc
similarity index 100%
rename from y2024/vision/modify_extrinsics.cc
rename to frc971/vision/modify_extrinsics.cc
diff --git a/y2024/vision/rename_calibration_file.sh b/frc971/vision/rename_calibration_file.sh
similarity index 100%
copy from y2024/vision/rename_calibration_file.sh
copy to frc971/vision/rename_calibration_file.sh
diff --git a/frc971/wpilib/swerve/BUILD b/frc971/wpilib/swerve/BUILD
index 4493781..8eb5651 100644
--- a/frc971/wpilib/swerve/BUILD
+++ b/frc971/wpilib/swerve/BUILD
@@ -12,7 +12,7 @@
":swerve_module",
"//aos/logging",
"//frc971:can_configuration_fbs",
- "//frc971/control_loops/drivetrain/swerve:swerve_drivetrain_output_fbs",
+ "//frc971/control_loops/swerve:swerve_drivetrain_output_fbs",
"//frc971/wpilib:loop_output_handler",
"//frc971/wpilib:talonfx",
"//third_party:phoenix6",
diff --git a/frc971/wpilib/swerve/swerve_drivetrain_writer.h b/frc971/wpilib/swerve/swerve_drivetrain_writer.h
index 8e9e616..16db158 100644
--- a/frc971/wpilib/swerve/swerve_drivetrain_writer.h
+++ b/frc971/wpilib/swerve/swerve_drivetrain_writer.h
@@ -4,7 +4,7 @@
#include "ctre/phoenix6/TalonFX.hpp"
#include "frc971/can_configuration_generated.h"
-#include "frc971/control_loops/drivetrain/swerve/swerve_drivetrain_output_generated.h"
+#include "frc971/control_loops/swerve/swerve_drivetrain_output_generated.h"
#include "frc971/wpilib/loop_output_handler.h"
#include "frc971/wpilib/swerve/swerve_module.h"
#include "frc971/wpilib/talonfx.h"
diff --git a/frc971/wpilib/swerve/swerve_module.h b/frc971/wpilib/swerve/swerve_module.h
index c373a31..729ebab 100644
--- a/frc971/wpilib/swerve/swerve_module.h
+++ b/frc971/wpilib/swerve/swerve_module.h
@@ -1,6 +1,7 @@
#ifndef FRC971_WPILIB_SWERVE_SWERVE_MODULE_H_
#define FRC971_WPILIB_SWERVE_SWERVE_MODULE_H_
+#include "frc971/control_loops/swerve/swerve_drivetrain_output_generated.h"
#include "frc971/wpilib/talonfx.h"
namespace frc971::wpilib::swerve {
diff --git a/frc971/www/BUILD b/frc971/www/BUILD
new file mode 100644
index 0000000..060912b
--- /dev/null
+++ b/frc971/www/BUILD
@@ -0,0 +1,37 @@
+load("//tools/build_rules:js.bzl", "rollup_bundle", "ts_project")
+
+package(default_visibility = ["//visibility:public"])
+
+filegroup(
+ name = "starter_files",
+ srcs = [
+ "starter.html",
+ "starter_main_bundle.min.js",
+ "styles.css",
+ ],
+)
+
+ts_project(
+ name = "starter_main",
+ srcs = [
+ "starter_handler.ts",
+ "starter_main.ts",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos/network:connect_ts_fbs",
+ "//aos/network:message_bridge_client_ts_fbs",
+ "//aos/network/www:proxy",
+ "//aos/starter:starter_ts_fbs",
+ "@com_github_google_flatbuffers//ts:flatbuffers_ts",
+ ],
+)
+
+rollup_bundle(
+ name = "starter_main_bundle",
+ entry_point = "starter_main.ts",
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":starter_main",
+ ],
+)
diff --git a/y2024/www/starter.html b/frc971/www/starter.html
similarity index 100%
rename from y2024/www/starter.html
rename to frc971/www/starter.html
diff --git a/y2024/www/starter_handler.ts b/frc971/www/starter_handler.ts
similarity index 100%
rename from y2024/www/starter_handler.ts
rename to frc971/www/starter_handler.ts
diff --git a/y2024/www/starter_main.ts b/frc971/www/starter_main.ts
similarity index 100%
rename from y2024/www/starter_main.ts
rename to frc971/www/starter_main.ts
diff --git a/y2024/www/styles.css b/frc971/www/styles.css
similarity index 100%
rename from y2024/www/styles.css
rename to frc971/www/styles.css
diff --git a/third_party/flatbuffers/tests/ts/test_dir/BUILD.bazel b/third_party/flatbuffers/tests/ts/test_dir/BUILD.bazel
index 768fe83..8b0acca 100644
--- a/third_party/flatbuffers/tests/ts/test_dir/BUILD.bazel
+++ b/third_party/flatbuffers/tests/ts/test_dir/BUILD.bazel
@@ -10,5 +10,5 @@
name = "include_ts_fbs",
srcs = ["typescript_include.fbs"],
visibility = ["//visibility:public"],
- deps = [":typescript_transitive_ts_ts_fbs"],
+ deps = [":typescript_transitive_ts_fbs"],
)
diff --git a/third_party/julia/julia.BUILD b/third_party/julia/julia.BUILD
index 94988ca..7f25671 100644
--- a/third_party/julia/julia.BUILD
+++ b/third_party/julia/julia.BUILD
@@ -1,18 +1,21 @@
load("@rules_pkg//:pkg.bzl", "pkg_tar")
-load(":files.bzl", "LIB_SYMLINKS", "LIBS")
+load(":files.bzl", "LIBS", "LIB_SYMLINKS")
pkg_tar(
name = "runtime",
srcs = LIBS + [
"bin/julia",
- ] + glob([
- "share/julia/**/*.jl",
- "share/julia/**/*.toml",
- "include/julia/**/*",
- ], exclude = [
- "**/test/**",
- ]),
- symlinks = LIB_SYMLINKS,
+ ] + glob(
+ [
+ "share/julia/**/*.jl",
+ "share/julia/**/*.toml",
+ "include/julia/**/*",
+ ],
+ exclude = [
+ "**/test/**",
+ ],
+ ),
strip_prefix = "external/julia",
+ symlinks = LIB_SYMLINKS,
visibility = ["//visibility:public"],
)
diff --git a/third_party/python/BUILD b/third_party/python/BUILD
index 46c464c..5e3278a 100644
--- a/third_party/python/BUILD
+++ b/third_party/python/BUILD
@@ -2,10 +2,6 @@
cc_library(
name = "python",
- deps = [
- "@python3_9_x86_64-unknown-linux-gnu//:python_headers",
- "@python3_9_x86_64-unknown-linux-gnu//:libpython",
- ],
defines = [
"FRC971_UPSTREAM_BUNDLED_PYTHON",
"FRC971_PYTHON_HOME=../python3_9_x86_64-unknown-linux-gnu/",
@@ -15,6 +11,10 @@
"@platforms//os:linux",
],
visibility = ["//visibility:public"],
+ deps = [
+ "@python3_9_x86_64-unknown-linux-gnu//:libpython",
+ "@python3_9_x86_64-unknown-linux-gnu//:python_headers",
+ ],
)
filegroup(
@@ -31,8 +31,8 @@
extract_numpy_headers(
name = "numpy_headers",
- numpy = "@pip//numpy",
header_prefix = "numpy_headers",
+ numpy = "@pip//numpy",
visibility = ["//visibility:private"],
)
@@ -44,8 +44,8 @@
includes = [
"numpy_headers",
],
+ visibility = ["//visibility:public"],
deps = [
":python",
],
- visibility = ["//visibility:public"],
)
diff --git a/third_party/vl53l1x/vl53l1x.BUILD b/third_party/vl53l1x/vl53l1x.BUILD
index 13ea7fd..9d7e776 100644
--- a/third_party/vl53l1x/vl53l1x.BUILD
+++ b/third_party/vl53l1x/vl53l1x.BUILD
@@ -1,26 +1,25 @@
-
cc_library(
- name = "VL53L1X",
- srcs = [
- "core/VL53L1X_api.c",
- "core/VL53L1X_calibration.c",
- "platform/vl53l1_platform.c",
- ],
- hdrs = [
- "core/VL53L1X_api.h",
- "core/VL53L1X_calibration.h",
- "platform/vl53l1_platform.h",
- "platform/vl53l1_types.h",
- ],
- target_compatible_with = [
- "@platforms//os:none",
- "@//tools/platforms/hardware:cortex_m0plus",
- ],
- copts = [
- "-Wno-unused-parameter",
- ],
- visibility = ["//visibility:public"],
- deps = [
- "@//third_party/pico-sdk",
- ],
+ name = "VL53L1X",
+ srcs = [
+ "core/VL53L1X_api.c",
+ "core/VL53L1X_calibration.c",
+ "platform/vl53l1_platform.c",
+ ],
+ hdrs = [
+ "core/VL53L1X_api.h",
+ "core/VL53L1X_calibration.h",
+ "platform/vl53l1_platform.h",
+ "platform/vl53l1_types.h",
+ ],
+ copts = [
+ "-Wno-unused-parameter",
+ ],
+ target_compatible_with = [
+ "@platforms//os:none",
+ "@//tools/platforms/hardware:cortex_m0plus",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "@//third_party/pico-sdk",
+ ],
)
diff --git a/third_party/websocketpp/websocketpp.BUILD b/third_party/websocketpp/websocketpp.BUILD
index ecf12cd..436e47f 100644
--- a/third_party/websocketpp/websocketpp.BUILD
+++ b/third_party/websocketpp/websocketpp.BUILD
@@ -4,7 +4,10 @@
cc_library(
name = "com_github_zaphoyd_websocketpp",
hdrs = glob(["websocketpp/**/*.hpp"]),
- defines = ["_WEBSOCKETPP_CPP11_STL_", "ASIO_STANDALONE"],
+ defines = [
+ "_WEBSOCKETPP_CPP11_STL_",
+ "ASIO_STANDALONE",
+ ],
includes = ["."],
visibility = ["//visibility:public"],
deps = ["@asio"],
diff --git a/third_party/xvfb/xvfb.BUILD b/third_party/xvfb/xvfb.BUILD
index 91332ff..f03e322 100644
--- a/third_party/xvfb/xvfb.BUILD
+++ b/third_party/xvfb/xvfb.BUILD
@@ -31,13 +31,13 @@
sh_binary(
name = "wrapped_bin/Xvfb",
srcs = ["wrapped_bin/Xvfb.sh"],
- deps = [
- "@bazel_tools//tools/bash/runfiles",
- ],
data = glob([
"usr/lib/**/*",
"lib/**/*",
"usr/bin/*",
]),
visibility = ["//visibility:public"],
+ deps = [
+ "@bazel_tools//tools/bash/runfiles",
+ ],
)
diff --git a/third_party/y2023/field/BUILD b/third_party/y2023/field/BUILD
index de1d382..a806293 100644
--- a/third_party/y2023/field/BUILD
+++ b/third_party/y2023/field/BUILD
@@ -1,14 +1,14 @@
filegroup(
name = "pictures",
srcs = [
- # Picture from the FIRST inspires field drawings.
- # https://www.firstinspires.org/robotics/frc/playing-field
- # Copyright 2023 FIRST
- "2023.png",
- # Picture from FIRST modified by Tea Fazio.
- # https://firstfrc.blob.core.windows.net/frc2023/Manual/2023FRCGameManual.pdf
- # Copyright 2023 FIRST
- "field.jpg",
- ],
+ # Picture from the FIRST inspires field drawings.
+ # https://www.firstinspires.org/robotics/frc/playing-field
+ # Copyright 2023 FIRST
+ "2023.png",
+ # Picture from FIRST modified by Tea Fazio.
+ # https://firstfrc.blob.core.windows.net/frc2023/Manual/2023FRCGameManual.pdf
+ # Copyright 2023 FIRST
+ "field.jpg",
+ ],
visibility = ["//visibility:public"],
)
diff --git a/third_party/y2024/field/BUILD b/third_party/y2024/field/BUILD
index 04db591..4b2bbff 100644
--- a/third_party/y2024/field/BUILD
+++ b/third_party/y2024/field/BUILD
@@ -1,11 +1,11 @@
filegroup(
name = "pictures",
srcs = [
- # Picture from the FIRST inspires field drawings.
- "2024.png",
- # https://www.firstinspires.org/robotics/frc/playing-field
- # Copyright 2024 FIRST
- "2024_field.png",
- ],
+ # Picture from the FIRST inspires field drawings.
+ "2024.png",
+ # https://www.firstinspires.org/robotics/frc/playing-field
+ # Copyright 2024 FIRST
+ "2024_field.png",
+ ],
visibility = ["//visibility:public"],
-)
\ No newline at end of file
+)
diff --git a/y2024/BUILD b/y2024/BUILD
index 26727a5..6cb7156 100644
--- a/y2024/BUILD
+++ b/y2024/BUILD
@@ -63,7 +63,7 @@
"//y2024/constants:constants_sender",
"//y2024/localizer:localizer_main",
"//y2024/localizer:localizer_logger",
- "//y2024/vision:foxglove_image_converter",
+ "//frc971/vision:foxglove_image_converter",
],
data = [
":aos_config",
@@ -87,7 +87,7 @@
"//frc971/orin:argus_camera",
"//y2024/orin:can_logger",
"//y2024/vision:apriltag_detector",
- "//y2024/vision:image_logger",
+ "//frc971/vision:image_logger",
],
target_compatible_with = ["//tools/platforms/hardware:raspberry_pi"],
target_type = "pi",
@@ -339,9 +339,9 @@
data = [
":aos_config",
"//aos/network:log_web_proxy_main",
+ "//frc971/www:starter_main_bundle.min.js",
"//y2024/www:field_main_bundle.min.js",
"//y2024/www:files",
- "//y2024/www:starter_main_bundle.min.js",
],
target_compatible_with = ["@platforms//os:linux"],
)
diff --git a/y2024/vision/BUILD b/y2024/vision/BUILD
index bd4fe76..8f0a543 100644
--- a/y2024/vision/BUILD
+++ b/y2024/vision/BUILD
@@ -5,17 +5,6 @@
)
cc_binary(
- name = "foxglove_image_converter",
- srcs = ["foxglove_image_converter.cc"],
- visibility = ["//y2024:__subpackages__"],
- deps = [
- "//aos:init",
- "//aos/events:shm_event_loop",
- "//frc971/vision:foxglove_image_converter_lib",
- ],
-)
-
-cc_binary(
name = "target_mapping",
srcs = [
"target_mapping.cc",
@@ -48,26 +37,6 @@
)
cc_binary(
- name = "image_logger",
- srcs = [
- "image_logger.cc",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//visibility:public"],
- deps = [
- "//aos:configuration",
- "//aos:init",
- "//aos/events:shm_event_loop",
- "//aos/events/logging:log_writer",
- "//aos/logging:log_namer",
- "//aos/util:filesystem_fbs",
- "//frc971/input:joystick_state_fbs",
- "@com_github_gflags_gflags//:gflags",
- "@com_github_google_glog//:glog",
- ],
-)
-
-cc_binary(
name = "apriltag_detector",
srcs = [
"apriltag_detector.cc",
@@ -148,39 +117,3 @@
"@org_tuxfamily_eigen//:eigen",
],
)
-
-cc_binary(
- name = "modify_extrinsics",
- srcs = [
- "modify_extrinsics.cc",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//y2024:__subpackages__"],
- deps = [
- "//aos:configuration",
- "//aos:init",
- "//aos/events:event_loop",
- "//frc971/vision:calibration_fbs",
- "//frc971/vision:vision_util_lib",
- "//y2024/constants:constants_fbs",
- "@com_google_absl//absl/strings:str_format",
- "@org_tuxfamily_eigen//:eigen",
- ],
-)
-
-cc_binary(
- name = "image_replay",
- srcs = [
- "image_replay.cc",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//y2024:__subpackages__"],
- deps = [
- "//aos:configuration",
- "//aos:init",
- "//aos/events:simulated_event_loop",
- "//aos/events/logging:log_reader",
- "//frc971/vision:vision_fbs",
- "//third_party:opencv",
- ],
-)
diff --git a/y2024/vision/localization_verifier.cc b/y2024/vision/localization_verifier.cc
deleted file mode 100644
index c16f874..0000000
--- a/y2024/vision/localization_verifier.cc
+++ /dev/null
@@ -1,108 +0,0 @@
-#include "glog/logging.h"
-
-#include "aos/init.h"
-#include "frc971/constants/constants_sender_lib.h"
-#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
-#include "frc971/vision/vision_generated.h"
-#include "y2023/localizer/localizer.h"
-#include "y2023/localizer/utils.h"
-
-DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
-
-DEFINE_uint64(min_april_id, 1,
- "Minimum april id to consider as part of the field and ignore");
-DEFINE_uint64(max_april_id, 8,
- "Maximum april id to consider as part of the field and ignore");
-
-// This binary allows us to place extra april tags on the field and verify
-// that we can compute their field pose correctly
-
-namespace y2023::vision {
-
-using localizer::Localizer;
-
-Localizer::Transform LocalizerOutputToTransform(
- const frc971::controls::LocalizerOutput &localizer) {
- const auto T_field_robot =
- Eigen::Translation3d(localizer.x(), localizer.y(), 0.0);
-
- Eigen::AngleAxisd robot_yaw_angle(localizer.theta(),
- Eigen::Vector3d::UnitZ());
- const auto R_field_robot = Eigen::Quaterniond(robot_yaw_angle);
- const Localizer::Transform H_field_robot =
- (T_field_robot * R_field_robot).matrix();
- return H_field_robot;
-}
-
-void HandleDetections(
- const frc971::vision::TargetMap &detections,
- const Localizer::Transform &H_robot_camera,
- aos::Fetcher<frc971::controls::LocalizerOutput> *localizer_fetcher) {
- localizer_fetcher->Fetch();
- CHECK(localizer_fetcher->get());
-
- for (const auto *target_pose : *detections.target_poses()) {
- // Only look at april tags not part of the field
- if (target_pose->id() >= FLAGS_min_april_id &&
- target_pose->id() <= FLAGS_max_april_id) {
- continue;
- }
-
- const Localizer::Transform H_camera_target =
- localizer::PoseToTransform(target_pose);
- const Localizer::Transform H_field_robot =
- LocalizerOutputToTransform(*localizer_fetcher->get());
-
- // Get the field-based target pose using the detection, extrinsics, and
- // localizer output
- const Localizer::Transform H_field_target =
- H_field_robot * H_robot_camera * H_camera_target;
-
- LOG(INFO) << "Field to target " << target_pose->id();
- LOG(INFO) << "H_field_robot " << H_field_robot;
- LOG(INFO) << "H_robot_camera " << H_robot_camera;
- LOG(INFO) << "H_camera_target " << H_camera_target;
- LOG(INFO) << "Transform: " << H_field_target;
- LOG(INFO) << "Translation: "
- << Eigen::Affine3d(H_field_target).translation();
- LOG(INFO);
- }
-}
-
-void LocalizationVerifierMain() {
- aos::FlatbufferDetachedBuffer<aos::Configuration> config =
- aos::configuration::ReadConfig(FLAGS_config);
-
- frc971::constants::WaitForConstants<Constants>(&config.message());
-
- aos::ShmEventLoop event_loop(&config.message());
-
- frc971::constants::ConstantsFetcher<Constants> constants_fetcher(&event_loop);
-
- aos::Fetcher<frc971::controls::LocalizerOutput> localizer_fetcher =
- event_loop.MakeFetcher<frc971::controls::LocalizerOutput>("/localizer");
-
- for (const auto *camera : *constants_fetcher.constants().cameras()) {
- CHECK(camera->has_calibration());
- Localizer::Transform H_robot_camera =
- frc971::control_loops::drivetrain::FlatbufferToTransformationMatrix(
- *camera->calibration()->fixed_extrinsics());
-
- const std::string_view pi_name =
- camera->calibration()->node_name()->string_view();
- event_loop.MakeWatcher(
- absl::StrCat("/", pi_name, "/camera"),
- [H_robot_camera,
- &localizer_fetcher](const frc971::vision::TargetMap &target_map) {
- HandleDetections(target_map, H_robot_camera, &localizer_fetcher);
- });
- }
- event_loop.Run();
-}
-
-} // namespace y2023::vision
-
-int main(int argc, char **argv) {
- aos::InitGoogle(&argc, &argv);
- y2023::vision::LocalizationVerifierMain();
-}
diff --git a/y2024/www/BUILD b/y2024/www/BUILD
index 6b751ed..b9bf06a 100644
--- a/y2024/www/BUILD
+++ b/y2024/www/BUILD
@@ -56,39 +56,13 @@
],
)
-ts_project(
- name = "starter_main",
- srcs = [
- "starter_handler.ts",
- "starter_main.ts",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- deps = [
- "//aos/network:connect_ts_fbs",
- "//aos/network:message_bridge_client_ts_fbs",
- "//aos/network/www:proxy",
- "//aos/starter:starter_ts_fbs",
- "@com_github_google_flatbuffers//ts:flatbuffers_ts",
- ],
-)
-
-rollup_bundle(
- name = "starter_main_bundle",
- entry_point = "starter_main.ts",
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//y2024:__subpackages__"],
- deps = [
- ":starter_main",
- ],
-)
-
aos_downloader_dir(
name = "www_files",
srcs = [
":field_main_bundle.min.js",
":files",
- ":starter_main_bundle.min.js",
"//frc971/analysis:plot_index_bundle.min.js",
+ "//frc971/www:starter_main_bundle.min.js",
],
dir = "www",
target_compatible_with = ["@platforms//os:linux"],
diff --git a/y2024_swerve/BUILD b/y2024_swerve/BUILD
index c696c5a..0670038 100644
--- a/y2024_swerve/BUILD
+++ b/y2024_swerve/BUILD
@@ -109,7 +109,7 @@
deps = [
"//aos:init",
"//aos/events:event_loop",
- "//frc971/control_loops/drivetrain/swerve:swerve_drivetrain_output_fbs",
+ "//frc971/control_loops/swerve:swerve_drivetrain_output_fbs",
"@com_github_google_glog//:glog",
],
)
@@ -141,6 +141,7 @@
"//aos:init",
"//aos/events:shm_event_loop",
"//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops/swerve:swerve_drivetrain_position_fbs",
"//frc971/wpilib:can_sensor_reader",
"//frc971/wpilib:sensor_reader",
"//frc971/wpilib:talonfx",
@@ -178,8 +179,8 @@
"//aos/network:message_bridge_client_fbs",
"//aos/network:message_bridge_server_fbs",
"//aos/network:timestamp_fbs",
- "//frc971/control_loops/drivetrain/swerve:swerve_drivetrain_output_fbs",
- "//frc971/control_loops/drivetrain/swerve:swerve_drivetrain_position_fbs",
+ "//frc971/control_loops/swerve:swerve_drivetrain_output_fbs",
+ "//frc971/control_loops/swerve:swerve_drivetrain_position_fbs",
"//frc971/control_loops/drivetrain:drivetrain_can_position_fbs",
"//frc971/can_logger:can_logging_fbs",
],
diff --git a/y2024_swerve/constants/6971.json b/y2024_swerve/constants/6971.json
new file mode 100644
index 0000000..644af99
--- /dev/null
+++ b/y2024_swerve/constants/6971.json
@@ -0,0 +1,28 @@
+{% from 'y2024_swerve/constants/common.jinja2' import front_left_zero %}
+{% from 'y2024_swerve/constants/common.jinja2' import front_right_zero %}
+{% from 'y2024_swerve/constants/common.jinja2' import back_left_zero %}
+{% from 'y2024_swerve/constants/common.jinja2' import back_right_zero %}
+
+{
+ "robot": {
+ "front_left_zeroing_constants": {{ front_left_zero | tojson(indent=2)}},
+ "front_right_zeroing_constants": {{ front_right_zero | tojson(indent=2)}},
+ "back_left_zeroing_constants": {{ back_left_zero | tojson(indent=2)}},
+ "back_right_zeroing_constants": {{ back_right_zero | tojson(indent=2)}},
+ "cameras": [
+ {
+ "calibration": {% include 'y2024_swerve/constants/calib_files/calibration_imu-9971-0_cam-24-10_2024-02-24_16-44-05.json' %}
+ },
+ {
+ "calibration": {% include 'y2024_swerve/constants/calib_files/calibration_imu-9971-1_cam-24-12_2024-03-24_11-52-49.json' %}
+ },
+ {
+ "calibration": {% include 'y2024_swerve/constants/calib_files/calibration_orin1-9971-0_cam-24-09_2024-03-24_11-52-49.json' %}
+ },
+ {
+ "calibration": {% include 'y2024_swerve/constants/calib_files/calibration_orin1-9971-1_cam-24-11_2024-03-24_11-52-49.json' %}
+ }
+ ]
+ },
+ {% include 'y2024_swerve/constants/common.json' %}
+}
diff --git a/y2024_swerve/constants/7971.json b/y2024_swerve/constants/7971.json
new file mode 100644
index 0000000..644af99
--- /dev/null
+++ b/y2024_swerve/constants/7971.json
@@ -0,0 +1,28 @@
+{% from 'y2024_swerve/constants/common.jinja2' import front_left_zero %}
+{% from 'y2024_swerve/constants/common.jinja2' import front_right_zero %}
+{% from 'y2024_swerve/constants/common.jinja2' import back_left_zero %}
+{% from 'y2024_swerve/constants/common.jinja2' import back_right_zero %}
+
+{
+ "robot": {
+ "front_left_zeroing_constants": {{ front_left_zero | tojson(indent=2)}},
+ "front_right_zeroing_constants": {{ front_right_zero | tojson(indent=2)}},
+ "back_left_zeroing_constants": {{ back_left_zero | tojson(indent=2)}},
+ "back_right_zeroing_constants": {{ back_right_zero | tojson(indent=2)}},
+ "cameras": [
+ {
+ "calibration": {% include 'y2024_swerve/constants/calib_files/calibration_imu-9971-0_cam-24-10_2024-02-24_16-44-05.json' %}
+ },
+ {
+ "calibration": {% include 'y2024_swerve/constants/calib_files/calibration_imu-9971-1_cam-24-12_2024-03-24_11-52-49.json' %}
+ },
+ {
+ "calibration": {% include 'y2024_swerve/constants/calib_files/calibration_orin1-9971-0_cam-24-09_2024-03-24_11-52-49.json' %}
+ },
+ {
+ "calibration": {% include 'y2024_swerve/constants/calib_files/calibration_orin1-9971-1_cam-24-11_2024-03-24_11-52-49.json' %}
+ }
+ ]
+ },
+ {% include 'y2024_swerve/constants/common.json' %}
+}
diff --git a/y2024_swerve/constants/BUILD b/y2024_swerve/constants/BUILD
new file mode 100644
index 0000000..6de0fb0
--- /dev/null
+++ b/y2024_swerve/constants/BUILD
@@ -0,0 +1,107 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
+load("//tools/build_rules:template.bzl", "jinja2_template")
+load("//y2024_swerve/constants:validator.bzl", "constants_json")
+
+cc_library(
+ name = "simulated_constants_sender",
+ srcs = ["simulated_constants_sender.cc"],
+ hdrs = ["simulated_constants_sender.h"],
+ data = [":test_constants.json"],
+ visibility = ["//y2024_swerve:__subpackages__"],
+ deps = [
+ ":constants_fbs",
+ ":constants_list_fbs",
+ "//aos/events:simulated_event_loop",
+ "//aos/testing:path",
+ "//frc971/constants:constants_sender_lib",
+ ],
+)
+
+jinja2_template(
+ name = "test_constants_unvalidated.json",
+ src = "test_constants.jinja2.json",
+ includes = glob([
+ "test_data/*.json",
+ ]) + [
+ "common.jinja2",
+ "common.json",
+ "//y2024_swerve/constants/calib_files",
+ "//y2024_swerve/vision/maps",
+ ],
+ parameters = {},
+ visibility = ["//visibility:public"],
+)
+
+jinja2_template(
+ name = "constants_unvalidated.json",
+ src = "constants.jinja2.json",
+ includes = [
+ "6971.json",
+ "7971.json",
+ "common.jinja2",
+ "common.json",
+ "//y2024_swerve/constants/calib_files",
+ "//y2024_swerve/vision/maps",
+ ],
+ parameters = {},
+ visibility = ["//visibility:public"],
+)
+
+static_flatbuffer(
+ name = "constants_fbs",
+ srcs = ["constants.fbs"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_config_fbs",
+ "//frc971/vision:calibration_fbs",
+ "//frc971/vision:target_map_fbs",
+ "//frc971/zeroing:constants_fbs",
+ ],
+)
+
+static_flatbuffer(
+ name = "constants_list_fbs",
+ srcs = ["constants_list.fbs"],
+ visibility = ["//visibility:public"],
+ deps = [":constants_fbs"],
+)
+
+cc_binary(
+ name = "constants_sender",
+ srcs = ["constants_sender.cc"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":constants_fbs",
+ ":constants_list_fbs",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//aos/testing:path",
+ "//frc971/constants:constants_sender_lib",
+ ],
+)
+
+cc_binary(
+ name = "constants_formatter",
+ srcs = ["constants_formatter.cc"],
+ data = [":constants_unvalidated.json"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":constants_list_fbs",
+ "//aos:init",
+ "//aos:json_to_flatbuffer",
+ "@com_github_google_glog//:glog",
+ ],
+)
+
+constants_json(
+ name = "constants_json",
+ src = ":constants_unvalidated.json",
+ out = "constants.json",
+)
+
+constants_json(
+ name = "test_constants_json",
+ src = ":test_constants_unvalidated.json",
+ out = "test_constants.json",
+)
diff --git a/y2024_swerve/constants/calib_files/BUILD b/y2024_swerve/constants/calib_files/BUILD
new file mode 100644
index 0000000..1f33d50
--- /dev/null
+++ b/y2024_swerve/constants/calib_files/BUILD
@@ -0,0 +1,5 @@
+filegroup(
+ name = "calib_files",
+ srcs = glob(["*.json"]),
+ visibility = ["//visibility:public"],
+)
diff --git a/y2024_swerve/constants/calib_files/calibration_imu-9971-0_cam-24-10_2024-02-24_16-44-05.json b/y2024_swerve/constants/calib_files/calibration_imu-9971-0_cam-24-10_2024-02-24_16-44-05.json
new file mode 100755
index 0000000..e5f50e3
--- /dev/null
+++ b/y2024_swerve/constants/calib_files/calibration_imu-9971-0_cam-24-10_2024-02-24_16-44-05.json
@@ -0,0 +1,46 @@
+{
+ "node_name": "imu",
+ "team_number": 9971,
+ "intrinsics": [
+ 646.04834,
+ 0.0,
+ 703.327576,
+ 0.0,
+ 645.444458,
+ 527.86261,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ 0.0,
+ -0.258819,
+ -0.965926,
+ -0.323293,
+ 1.0,
+ 0.0,
+ -0.0,
+ 0.268249,
+ 0.0,
+ -0.965926,
+ 0.258819,
+ 0.471129,
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ]
+ },
+ "dist_coeffs": [
+ -0.251594,
+ 0.064935,
+ 0.000479,
+ 0.000036,
+ -0.007207
+ ],
+ "calibration_timestamp": 1708821845975708672,
+ "camera_id": "24-10",
+ "camera_number": 0,
+ "reprojection_error": 1.523209
+}
diff --git a/y2024_swerve/constants/calib_files/calibration_imu-9971-1_cam-24-12_2024-03-24_11-52-49.json b/y2024_swerve/constants/calib_files/calibration_imu-9971-1_cam-24-12_2024-03-24_11-52-49.json
new file mode 100755
index 0000000..530e88a
--- /dev/null
+++ b/y2024_swerve/constants/calib_files/calibration_imu-9971-1_cam-24-12_2024-03-24_11-52-49.json
@@ -0,0 +1,46 @@
+{
+ "node_name": "imu",
+ "team_number": 9971,
+ "intrinsics": [
+ 647.19928,
+ 0.0,
+ 690.698181,
+ 0.0,
+ 646.449158,
+ 530.162842,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ 0.99969,
+ 0.020217,
+ -0.014527,
+ 0.160342,
+ 0.009501,
+ 0.229548,
+ 0.973251,
+ 0.25208,
+ 0.023011,
+ -0.973088,
+ 0.229284,
+ 0.41504,
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ]
+ },
+ "dist_coeffs": [
+ -0.249799,
+ 0.062593,
+ 0.00003,
+ 0.000366,
+ -0.006532
+ ],
+ "calibration_timestamp": 1711306369592332476,
+ "camera_id": "24-12",
+ "camera_number": 1,
+ "reprojection_error": 1.23409
+}
\ No newline at end of file
diff --git a/y2024_swerve/constants/calib_files/calibration_orin1-9971-0_cam-24-09_2024-03-24_11-52-49.json b/y2024_swerve/constants/calib_files/calibration_orin1-9971-0_cam-24-09_2024-03-24_11-52-49.json
new file mode 100755
index 0000000..97234c9
--- /dev/null
+++ b/y2024_swerve/constants/calib_files/calibration_orin1-9971-0_cam-24-09_2024-03-24_11-52-49.json
@@ -0,0 +1,46 @@
+{
+ "node_name": "orin1",
+ "team_number": 9971,
+ "intrinsics": [
+ 648.187805,
+ 0.0,
+ 736.903137,
+ 0.0,
+ 648.028687,
+ 557.169861,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ -0.999204,
+ -0.034711,
+ -0.019682,
+ 0.162103,
+ 0.028118,
+ -0.262536,
+ -0.964512,
+ -0.329348,
+ 0.028312,
+ -0.964298,
+ 0.263303,
+ 0.562319,
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ]
+ },
+ "dist_coeffs": [
+ -0.265564,
+ 0.078084,
+ -0.000231,
+ 0.000386,
+ -0.010425
+ ],
+ "calibration_timestamp": 1711306369593360533,
+ "camera_id": "24-09",
+ "camera_number": 0,
+ "reprojection_error": 1.881098
+}
\ No newline at end of file
diff --git a/y2024_swerve/constants/calib_files/calibration_orin1-9971-1_cam-24-11_2024-03-24_11-52-49.json b/y2024_swerve/constants/calib_files/calibration_orin1-9971-1_cam-24-11_2024-03-24_11-52-49.json
new file mode 100755
index 0000000..daef89c
--- /dev/null
+++ b/y2024_swerve/constants/calib_files/calibration_orin1-9971-1_cam-24-11_2024-03-24_11-52-49.json
@@ -0,0 +1,46 @@
+{
+ "node_name": "orin1",
+ "team_number": 9971,
+ "intrinsics": [
+ 649.866699,
+ 0.0,
+ 709.355713,
+ 0.0,
+ 648.893066,
+ 576.101868,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ -0.014623,
+ 0.004459,
+ 0.999883,
+ 0.345385,
+ 0.997965,
+ 0.062137,
+ 0.014318,
+ 0.150131,
+ -0.062066,
+ 0.998058,
+ -0.005359,
+ 0.570236,
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ]
+ },
+ "dist_coeffs": [
+ -0.248092,
+ 0.060938,
+ 0.000313,
+ 0.00009,
+ -0.006163
+ ],
+ "calibration_timestamp": 1711306369592886702,
+ "camera_id": "24-11",
+ "camera_number": 1,
+ "reprojection_error": 1.450069
+}
\ No newline at end of file
diff --git a/y2024_swerve/constants/common.jinja2 b/y2024_swerve/constants/common.jinja2
new file mode 100644
index 0000000..48b843c
--- /dev/null
+++ b/y2024_swerve/constants/common.jinja2
@@ -0,0 +1,45 @@
+{% set M_PI = 3.14159265358979323846 %}
+
+{%
+set front_left_zero = {
+ "average_filter_size": 0,
+ "one_revolution_distance": 2 * M_PI,
+ "measured_absolute_position": 0.76761395509829,
+ "zeroing_threshold": 0.0,
+ "moving_buffer_size": 0,
+ "allowable_encoder_error": 0.0,
+}
+%}
+
+{%
+set front_right_zero = {
+ "average_filter_size": 0,
+ "one_revolution_distance": 2 * M_PI,
+ "measured_absolute_position": 0.779403958443922,
+ "zeroing_threshold": 0.0,
+ "moving_buffer_size": 0,
+ "allowable_encoder_error": 0.0,
+}
+%}
+
+{%
+set back_left_zero = {
+ "average_filter_size": 0,
+ "one_revolution_distance": 2 * M_PI,
+ "measured_absolute_position": 0.053439698061417,
+ "zeroing_threshold": 0.0,
+ "moving_buffer_size": 0,
+ "allowable_encoder_error": 0.0,
+}
+%}
+
+{%
+set back_right_zero = {
+ "average_filter_size": 0,
+ "one_revolution_distance": 2 * M_PI,
+ "measured_absolute_position": 0.719329333121509,
+ "zeroing_threshold": 0.0,
+ "moving_buffer_size": 0,
+ "allowable_encoder_error": 0.0,
+}
+%}
diff --git a/y2024_swerve/constants/common.json b/y2024_swerve/constants/common.json
new file mode 100644
index 0000000..48a4afa
--- /dev/null
+++ b/y2024_swerve/constants/common.json
@@ -0,0 +1,3 @@
+"common": {
+
+}
\ No newline at end of file
diff --git a/y2024_swerve/constants/constants.fbs b/y2024_swerve/constants/constants.fbs
new file mode 100644
index 0000000..a621f3b
--- /dev/null
+++ b/y2024_swerve/constants/constants.fbs
@@ -0,0 +1,28 @@
+include "frc971/zeroing/constants.fbs";
+include "frc971/vision/calibration.fbs";
+
+namespace y2024_swerve;
+
+table CameraConfiguration {
+ calibration:frc971.vision.calibration.CameraCalibration (id: 0);
+}
+
+table Common {
+
+}
+
+table RobotConstants {
+ front_left_zeroing_constants:frc971.zeroing.ContinuousAbsoluteEncoderZeroingConstants (id: 0);
+ front_right_zeroing_constants:frc971.zeroing.ContinuousAbsoluteEncoderZeroingConstants (id: 1);
+ back_left_zeroing_constants:frc971.zeroing.ContinuousAbsoluteEncoderZeroingConstants (id: 2);
+ back_right_zeroing_constants:frc971.zeroing.ContinuousAbsoluteEncoderZeroingConstants (id: 3);
+ cameras:[CameraConfiguration] (id: 4);
+}
+
+table Constants{
+ cameras:[CameraConfiguration] (id: 0, deprecated);
+ robot:RobotConstants (id: 1);
+ common:Common (id: 2);
+}
+
+root_type Constants;
diff --git a/y2024_swerve/constants/constants.jinja2.json b/y2024_swerve/constants/constants.jinja2.json
new file mode 100644
index 0000000..555c264
--- /dev/null
+++ b/y2024_swerve/constants/constants.jinja2.json
@@ -0,0 +1,12 @@
+{
+ "constants": [
+ {
+ "team": 6971,
+ "data": {% include 'y2024_swerve/constants/6971.json' %}
+ },
+ {
+ "team": 7971,
+ "data": {% include 'y2024_swerve/constants/7971.json' %}
+ }
+ ]
+}
diff --git a/y2024_swerve/constants/constants_formatter.cc b/y2024_swerve/constants/constants_formatter.cc
new file mode 100644
index 0000000..2ebb9b5
--- /dev/null
+++ b/y2024_swerve/constants/constants_formatter.cc
@@ -0,0 +1,26 @@
+#include "glog/logging.h"
+
+#include "aos/flatbuffers.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/util/file.h"
+#include "y2024_swerve/constants/constants_list_generated.h"
+
+int main(int argc, char **argv) {
+ ::aos::InitGoogle(&argc, &argv);
+
+ CHECK(argc == 3) << ": Expected input and output json files to be passed in.";
+
+ aos::FlatbufferDetachedBuffer<y2024_swerve::ConstantsList> constants =
+ aos::JsonFileToFlatbuffer<y2024_swerve::ConstantsList>(argv[1]);
+
+ // Make sure the file is valid json before we output a formatted version.
+ CHECK(constants.message().constants() != nullptr)
+ << ": Failed to parse " << std::string(argv[2]);
+
+ aos::util::WriteStringToFileOrDie(
+ std::string(argv[2]),
+ aos::FlatbufferToJson(constants, {.multi_line = true}));
+
+ return 0;
+}
diff --git a/y2024_swerve/constants/constants_list.fbs b/y2024_swerve/constants/constants_list.fbs
new file mode 100644
index 0000000..3a65b64
--- /dev/null
+++ b/y2024_swerve/constants/constants_list.fbs
@@ -0,0 +1,14 @@
+include "y2024_swerve/constants/constants.fbs";
+
+namespace y2024_swerve;
+
+table TeamAndConstants {
+ team:long (id: 0);
+ data:Constants (id: 1);
+}
+
+table ConstantsList {
+ constants:[TeamAndConstants] (id: 0);
+}
+
+root_type ConstantsList;
diff --git a/y2024_swerve/constants/constants_sender.cc b/y2024_swerve/constants/constants_sender.cc
new file mode 100644
index 0000000..c624572
--- /dev/null
+++ b/y2024_swerve/constants/constants_sender.cc
@@ -0,0 +1,25 @@
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+
+#include "aos/configuration.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "frc971/constants/constants_sender_lib.h"
+#include "y2024_swerve/constants/constants_generated.h"
+#include "y2024_swerve/constants/constants_list_generated.h"
+
+DEFINE_string(config, "aos_config.json", "Path to the AOS config.");
+DEFINE_string(constants_path, "constants.json", "Path to the constant file");
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+ aos::ShmEventLoop event_loop(&config.message());
+ frc971::constants::ConstantSender<y2024_swerve::Constants,
+ y2024_swerve::ConstantsList>
+ constants_sender(&event_loop, FLAGS_constants_path);
+ // Don't need to call Run().
+ return 0;
+}
diff --git a/y2024_swerve/constants/simulated_constants_sender.cc b/y2024_swerve/constants/simulated_constants_sender.cc
new file mode 100644
index 0000000..7782e62
--- /dev/null
+++ b/y2024_swerve/constants/simulated_constants_sender.cc
@@ -0,0 +1,23 @@
+#include "aos/events/simulated_event_loop.h"
+#include "aos/testing/path.h"
+#include "frc971/constants/constants_sender_lib.h"
+#include "y2024_swerve/constants/constants_generated.h"
+#include "y2024_swerve/constants/constants_list_generated.h"
+
+namespace y2024_swerve {
+bool SendSimulationConstants(aos::SimulatedEventLoopFactory *factory, int team,
+ std::string constants_path,
+ const std::set<std::string_view> &node_names) {
+ for (const aos::Node *node : factory->nodes()) {
+ if (!node_names.empty() &&
+ !node_names.contains(node->name()->string_view())) {
+ continue;
+ }
+ std::unique_ptr<aos::EventLoop> event_loop =
+ factory->MakeEventLoop("constants_sender", node);
+ frc971::constants::ConstantSender<Constants, ConstantsList> sender(
+ event_loop.get(), constants_path, team, "/constants");
+ }
+ return true;
+}
+} // namespace y2024_swerve
\ No newline at end of file
diff --git a/y2024_swerve/constants/simulated_constants_sender.h b/y2024_swerve/constants/simulated_constants_sender.h
new file mode 100644
index 0000000..5a64b75
--- /dev/null
+++ b/y2024_swerve/constants/simulated_constants_sender.h
@@ -0,0 +1,21 @@
+#ifndef Y2024_SWERVE_CONSTANTS_SIMULATED_CONFIG_SENDER_H_
+#define Y2024_SWERVE_CONSTANTS_SIMULATED_CONFIG_SENDER_H_
+
+#include <set>
+
+#include "aos/events/simulated_event_loop.h"
+#include "aos/testing/path.h"
+
+namespace y2024_swerve {
+// Returns true, to allow this to be easily called in the initializer list of a
+// constructor.
+// If node_names is specified, we limit ourselves to sending constants on the
+// specified nodes.
+bool SendSimulationConstants(
+ aos::SimulatedEventLoopFactory *factory, int team,
+ std::string constants_path = aos::testing::ArtifactPath(
+ "y2024_swerve/constants/test_constants.json"),
+ const std::set<std::string_view> &node_names = {});
+} // namespace y2024_swerve
+
+#endif // Y2024_SWERVE_CONSTANTS_SIMULATED_CONFIG_SENDER_H_
diff --git a/y2024_swerve/constants/test_constants.jinja2.json b/y2024_swerve/constants/test_constants.jinja2.json
new file mode 100644
index 0000000..6533a03
--- /dev/null
+++ b/y2024_swerve/constants/test_constants.jinja2.json
@@ -0,0 +1,8 @@
+{
+ "constants": [
+ {
+ "team": 6971,
+ "data": {% include 'y2024_swerve/constants/test_data/test_team.json' %}
+ }
+ ]
+}
diff --git a/y2024_swerve/constants/test_data/calibration_cam-1.json b/y2024_swerve/constants/test_data/calibration_cam-1.json
new file mode 100644
index 0000000..2d81347
--- /dev/null
+++ b/y2024_swerve/constants/test_data/calibration_cam-1.json
@@ -0,0 +1,46 @@
+{
+ "node_name": "orin1",
+ "camera_number": 0,
+ "team_number": 7971,
+ "intrinsics": [
+ 893.759521,
+ 0,
+ 645.470764,
+ 0,
+ 893.222351,
+ 388.150269,
+ 0,
+ 0,
+ 1
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ 0.0,
+ 0.0,
+ 1.0,
+ 1.0,
+
+ -1.0,
+ 0.0,
+ 0.0,
+ 0.0,
+
+ 0.0,
+ -1.0,
+ 0.0,
+ 0.0,
+
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ]
+ },
+ "dist_coeffs": [
+ -0.44902,
+ 0.248409,
+ -0.000537,
+ -0.000112,
+ -0.076989
+ ]
+}
diff --git a/y2024_swerve/constants/test_data/calibration_cam-2.json b/y2024_swerve/constants/test_data/calibration_cam-2.json
new file mode 100644
index 0000000..1128799
--- /dev/null
+++ b/y2024_swerve/constants/test_data/calibration_cam-2.json
@@ -0,0 +1,46 @@
+{
+ "node_name": "orin1",
+ "camera_number": 1,
+ "team_number": 7971,
+ "intrinsics": [
+ 893.759521,
+ 0,
+ 645.470764,
+ 0,
+ 893.222351,
+ 388.150269,
+ 0,
+ 0,
+ 1
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ 1.0,
+ 0.0,
+ 0.0,
+ 1.0,
+
+ 0.0,
+ 0.0,
+ -1.0,
+ 0.0,
+
+ 0.0,
+ 1.0,
+ 0.0,
+ 0.0,
+
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ]
+ },
+ "dist_coeffs": [
+ -0.44902,
+ 0.248409,
+ -0.000537,
+ -0.000112,
+ -0.076989
+ ]
+}
diff --git a/y2024_swerve/constants/test_data/calibration_cam-3.json b/y2024_swerve/constants/test_data/calibration_cam-3.json
new file mode 100644
index 0000000..16e67ec
--- /dev/null
+++ b/y2024_swerve/constants/test_data/calibration_cam-3.json
@@ -0,0 +1,46 @@
+{
+ "node_name": "imu",
+ "camera_number": 0,
+ "team_number": 7971,
+ "intrinsics": [
+ 893.759521,
+ 0,
+ 645.470764,
+ 0,
+ 893.222351,
+ 388.150269,
+ 0,
+ 0,
+ 1
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ 0.0,
+ 1.0,
+ 0.0,
+ 1.0,
+
+ 0.0,
+ 0.0,
+ -1.0,
+ 0.0,
+
+ -1.0,
+ 0.0,
+ 0.0,
+ 0.0,
+
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ]
+ },
+ "dist_coeffs": [
+ -0.44902,
+ 0.248409,
+ -0.000537,
+ -0.000112,
+ -0.076989
+ ]
+}
diff --git a/y2024_swerve/constants/test_data/calibration_cam-4.json b/y2024_swerve/constants/test_data/calibration_cam-4.json
new file mode 100644
index 0000000..1e5b623
--- /dev/null
+++ b/y2024_swerve/constants/test_data/calibration_cam-4.json
@@ -0,0 +1,46 @@
+{
+ "node_name": "imu",
+ "camera_number": 1,
+ "team_number": 7971,
+ "intrinsics": [
+ 893.759521,
+ 0,
+ 645.470764,
+ 0,
+ 893.222351,
+ 388.150269,
+ 0,
+ 0,
+ 1
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ -1.0,
+ 0.0,
+ 0.0,
+ 1.0,
+
+ 0.0,
+ 0.0,
+ -1.0,
+ 0.0,
+
+ 0.0,
+ -1.0,
+ 0.0,
+ 0.0,
+
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ]
+ },
+ "dist_coeffs": [
+ -0.44902,
+ 0.248409,
+ -0.000537,
+ -0.000112,
+ -0.076989
+ ]
+}
diff --git a/y2024_swerve/constants/test_data/test_team.json b/y2024_swerve/constants/test_data/test_team.json
new file mode 100644
index 0000000..644af99
--- /dev/null
+++ b/y2024_swerve/constants/test_data/test_team.json
@@ -0,0 +1,28 @@
+{% from 'y2024_swerve/constants/common.jinja2' import front_left_zero %}
+{% from 'y2024_swerve/constants/common.jinja2' import front_right_zero %}
+{% from 'y2024_swerve/constants/common.jinja2' import back_left_zero %}
+{% from 'y2024_swerve/constants/common.jinja2' import back_right_zero %}
+
+{
+ "robot": {
+ "front_left_zeroing_constants": {{ front_left_zero | tojson(indent=2)}},
+ "front_right_zeroing_constants": {{ front_right_zero | tojson(indent=2)}},
+ "back_left_zeroing_constants": {{ back_left_zero | tojson(indent=2)}},
+ "back_right_zeroing_constants": {{ back_right_zero | tojson(indent=2)}},
+ "cameras": [
+ {
+ "calibration": {% include 'y2024_swerve/constants/calib_files/calibration_imu-9971-0_cam-24-10_2024-02-24_16-44-05.json' %}
+ },
+ {
+ "calibration": {% include 'y2024_swerve/constants/calib_files/calibration_imu-9971-1_cam-24-12_2024-03-24_11-52-49.json' %}
+ },
+ {
+ "calibration": {% include 'y2024_swerve/constants/calib_files/calibration_orin1-9971-0_cam-24-09_2024-03-24_11-52-49.json' %}
+ },
+ {
+ "calibration": {% include 'y2024_swerve/constants/calib_files/calibration_orin1-9971-1_cam-24-11_2024-03-24_11-52-49.json' %}
+ }
+ ]
+ },
+ {% include 'y2024_swerve/constants/common.json' %}
+}
diff --git a/y2024_swerve/constants/validator.bzl b/y2024_swerve/constants/validator.bzl
new file mode 100644
index 0000000..92592df
--- /dev/null
+++ b/y2024_swerve/constants/validator.bzl
@@ -0,0 +1,13 @@
+load("@aspect_bazel_lib//lib:run_binary.bzl", "run_binary")
+
+# Validates the constants.json file and outputs a formatted version.
+# TODO(max): Make this generic/template it out into frc971
+def constants_json(name, src, out):
+ run_binary(
+ name = name,
+ tool = "//y2024_swerve/constants:constants_formatter",
+ srcs = [src],
+ outs = [out],
+ args = ["$(location %s)" % (src)] + ["$(location %s)" % (out)],
+ visibility = ["//visibility:public"],
+ )
diff --git a/y2024_swerve/swerve_publisher_lib.h b/y2024_swerve/swerve_publisher_lib.h
index 9206eab..1a07865 100644
--- a/y2024_swerve/swerve_publisher_lib.h
+++ b/y2024_swerve/swerve_publisher_lib.h
@@ -8,7 +8,7 @@
#include "aos/flatbuffer_merge.h"
#include "aos/init.h"
#include "aos/json_to_flatbuffer.h"
-#include "frc971/control_loops/drivetrain/swerve/swerve_drivetrain_output_generated.h"
+#include "frc971/control_loops/swerve/swerve_drivetrain_output_generated.h"
namespace y2024_swerve {
diff --git a/y2024_swerve/vision/BUILD b/y2024_swerve/vision/BUILD
new file mode 100644
index 0000000..2951130
--- /dev/null
+++ b/y2024_swerve/vision/BUILD
@@ -0,0 +1,86 @@
+filegroup(
+ name = "image_streamer_start",
+ srcs = ["image_streamer_start.sh"],
+ visibility = ["//visibility:public"],
+)
+
+cc_binary(
+ name = "target_mapping",
+ srcs = [
+ "target_mapping.cc",
+ "vision_util.cc",
+ "vision_util.h",
+ ],
+ data = [
+ "//y2024_swerve:aos_config",
+ "//y2024_swerve/constants:constants.json",
+ "//y2024_swerve/vision:maps",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//y2024_swerve:__subpackages__"],
+ deps = [
+ "//aos:init",
+ "//aos/events:simulated_event_loop",
+ "//aos/events/logging:log_reader",
+ "//aos/util:mcap_logger",
+ "//frc971/constants:constants_sender_lib",
+ "//frc971/control_loops:pose",
+ "//frc971/vision:calibration_fbs",
+ "//frc971/vision:charuco_lib",
+ "//frc971/vision:target_mapper",
+ "//frc971/vision:vision_util_lib",
+ "//frc971/vision:visualize_robot",
+ "//third_party:opencv",
+ "//y2024_swerve/constants:constants_fbs",
+ "//y2024_swerve/constants:simulated_constants_sender",
+ ],
+)
+
+cc_binary(
+ name = "apriltag_detector",
+ srcs = [
+ "apriltag_detector.cc",
+ "vision_util.cc",
+ "vision_util.h",
+ ],
+ features = ["cuda"],
+ target_compatible_with = ["@platforms//cpu:arm64"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos:configuration",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//frc971/orin:gpu_apriltag_lib",
+ "//third_party:cudart",
+ "//third_party/apriltag",
+ "//y2024_swerve/constants:constants_fbs",
+ "@com_github_gflags_gflags//:gflags",
+ "@com_github_google_glog//:glog",
+ "@com_github_nvidia_cccl//:cccl",
+ "@com_github_nvidia_cuco//:cuco",
+ ],
+)
+
+cc_binary(
+ name = "viewer",
+ srcs = [
+ "viewer.cc",
+ "vision_util.cc",
+ "vision_util.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = [
+ "//y2024_swerve:__subpackages__",
+ ],
+ deps = [
+ "//aos:init",
+ "//aos:json_to_flatbuffer",
+ "//aos/events:shm_event_loop",
+ "//frc971/constants:constants_sender_lib",
+ "//frc971/vision:vision_fbs",
+ "//frc971/vision:vision_util_lib",
+ "//third_party:opencv",
+ "//y2024_swerve/constants:constants_fbs",
+ "@com_google_absl//absl/strings",
+ ],
+)
diff --git a/y2024_swerve/vision/README.md b/y2024_swerve/vision/README.md
new file mode 100644
index 0000000..c81581f
--- /dev/null
+++ b/y2024_swerve/vision/README.md
@@ -0,0 +1,25 @@
+How to use the extrinsic calibration for camera-to-camera calibration
+
+This all assumes you have cameras that have been intrinsically
+calibrated, and that pi1 has a valid extrinsic calibration (from robot
+origin / IMU to the pi1 camera).
+
+It by default will compute the extrinsics for each of the other cameras (pi2,
+pi3, pi4) relative to the robot origin (IMU origin).
+
+Steps:
+* Place two Aruco Diamond markers about 1 meter apart
+ (center-to-center) at a height so that they will be in view of the
+ cameras when the robot is about 1-2m away
+* Start with the robot in a position that both markers are fully in
+ view by one camera
+* Enable logging for all cameras
+* Rotate the robot in place through a full circle, so that each camera sees both tags, and at times just one tag.
+* Stop the logging and copy the files to your laptop
+* Run the calibration code on the resulting files, e.g.,
+ * `bazel run -c opt //y2023/vision:calibrate_multi_cameras -- /data/PATH_TO_LOGS --team_number 971 --target_type charuco_diamond
+ * Can add "--visualize" flag to see the camera views and marker detections
+ * I've sometimes found it necessary to add the "--skip_missing_forwarding_entries" flag-- as guided by the logging messages
+* From the output, copy the calculated ("Updated") extrinsic into the
+ corresponding calibration file for the right robot and camera in the
+ calib_files directory
diff --git a/y2024_swerve/vision/apriltag_detector.cc b/y2024_swerve/vision/apriltag_detector.cc
new file mode 100644
index 0000000..4d4025e
--- /dev/null
+++ b/y2024_swerve/vision/apriltag_detector.cc
@@ -0,0 +1,52 @@
+
+#include <string>
+
+#include "aos/init.h"
+#include "frc971/orin/gpu_apriltag.h"
+#include "y2024_swerve/constants/constants_generated.h"
+#include "y2024_swerve/vision/vision_util.h"
+
+DEFINE_string(channel, "/camera", "Channel name");
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+
+void GpuApriltagDetector() {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ frc971::constants::WaitForConstants<y2024_swerve::Constants>(
+ &config.message());
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ const frc971::constants::ConstantsFetcher<y2024_swerve::Constants>
+ calibration_data(&event_loop);
+
+ CHECK(FLAGS_channel.length() == 8);
+ int camera_id = std::stoi(FLAGS_channel.substr(7, 1));
+ const frc971::vision::calibration::CameraCalibration *calibration =
+ y2024_swerve::vision::FindCameraCalibration(
+ calibration_data.constants(),
+ event_loop.node()->name()->string_view(), camera_id);
+
+ frc971::apriltag::ApriltagDetector detector(&event_loop, FLAGS_channel,
+ calibration);
+
+ // TODO(austin): Figure out our core pinning strategy.
+ // event_loop.SetRuntimeAffinity(aos::MakeCpusetFromCpus({5}));
+
+ LOG(INFO) << "Setting scheduler priority";
+ struct sched_param param;
+ param.sched_priority = 21;
+ PCHECK(sched_setscheduler(0, SCHED_FIFO, ¶m) == 0);
+
+ LOG(INFO) << "Running event loop";
+ // TODO(austin): Pre-warm it...
+ event_loop.Run();
+} // namespace frc971::apriltag
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ GpuApriltagDetector();
+
+ return 0;
+}
diff --git a/y2024_swerve/vision/image_streamer_start.sh b/y2024_swerve/vision/image_streamer_start.sh
new file mode 100755
index 0000000..48d9da7
--- /dev/null
+++ b/y2024_swerve/vision/image_streamer_start.sh
@@ -0,0 +1,22 @@
+#!/bin/bash
+
+# Some configurations to avoid dropping frames
+# 640x480@30fps, 400x300@60fps.
+# Bitrate 500000-1500000
+WIDTH=640
+HEIGHT=480
+BITRATE=1500000
+LISTEN_ON="/camera/downsized"
+# Don't interfere with field webpage
+STREAMING_PORT=1181
+
+# Handle weirdness with openssl and gstreamer
+export OPENSSL_CONF=""
+
+# Enable for verbose logging
+#export GST_DEBUG=4
+
+export LD_LIBRARY_PATH=/usr/lib/aarch64-linux-gnu/gstreamer-1.0
+
+exec ./image_streamer --width=$WIDTH --height=$HEIGHT --bitrate=$BITRATE --listen_on=$LISTEN_ON --config=aos_config.json --streaming_port=$STREAMING_PORT
+
diff --git a/y2024_swerve/vision/maps/BUILD b/y2024_swerve/vision/maps/BUILD
new file mode 100644
index 0000000..38191a4
--- /dev/null
+++ b/y2024_swerve/vision/maps/BUILD
@@ -0,0 +1,7 @@
+filegroup(
+ name = "maps",
+ srcs = glob([
+ "*.json",
+ ]),
+ visibility = ["//visibility:public"],
+)
diff --git a/y2024_swerve/vision/maps/target_map.json b/y2024_swerve/vision/maps/target_map.json
new file mode 100644
index 0000000..2a8dfef
--- /dev/null
+++ b/y2024_swerve/vision/maps/target_map.json
@@ -0,0 +1,236 @@
+{
+/* Targets have positive Z axis pointing into the board, positive X to the right
+ when looking at the board, and positive Y is down when looking at the board.
+ This means that you will get an identity rotation from the camera to target
+ frame when the target is upright, flat, and centered in the camera's view.
+
+ The global frame as the origin at the center of the field, positive X points
+ at the red driver's station, and positive Z points straight up.
+ */
+ "target_poses": [
+ {
+ "id": 1,
+ "position": {
+ "x": 6.809,
+ "y": -3.860,
+ "z": 1.361
+ },
+ "orientation": {
+ "w": 0.1830127,
+ "x": -0.1830127,
+ "y": 0.6830127,
+ "z": -0.6830127
+ }
+ },
+ {
+ "id": 2,
+ "position": {
+ "x": 7.915,
+ "y": -3.223,
+ "z": 1.361
+ },
+ "orientation": {
+ "w": 0.1830127,
+ "x": -0.1830127,
+ "y": 0.6830127,
+ "z": -0.6830127
+ }
+ },
+ {
+ "id": 3,
+ "position": {
+ "x": 8.309,
+ "y": 0.877,
+ "z": 1.456
+ },
+ "orientation": {
+ "w": 0.5,
+ "x": -0.5,
+ "y": 0.5,
+ "z": -0.5
+ }
+ },
+ {
+ "id": 4,
+ "position": {
+ "x": 8.309,
+ "y": 1.442,
+ "z": 1.456
+ },
+ "orientation": {
+ "w": 0.5,
+ "x": -0.5,
+ "y": 0.5,
+ "z": -0.5
+ }
+ },
+ {
+ "id": 5,
+ "position": {
+ "x": 6.428,
+ "y": 4.099,
+ "z": 1.361
+ },
+ "orientation": {
+ "w": 0.7071068,
+ "x": -0.7071068,
+ "y": 0.0,
+ "z": 0.0
+ }
+ },
+ {
+ "id": 6,
+ "position": {
+ "x": -6.430,
+ "y": 4.099,
+ "z": 1.361
+ },
+ "orientation": {
+ "w": 0.7071068,
+ "x": -0.7071068,
+ "y": 0.0,
+ "z": 0.0
+ }
+ },
+ {
+ "id": 7,
+ "position": {
+ "x": -8.309,
+ "y": 1.442,
+ "z": 1.474
+ },
+ "orientation": {
+ "w": 0.5,
+ "x": -0.5,
+ "y": -0.5,
+ "z": 0.5
+ }
+ },
+ {
+ "id": 8,
+ "position": {
+ "x": -8.309,
+ "y": 0.877,
+ "z": 1.474
+ },
+ "orientation": {
+ "w": 0.5,
+ "x": -0.5,
+ "y": -0.5,
+ "z": 0.5
+ }
+ },
+ {
+ "id": 9,
+ "position": {
+ "x": -7.915,
+ "y": -3.223,
+ "z": 1.361
+ },
+ "orientation": {
+ "w": 0.1830127,
+ "x": -0.1830127,
+ "y": -0.6830127,
+ "z": 0.6830127
+ }
+ },
+ {
+ "id": 10,
+ "position": {
+ "x": -6.809,
+ "y": -3.860,
+ "z": 1.361
+ },
+ "orientation": {
+ "w": 0.1830127,
+ "x": -0.1830127,
+ "y": -0.6830127,
+ "z": 0.6830127
+ }
+ },
+ {
+ "id": 11,
+ "position": {
+ "x": 3.629,
+ "y": -0.393,
+ "z": 1.326
+ },
+ "orientation": {
+ "w": 0.6830127,
+ "x": -0.6830127,
+ "y": -0.1830127,
+ "z": 0.1830127
+ }
+ },
+ {
+ "id": 12,
+ "position": {
+ "x": 3.630,
+ "y": 0.392,
+ "z": 1.326
+ },
+ "orientation": {
+ "w": 0.1830127,
+ "x": -0.1830127,
+ "y": -0.6830127,
+ "z": 0.6830127
+ }
+ },
+ {
+ "id": 13,
+ "position": {
+ "x": 2.949,
+ "y": -0.000,
+ "z": 1.326
+ },
+ "orientation": {
+ "w": 0.5,
+ "x": -0.5,
+ "y": 0.5,
+ "z": -0.5
+ }
+ },
+ {
+ "id": 14,
+ "position": {
+ "x": -2.949,
+ "y": -0.000,
+ "z": 1.326
+ },
+ "orientation": {
+ "w": 0.5,
+ "x": -0.5,
+ "y": -0.5,
+ "z": 0.5
+ }
+ },
+ {
+ "id": 15,
+ "position": {
+ "x": -3.629,
+ "y": 0.393,
+ "z": 1.326
+ },
+ "orientation": {
+ "w": 0.1830127,
+ "x": -0.1830127,
+ "y": 0.6830127,
+ "z": -0.6830127
+ }
+ },
+ {
+ "id": 16,
+ "position": {
+ "x": -3.630,
+ "y": -0.392,
+ "z": 1.326
+ },
+ "orientation": {
+ "w": 0.6830127,
+ "x": -0.6830127,
+ "y": 0.1830127,
+ "z": -0.1830127
+ }
+ }
+ ]
+}
diff --git a/y2024/vision/rename_calibration_file.sh b/y2024_swerve/vision/rename_calibration_file.sh
similarity index 100%
rename from y2024/vision/rename_calibration_file.sh
rename to y2024_swerve/vision/rename_calibration_file.sh
diff --git a/y2024_swerve/vision/target_mapping.cc b/y2024_swerve/vision/target_mapping.cc
new file mode 100644
index 0000000..c540654
--- /dev/null
+++ b/y2024_swerve/vision/target_mapping.cc
@@ -0,0 +1,485 @@
+#include <string>
+
+#include "Eigen/Dense"
+#include "opencv2/aruco.hpp"
+#include "opencv2/calib3d.hpp"
+#include "opencv2/core/eigen.hpp"
+#include "opencv2/features2d.hpp"
+#include "opencv2/highgui.hpp"
+#include "opencv2/highgui/highgui.hpp"
+#include "opencv2/imgproc.hpp"
+
+#include "aos/configuration.h"
+#include "aos/events/logging/log_reader.h"
+#include "aos/events/simulated_event_loop.h"
+#include "aos/init.h"
+#include "aos/util/mcap_logger.h"
+#include "frc971/constants/constants_sender_lib.h"
+#include "frc971/control_loops/pose.h"
+#include "frc971/vision/calibration_generated.h"
+#include "frc971/vision/charuco_lib.h"
+#include "frc971/vision/target_mapper.h"
+#include "frc971/vision/vision_generated.h"
+#include "frc971/vision/vision_util_lib.h"
+#include "frc971/vision/visualize_robot.h"
+#include "y2024_swerve/constants/simulated_constants_sender.h"
+#include "y2024_swerve/vision/vision_util.h"
+
+DEFINE_string(config, "",
+ "If set, override the log's config file with this one.");
+DEFINE_string(constants_path, "y2024_swerve/constants/constants.json",
+ "Path to the constant file");
+DEFINE_string(dump_constraints_to, "/tmp/mapping_constraints.txt",
+ "Write the target constraints to this path");
+DEFINE_string(dump_stats_to, "/tmp/mapping_stats.txt",
+ "Write the mapping stats to this path");
+DEFINE_string(field_name, "crescendo",
+ "Field name, for the output json filename and flatbuffer field");
+DEFINE_string(json_path, "y2024_swerve/vision/maps/target_map.json",
+ "Specify path for json with initial pose guesses.");
+DEFINE_double(max_pose_error, 1e-6,
+ "Throw out target poses with a higher pose error than this");
+DEFINE_double(
+ max_pose_error_ratio, 0.4,
+ "Throw out target poses with a higher pose error ratio than this");
+DEFINE_string(mcap_output_path, "", "Log to output.");
+DEFINE_string(output_dir, "y2024_swerve/vision/maps",
+ "Directory to write solved target map to");
+DEFINE_double(pause_on_distance, 2.0,
+ "Pause if two consecutive implied robot positions differ by more "
+ "than this many meters");
+DEFINE_string(orin, "orin1",
+ "Orin name to generate mcap log for; defaults to orin1.");
+DEFINE_uint64(skip_to, 1,
+ "Start at combined image of this number (1 is the first image)");
+DEFINE_bool(solve, true, "Whether to solve for the field's target map.");
+DEFINE_bool(split_field, false,
+ "Whether to break solve into two sides of field");
+DEFINE_int32(team_number, 0,
+ "Required: Use the calibration for a node with this team number");
+DEFINE_uint64(wait_key, 1,
+ "Time in ms to wait between images, if no click (0 to wait "
+ "indefinitely until click).");
+
+DECLARE_int32(frozen_target_id);
+DECLARE_int32(min_target_id);
+DECLARE_int32(max_target_id);
+DECLARE_bool(visualize_solver);
+
+namespace y2024_swerve::vision {
+using frc971::vision::DataAdapter;
+using frc971::vision::ImageCallback;
+using frc971::vision::PoseUtils;
+using frc971::vision::TargetMap;
+using frc971::vision::TargetMapper;
+using frc971::vision::VisualizeRobot;
+namespace calibration = frc971::vision::calibration;
+
+// Class to handle reading target poses from a replayed log,
+// displaying various debug info, and passing the poses to
+// frc971::vision::TargetMapper for field mapping.
+class TargetMapperReplay {
+ public:
+ TargetMapperReplay(aos::logger::LogReader *reader);
+
+ // Solves for the target poses with the accumulated detections if FLAGS_solve.
+ void MaybeSolve();
+
+ private:
+ static constexpr int kImageWidth = 1280;
+
+ // Contains fixed target poses without solving, for use with visualization
+ static const TargetMapper kFixedTargetMapper;
+
+ // Map of TargetId to alliance "color" for splitting field
+ static std::map<uint, std::string> kIdAllianceMap;
+
+ // Change reference frame from camera to robot
+ static Eigen::Affine3d CameraToRobotDetection(Eigen::Affine3d H_camera_target,
+ Eigen::Affine3d extrinsics);
+
+ // Adds april tag detections into the detection list, and handles
+ // visualization
+ void HandleAprilTags(const TargetMap &map,
+ aos::distributed_clock::time_point node_distributed_time,
+ std::string camera_name, Eigen::Affine3d extrinsics);
+ // Gets images from the given pi and passes apriltag positions to
+ // HandleAprilTags()
+ void HandleNodeCaptures(
+ aos::EventLoop *mapping_event_loop,
+ frc971::constants::ConstantsFetcher<y2024_swerve::Constants>
+ *constants_fetcher,
+ int camera_number);
+
+ aos::logger::LogReader *reader_;
+ // April tag detections from all pis
+ std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections_;
+
+ VisualizeRobot vis_robot_;
+ // Set of camera names which are currently drawn on the display
+ std::set<std::string> drawn_cameras_;
+ // Number of frames displayed
+ size_t display_count_;
+ // Last time we drew onto the display image.
+ // This is different from when we actually call imshow() to update
+ // the display window
+ aos::distributed_clock::time_point last_draw_time_;
+
+ Eigen::Affine3d last_H_world_robot_;
+ // Maximum distance between consecutive T_world_robot's in one display frame,
+ // used to determine if we need to pause for the user to see this frame
+ // clearly
+ double max_delta_T_world_robot_;
+ double ignore_count_;
+
+ std::vector<std::unique_ptr<aos::EventLoop>> mapping_event_loops_;
+
+ std::unique_ptr<aos::EventLoop> mcap_event_loop_;
+ std::unique_ptr<aos::McapLogger> relogger_;
+};
+
+std::vector<CameraNode> node_list(y2024_swerve::vision::CreateNodeList());
+
+std::map<std::string, int> camera_ordering_map(
+ y2024_swerve::vision::CreateOrderingMap(node_list));
+
+std::map<uint, std::string> TargetMapperReplay::kIdAllianceMap = {
+ {1, "red"}, {2, "red"}, {3, "red"}, {4, "red"},
+ {5, "red"}, {6, "blue"}, {7, "blue"}, {8, "blue"},
+ {9, "blue"}, {10, "blue"}, {11, "red"}, {12, "red"},
+ {13, "red"}, {14, "blue"}, {15, "blue"}, {16, "blue"}};
+
+const auto TargetMapperReplay::kFixedTargetMapper =
+ TargetMapper(FLAGS_json_path, ceres::examples::VectorOfConstraints{});
+
+Eigen::Affine3d TargetMapperReplay::CameraToRobotDetection(
+ Eigen::Affine3d H_camera_target, Eigen::Affine3d extrinsics) {
+ const Eigen::Affine3d H_robot_camera = extrinsics;
+ const Eigen::Affine3d H_robot_target = H_robot_camera * H_camera_target;
+ return H_robot_target;
+}
+
+TargetMapperReplay::TargetMapperReplay(aos::logger::LogReader *reader)
+ : reader_(reader),
+ timestamped_target_detections_(),
+ vis_robot_(cv::Size(1280, 1000)),
+ drawn_cameras_(),
+ display_count_(0),
+ last_draw_time_(aos::distributed_clock::min_time),
+ last_H_world_robot_(Eigen::Matrix4d::Identity()),
+ max_delta_T_world_robot_(0.0) {
+ reader_->RemapLoggedChannel("/orin1/constants", "y2024_swerve.Constants");
+ reader_->RemapLoggedChannel("/imu/constants", "y2024_swerve.Constants");
+ // If it's Box of Orins, don't remap roborio constants
+ reader_->MaybeRemapLoggedChannel<Constants>("/roborio/constants");
+ reader_->Register();
+
+ SendSimulationConstants(reader_->event_loop_factory(), FLAGS_team_number,
+ FLAGS_constants_path);
+
+ if (FLAGS_visualize_solver) {
+ vis_robot_.ClearImage();
+ // Set focal length to zoomed in, to view extrinsics
+ const double kFocalLength = 1500.0;
+ vis_robot_.SetDefaultViewpoint(kImageWidth, kFocalLength);
+ }
+
+ for (const CameraNode &camera_node : node_list) {
+ const aos::Node *node = aos::configuration::GetNode(
+ reader_->configuration(), camera_node.node_name.c_str());
+
+ mapping_event_loops_.emplace_back(
+ reader_->event_loop_factory()->MakeEventLoop(
+ camera_node.node_name + "mapping", node));
+
+ frc971::constants::ConstantsFetcher<y2024_swerve::Constants>
+ constants_fetcher(
+ mapping_event_loops_[mapping_event_loops_.size() - 1].get());
+ HandleNodeCaptures(
+ mapping_event_loops_[mapping_event_loops_.size() - 1].get(),
+ &constants_fetcher, camera_node.camera_number);
+
+ if (FLAGS_visualize_solver) {
+ // Show the extrinsics calibration to start, for reference to confirm
+ const auto *calibration = FindCameraCalibration(
+ constants_fetcher.constants(),
+ mapping_event_loops_.back()->node()->name()->string_view(),
+ camera_node.camera_number);
+ cv::Mat extrinsics_cv =
+ frc971::vision::CameraExtrinsics(calibration).value();
+ Eigen::Matrix4d extrinsics_matrix;
+ cv::cv2eigen(extrinsics_cv, extrinsics_matrix);
+ const auto extrinsics = Eigen::Affine3d(extrinsics_matrix);
+
+ vis_robot_.DrawRobotOutline(extrinsics, camera_node.camera_name(),
+ kOrinColors.at(camera_node.camera_name()));
+ }
+ }
+
+ if (FLAGS_visualize_solver) {
+ cv::imshow("Extrinsics", vis_robot_.image_);
+ cv::waitKey(0);
+ vis_robot_.ClearImage();
+ // Reset focal length to more zoomed out view for field
+ const double kFocalLength = 500.0;
+ vis_robot_.SetDefaultViewpoint(kImageWidth, kFocalLength);
+ }
+}
+
+// Add detected apriltag poses relative to the robot to
+// timestamped_target_detections
+void TargetMapperReplay::HandleAprilTags(
+ const TargetMap &map,
+ aos::distributed_clock::time_point node_distributed_time,
+ std::string camera_name, Eigen::Affine3d extrinsics) {
+ bool drew = false;
+ std::stringstream label;
+ label << camera_name << " - ";
+
+ if (map.target_poses()->size() == 0) {
+ VLOG(2) << "Got 0 AprilTags for camera " << camera_name;
+ return;
+ }
+
+ for (const auto *target_pose_fbs : *map.target_poses()) {
+ // Skip detections with invalid ids
+ if (static_cast<TargetMapper::TargetId>(target_pose_fbs->id()) <
+ FLAGS_min_target_id ||
+ static_cast<TargetMapper::TargetId>(target_pose_fbs->id()) >
+ FLAGS_max_target_id) {
+ VLOG(1) << "Skipping tag with invalid id of " << target_pose_fbs->id();
+ continue;
+ }
+
+ // Skip detections with high pose errors
+ if (target_pose_fbs->pose_error() > FLAGS_max_pose_error) {
+ VLOG(1) << "Skipping tag " << target_pose_fbs->id()
+ << " due to pose error of " << target_pose_fbs->pose_error();
+ continue;
+ }
+ // Skip detections with high pose error ratios
+ if (target_pose_fbs->pose_error_ratio() > FLAGS_max_pose_error_ratio) {
+ VLOG(1) << "Skipping tag " << target_pose_fbs->id()
+ << " due to pose error ratio of "
+ << target_pose_fbs->pose_error_ratio();
+ continue;
+ }
+
+ const TargetMapper::TargetPose target_pose =
+ PoseUtils::TargetPoseFromFbs(*target_pose_fbs);
+
+ Eigen::Affine3d H_camera_target =
+ Eigen::Translation3d(target_pose.pose.p) * target_pose.pose.q;
+ Eigen::Affine3d H_robot_target =
+ CameraToRobotDetection(H_camera_target, extrinsics);
+
+ ceres::examples::Pose3d target_pose_camera =
+ PoseUtils::Affine3dToPose3d(H_camera_target);
+ double distance_from_camera = target_pose_camera.p.norm();
+ double distortion_factor = target_pose_fbs->distortion_factor();
+
+ double distance_threshold = 5.0;
+ if (distance_from_camera > distance_threshold) {
+ ignore_count_++;
+ LOG(INFO) << "Ignored " << ignore_count_ << " AprilTags with distance "
+ << distance_from_camera << " > " << distance_threshold;
+ continue;
+ }
+
+ CHECK(map.has_monotonic_timestamp_ns())
+ << "Need detection timestamps for mapping";
+
+ // Detection is usable, so store it
+ timestamped_target_detections_.emplace_back(
+ DataAdapter::TimestampedDetection{
+ .time = node_distributed_time,
+ .H_robot_target = H_robot_target,
+ .distance_from_camera = distance_from_camera,
+ .distortion_factor = distortion_factor,
+ .id = static_cast<TargetMapper::TargetId>(target_pose.id)});
+
+ if (FLAGS_visualize_solver) {
+ // If we've already drawn this camera_name in the current image,
+ // display the image before clearing and adding the new poses
+ if (drawn_cameras_.count(camera_name) != 0) {
+ display_count_++;
+ cv::putText(vis_robot_.image_,
+ "Poses #" + std::to_string(display_count_),
+ cv::Point(600, 10), cv::FONT_HERSHEY_PLAIN, 1.0,
+ cv::Scalar(255, 255, 255));
+
+ if (display_count_ >= FLAGS_skip_to) {
+ VLOG(1) << "Showing image for camera " << camera_name
+ << " since we've drawn it already";
+ cv::imshow("View", vis_robot_.image_);
+ // Pause if delta_T is too large, but only after first image (to make
+ // sure the delta's are correct)
+ if (max_delta_T_world_robot_ > FLAGS_pause_on_distance &&
+ display_count_ > 1) {
+ LOG(INFO) << "Pausing since the delta between robot estimates is "
+ << max_delta_T_world_robot_ << " which is > threshold of "
+ << FLAGS_pause_on_distance;
+ cv::waitKey(0);
+ } else {
+ cv::waitKey(FLAGS_wait_key);
+ }
+ max_delta_T_world_robot_ = 0.0;
+ } else {
+ VLOG(2) << "At poses #" << std::to_string(display_count_);
+ }
+ vis_robot_.ClearImage();
+ drawn_cameras_.clear();
+ }
+
+ Eigen::Affine3d H_world_target = PoseUtils::Pose3dToAffine3d(
+ kFixedTargetMapper.GetTargetPoseById(target_pose_fbs->id())->pose);
+ Eigen::Affine3d H_world_robot = H_world_target * H_robot_target.inverse();
+ VLOG(2) << camera_name << ", id " << target_pose_fbs->id()
+ << ", t = " << node_distributed_time
+ << ", pose_error = " << target_pose_fbs->pose_error()
+ << ", pose_error_ratio = " << target_pose_fbs->pose_error_ratio()
+ << ", robot_pos (x,y,z) = "
+ << H_world_robot.translation().transpose();
+
+ label << "id " << target_pose_fbs->id()
+ << ": err (% of max): " << target_pose_fbs->pose_error() << " ("
+ << (target_pose_fbs->pose_error() / FLAGS_max_pose_error)
+ << ") err_ratio: " << target_pose_fbs->pose_error_ratio() << " ";
+
+ vis_robot_.DrawRobotOutline(H_world_robot, camera_name,
+ kOrinColors.at(camera_name));
+ vis_robot_.DrawFrameAxes(H_world_target,
+ std::to_string(target_pose_fbs->id()),
+ kOrinColors.at(camera_name));
+
+ double delta_T_world_robot =
+ (H_world_robot.translation() - last_H_world_robot_.translation())
+ .norm();
+ max_delta_T_world_robot_ =
+ std::max(delta_T_world_robot, max_delta_T_world_robot_);
+
+ VLOG(1) << "Drew in info for camera " << camera_name << " and target #"
+ << target_pose_fbs->id();
+ drew = true;
+ last_draw_time_ = node_distributed_time;
+ last_H_world_robot_ = H_world_robot;
+ }
+ }
+ if (FLAGS_visualize_solver) {
+ if (drew) {
+ // Collect all the labels from a given camera, and add the text
+ // TODO: Need to fix this one
+ int position_number = camera_ordering_map[camera_name];
+ cv::putText(vis_robot_.image_, label.str(),
+ cv::Point(10, 30 + 20 * position_number),
+ cv::FONT_HERSHEY_PLAIN, 1.0, kOrinColors.at(camera_name));
+ drawn_cameras_.emplace(camera_name);
+ } else if (node_distributed_time - last_draw_time_ >
+ std::chrono::milliseconds(30) &&
+ display_count_ >= FLAGS_skip_to && drew) {
+ // TODO: Check on 30ms value-- does this make sense?
+ double delta_t = (node_distributed_time - last_draw_time_).count() / 1e6;
+ VLOG(1) << "Last result was " << delta_t << "ms ago";
+ cv::putText(vis_robot_.image_, "No detections in last 30ms",
+ cv::Point(10, 0), cv::FONT_HERSHEY_PLAIN, 1.0,
+ kOrinColors.at(camera_name));
+ // Display and clear the image if we haven't draw in a while
+ VLOG(1) << "Displaying image due to time lapse";
+ cv::imshow("View", vis_robot_.image_);
+ cv::waitKey(FLAGS_wait_key);
+ max_delta_T_world_robot_ = 0.0;
+ drawn_cameras_.clear();
+ }
+ }
+}
+
+void TargetMapperReplay::HandleNodeCaptures(
+ aos::EventLoop *mapping_event_loop,
+ frc971::constants::ConstantsFetcher<y2024_swerve::Constants>
+ *constants_fetcher,
+ int camera_number) {
+ // Get the camera extrinsics
+ std::string node_name =
+ std::string(mapping_event_loop->node()->name()->string_view());
+ const auto *calibration = FindCameraCalibration(
+ constants_fetcher->constants(), node_name, camera_number);
+ cv::Mat extrinsics_cv = frc971::vision::CameraExtrinsics(calibration).value();
+ Eigen::Matrix4d extrinsics_matrix;
+ cv::cv2eigen(extrinsics_cv, extrinsics_matrix);
+ const auto extrinsics = Eigen::Affine3d(extrinsics_matrix);
+ std::string camera_name = absl::StrFormat(
+ "/%s/camera%d", mapping_event_loop->node()->name()->str(), camera_number);
+
+ mapping_event_loop->MakeWatcher(
+ camera_name.c_str(), [this, mapping_event_loop, extrinsics,
+ camera_name](const TargetMap &map) {
+ aos::distributed_clock::time_point node_distributed_time =
+ reader_->event_loop_factory()
+ ->GetNodeEventLoopFactory(mapping_event_loop->node())
+ ->ToDistributedClock(aos::monotonic_clock::time_point(
+ aos::monotonic_clock::duration(
+ map.monotonic_timestamp_ns())));
+
+ HandleAprilTags(map, node_distributed_time, camera_name, extrinsics);
+ });
+}
+
+void TargetMapperReplay::MaybeSolve() {
+ if (FLAGS_solve) {
+ auto target_constraints =
+ DataAdapter::MatchTargetDetections(timestamped_target_detections_);
+
+ if (FLAGS_split_field) {
+ // Remove constraints between the two sides of the field - these are
+ // basically garbage because of how far the camera is. We will use seeding
+ // below to connect the two sides
+ target_constraints.erase(
+ std::remove_if(
+ target_constraints.begin(), target_constraints.end(),
+ [](const auto &constraint) {
+ return (
+ kIdAllianceMap[static_cast<uint>(constraint.id_begin)] !=
+ kIdAllianceMap[static_cast<uint>(constraint.id_end)]);
+ }),
+ target_constraints.end());
+ }
+
+ LOG(INFO) << "Solving for locations of tags with "
+ << target_constraints.size() << " constraints";
+ TargetMapper mapper(FLAGS_json_path, target_constraints);
+ mapper.Solve(FLAGS_field_name, FLAGS_output_dir);
+
+ if (!FLAGS_dump_constraints_to.empty()) {
+ mapper.DumpConstraints(FLAGS_dump_constraints_to);
+ }
+ if (!FLAGS_dump_stats_to.empty()) {
+ mapper.DumpStats(FLAGS_dump_stats_to);
+ }
+ mapper.PrintDiffs();
+ }
+}
+
+void MappingMain(int argc, char *argv[]) {
+ std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections;
+
+ std::optional<aos::FlatbufferDetachedBuffer<aos::Configuration>> config =
+ (FLAGS_config.empty()
+ ? std::nullopt
+ : std::make_optional(aos::configuration::ReadConfig(FLAGS_config)));
+
+ // Open logfiles
+ aos::logger::LogReader reader(
+ aos::logger::SortParts(aos::logger::FindLogs(argc, argv)),
+ config.has_value() ? &config->message() : nullptr);
+
+ TargetMapperReplay mapper_replay(&reader);
+ reader.event_loop_factory()->Run();
+ mapper_replay.MaybeSolve();
+}
+
+} // namespace y2024_swerve::vision
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ y2024_swerve::vision::MappingMain(argc, argv);
+}
diff --git a/y2024_swerve/vision/viewer.cc b/y2024_swerve/vision/viewer.cc
new file mode 100644
index 0000000..143868d
--- /dev/null
+++ b/y2024_swerve/vision/viewer.cc
@@ -0,0 +1,122 @@
+#include "absl/strings/match.h"
+#include "opencv2/calib3d.hpp"
+#include "opencv2/highgui/highgui.hpp"
+#include "opencv2/imgproc.hpp"
+
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/time/time.h"
+#include "frc971/constants/constants_sender_lib.h"
+#include "frc971/vision/vision_generated.h"
+#include "frc971/vision/vision_util_lib.h"
+#include "y2024_swerve/vision/vision_util.h"
+
+DEFINE_string(capture, "",
+ "If set, capture a single image and save it to this filename.");
+DEFINE_string(channel, "/camera", "Channel name for the image.");
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+DEFINE_int32(rate, 100, "Time in milliseconds to wait between images");
+DEFINE_double(scale, 1.0, "Scale factor for images being displayed");
+
+namespace y2024_swerve::vision {
+namespace {
+
+using frc971::vision::CameraImage;
+
+bool DisplayLoop(const cv::Mat intrinsics, const cv::Mat dist_coeffs,
+ aos::Fetcher<CameraImage> *image_fetcher) {
+ const CameraImage *image;
+
+ // Read next image
+ if (!image_fetcher->Fetch()) {
+ VLOG(2) << "Couldn't fetch next image";
+ return true;
+ }
+ image = image_fetcher->get();
+ CHECK(image != nullptr) << "Couldn't read image";
+
+ // Create color image:
+ cv::Mat image_color_mat(cv::Size(image->cols(), image->rows()), CV_8UC2,
+ (void *)image->data()->data());
+ cv::Mat bgr_image(cv::Size(image->cols(), image->rows()), CV_8UC3);
+ cv::cvtColor(image_color_mat, bgr_image, cv::COLOR_YUV2BGR_YUYV);
+
+ if (!FLAGS_capture.empty()) {
+ if (absl::EndsWith(FLAGS_capture, ".bfbs")) {
+ aos::WriteFlatbufferToFile(FLAGS_capture,
+ image_fetcher->CopyFlatBuffer());
+ } else {
+ cv::imwrite(FLAGS_capture, bgr_image);
+ }
+
+ return false;
+ }
+
+ cv::Mat undistorted_image;
+ cv::undistort(bgr_image, undistorted_image, intrinsics, dist_coeffs);
+ if (FLAGS_scale != 1.0) {
+ cv::resize(undistorted_image, undistorted_image, cv::Size(), FLAGS_scale,
+ FLAGS_scale);
+ }
+ cv::imshow("Display", undistorted_image);
+
+ int keystroke = cv::waitKey(1);
+ if ((keystroke & 0xFF) == static_cast<int>('c')) {
+ // Convert again, to get clean image
+ cv::cvtColor(image_color_mat, bgr_image, cv::COLOR_YUV2BGR_YUYV);
+ std::stringstream name;
+ name << "capture-" << aos::realtime_clock::now() << ".png";
+ cv::imwrite(name.str(), bgr_image);
+ LOG(INFO) << "Saved image file: " << name.str();
+ } else if ((keystroke & 0xFF) == static_cast<int>('q')) {
+ return false;
+ }
+ return true;
+}
+
+void ViewerMain() {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ frc971::constants::WaitForConstants<y2024_swerve::Constants>(
+ &config.message());
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ frc971::constants::ConstantsFetcher<y2024_swerve::Constants>
+ constants_fetcher(&event_loop);
+ CHECK(FLAGS_channel.length() == 8);
+ int camera_id = std::stoi(FLAGS_channel.substr(7, 1));
+ const auto *calibration_data = FindCameraCalibration(
+ constants_fetcher.constants(), event_loop.node()->name()->string_view(),
+ camera_id);
+ const cv::Mat intrinsics = frc971::vision::CameraIntrinsics(calibration_data);
+ const cv::Mat dist_coeffs =
+ frc971::vision::CameraDistCoeffs(calibration_data);
+
+ aos::Fetcher<CameraImage> image_fetcher =
+ event_loop.MakeFetcher<CameraImage>(FLAGS_channel);
+
+ // Run the display loop
+ event_loop.AddPhasedLoop(
+ [&](int) {
+ if (!DisplayLoop(intrinsics, dist_coeffs, &image_fetcher)) {
+ LOG(INFO) << "Calling event_loop Exit";
+ event_loop.Exit();
+ };
+ },
+ ::std::chrono::milliseconds(FLAGS_rate));
+
+ event_loop.Run();
+
+ image_fetcher = aos::Fetcher<CameraImage>();
+}
+
+} // namespace
+} // namespace y2024_swerve::vision
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ y2024_swerve::vision::ViewerMain();
+}
diff --git a/y2024_swerve/vision/vision_util.cc b/y2024_swerve/vision/vision_util.cc
new file mode 100644
index 0000000..e08bfaa
--- /dev/null
+++ b/y2024_swerve/vision/vision_util.cc
@@ -0,0 +1,48 @@
+#include "y2024_swerve/vision/vision_util.h"
+
+#include "glog/logging.h"
+
+namespace y2024_swerve::vision {
+
+// Store a list of ordered cameras as you progress around the robot/box of orins
+std::vector<CameraNode> CreateNodeList() {
+ std::vector<CameraNode> list;
+
+ list.push_back({.node_name = "imu", .camera_number = 0});
+ list.push_back({.node_name = "imu", .camera_number = 1});
+ list.push_back({.node_name = "orin1", .camera_number = 1});
+ list.push_back({.node_name = "orin1", .camera_number = 0});
+
+ return list;
+}
+
+// From the node_list, create a numbering scheme from 0 to 3
+std::map<std::string, int> CreateOrderingMap(
+ std::vector<CameraNode> &node_list) {
+ std::map<std::string, int> map;
+
+ for (uint i = 0; i < node_list.size(); i++) {
+ map.insert({node_list.at(i).camera_name(), i});
+ }
+
+ return map;
+}
+
+const frc971::vision::calibration::CameraCalibration *FindCameraCalibration(
+ const y2024_swerve::Constants &calibration_data, std::string_view node_name,
+ int camera_number) {
+ CHECK(calibration_data.robot()->has_cameras());
+ for (const y2024_swerve::CameraConfiguration *candidate :
+ *calibration_data.robot()->cameras()) {
+ CHECK(candidate->has_calibration());
+ if (candidate->calibration()->node_name()->string_view() != node_name ||
+ candidate->calibration()->camera_number() != camera_number) {
+ continue;
+ }
+ return candidate->calibration();
+ }
+ LOG(FATAL) << ": Failed to find camera calibration for " << node_name
+ << " and camera number " << camera_number;
+}
+
+} // namespace y2024_swerve::vision
diff --git a/y2024_swerve/vision/vision_util.h b/y2024_swerve/vision/vision_util.h
new file mode 100644
index 0000000..6fed3c8
--- /dev/null
+++ b/y2024_swerve/vision/vision_util.h
@@ -0,0 +1,41 @@
+#ifndef Y2024_SWERVE_VISION_VISION_UTIL_H_
+#define Y2024_SWERVE_VISION_VISION_UTIL_H_
+#include <map>
+#include <string_view>
+
+#include "opencv2/imgproc.hpp"
+
+#include "y2024_swerve/constants/constants_generated.h"
+
+namespace y2024_swerve::vision {
+
+// Generate unique colors for each camera
+const auto kOrinColors = std::map<std::string, cv::Scalar>{
+ {"/orin1/camera0", cv::Scalar(255, 0, 255)},
+ {"/orin1/camera1", cv::Scalar(255, 255, 0)},
+ {"/imu/camera0", cv::Scalar(0, 255, 255)},
+ {"/imu/camera1", cv::Scalar(255, 165, 0)},
+};
+
+// Structure to store node name (e.g., orin1, imu), number, and a usable string
+struct CameraNode {
+ std::string node_name;
+ int camera_number;
+
+ inline const std::string camera_name() const {
+ return "/" + node_name + "/camera" + std::to_string(camera_number);
+ }
+};
+
+std::vector<CameraNode> CreateNodeList();
+
+std::map<std::string, int> CreateOrderingMap(
+ std::vector<CameraNode> &node_list);
+
+const frc971::vision::calibration::CameraCalibration *FindCameraCalibration(
+ const y2024_swerve::Constants &calibration_data, std::string_view node_name,
+ int camera_number);
+
+} // namespace y2024_swerve::vision
+
+#endif // y2024_SWERVE_VISION_VISION_UTIL_H_