Create y2024_bot3 folder
Change-Id: Iee232bde58c0425920a49eee145a0f93c7485391
Signed-off-by: Yash Maheshwari <yashmahe2018@gmail.com>
Signed-off-by: James (Peilun) Li <jamespeilunli@gmail.com>
diff --git a/y2024_bot3/constants/9971.json b/y2024_bot3/constants/9971.json
new file mode 100644
index 0000000..5583b44
--- /dev/null
+++ b/y2024_bot3/constants/9971.json
@@ -0,0 +1,18 @@
+{
+ "cameras": [
+ {
+ "calibration": {% include 'y2024_bot3/constants/calib_files/calibration_imu-9971-0_cam-24-10_2024-02-24_16-44-05.json' %}
+ },
+ {
+ "calibration": {% include 'y2024_bot3/constants/calib_files/calibration_imu-9971-1_cam-24-12_2024-03-24_11-52-49.json' %}
+ },
+ {
+ "calibration": {% include 'y2024_bot3/constants/calib_files/calibration_orin1-9971-0_cam-24-09_2024-03-24_11-52-49.json' %}
+ },
+ {
+ "calibration": {% include 'y2024_bot3/constants/calib_files/calibration_orin1-9971-1_cam-24-11_2024-03-24_11-52-49.json' %}
+ }
+ ],
+ "robot": {},
+ {% include 'y2024_bot3/constants/common.json' %}
+}
diff --git a/y2024_bot3/constants/BUILD b/y2024_bot3/constants/BUILD
new file mode 100644
index 0000000..1ce69a0
--- /dev/null
+++ b/y2024_bot3/constants/BUILD
@@ -0,0 +1,106 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
+load("//tools/build_rules:template.bzl", "jinja2_template")
+load("//y2024_bot3/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_bot3:__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_bot3/constants/calib_files",
+ "//y2024_bot3/vision/maps",
+ ],
+ parameters = {},
+ visibility = ["//visibility:public"],
+)
+
+jinja2_template(
+ name = "constants_unvalidated.json",
+ src = "constants.jinja2.json",
+ includes = [
+ "9971.json",
+ "common.jinja2",
+ "common.json",
+ "//y2024_bot3/constants/calib_files",
+ "//y2024_bot3/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/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_google_absl//absl/log",
+ "@com_google_absl//absl/log:check",
+ ],
+)
+
+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_bot3/constants/calib_files/BUILD b/y2024_bot3/constants/calib_files/BUILD
new file mode 100644
index 0000000..1f33d50
--- /dev/null
+++ b/y2024_bot3/constants/calib_files/BUILD
@@ -0,0 +1,5 @@
+filegroup(
+ name = "calib_files",
+ srcs = glob(["*.json"]),
+ visibility = ["//visibility:public"],
+)
diff --git a/y2024_bot3/constants/calib_files/calibration_imu-9971-0_cam-24-10_2024-02-24_16-44-05.json b/y2024_bot3/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_bot3/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_bot3/constants/calib_files/calibration_imu-9971-1_cam-24-12_2024-03-24_11-52-49.json b/y2024_bot3/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_bot3/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_bot3/constants/calib_files/calibration_orin1-9971-0_cam-24-09_2024-03-24_11-52-49.json b/y2024_bot3/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_bot3/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_bot3/constants/calib_files/calibration_orin1-9971-1_cam-24-11_2024-03-24_11-52-49.json b/y2024_bot3/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_bot3/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_bot3/constants/common.jinja2 b/y2024_bot3/constants/common.jinja2
new file mode 100644
index 0000000..9b7af82
--- /dev/null
+++ b/y2024_bot3/constants/common.jinja2
@@ -0,0 +1,3 @@
+{% set pi = 3.14159265 %}
+
+{%set zeroing_sample_size = 200 %}
diff --git a/y2024_bot3/constants/common.json b/y2024_bot3/constants/common.json
new file mode 100644
index 0000000..e7b70b4
--- /dev/null
+++ b/y2024_bot3/constants/common.json
@@ -0,0 +1,2 @@
+"common": {
+}
diff --git a/y2024_bot3/constants/constants.fbs b/y2024_bot3/constants/constants.fbs
new file mode 100644
index 0000000..0c7a792
--- /dev/null
+++ b/y2024_bot3/constants/constants.fbs
@@ -0,0 +1,27 @@
+include "frc971/vision/calibration.fbs";
+include "frc971/vision/target_map.fbs";
+include "frc971/control_loops/profiled_subsystem.fbs";
+include "frc971/zeroing/constants.fbs";
+include "frc971/math/matrix.fbs";
+
+namespace y2024_bot3;
+
+table CameraConfiguration {
+ calibration:frc971.vision.calibration.CameraCalibration (id: 0);
+}
+
+table RobotConstants {
+}
+
+// Common table for constants unrelated to the robot
+table Common {
+ target_map:frc971.vision.TargetMap (id: 0);
+}
+
+table Constants {
+ cameras:[CameraConfiguration] (id: 0);
+ robot:RobotConstants (id: 1);
+ common:Common (id: 2);
+}
+
+root_type Constants;
diff --git a/y2024_bot3/constants/constants.jinja2.json b/y2024_bot3/constants/constants.jinja2.json
new file mode 100644
index 0000000..63fad42
--- /dev/null
+++ b/y2024_bot3/constants/constants.jinja2.json
@@ -0,0 +1,8 @@
+{
+ "constants": [
+ {
+ "team": 9971,
+ "data": {% include 'y2024_bot3/constants/9971.json' %}
+ }
+ ]
+}
diff --git a/y2024_bot3/constants/constants_formatter.cc b/y2024_bot3/constants/constants_formatter.cc
new file mode 100644
index 0000000..4b2a601
--- /dev/null
+++ b/y2024_bot3/constants/constants_formatter.cc
@@ -0,0 +1,27 @@
+#include "absl/log/check.h"
+#include "absl/log/log.h"
+
+#include "aos/flatbuffers.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/util/file.h"
+#include "y2024_bot3/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_bot3::ConstantsList> constants =
+ aos::JsonFileToFlatbuffer<y2024_bot3::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_bot3/constants/constants_list.fbs b/y2024_bot3/constants/constants_list.fbs
new file mode 100644
index 0000000..af2b49c
--- /dev/null
+++ b/y2024_bot3/constants/constants_list.fbs
@@ -0,0 +1,14 @@
+include "y2024_bot3/constants/constants.fbs";
+
+namespace y2024_bot3;
+
+table TeamAndConstants {
+ team:long (id: 0);
+ data:Constants (id: 1);
+}
+
+table ConstantsList {
+ constants:[TeamAndConstants] (id: 0);
+}
+
+root_type ConstantsList;
diff --git a/y2024_bot3/constants/constants_sender.cc b/y2024_bot3/constants/constants_sender.cc
new file mode 100644
index 0000000..d74f0d0
--- /dev/null
+++ b/y2024_bot3/constants/constants_sender.cc
@@ -0,0 +1,25 @@
+#include "absl/flags/flag.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_bot3/constants/constants_generated.h"
+#include "y2024_bot3/constants/constants_list_generated.h"
+
+ABSL_FLAG(std::string, config, "aos_config.json", "Path to the AOS config.");
+ABSL_FLAG(std::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(absl::GetFlag(FLAGS_config));
+ aos::ShmEventLoop event_loop(&config.message());
+ frc971::constants::ConstantSender<y2024_bot3::Constants,
+ y2024_bot3::ConstantsList>
+ constants_sender(&event_loop, absl::GetFlag(FLAGS_constants_path));
+ // Don't need to call Run().
+ return 0;
+}
diff --git a/y2024_bot3/constants/simulated_constants_sender.cc b/y2024_bot3/constants/simulated_constants_sender.cc
new file mode 100644
index 0000000..77d818a
--- /dev/null
+++ b/y2024_bot3/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_bot3/constants/constants_generated.h"
+#include "y2024_bot3/constants/constants_list_generated.h"
+
+namespace y2024_bot3 {
+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_bot3
diff --git a/y2024_bot3/constants/simulated_constants_sender.h b/y2024_bot3/constants/simulated_constants_sender.h
new file mode 100644
index 0000000..90b3724
--- /dev/null
+++ b/y2024_bot3/constants/simulated_constants_sender.h
@@ -0,0 +1,21 @@
+#ifndef Y2024_BOT3_CONSTANTS_SIMULATED_CONFIG_SENDER_H_
+#define Y2024_BOT3_CONSTANTS_SIMULATED_CONFIG_SENDER_H_
+
+#include <set>
+
+#include "aos/events/simulated_event_loop.h"
+#include "aos/testing/path.h"
+
+namespace y2024_bot3 {
+// 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_bot3/constants/test_constants.json"),
+ const std::set<std::string_view> &node_names = {});
+} // namespace y2024_bot3
+
+#endif // Y2024_BOT3_CONSTANTS_SIMULATED_CONFIG_SENDER_H_
diff --git a/y2024_bot3/constants/test_constants.jinja2.json b/y2024_bot3/constants/test_constants.jinja2.json
new file mode 100644
index 0000000..fdb764a
--- /dev/null
+++ b/y2024_bot3/constants/test_constants.jinja2.json
@@ -0,0 +1,8 @@
+{
+ "constants": [
+ {
+ "team": 9971,
+ "data": {% include 'y2024_bot3/constants/test_data/test_team.json' %}
+ }
+ ]
+}
diff --git a/y2024_bot3/constants/test_data/test_team.json b/y2024_bot3/constants/test_data/test_team.json
new file mode 100644
index 0000000..deacef4
--- /dev/null
+++ b/y2024_bot3/constants/test_data/test_team.json
@@ -0,0 +1,4 @@
+{
+ "robot": {},
+ {% include 'y2024_bot3/constants/common.json' %}
+}
diff --git a/y2024_bot3/constants/validator.bzl b/y2024_bot3/constants/validator.bzl
new file mode 100644
index 0000000..7224e13
--- /dev/null
+++ b/y2024_bot3/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_bot3/constants:constants_formatter",
+ srcs = [src],
+ outs = [out],
+ args = ["$(location %s)" % (src)] + ["$(location %s)" % (out)],
+ visibility = ["//visibility:public"],
+ )