Move cameras to RobotConstants in y2024
Signed-off-by: Yash Maheshwari <yashmahe2018@gmail.com>
Change-Id: Iba79b172e1161565ca2be4960f953bfda6d29ffc
Signed-off-by: Yash Maheshwari <yashmahe2018@gmail.com>
diff --git a/y2024/constants/7971.json b/y2024/constants/7971.json
index c924174..4aed3cb 100644
--- a/y2024/constants/7971.json
+++ b/y2024/constants/7971.json
@@ -6,20 +6,6 @@
{% from 'y2024/constants/common.jinja2' import extend_zero %}
{
- "cameras": [
- {
- "calibration": {% include 'y2024/constants/calib_files/calibration_imu-7971-0_cam-24-01_2024-03-02_19-44-12.098903651.json' %}
- },
- {
- "calibration": {% include 'y2024/constants/calib_files/calibration_orin1-7971-0_cam-24-04_2024-04-07_19-59-36.json' %}
- },
- {
- "calibration": {% include 'y2024/constants/calib_files/calibration_orin1-7971-1_cam-24-02_2024-04-07_19-59-36.json' %}
- },
- {
- "calibration": {% include 'y2024/constants/calib_files/calibration_imu-7971-1_cam-24-03_2024-04-07_19-59-36.json' %}
- }
- ],
"robot": {
{% set _ = intake_pivot_zero.update(
{
@@ -65,7 +51,21 @@
"potentiometer_offset": 0.0
},
"disable_extend": false,
- "disable_climber": false
+ "disable_climber": false,
+ "cameras": [
+ {
+ "calibration": {% include 'y2024/constants/calib_files/calibration_imu-7971-0_cam-24-01_2024-03-02_19-44-12.098903651.json' %}
+ },
+ {
+ "calibration": {% include 'y2024/constants/calib_files/calibration_orin1-7971-0_cam-24-04_2024-04-07_19-59-36.json' %}
+ },
+ {
+ "calibration": {% include 'y2024/constants/calib_files/calibration_orin1-7971-1_cam-24-02_2024-04-07_19-59-36.json' %}
+ },
+ {
+ "calibration": {% include 'y2024/constants/calib_files/calibration_imu-7971-1_cam-24-03_2024-04-07_19-59-36.json' %}
+ }
+ ]
},
{% include 'y2024/constants/common.json' %}
}
diff --git a/y2024/constants/971.json b/y2024/constants/971.json
index 5145613..ce70e07 100644
--- a/y2024/constants/971.json
+++ b/y2024/constants/971.json
@@ -6,20 +6,6 @@
{% from 'y2024/constants/common.jinja2' import extend_zero %}
{
- "cameras": [
- {
- "calibration": {% include 'y2024/constants/calib_files/calibration_imu-971-1_cam-24-05_2024-03-24_14-54-27.json' %}
- },
- {
- "calibration": {% include 'y2024/constants/calib_files/calibration_orin1-971-0_cam-24-06_2024-03-24_14-54-27.json' %}
- },
- {
- "calibration": {% include 'y2024/constants/calib_files/calibration_orin1-971-1_cam-24-07_2024-03-24_14-54-27.json' %}
- },
- {
- "calibration": {% include 'y2024/constants/calib_files/calibration_imu-971-0_cam-24-08_2024-03-01_11-02-11.json' %}
- }
- ],
"robot": {
{% set _ = intake_pivot_zero.update(
{
@@ -65,7 +51,21 @@
"potentiometer_offset": {{ -0.2574404033256 + 0.0170793439542 - 0.177097393974999 + 0.3473623911879 - 0.1577 - 0.0214825988393}}
},
"disable_extend": false,
- "disable_climber": false
+ "disable_climber": false,
+ "cameras": [
+ {
+ "calibration": {% include 'y2024/constants/calib_files/calibration_imu-971-1_cam-24-05_2024-03-24_14-54-27.json' %}
+ },
+ {
+ "calibration": {% include 'y2024/constants/calib_files/calibration_orin1-971-0_cam-24-06_2024-03-24_14-54-27.json' %}
+ },
+ {
+ "calibration": {% include 'y2024/constants/calib_files/calibration_orin1-971-1_cam-24-07_2024-03-24_14-54-27.json' %}
+ },
+ {
+ "calibration": {% include 'y2024/constants/calib_files/calibration_imu-971-0_cam-24-08_2024-03-01_11-02-11.json' %}
+ }
+ ]
},
{% include 'y2024/constants/common.json' %}
}
diff --git a/y2024/constants/9971.json b/y2024/constants/9971.json
index 0cde2ad..5a3e7fc 100644
--- a/y2024/constants/9971.json
+++ b/y2024/constants/9971.json
@@ -6,20 +6,6 @@
{% from 'y2024/constants/common.jinja2' import extend_zero %}
{
- "cameras": [
- {
- "calibration": {% include 'y2024/constants/calib_files/calibration_imu-9971-0_cam-24-10_2024-02-24_16-44-05.json' %}
- },
- {
- "calibration": {% include 'y2024/constants/calib_files/calibration_imu-9971-1_cam-24-12_2024-03-24_11-52-49.json' %}
- },
- {
- "calibration": {% include 'y2024/constants/calib_files/calibration_orin1-9971-0_cam-24-09_2024-03-24_11-52-49.json' %}
- },
- {
- "calibration": {% include 'y2024/constants/calib_files/calibration_orin1-9971-1_cam-24-11_2024-03-24_11-52-49.json' %}
- },
- ],
"robot": {
{% set _ = intake_pivot_zero.update(
{
@@ -65,7 +51,21 @@
"potentiometer_offset": 0.0
},
"disable_extend": false,
- "disable_climber": false
+ "disable_climber": false,
+ "cameras": [
+ {
+ "calibration": {% include 'y2024/constants/calib_files/calibration_imu-9971-0_cam-24-10_2024-02-24_16-44-05.json' %}
+ },
+ {
+ "calibration": {% include 'y2024/constants/calib_files/calibration_imu-9971-1_cam-24-12_2024-03-24_11-52-49.json' %}
+ },
+ {
+ "calibration": {% include 'y2024/constants/calib_files/calibration_orin1-9971-0_cam-24-09_2024-03-24_11-52-49.json' %}
+ },
+ {
+ "calibration": {% include 'y2024/constants/calib_files/calibration_orin1-9971-1_cam-24-11_2024-03-24_11-52-49.json' %}
+ },
+ ]
},
{% include 'y2024/constants/common.json' %}
}
diff --git a/y2024/constants/constants.fbs b/y2024/constants/constants.fbs
index 6c1480c..d4d05b7 100644
--- a/y2024/constants/constants.fbs
+++ b/y2024/constants/constants.fbs
@@ -150,6 +150,7 @@
extend_constants:PotAndAbsEncoderConstants (id: 5);
disable_extend:bool (id: 6);
disable_climber:bool (id: 7);
+ cameras:[CameraConfiguration] (id: 9);
}
table ShooterSetPoint {
@@ -223,9 +224,8 @@
}
table Constants {
- cameras:[CameraConfiguration] (id: 0);
- robot:RobotConstants (id: 1);
- common:Common (id: 2);
+ robot:RobotConstants (id: 0);
+ common:Common (id: 1);
}
root_type Constants;
diff --git a/y2024/constants/test_data/test_team.json b/y2024/constants/test_data/test_team.json
index 9d40027..4601f50 100644
--- a/y2024/constants/test_data/test_team.json
+++ b/y2024/constants/test_data/test_team.json
@@ -6,20 +6,6 @@
{% from 'y2024/constants/common.jinja2' import extend_zero %}
{
- "cameras": [
- {
- "calibration": {% include 'y2024/constants/test_data/calibration_cam-1.json' %}
- },
- {
- "calibration": {% include 'y2024/constants/test_data/calibration_cam-2.json' %}
- },
- {
- "calibration": {% include 'y2024/constants/test_data/calibration_cam-3.json' %}
- },
- {
- "calibration": {% include 'y2024/constants/test_data/calibration_cam-4.json' %}
- }
- ],
"robot": {
{% set _ = intake_pivot_zero.update(
{
@@ -65,7 +51,21 @@
"potentiometer_offset": 0.0
},
"disable_extend": false,
- "disable_climber": false
+ "disable_climber": false,
+ "cameras": [
+ {
+ "calibration": {% include 'y2024/constants/test_data/calibration_cam-1.json' %}
+ },
+ {
+ "calibration": {% include 'y2024/constants/test_data/calibration_cam-2.json' %}
+ },
+ {
+ "calibration": {% include 'y2024/constants/test_data/calibration_cam-3.json' %}
+ },
+ {
+ "calibration": {% include 'y2024/constants/test_data/calibration_cam-4.json' %}
+ }
+ ]
},
{% include 'y2024/constants/common.json' %}
}
diff --git a/y2024/localizer/localizer.cc b/y2024/localizer/localizer.cc
index d73d171..2b72f94 100644
--- a/y2024/localizer/localizer.cc
+++ b/y2024/localizer/localizer.cc
@@ -140,9 +140,9 @@
std::array<Localizer::CameraState, Localizer::kNumCameras>
Localizer::MakeCameras(const Constants &constants, aos::EventLoop *event_loop) {
- CHECK(constants.has_cameras());
+ CHECK(constants.robot()->has_cameras());
std::array<Localizer::CameraState, Localizer::kNumCameras> cameras;
- for (const CameraConfiguration *camera : *constants.cameras()) {
+ for (const CameraConfiguration *camera : *constants.robot()->cameras()) {
CHECK(camera->has_calibration());
const frc971::vision::calibration::CameraCalibration *calibration =
camera->calibration();
diff --git a/y2024/localizer/localizer_test.cc b/y2024/localizer/localizer_test.cc
index e5815e0..bd97059 100644
--- a/y2024/localizer/localizer_test.cc
+++ b/y2024/localizer/localizer_test.cc
@@ -104,20 +104,23 @@
// Sanity check that the test calibration files look like what we
// expect.
CHECK_EQ("orin1", constants_fetcher_.constants()
- .cameras()
+ .robot()
+ ->cameras()
->Get(0)
->calibration()
->node_name()
->string_view());
CHECK_EQ(0, constants_fetcher_.constants()
- .cameras()
+ .robot()
+ ->cameras()
->Get(0)
->calibration()
->camera_number());
const Eigen::Matrix<double, 4, 4> H_robot_camera =
frc971::control_loops::drivetrain::FlatbufferToTransformationMatrix(
*constants_fetcher_.constants()
- .cameras()
+ .robot()
+ ->cameras()
->Get(0)
->calibration()
->fixed_extrinsics());
diff --git a/y2024/vision/vision_util.cc b/y2024/vision/vision_util.cc
index becb1cf..cf52507 100644
--- a/y2024/vision/vision_util.cc
+++ b/y2024/vision/vision_util.cc
@@ -31,9 +31,9 @@
const frc971::vision::calibration::CameraCalibration *FindCameraCalibration(
const y2024::Constants &calibration_data, std::string_view node_name,
int camera_number) {
- CHECK(calibration_data.has_cameras());
+ CHECK(calibration_data.robot()->has_cameras());
for (const y2024::CameraConfiguration *candidate :
- *calibration_data.cameras()) {
+ *calibration_data.robot()->cameras()) {
CHECK(candidate->has_calibration());
if (candidate->calibration()->node_name()->string_view() != node_name ||
candidate->calibration()->camera_number() != camera_number) {