Move y2023 vision code over to using constants sender
This removes the need for the generated calibration_data.h
Not tested on a pi yet, so I may've messed something up with the
deployment.
Change-Id: Ic46ba861db25033ac21f33f4898cf52afe02f1ab
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2023/BUILD b/y2023/BUILD
index eb265a4..e997d8c 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -9,13 +9,15 @@
"//y2023/vision:viewer",
"//y2023/vision:aprilrobotics",
"//y2022/localizer:localizer_main",
+ "//y2023/constants:constants_sender",
"//aos/network:web_proxy_main",
"//aos/events/logging:log_cat",
],
data = [
":aos_config",
":message_bridge_client.sh",
- "//y2022/www:www_files",
+ "//y2023/constants:constants.json",
+ "//y2023/www:www_files",
],
dirs = [
"//y2023/www:www_files",
@@ -65,6 +67,7 @@
"//aos/network:message_bridge_server_fbs",
"//aos/network:timestamp_fbs",
"//aos/network:remote_message_fbs",
+ "//y2023/constants:constants_fbs",
"//y2022/localizer:localizer_output_fbs",
"//frc971/vision:calibration_fbs",
"//frc971/vision:target_map_fbs",
@@ -93,6 +96,7 @@
flatbuffers = [
"//aos/network:message_bridge_client_fbs",
"//aos/network:message_bridge_server_fbs",
+ "//y2023/constants:constants_fbs",
"//aos/network:timestamp_fbs",
"//aos/network:remote_message_fbs",
"//y2022/localizer:localizer_status_fbs",
@@ -119,6 +123,7 @@
"//frc971/vision:calibration_fbs",
"//frc971/vision:vision_fbs",
"//frc971/vision:target_map_fbs",
+ "//y2023/constants:constants_fbs",
],
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
@@ -136,6 +141,7 @@
"//aos/network:remote_message_fbs",
"//aos/network:message_bridge_client_fbs",
"//aos/network:message_bridge_server_fbs",
+ "//y2023/constants:constants_fbs",
"//aos/network:timestamp_fbs",
"//y2019/control_loops/drivetrain:target_selector_fbs",
"//y2023/control_loops/superstructure:superstructure_goal_fbs",
diff --git a/y2023/constants/7971.json b/y2023/constants/7971.json
new file mode 100644
index 0000000..ded4869
--- /dev/null
+++ b/y2023/constants/7971.json
@@ -0,0 +1,16 @@
+{
+ "cameras": [
+ {
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-7971-1_2021-06-12_15-35-39.636386620.json' %}
+ },
+ {
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-7971-2_2021-06-12_15-30-20.325393444.json' %}
+ },
+ {
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-7971-3_2021-06-12_15-33-31.977365877.json' %}
+ },
+ {
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-7971-4_2021-06-12_15-37-25.706564865.json' %}
+ }
+ ]
+}
diff --git a/y2023/constants/BUILD b/y2023/constants/BUILD
new file mode 100644
index 0000000..360ecc2
--- /dev/null
+++ b/y2023/constants/BUILD
@@ -0,0 +1,54 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//tools/build_rules:template.bzl", "jinja2_template")
+
+cc_library(
+ name = "simulated_constants_sender",
+ testonly = True,
+ srcs = ["simulated_constants_sender.h"],
+ hdrs = ["simulated_constants_sender.cc"],
+ deps = [
+ ":constants_fbs",
+ ":constants_list_fbs",
+ "//aos/events:simulated_event_loop",
+ "//aos/testing:path",
+ "//frc971/constants:constants_sender_lib",
+ ],
+)
+
+jinja2_template(
+ name = "constants.json",
+ src = "constants.jinja2.json",
+ includes = ["//y2023/vision/calib_files"] + ["7971.json"],
+ parameters = {},
+ visibility = ["//visibility:public"],
+)
+
+flatbuffer_cc_library(
+ name = "constants_fbs",
+ srcs = ["constants.fbs"],
+ gen_reflections = True,
+ visibility = ["//visibility:public"],
+ deps = ["//frc971/vision:calibration_fbs"],
+)
+
+flatbuffer_cc_library(
+ name = "constants_list_fbs",
+ srcs = ["constants_list.fbs"],
+ gen_reflections = True,
+ 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",
+ ],
+)
diff --git a/y2023/constants/constants.fbs b/y2023/constants/constants.fbs
new file mode 100644
index 0000000..e62e253
--- /dev/null
+++ b/y2023/constants/constants.fbs
@@ -0,0 +1,13 @@
+include "frc971/vision/calibration.fbs";
+
+namespace y2023;
+
+table CameraConfiguration {
+ calibration:frc971.vision.calibration.CameraCalibration (id: 0);
+}
+
+table Constants {
+ cameras:[CameraConfiguration] (id: 0);
+}
+
+root_type Constants;
diff --git a/y2023/constants/constants.jinja2.json b/y2023/constants/constants.jinja2.json
new file mode 100644
index 0000000..6a8cde9
--- /dev/null
+++ b/y2023/constants/constants.jinja2.json
@@ -0,0 +1,16 @@
+{
+ "constants": [
+ {
+ "team": 7971,
+ "data": {% include 'y2023/constants/7971.json' %}
+ },
+ {
+ "team": 971,
+ "data": {}
+ },
+ {
+ "team": 9971,
+ "data": {}
+ }
+ ]
+}
diff --git a/y2023/constants/constants_list.fbs b/y2023/constants/constants_list.fbs
new file mode 100644
index 0000000..aec10d8
--- /dev/null
+++ b/y2023/constants/constants_list.fbs
@@ -0,0 +1,14 @@
+include "y2023/constants/constants.fbs";
+
+namespace y2023;
+
+table TeamAndConstants {
+ team:long (id: 0);
+ data:Constants (id: 1);
+}
+
+table ConstantsList {
+ constants:[TeamAndConstants] (id: 0);
+}
+
+root_type ConstantsList;
diff --git a/y2023/constants/constants_sender.cc b/y2023/constants/constants_sender.cc
new file mode 100644
index 0000000..77fb24d
--- /dev/null
+++ b/y2023/constants/constants_sender.cc
@@ -0,0 +1,23 @@
+#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 "gflags/gflags.h"
+#include "glog/logging.h"
+#include "y2023/constants/constants_generated.h"
+#include "y2023/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<y2023::Constants, y2023::ConstantsList>
+ constants_sender(&event_loop, FLAGS_constants_path);
+ // Don't need to call Run().
+ return 0;
+}
diff --git a/y2023/constants/simulated_constants_sender.cc b/y2023/constants/simulated_constants_sender.cc
new file mode 100644
index 0000000..3086e99
--- /dev/null
+++ b/y2023/constants/simulated_constants_sender.cc
@@ -0,0 +1,18 @@
+#include "y2023/constants/constants_generated.h"
+#include "y2023/constants/constants_list_generated.h"
+#include "aos/events/simulated_event_loop.h"
+#include "aos/testing/path.h"
+
+namespace y2023 {
+void SendSimulationConstants(aos::SimulatedEventLoopFactory *factory, int team,
+ std::string constants_path) {
+ for (const aos::Node *node : factory->nodes()) {
+ 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");
+ }
+}
+} // namespace y2023
+
+#endif // Y2023_CONFIGURATION_SIMULATED_CONFIG_SENDER_H_
diff --git a/y2023/constants/simulated_constants_sender.h b/y2023/constants/simulated_constants_sender.h
new file mode 100644
index 0000000..10ea793
--- /dev/null
+++ b/y2023/constants/simulated_constants_sender.h
@@ -0,0 +1,13 @@
+#ifndef Y2023_CONSTANTS_SIMULATED_CONFIG_SENDER_H_
+#define Y2023_CONSTANTS_SIMULATED_CONFIG_SENDER_H_
+
+#include "aos/events/simulated_event_loop.h"
+#include "aos/testing/path.h"
+
+namespace y2023 {
+void SendSimulationConstants(aos::SimulatedEventLoopFactory *factory, int team,
+ std::string constants_path = testing::ArtifactPath(
+ "y2023/constants/constants.json"));
+} // namespace y2023
+
+#endif // Y2023_CONSTANTS_SIMULATED_CONFIG_SENDER_H_
diff --git a/y2023/vision/BUILD b/y2023/vision/BUILD
index 96ea632..5c98d7c 100644
--- a/y2023/vision/BUILD
+++ b/y2023/vision/BUILD
@@ -1,48 +1,5 @@
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
-py_binary(
- name = "create_calib_file",
- srcs = [
- "create_calib_file.py",
- ],
- args = [
- "calibration_data.h",
- ],
- data = glob(["calib_files/*.json"]),
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//visibility:public"],
- deps = [
- "//frc971/vision:create_calib_file",
- ],
-)
-
-genrule(
- name = "run_calibration_data",
- outs = [
- "calibration_data.h",
- ],
- cmd = " ".join([
- "$(location :create_calib_file)",
- "$(location calibration_data.h)",
- ]),
- target_compatible_with = ["@platforms//os:linux"],
- tools = [
- ":create_calib_file",
- ],
-)
-
-cc_library(
- name = "calibration_data",
- hdrs = [
- "calibration_data.h",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//visibility:public"],
- deps = [
- "@com_google_absl//absl/types:span",
- ],
-)
-
cc_binary(
name = "camera_reader",
srcs = [
@@ -91,15 +48,26 @@
visibility = ["//y2023:__subpackages__"],
deps = [
":aprilrobotics_lib",
- ":calibration_data",
"//aos:init",
"//aos/events:simulated_event_loop",
"//aos/events/logging:log_reader",
+ "//frc971/constants:constants_sender_lib",
"//frc971/control_loops:pose",
"//frc971/vision:calibration_fbs",
"//frc971/vision:charuco_lib",
"//frc971/vision:target_mapper",
"//third_party:opencv",
+ "//y2023/constants:constants_fbs",
+ ],
+)
+
+cc_library(
+ name = "vision_util",
+ srcs = ["vision_util.cc"],
+ hdrs = ["vision_util.h"],
+ deps = [
+ "//y2023/constants:constants_fbs",
+ "@com_github_google_glog//:glog",
],
)
@@ -121,9 +89,10 @@
visibility = ["//y2023:__subpackages__"],
deps = [
":april_debug_fbs",
- ":calibration_data",
+ ":vision_util",
"//aos:init",
"//aos/events:shm_event_loop",
+ "//frc971/constants:constants_sender_lib",
"//frc971/vision:calibration_fbs",
"//frc971/vision:charuco_lib",
"//frc971/vision:target_map_fbs",
@@ -131,6 +100,7 @@
"//frc971/vision:vision_fbs",
"//third_party:opencv",
"//third_party/apriltag",
+ "//y2023/constants:constants_fbs",
],
)
diff --git a/y2023/vision/aprilrobotics.cc b/y2023/vision/aprilrobotics.cc
index 23c5ada..cb6a5bc 100644
--- a/y2023/vision/aprilrobotics.cc
+++ b/y2023/vision/aprilrobotics.cc
@@ -1,11 +1,11 @@
#include "y2023/vision/aprilrobotics.h"
+#include "y2023/vision/vision_util.h"
+
DEFINE_bool(
debug, false,
"If true, dump a ton of debug and crash on the first valid detection.");
-DEFINE_int32(team_number, 971,
- "Use the calibration for a node with this team number");
namespace y2023 {
namespace vision {
@@ -13,7 +13,7 @@
AprilRoboticsDetector::AprilRoboticsDetector(aos::EventLoop *event_loop,
std::string_view channel_name)
- : calibration_data_(CalibrationData()),
+ : calibration_data_(event_loop),
ftrace_(),
image_callback_(
event_loop, channel_name,
@@ -38,14 +38,8 @@
std::string hostname = aos::network::GetHostname();
// Check team string is valid
- std::optional<uint16_t> pi_number = aos::network::ParsePiNumber(hostname);
- std::optional<uint16_t> team_number =
- aos::network::team_number_internal::ParsePiTeamNumber(hostname);
- CHECK(pi_number) << "Unable to parse pi number from '" << hostname << "'";
- CHECK(team_number);
-
- calibration_ = FindCameraCalibration(&calibration_data_.message(),
- "pi" + std::to_string(*pi_number));
+ calibration_ = FindCameraCalibration(
+ calibration_data_.constants(), event_loop->node()->name()->string_view());
intrinsics_ = CameraIntrinsics(calibration_);
camera_distortion_coeffs_ = CameraDistCoeffs(calibration_);
diff --git a/y2023/vision/aprilrobotics.h b/y2023/vision/aprilrobotics.h
index d5172c7..8c2c0ad 100644
--- a/y2023/vision/aprilrobotics.h
+++ b/y2023/vision/aprilrobotics.h
@@ -16,7 +16,8 @@
#include "third_party/apriltag/apriltag_pose.h"
#include "third_party/apriltag/tag16h5.h"
#include "y2023/vision/april_debug_generated.h"
-#include "y2023/vision/calibration_data.h"
+#include "y2023/constants/constants_generated.h"
+#include "frc971/constants/constants_sender_lib.h"
DECLARE_int32(team_number);
@@ -43,23 +44,6 @@
frc971::vision::TargetMapper::TargetId target_id,
flatbuffers::FlatBufferBuilder *fbb);
- static const frc971::vision::calibration::CameraCalibration *
- FindCameraCalibration(
- const frc971::vision::calibration::CalibrationData *calibration_data,
- std::string_view node_name) {
- for (const frc971::vision::calibration::CameraCalibration *candidate :
- *calibration_data->camera_calibrations()) {
- if (candidate->node_name()->string_view() != node_name) {
- continue;
- }
- if (candidate->team_number() != FLAGS_team_number) {
- continue;
- }
- return candidate;
- }
- LOG(FATAL) << ": Failed to find camera calibration for " << node_name
- << " on " << FLAGS_team_number;
- }
static cv::Mat CameraIntrinsics(
const frc971::vision::calibration::CameraCalibration
@@ -86,8 +70,7 @@
apriltag_family_t *tag_family_;
apriltag_detector_t *tag_detector_;
- const aos::FlatbufferSpan<frc971::vision::calibration::CalibrationData>
- calibration_data_;
+ const frc971::constants::ConstantsFetcher<Constants> calibration_data_;
const frc971::vision::calibration::CameraCalibration *calibration_;
cv::Mat intrinsics_;
cv::Mat camera_distortion_coeffs_;
diff --git a/y2023/vision/aprilrobotics_main.cc b/y2023/vision/aprilrobotics_main.cc
index 67b853f..6870ae1 100644
--- a/y2023/vision/aprilrobotics_main.cc
+++ b/y2023/vision/aprilrobotics_main.cc
@@ -1,6 +1,7 @@
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "y2023/vision/aprilrobotics.h"
+#include "frc971/constants/constants_sender_lib.h"
DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
@@ -9,6 +10,8 @@
aos::FlatbufferDetachedBuffer<aos::Configuration> config =
aos::configuration::ReadConfig(FLAGS_config);
+ frc971::constants::WaitForConstants<Constants>(&config.message());
+
aos::ShmEventLoop event_loop(&config.message());
AprilRoboticsDetector detector(&event_loop, "/camera");
diff --git a/y2023/vision/calib_files/BUILD b/y2023/vision/calib_files/BUILD
new file mode 100644
index 0000000..1f33d50
--- /dev/null
+++ b/y2023/vision/calib_files/BUILD
@@ -0,0 +1,5 @@
+filegroup(
+ name = "calib_files",
+ srcs = glob(["*.json"]),
+ visibility = ["//visibility:public"],
+)
diff --git a/y2023/vision/calib_files/calibration_pi-7971-1_2021-06-12_15-35-39.636386620.json b/y2023/vision/calib_files/calibration_pi-7971-1_2021-06-12_15-35-39.636386620.json
old mode 100755
new mode 100644
index a8d8816..fc739fd
--- a/y2023/vision/calib_files/calibration_pi-7971-1_2021-06-12_15-35-39.636386620.json
+++ b/y2023/vision/calib_files/calibration_pi-7971-1_2021-06-12_15-35-39.636386620.json
@@ -1,41 +1,43 @@
{
- "node_name": "pi1",
- "team_number": 7971,
- "intrinsics": [
- 388.369812,
- 0.0,
- 292.325653,
- 0.0,
- 388.513733,
- 224.371063,
- 0.0,
- 0.0,
- 1.0
- ],
- "dist_coeffs": [
- 0.126935,
- -0.218447,
- -0.000152,
- 0.001158,
- 0.06266
- ],
- "fixed_extrinsics": [
- -1.0,
- -1.57586107256918e-16,
- 5.0158596452676243e-17,
- -0.15239999999999998,
- 1.3147519464173305e-16,
- -0.5735764363510459,
- 0.8191520442889919,
- -0.2032,
- -1.0031719290535249e-16,
- 0.8191520442889919,
- 0.5735764363510459,
- 0.0127,
- 0.0,
- 0.0,
- 0.0,
- 1.0
- ],
- "calibration_timestamp": 1623537339636386620
+ "node_name": "pi1",
+ "team_number": 7971,
+ "intrinsics": [
+ 388.369812,
+ 0,
+ 292.325653,
+ 0,
+ 388.513733,
+ 224.371063,
+ 0,
+ 0,
+ 1
+ ],
+ "dist_coeffs": [
+ 0.126935,
+ -0.218447,
+ -0.000152,
+ 0.001158,
+ 0.06266
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ -1,
+ -1.57586107256918e-16,
+ 5.0158596452676243e-17,
+ -0.15239999999999998,
+ 1.3147519464173305e-16,
+ -0.5735764363510459,
+ 0.8191520442889919,
+ -0.2032,
+ -1.0031719290535249e-16,
+ 0.8191520442889919,
+ 0.5735764363510459,
+ 0.0127,
+ 0,
+ 0,
+ 0,
+ 1
+ ]
+ },
+ "calibration_timestamp": 1623537339636386600
}
diff --git a/y2023/vision/calib_files/calibration_pi-7971-2_2021-06-12_15-30-20.325393444.json b/y2023/vision/calib_files/calibration_pi-7971-2_2021-06-12_15-30-20.325393444.json
old mode 100755
new mode 100644
index d994a07..3cf285c
--- a/y2023/vision/calib_files/calibration_pi-7971-2_2021-06-12_15-30-20.325393444.json
+++ b/y2023/vision/calib_files/calibration_pi-7971-2_2021-06-12_15-30-20.325393444.json
@@ -1,41 +1,43 @@
{
- "node_name": "pi2",
- "team_number": 7971,
- "intrinsics": [
- 388.7565,
- 0.0,
- 285.024506,
- 0.0,
- 388.915039,
- 222.227539,
- 0.0,
- 0.0,
- 1.0
- ],
- "dist_coeffs": [
- 0.128415,
- -0.212528,
- 0.001165,
- 0.000579,
- 0.054853
- ],
- "fixed_extrinsics": [
- 7.02428546843654e-17,
- -0.5735764363510459,
- 0.8191520442889919,
- 0.09525,
- 1.0,
- 1.2246467991473532e-16,
- 0.0,
- 0.1905,
- -1.0031719290535249e-16,
- 0.8191520442889919,
- 0.5735764363510459,
- 0.0127,
- 0.0,
- 0.0,
- 0.0,
- 1.0
- ],
- "calibration_timestamp": 1623537020325393444
+ "node_name": "pi2",
+ "team_number": 7971,
+ "intrinsics": [
+ 388.7565,
+ 0,
+ 285.024506,
+ 0,
+ 388.915039,
+ 222.227539,
+ 0,
+ 0,
+ 1
+ ],
+ "dist_coeffs": [
+ 0.128415,
+ -0.212528,
+ 0.001165,
+ 0.000579,
+ 0.054853
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ 7.02428546843654e-17,
+ -0.5735764363510459,
+ 0.8191520442889919,
+ 0.09525,
+ 1,
+ 1.2246467991473532e-16,
+ 0,
+ 0.1905,
+ -1.0031719290535249e-16,
+ 0.8191520442889919,
+ 0.5735764363510459,
+ 0.0127,
+ 0,
+ 0,
+ 0,
+ 1
+ ]
+ },
+ "calibration_timestamp": 1623537020325393400
}
diff --git a/y2023/vision/calib_files/calibration_pi-7971-3_2021-06-12_15-33-31.977365877.json b/y2023/vision/calib_files/calibration_pi-7971-3_2021-06-12_15-33-31.977365877.json
old mode 100755
new mode 100644
index 241957e..c7a7581
--- a/y2023/vision/calib_files/calibration_pi-7971-3_2021-06-12_15-33-31.977365877.json
+++ b/y2023/vision/calib_files/calibration_pi-7971-3_2021-06-12_15-33-31.977365877.json
@@ -1,41 +1,43 @@
{
- "node_name": "pi3",
- "team_number": 7971,
- "intrinsics": [
- 389.35611,
- 0.0,
- 339.345673,
- 0.0,
- 389.516235,
- 240.247787,
- 0.0,
- 0.0,
- 1.0
- ],
- "dist_coeffs": [
- 0.122511,
- -0.209383,
- -0.001212,
- 0.000041,
- 0.05674
- ],
- "fixed_extrinsics": [
- 7.02428546843654e-17,
- -0.5735764363510459,
- 0.8191520442889919,
- 0.09525,
- 1.0,
- 1.2246467991473532e-16,
- 0.0,
- -0.10794999999999999,
- -1.0031719290535249e-16,
- 0.8191520442889919,
- 0.5735764363510459,
- 0.0127,
- 0.0,
- 0.0,
- 0.0,
- 1.0
- ],
- "calibration_timestamp": 1623537211977365877
+ "node_name": "pi3",
+ "team_number": 7971,
+ "intrinsics": [
+ 389.35611,
+ 0,
+ 339.345673,
+ 0,
+ 389.516235,
+ 240.247787,
+ 0,
+ 0,
+ 1
+ ],
+ "dist_coeffs": [
+ 0.122511,
+ -0.209383,
+ -0.001212,
+ 4.1e-05,
+ 0.05674
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ 7.02428546843654e-17,
+ -0.5735764363510459,
+ 0.8191520442889919,
+ 0.09525,
+ 1,
+ 1.2246467991473532e-16,
+ 0,
+ -0.10794999999999999,
+ -1.0031719290535249e-16,
+ 0.8191520442889919,
+ 0.5735764363510459,
+ 0.0127,
+ 0,
+ 0,
+ 0,
+ 1
+ ]
+ },
+ "calibration_timestamp": 1623537211977365800
}
diff --git a/y2023/vision/calib_files/calibration_pi-7971-4_2021-06-12_15-37-25.706564865.json b/y2023/vision/calib_files/calibration_pi-7971-4_2021-06-12_15-37-25.706564865.json
old mode 100755
new mode 100644
index 6e04089..f8f6b1f
--- a/y2023/vision/calib_files/calibration_pi-7971-4_2021-06-12_15-37-25.706564865.json
+++ b/y2023/vision/calib_files/calibration_pi-7971-4_2021-06-12_15-37-25.706564865.json
@@ -1,41 +1,43 @@
{
- "node_name": "pi4",
- "team_number": 7971,
- "intrinsics": [
- 390.301514,
- 0.0,
- 356.104095,
- 0.0,
- 389.884491,
- 231.157303,
- 0.0,
- 0.0,
- 1.0
- ],
- "dist_coeffs": [
- 0.128595,
- -0.229324,
- -0.001145,
- 0.001602,
- 0.079774
- ],
- "fixed_extrinsics": [
- 7.02428546843654e-17,
- -0.5735764363510459,
- 0.8191520442889919,
- -0.15239999999999998,
- 1.0,
- 1.2246467991473532e-16,
- 0.0,
- -0.17779999999999999,
- -1.0031719290535249e-16,
- 0.8191520442889919,
- 0.5735764363510459,
- 0.0127,
- 0.0,
- 0.0,
- 0.0,
- 1.0
- ],
- "calibration_timestamp": 1623537445706564865
+ "node_name": "pi4",
+ "team_number": 7971,
+ "intrinsics": [
+ 390.301514,
+ 0,
+ 356.104095,
+ 0,
+ 389.884491,
+ 231.157303,
+ 0,
+ 0,
+ 1
+ ],
+ "dist_coeffs": [
+ 0.128595,
+ -0.229324,
+ -0.001145,
+ 0.001602,
+ 0.079774
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ 7.02428546843654e-17,
+ -0.5735764363510459,
+ 0.8191520442889919,
+ -0.15239999999999998,
+ 1,
+ 1.2246467991473532e-16,
+ 0,
+ -0.17779999999999999,
+ -1.0031719290535249e-16,
+ 0.8191520442889919,
+ 0.5735764363510459,
+ 0.0127,
+ 0,
+ 0,
+ 0,
+ 1
+ ]
+ },
+ "calibration_timestamp": 1623537445706564900
}
diff --git a/y2023/vision/create_calib_file.py b/y2023/vision/create_calib_file.py
deleted file mode 100644
index b5e620d..0000000
--- a/y2023/vision/create_calib_file.py
+++ /dev/null
@@ -1,4 +0,0 @@
-import frc971.vision.create_calib_file
-
-if __name__ == "__main__":
- frc971.vision.create_calib_file.generate_header("2023")
diff --git a/y2023/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
index 6d32dbd..43b5353 100644
--- a/y2023/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -13,7 +13,7 @@
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc.hpp"
#include "y2023/vision/aprilrobotics.h"
-#include "y2023/vision/calibration_data.h"
+#include "y2023/vision/vision_util.h"
DEFINE_string(json_path, "target_map.json",
"Specify path for json with initial pose guesses.");
@@ -76,23 +76,6 @@
}
}
-const calibration::CameraCalibration *FindCameraCalibration(
- const calibration::CalibrationData *calibration_data,
- std::string_view node_name) {
- for (const calibration::CameraCalibration *candidate :
- *calibration_data->camera_calibrations()) {
- if (candidate->node_name()->string_view() != node_name) {
- continue;
- }
- if (candidate->team_number() != FLAGS_team_number) {
- continue;
- }
- return candidate;
- }
- LOG(FATAL) << ": Failed to find camera calibration for " << node_name
- << " on " << FLAGS_team_number;
-}
-
Eigen::Affine3d CameraExtrinsics(
const calibration::CameraCalibration *camera_calibration) {
const frc971::vision::calibration::TransformationMatrix *transform =
@@ -113,16 +96,15 @@
// Get images from pi and pass apriltag positions to HandleAprilTag()
void HandlePiCaptures(
+ const frc971::constants::ConstantsFetcher<Constants> &constants,
aos::EventLoop *pi_event_loop, aos::logger::LogReader *reader,
std::vector<DataAdapter::TimestampedDetection>
*timestamped_target_detections,
std::vector<std::unique_ptr<AprilRoboticsDetector>> *detectors) {
- const aos::FlatbufferSpan<calibration::CalibrationData> calibration_data(
- CalibrationData());
const auto node_name = pi_event_loop->node()->name()->string_view();
const calibration::CameraCalibration *calibration =
- FindCameraCalibration(&calibration_data.message(), node_name);
+ FindCameraCalibration(constants.constants(), node_name);
const auto extrinsics = CameraExtrinsics(calibration);
// TODO(milind): change to /camera once we log at full frequency
@@ -158,28 +140,36 @@
aos::configuration::GetNode(reader.configuration(), "pi1");
std::unique_ptr<aos::EventLoop> pi1_event_loop =
reader.event_loop_factory()->MakeEventLoop("pi1", pi1);
- HandlePiCaptures(pi1_event_loop.get(), &reader,
+ frc971::constants::ConstantsFetcher<Constants> pi1_constants(
+ pi1_event_loop.get());
+ HandlePiCaptures(pi1_constants, pi1_event_loop.get(), &reader,
×tamped_target_detections, &detectors);
const aos::Node *pi2 =
aos::configuration::GetNode(reader.configuration(), "pi2");
std::unique_ptr<aos::EventLoop> pi2_event_loop =
reader.event_loop_factory()->MakeEventLoop("pi2", pi2);
- HandlePiCaptures(pi2_event_loop.get(), &reader,
+ frc971::constants::ConstantsFetcher<Constants> pi2_constants(
+ pi2_event_loop.get());
+ HandlePiCaptures(pi2_constants, pi2_event_loop.get(), &reader,
×tamped_target_detections, &detectors);
const aos::Node *pi3 =
aos::configuration::GetNode(reader.configuration(), "pi3");
std::unique_ptr<aos::EventLoop> pi3_event_loop =
reader.event_loop_factory()->MakeEventLoop("pi3", pi3);
- HandlePiCaptures(pi3_event_loop.get(), &reader,
+ frc971::constants::ConstantsFetcher<Constants> pi3_constants(
+ pi3_event_loop.get());
+ HandlePiCaptures(pi3_constants, pi3_event_loop.get(), &reader,
×tamped_target_detections, &detectors);
const aos::Node *pi4 =
aos::configuration::GetNode(reader.configuration(), "pi4");
std::unique_ptr<aos::EventLoop> pi4_event_loop =
reader.event_loop_factory()->MakeEventLoop("pi4", pi4);
- HandlePiCaptures(pi4_event_loop.get(), &reader,
+ frc971::constants::ConstantsFetcher<Constants> pi4_constants(
+ pi4_event_loop.get());
+ HandlePiCaptures(pi4_constants, pi4_event_loop.get(), &reader,
×tamped_target_detections, &detectors);
reader.event_loop_factory()->Run();
diff --git a/y2023/vision/vision_util.cc b/y2023/vision/vision_util.cc
new file mode 100644
index 0000000..eed315a
--- /dev/null
+++ b/y2023/vision/vision_util.cc
@@ -0,0 +1,19 @@
+#include "y2023/vision/vision_util.h"
+
+#include "glog/logging.h"
+
+namespace y2023::vision {
+const frc971::vision::calibration::CameraCalibration *FindCameraCalibration(
+ const y2023::Constants &calibration_data, std::string_view node_name) {
+ CHECK(calibration_data.has_cameras());
+ for (const y2023::CameraConfiguration *candidate :
+ *calibration_data.cameras()) {
+ CHECK(candidate->has_calibration());
+ if (candidate->calibration()->node_name()->string_view() != node_name) {
+ continue;
+ }
+ return candidate->calibration();
+ }
+ LOG(FATAL) << ": Failed to find camera calibration for " << node_name;
+}
+} // namespace y2023::vision
diff --git a/y2023/vision/vision_util.h b/y2023/vision/vision_util.h
new file mode 100644
index 0000000..58f9a7f
--- /dev/null
+++ b/y2023/vision/vision_util.h
@@ -0,0 +1,11 @@
+#ifndef Y2023_VISION_VISION_UTIL_H_
+#define Y2023_VISION_VISION_UTIL_H_
+#include <string_view>
+
+#include "y2023/constants/constants_generated.h"
+namespace y2023::vision {
+
+const frc971::vision::calibration::CameraCalibration *FindCameraCalibration(
+ const y2023::Constants &calibration_data, std::string_view node_name);
+}
+#endif // Y2023_VISION_VISION_UTIL_H_
diff --git a/y2023/y2023_imu.json b/y2023/y2023_imu.json
index 7744281..9636003 100644
--- a/y2023/y2023_imu.json
+++ b/y2023/y2023_imu.json
@@ -298,12 +298,21 @@
"frequency": 2200,
"max_size": 1600,
"num_senders": 2
+ },
+ {
+ "name": "/imu/constants",
+ "type": "y2023.Constants",
+ "source_node": "imu",
+ "frequency": 1,
+ "num_senders": 2,
+ "max_size": 4096
}
],
"applications": [
{
"name": "message_bridge_client",
"executable_name": "message_bridge_client.sh",
+ "user": "pi",
"nodes": [
"imu"
]
@@ -313,6 +322,7 @@
"executable_name": "localizer_main",
/* TODO(james): Remove this once confident in the accelerometer code. */
"args": ["--ignore_accelerometer"],
+ "user": "pi",
"nodes": [
"imu"
]
@@ -320,6 +330,7 @@
{
"name": "imu",
"executable_name": "imu_main",
+ "user": "pi",
"nodes": [
"imu"
]
@@ -327,6 +338,7 @@
{
"name": "message_bridge_server",
"executable_name": "message_bridge_server",
+ "user": "pi",
"nodes": [
"imu"
]
@@ -335,6 +347,7 @@
"name": "localizer_logger",
"executable_name": "logger_main",
"args": ["--logging_folder", "", "--snappy_compress"],
+ "user": "pi",
"nodes": [
"imu"
]
@@ -342,6 +355,15 @@
{
"name": "web_proxy",
"executable_name": "web_proxy_main",
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "constants_sender",
+ "autorestart": false,
+ "user": "pi",
"nodes": [
"imu"
]
@@ -350,6 +372,15 @@
"maps": [
{
"match": {
+ "name": "/constants*",
+ "source_node": "imu"
+ },
+ "rename": {
+ "name": "/imu/constants"
+ }
+ },
+ {
+ "match": {
"name": "/aos*",
"source_node": "imu"
},
diff --git a/y2023/y2023_logger.json b/y2023/y2023_logger.json
index db313d2..793c101 100644
--- a/y2023/y2023_logger.json
+++ b/y2023/y2023_logger.json
@@ -391,6 +391,14 @@
"time_to_live": 500000000
}
]
+ },
+ {
+ "name": "/logger/constants",
+ "type": "y2023.Constants",
+ "source_node": "logger",
+ "frequency": 1,
+ "num_senders": 2,
+ "max_size": 4096
}
],
"maps": [
@@ -405,6 +413,15 @@
},
{
"match": {
+ "name": "/constants*",
+ "source_node": "logger"
+ },
+ "rename": {
+ "name": "/logger/constants"
+ }
+ },
+ {
+ "match": {
"name": "/camera*",
"source_node": "logger"
},
@@ -460,6 +477,13 @@
"nodes": [
"logger"
]
+ },
+ {
+ "name": "constants_sender",
+ "autorestart": false,
+ "nodes": [
+ "logger"
+ ]
}
],
"nodes": [
@@ -469,7 +493,9 @@
"hostnames": [
"pi-971-6",
"pi-9971-6",
+ "pi-7971-6",
"ASchuh-T480s",
+ "tarvalon",
"aschuh-3950x"
],
"port": 9971
diff --git a/y2023/y2023_pi_template.json b/y2023/y2023_pi_template.json
index 05aaa96..911c795 100644
--- a/y2023/y2023_pi_template.json
+++ b/y2023/y2023_pi_template.json
@@ -324,6 +324,14 @@
"frequency": 50,
"num_senders": 2,
"max_size": 200
+ },
+ {
+ "name": "/pi{{ NUM }}/constants",
+ "type": "y2023.Constants",
+ "source_node": "pi{{ NUM }}",
+ "frequency": 1,
+ "num_senders": 2,
+ "max_size": 4096
}
],
"applications": [
@@ -378,6 +386,14 @@
"nodes": [
"pi{{ NUM }}"
]
+ },
+ {
+ "name": "constants_sender",
+ "autorestart": false,
+ "user": "pi",
+ "nodes": [
+ "pi{{ NUM }}"
+ ]
}
],
"maps": [
@@ -392,6 +408,15 @@
},
{
"match": {
+ "name": "/constants*",
+ "source_node": "pi{{ NUM }}"
+ },
+ "rename": {
+ "name": "/pi{{ NUM }}/constants"
+ }
+ },
+ {
+ "match": {
"name": "/camera*",
"source_node": "pi{{ NUM }}"
},
diff --git a/y2023/y2023_roborio.json b/y2023/y2023_roborio.json
index e1306c2..d0b2251 100644
--- a/y2023/y2023_roborio.json
+++ b/y2023/y2023_roborio.json
@@ -435,6 +435,14 @@
"type": "frc971.wpilib.PneumaticsToLog",
"source_node": "roborio",
"frequency": 50
+ },
+ {
+ "name": "/roborio/constants",
+ "type": "y2023.Constants",
+ "source_node": "roborio",
+ "frequency": 1,
+ "num_senders": 2,
+ "max_size": 4096
}
],
"applications": [
@@ -511,11 +519,27 @@
"nodes": [
"roborio"
]
+ },
+ {
+ "name": "constants_sender",
+ "autorestart": false,
+ "nodes": [
+ "roborio"
+ ]
}
],
"maps": [
{
"match": {
+ "name": "/constants*",
+ "source_node": "roborio"
+ },
+ "rename": {
+ "name": "/roborio/constants"
+ }
+ },
+ {
+ "match": {
"name": "/aos*",
"source_node": "roborio"
},