Create y2023 localizer
This creates a localizer implementation for y2023, and implements simple
tests. This is actually more similar to the y2020 iteration than the
y2022 iteration in implementation, as I backed away from trying to
maintain the more complex y2022 logic (since I wasn't actually using the
features of that code). Somewhat notable changes from past years:
* New target format using the TargetMap that the april robotics detector
sends out.
* Does not attempt to entirely ignore the implied yaw indicated by the
target detections, since unlike in 2020/2022, we are not shooting at
the target and instead attempting to go somewhere offset from the
target.
Currently has no way of getting confidence values from the april
robotics code, so treats all detections the same. Will need to tune
this.
Change-Id: I072cd3fb2657081bca74c55570842960c5ad7b1b
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2023/constants/BUILD b/y2023/constants/BUILD
index 09e57c7..344fa75 100644
--- a/y2023/constants/BUILD
+++ b/y2023/constants/BUILD
@@ -4,8 +4,10 @@
cc_library(
name = "simulated_constants_sender",
testonly = True,
- srcs = ["simulated_constants_sender.h"],
- hdrs = ["simulated_constants_sender.cc"],
+ srcs = ["simulated_constants_sender.cc"],
+ hdrs = ["simulated_constants_sender.h"],
+ data = [":test_constants.json"],
+ visibility = ["//y2023:__subpackages__"],
deps = [
":constants_fbs",
":constants_list_fbs",
@@ -16,6 +18,15 @@
)
jinja2_template(
+ name = "test_constants.json",
+ testonly = True,
+ src = "test_constants.jinja2.json",
+ includes = glob(["test_data/*.json"]),
+ parameters = {},
+ visibility = ["//visibility:public"],
+)
+
+jinja2_template(
name = "constants.json",
src = "constants.jinja2.json",
includes = [
diff --git a/y2023/constants/simulated_constants_sender.cc b/y2023/constants/simulated_constants_sender.cc
index 3086e99..f18c3c1 100644
--- a/y2023/constants/simulated_constants_sender.cc
+++ b/y2023/constants/simulated_constants_sender.cc
@@ -2,6 +2,7 @@
#include "y2023/constants/constants_list_generated.h"
#include "aos/events/simulated_event_loop.h"
#include "aos/testing/path.h"
+#include "frc971/constants/constants_sender_lib.h"
namespace y2023 {
void SendSimulationConstants(aos::SimulatedEventLoopFactory *factory, int team,
@@ -14,5 +15,3 @@
}
}
} // 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
index 10ea793..44a868c 100644
--- a/y2023/constants/simulated_constants_sender.h
+++ b/y2023/constants/simulated_constants_sender.h
@@ -5,9 +5,10 @@
#include "aos/testing/path.h"
namespace y2023 {
-void SendSimulationConstants(aos::SimulatedEventLoopFactory *factory, int team,
- std::string constants_path = testing::ArtifactPath(
- "y2023/constants/constants.json"));
+void SendSimulationConstants(
+ aos::SimulatedEventLoopFactory *factory, int team,
+ std::string constants_path =
+ aos::testing::ArtifactPath("y2023/constants/test_constants.json"));
} // namespace y2023
#endif // Y2023_CONSTANTS_SIMULATED_CONFIG_SENDER_H_
diff --git a/y2023/constants/test_constants.jinja2.json b/y2023/constants/test_constants.jinja2.json
new file mode 100644
index 0000000..86bd834
--- /dev/null
+++ b/y2023/constants/test_constants.jinja2.json
@@ -0,0 +1,8 @@
+{
+ "constants": [
+ {
+ "team": 7971,
+ "data": {% include 'y2023/constants/test_data/test_team.json' %}
+ }
+ ]
+}
diff --git a/y2023/constants/test_data/calibration_pi-1.json b/y2023/constants/test_data/calibration_pi-1.json
new file mode 100644
index 0000000..1902740
--- /dev/null
+++ b/y2023/constants/test_data/calibration_pi-1.json
@@ -0,0 +1,45 @@
+{
+ "node_name": "pi1",
+ "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/y2023/constants/test_data/calibration_pi-2.json b/y2023/constants/test_data/calibration_pi-2.json
new file mode 100644
index 0000000..289337d
--- /dev/null
+++ b/y2023/constants/test_data/calibration_pi-2.json
@@ -0,0 +1,45 @@
+{
+ "node_name": "pi2",
+ "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/y2023/constants/test_data/calibration_pi-3.json b/y2023/constants/test_data/calibration_pi-3.json
new file mode 100644
index 0000000..073a88a
--- /dev/null
+++ b/y2023/constants/test_data/calibration_pi-3.json
@@ -0,0 +1,45 @@
+{
+ "node_name": "pi3",
+ "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/y2023/constants/test_data/calibration_pi-4.json b/y2023/constants/test_data/calibration_pi-4.json
new file mode 100644
index 0000000..0097bdb
--- /dev/null
+++ b/y2023/constants/test_data/calibration_pi-4.json
@@ -0,0 +1,45 @@
+{
+ "node_name": "pi4",
+ "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/y2023/constants/test_data/target_map.json b/y2023/constants/test_data/target_map.json
new file mode 100644
index 0000000..6bd2947
--- /dev/null
+++ b/y2023/constants/test_data/target_map.json
@@ -0,0 +1,18 @@
+{
+ "target_poses": [
+ {
+ "id": 1,
+ "position": {
+ "x": 10.0,
+ "y": 10.0,
+ "z": 10.0
+ },
+ "orientation": {
+ "w": 0.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 1.0
+ }
+ }
+ ]
+}
diff --git a/y2023/constants/test_data/test_team.json b/y2023/constants/test_data/test_team.json
new file mode 100644
index 0000000..adc9eae
--- /dev/null
+++ b/y2023/constants/test_data/test_team.json
@@ -0,0 +1,17 @@
+{
+ "cameras": [
+ {
+ "calibration": {% include 'y2023/constants/test_data/calibration_pi-1.json' %}
+ },
+ {
+ "calibration": {% include 'y2023/constants/test_data/calibration_pi-2.json' %}
+ },
+ {
+ "calibration": {% include 'y2023/constants/test_data/calibration_pi-3.json' %}
+ },
+ {
+ "calibration": {% include 'y2023/constants/test_data/calibration_pi-4.json' %}
+ }
+ ],
+ "target_map": {% include 'y2023/constants/test_data/target_map.json' %}
+}