Populate a JSON file with all the possible scoring locations.
Note: I have not double-checked these actual locations. Needs further
checking.
Change-Id: I8338f9389dad7f4e81fcf7cbb7d55faa3c8c064e
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2023/constants/BUILD b/y2023/constants/BUILD
index 0f57f05..02a5968 100644
--- a/y2023/constants/BUILD
+++ b/y2023/constants/BUILD
@@ -31,6 +31,7 @@
"7971.json",
"971.json",
"9971.json",
+ ":scoring_map",
"//y2023/vision/calib_files",
"//y2023/vision/maps",
],
@@ -38,6 +39,15 @@
visibility = ["//visibility:public"],
)
+filegroup(
+ name = "scoring_map",
+ srcs = [
+ "relative_scoring_map.json",
+ "scoring_map.json",
+ ],
+ visibility = ["//y2023:__subpackages__"],
+)
+
flatbuffer_cc_library(
name = "constants_fbs",
srcs = ["constants.fbs"],
diff --git a/y2023/constants/relative_scoring_map.json b/y2023/constants/relative_scoring_map.json
new file mode 100644
index 0000000..b38ff46
--- /dev/null
+++ b/y2023/constants/relative_scoring_map.json
@@ -0,0 +1,80 @@
+{
+ "substation": {
+ /* TODO: Tune substation numbers once we pick a "good" spot. */
+ "left": {
+ "x": -1.0,
+ "y": -0.255,
+ "z": 0.16
+ },
+ "right": {
+ "x": 1.0,
+ "y": -0.255,
+ "z": 0.16
+ }
+ },
+ "nominal_grid": {
+ "bottom": {
+ "left_cone": {
+ "x": -0.559,
+ "y": 0.463,
+ "z": -0.254
+ },
+ "cube": {
+ "x": 0.0,
+ "y": 0.463,
+ "z": -0.254
+ },
+ "right_cone": {
+ "x": 0.559,
+ "y": 0.463,
+ "z": -0.254
+ }
+ },
+ "middle": {
+ "left_cone": {
+ "x": -0.559,
+ "y": -0.407,
+ "z": 0.217
+ },
+ "cube": {
+ "x": 0.0,
+ "y": -0.407,
+ "z": 0.217
+ },
+ "right_cone": {
+ "x": 0.559,
+ "y": -0.407,
+ "z": 0.217
+ }
+ },
+ "top": {
+ "left_cone": {
+ "x": -0.559,
+ "y": -0.707,
+ "z": 0.647
+ },
+ "cube": {
+ "x": 0.0,
+ "y": -0.707,
+ "z": 0.647
+ },
+ "right_cone": {
+ "x": 0.559,
+ "y": -0.707,
+ "z": 0.647
+ }
+ }
+ },
+ "red": {
+ "substation": 5,
+ "left": 3,
+ "middle": 2,
+ "right": 1
+ },
+ "blue": {
+ "substation": 4,
+ "left": 6,
+ "middle": 7,
+ "right": 8
+ }
+}
diff --git a/y2023/constants/scoring_map.json b/y2023/constants/scoring_map.json
new file mode 100644
index 0000000..ebb9b38
--- /dev/null
+++ b/y2023/constants/scoring_map.json
@@ -0,0 +1,348 @@
+{
+ "red": {
+ "substation": {
+ "left": {
+ "x": -8.068,
+ "y": 1.74,
+ "z": 0.95
+ },
+ "right": {
+ "x": -8.068,
+ "y": 3.74,
+ "z": 0.95
+ }
+ },
+ "left_grid": {
+ "bottom": {
+ "left_cone": {
+ "x": 6.99,
+ "y": 0.973,
+ "z": 0.0
+ },
+ "cube": {
+ "x": 6.99,
+ "y": 0.414,
+ "z": 0.0
+ },
+ "right_cone": {
+ "x": 6.99,
+ "y": -0.145,
+ "z": 0.0
+ }
+ },
+ "middle": {
+ "left_cone": {
+ "x": 7.461,
+ "y": 0.973,
+ "z": 0.87
+ },
+ "cube": {
+ "x": 7.461,
+ "y": 0.414,
+ "z": 0.87
+ },
+ "right_cone": {
+ "x": 7.461,
+ "y": -0.145,
+ "z": 0.87
+ }
+ },
+ "top": {
+ "left_cone": {
+ "x": 7.891,
+ "y": 0.973,
+ "z": 1.17
+ },
+ "cube": {
+ "x": 7.891,
+ "y": 0.414,
+ "z": 1.17
+ },
+ "right_cone": {
+ "x": 7.891,
+ "y": -0.145,
+ "z": 1.17
+ }
+ }
+ },
+ "middle_grid": {
+ "bottom": {
+ "left_cone": {
+ "x": 6.99,
+ "y": -0.703,
+ "z": 0.0
+ },
+ "cube": {
+ "x": 6.99,
+ "y": -1.262,
+ "z": 0.0
+ },
+ "right_cone": {
+ "x": 6.99,
+ "y": -1.821,
+ "z": 0.0
+ }
+ },
+ "middle": {
+ "left_cone": {
+ "x": 7.461,
+ "y": -0.703,
+ "z": 0.87
+ },
+ "cube": {
+ "x": 7.461,
+ "y": -1.262,
+ "z": 0.87
+ },
+ "right_cone": {
+ "x": 7.461,
+ "y": -1.821,
+ "z": 0.87
+ }
+ },
+ "top": {
+ "left_cone": {
+ "x": 7.891,
+ "y": -0.703,
+ "z": 1.17
+ },
+ "cube": {
+ "x": 7.891,
+ "y": -1.262,
+ "z": 1.17
+ },
+ "right_cone": {
+ "x": 7.891,
+ "y": -1.821,
+ "z": 1.17
+ }
+ }
+ },
+ "right_grid": {
+ "bottom": {
+ "left_cone": {
+ "x": 6.99,
+ "y": -2.379,
+ "z": 0.0
+ },
+ "cube": {
+ "x": 6.99,
+ "y": -2.938,
+ "z": 0.0
+ },
+ "right_cone": {
+ "x": 6.99,
+ "y": -3.497,
+ "z": 0.0
+ }
+ },
+ "middle": {
+ "left_cone": {
+ "x": 7.461,
+ "y": -2.379,
+ "z": 0.87
+ },
+ "cube": {
+ "x": 7.461,
+ "y": -2.938,
+ "z": 0.87
+ },
+ "right_cone": {
+ "x": 7.461,
+ "y": -3.497,
+ "z": 0.87
+ }
+ },
+ "top": {
+ "left_cone": {
+ "x": 7.891,
+ "y": -2.379,
+ "z": 1.17
+ },
+ "cube": {
+ "x": 7.891,
+ "y": -2.938,
+ "z": 1.17
+ },
+ "right_cone": {
+ "x": 7.891,
+ "y": -3.497,
+ "z": 1.17
+ }
+ }
+ }
+ },
+ "blue": {
+ "substation": {
+ "left": {
+ "x": 8.069,
+ "y": 3.74,
+ "z": 0.95
+ },
+ "right": {
+ "x": 8.069,
+ "y": 1.74,
+ "z": 0.95
+ }
+ },
+ "left_grid": {
+ "bottom": {
+ "left_cone": {
+ "x": -6.989,
+ "y": -0.145,
+ "z": 0.0
+ },
+ "cube": {
+ "x": -6.989,
+ "y": 0.414,
+ "z": 0.0
+ },
+ "right_cone": {
+ "x": -6.989,
+ "y": 0.973,
+ "z": 0.0
+ }
+ },
+ "middle": {
+ "left_cone": {
+ "x": -7.46,
+ "y": -0.145,
+ "z": 0.87
+ },
+ "cube": {
+ "x": -7.46,
+ "y": 0.414,
+ "z": 0.87
+ },
+ "right_cone": {
+ "x": -7.46,
+ "y": 0.973,
+ "z": 0.87
+ }
+ },
+ "top": {
+ "left_cone": {
+ "x": -7.89,
+ "y": -0.145,
+ "z": 1.17
+ },
+ "cube": {
+ "x": -7.89,
+ "y": 0.414,
+ "z": 1.17
+ },
+ "right_cone": {
+ "x": -7.89,
+ "y": 0.973,
+ "z": 1.17
+ }
+ }
+ },
+ "middle_grid": {
+ "bottom": {
+ "left_cone": {
+ "x": -6.989,
+ "y": -1.821,
+ "z": 0.0
+ },
+ "cube": {
+ "x": -6.989,
+ "y": -1.262,
+ "z": 0.0
+ },
+ "right_cone": {
+ "x": -6.989,
+ "y": -0.703,
+ "z": 0.0
+ }
+ },
+ "middle": {
+ "left_cone": {
+ "x": -7.46,
+ "y": -1.821,
+ "z": 0.87
+ },
+ "cube": {
+ "x": -7.46,
+ "y": -1.262,
+ "z": 0.87
+ },
+ "right_cone": {
+ "x": -7.46,
+ "y": -0.703,
+ "z": 0.87
+ }
+ },
+ "top": {
+ "left_cone": {
+ "x": -7.89,
+ "y": -1.821,
+ "z": 1.17
+ },
+ "cube": {
+ "x": -7.89,
+ "y": -1.262,
+ "z": 1.17
+ },
+ "right_cone": {
+ "x": -7.89,
+ "y": -0.703,
+ "z": 1.17
+ }
+ }
+ },
+ "right_grid": {
+ "bottom": {
+ "left_cone": {
+ "x": -6.989,
+ "y": -3.497,
+ "z": 0.0
+ },
+ "cube": {
+ "x": -6.989,
+ "y": -2.938,
+ "z": 0.0
+ },
+ "right_cone": {
+ "x": -6.989,
+ "y": -2.379,
+ "z": 0.0
+ }
+ },
+ "middle": {
+ "left_cone": {
+ "x": -7.46,
+ "y": -3.497,
+ "z": 0.87
+ },
+ "cube": {
+ "x": -7.46,
+ "y": -2.938,
+ "z": 0.87
+ },
+ "right_cone": {
+ "x": -7.46,
+ "y": -2.379,
+ "z": 0.87
+ }
+ },
+ "top": {
+ "left_cone": {
+ "x": -7.89,
+ "y": -3.497,
+ "z": 1.17
+ },
+ "cube": {
+ "x": -7.89,
+ "y": -2.938,
+ "z": 1.17
+ },
+ "right_cone": {
+ "x": -7.89,
+ "y": -2.379,
+ "z": 1.17
+ }
+ }
+ }
+ }
+}
\ No newline at end of file
diff --git a/y2023/localizer/BUILD b/y2023/localizer/BUILD
index 8f1237c..2345900 100644
--- a/y2023/localizer/BUILD
+++ b/y2023/localizer/BUILD
@@ -31,7 +31,6 @@
"visualization.fbs",
],
gen_reflections = True,
- target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
deps = [
":status_fbs",
@@ -47,12 +46,87 @@
],
)
+flatbuffer_cc_library(
+ name = "scoring_map_fbs",
+ srcs = [
+ "scoring_map.fbs",
+ ],
+ gen_reflections = True,
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/vision:target_map_fbs",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "relative_scoring_map_fbs",
+ srcs = [
+ "relative_scoring_map.fbs",
+ ],
+ gen_reflections = True,
+ visibility = ["//visibility:public"],
+ deps = [
+ ":scoring_map_fbs",
+ ],
+)
+
+cc_library(
+ name = "utils",
+ srcs = ["utils.cc"],
+ hdrs = ["utils.h"],
+ deps = [
+ "//frc971/vision:target_map_fbs",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_library(
+ name = "map_expander_lib",
+ srcs = ["map_expander_lib.cc"],
+ hdrs = ["map_expander_lib.h"],
+ deps = [
+ ":relative_scoring_map_fbs",
+ ":scoring_map_fbs",
+ ":utils",
+ "//aos:flatbuffers",
+ "//aos:json_to_flatbuffer",
+ ],
+)
+
+cc_binary(
+ name = "map_expander",
+ srcs = ["map_expander.cc"],
+ data = [
+ "//y2023/constants:scoring_map",
+ "//y2023/vision/maps",
+ ],
+ deps = [
+ ":map_expander_lib",
+ "//aos:init",
+ ],
+)
+
+cc_test(
+ name = "map_expander_lib_test",
+ srcs = ["map_expander_lib_test.cc"],
+ data = [
+ "//y2023/constants:scoring_map",
+ "//y2023/vision/maps",
+ ],
+ deps = [
+ ":map_expander_lib",
+ "//aos/testing:flatbuffer_eq",
+ "//aos/testing:googletest",
+ ],
+)
+
cc_library(
name = "localizer",
srcs = ["localizer.cc"],
hdrs = ["localizer.h"],
deps = [
":status_fbs",
+ ":utils",
":visualization_fbs",
"//aos/containers:sized_array",
"//aos/events:event_loop",
@@ -77,6 +151,7 @@
deps = [
":localizer",
":status_fbs",
+ ":utils",
"//aos/events:simulated_event_loop",
"//aos/events/logging:log_writer",
"//aos/testing:googletest",
diff --git a/y2023/localizer/localizer.cc b/y2023/localizer/localizer.cc
index 30d15e8..4378f61 100644
--- a/y2023/localizer/localizer.cc
+++ b/y2023/localizer/localizer.cc
@@ -4,6 +4,7 @@
#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "frc971/control_loops/pose.h"
#include "y2023/constants.h"
+#include "y2023/localizer/utils.h"
namespace y2023::localizer {
namespace {
@@ -36,16 +37,6 @@
}
} // namespace
-Localizer::Transform PoseToTransform(
- const frc971::vision::TargetPoseFbs *pose) {
- const frc971::vision::Position *position = pose->position();
- const frc971::vision::Quaternion *quaternion = pose->orientation();
- return (Eigen::Translation3d(
- Eigen::Vector3d(position->x(), position->y(), position->z())) *
- Eigen::Quaterniond(quaternion->w(), quaternion->x(), quaternion->y(),
- quaternion->z()))
- .matrix();
-}
std::array<Localizer::CameraState, Localizer::kNumCameras>
Localizer::MakeCameras(const Constants &constants, aos::EventLoop *event_loop) {
diff --git a/y2023/localizer/localizer.h b/y2023/localizer/localizer.h
index 8c53467..ba5b1cf 100644
--- a/y2023/localizer/localizer.h
+++ b/y2023/localizer/localizer.h
@@ -116,8 +116,5 @@
// For the status message.
std::optional<Eigen::Vector2d> last_encoder_readings_;
};
-
-// Converts a TargetPoseFbs into a transformation matrix.
-Localizer::Transform PoseToTransform(const frc971::vision::TargetPoseFbs *pose);
} // namespace y2023::localizer
#endif // Y2023_LOCALIZER_LOCALIZER_H_
diff --git a/y2023/localizer/localizer_test.cc b/y2023/localizer/localizer_test.cc
index 77af69b..554ab5d 100644
--- a/y2023/localizer/localizer_test.cc
+++ b/y2023/localizer/localizer_test.cc
@@ -10,6 +10,7 @@
#include "y2023/constants/simulated_constants_sender.h"
#include "y2023/control_loops/drivetrain/drivetrain_base.h"
#include "y2023/localizer/status_generated.h"
+#include "y2023/localizer/utils.h"
DEFINE_string(output_folder, "",
"If set, logs all channels to the provided logfile.");
diff --git a/y2023/localizer/map_expander.cc b/y2023/localizer/map_expander.cc
new file mode 100644
index 0000000..e13c0a1
--- /dev/null
+++ b/y2023/localizer/map_expander.cc
@@ -0,0 +1,23 @@
+#include "aos/init.h"
+#include "aos/util/file.h"
+#include "gflags/gflags.h"
+#include "y2023/localizer/map_expander_lib.h"
+
+DEFINE_string(target_map, "y2023/vision/maps/target_map.json",
+ "Path to the target map JSON file.");
+DEFINE_string(relative_map, "y2023/constants/relative_scoring_map.json",
+ "Path to the relative scoring map JSON file.");
+DEFINE_string(output, "y2023/constants/scoring_map.json",
+ "Path to the output scoring map JSON file.");
+
+int main(int argc, char *argv[]) {
+ aos::InitGoogle(&argc, &argv);
+ aos::FlatbufferDetachedBuffer<y2023::localizer::ScoringMap> map =
+ y2023::localizer::ExpandMap(
+ aos::util::ReadFileToStringOrDie(FLAGS_relative_map),
+ aos::util::ReadFileToStringOrDie(FLAGS_target_map));
+ aos::util::WriteStringToFileOrDie(
+ FLAGS_output,
+ aos::FlatbufferToJson(map, {.multi_line = true, .max_multi_line = true}));
+ return EXIT_SUCCESS;
+}
diff --git a/y2023/localizer/map_expander_lib.cc b/y2023/localizer/map_expander_lib.cc
new file mode 100644
index 0000000..5a94985
--- /dev/null
+++ b/y2023/localizer/map_expander_lib.cc
@@ -0,0 +1,124 @@
+#include "y2023/localizer/map_expander_lib.h"
+
+#include "y2023/localizer/utils.h"
+
+namespace y2023::localizer {
+namespace {
+flatbuffers::Offset<frc971::vision::Position> AbsolutePositionForTagAndRelative(
+ const Eigen::Affine3d &tag, const frc971::vision::Position *relative,
+ flatbuffers::FlatBufferBuilder *fbb) {
+ const Eigen::Vector3d absolute =
+ tag * Eigen::Vector3d(relative->x(), relative->y(), relative->z());
+ return frc971::vision::CreatePosition(*fbb, absolute.x(), absolute.y(),
+ absolute.z());
+}
+
+flatbuffers::Offset<ScoringRow> AbsoluteRowForTagAndRelative(
+ const Eigen::Affine3d &tag, const ScoringRow *relative_row,
+ flatbuffers::FlatBufferBuilder *fbb) {
+ auto left_offset =
+ AbsolutePositionForTagAndRelative(tag, relative_row->left_cone(), fbb);
+ auto cube_offset =
+ AbsolutePositionForTagAndRelative(tag, relative_row->cube(), fbb);
+ auto right_offset =
+ AbsolutePositionForTagAndRelative(tag, relative_row->right_cone(), fbb);
+ return CreateScoringRow(*fbb, left_offset, cube_offset, right_offset);
+}
+
+flatbuffers::Offset<ScoringGrid> AbsoluteGridForTagAndRelative(
+ const Eigen::Affine3d &tag, const ScoringGrid *relative_grid,
+ flatbuffers::FlatBufferBuilder *fbb) {
+ auto bottom_offset =
+ AbsoluteRowForTagAndRelative(tag, relative_grid->bottom(), fbb);
+ auto middle_offset =
+ AbsoluteRowForTagAndRelative(tag, relative_grid->middle(), fbb);
+ auto top_offset =
+ AbsoluteRowForTagAndRelative(tag, relative_grid->top(), fbb);
+ return CreateScoringGrid(*fbb, bottom_offset, middle_offset, top_offset);
+}
+
+flatbuffers::Offset<ScoringGrid> RelativeGridForTagAndAbsolute(
+ const Eigen::Affine3d &tag, const ScoringGrid *absolute_grid,
+ flatbuffers::FlatBufferBuilder *fbb) {
+ return AbsoluteGridForTagAndRelative(tag.inverse(), absolute_grid, fbb);
+}
+
+flatbuffers::Offset<DoubleSubstation> AbsoluteSubstationForTagAndRelative(
+ const Eigen::Affine3d &tag, const DoubleSubstation *relative_station,
+ flatbuffers::FlatBufferBuilder *fbb) {
+ auto left_offset =
+ AbsolutePositionForTagAndRelative(tag, relative_station->left(), fbb);
+ auto right_offset =
+ AbsolutePositionForTagAndRelative(tag, relative_station->right(), fbb);
+ return CreateDoubleSubstation(*fbb, left_offset, right_offset);
+}
+
+flatbuffers::Offset<DoubleSubstation> RelativeSubstationForTagAndAbsolute(
+ const Eigen::Affine3d &tag, const DoubleSubstation *absolute_station,
+ flatbuffers::FlatBufferBuilder *fbb) {
+ return AbsoluteSubstationForTagAndRelative(tag.inverse(), absolute_station,
+ fbb);
+}
+
+flatbuffers::Offset<HalfField> AbsoluteHalfForTagsAndRelative(
+ const std::map<uint64_t, Eigen::Affine3d> &april_tags,
+ const RelativeHalfField *relative_half, const RelativeScoringMap *map,
+ flatbuffers::FlatBufferBuilder *fbb) {
+ auto substation_offset = AbsoluteSubstationForTagAndRelative(
+ april_tags.at(relative_half->substation()), map->substation(), fbb);
+ auto left_offset = AbsoluteGridForTagAndRelative(
+ april_tags.at(relative_half->left()), map->nominal_grid(), fbb);
+ auto middle_offset = AbsoluteGridForTagAndRelative(
+ april_tags.at(relative_half->middle()), map->nominal_grid(), fbb);
+ auto right_offset = AbsoluteGridForTagAndRelative(
+ april_tags.at(relative_half->right()), map->nominal_grid(), fbb);
+ return CreateHalfField(*fbb, substation_offset, left_offset, middle_offset,
+ right_offset);
+}
+} // namespace
+
+std::map<uint64_t, Eigen::Affine3d> AprilTagPoses(
+ const frc971::vision::TargetMap *map) {
+ std::map<uint64_t, Eigen::Affine3d> april_tags;
+ for (const frc971::vision::TargetPoseFbs *target : *map->target_poses()) {
+ const uint64_t id = target->id();
+ CHECK(april_tags.count(id) == 0);
+ april_tags[id] = PoseToTransform(target);
+ }
+ return april_tags;
+}
+
+aos::FlatbufferDetachedBuffer<ScoringMap> ExpandMap(
+ const RelativeScoringMap *relative_map,
+ const frc971::vision::TargetMap *target_map) {
+ std::map<uint64_t, Eigen::Affine3d> april_tags = AprilTagPoses(target_map);
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.Finish(CreateScoringMap(
+ fbb,
+ AbsoluteHalfForTagsAndRelative(april_tags, relative_map->red(),
+ relative_map, &fbb),
+ AbsoluteHalfForTagsAndRelative(april_tags, relative_map->blue(),
+ relative_map, &fbb)));
+ return fbb.Release();
+}
+
+aos::FlatbufferDetachedBuffer<ScoringGrid> RelativeGridForTag(
+ const ScoringGrid *absolute_grid,
+ const frc971::vision::TargetMap *target_map, uint64_t tag) {
+ const Eigen::Affine3d tag_pose = AprilTagPoses(target_map).at(tag);
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.Finish(RelativeGridForTagAndAbsolute(tag_pose, absolute_grid, &fbb));
+ return fbb.Release();
+}
+
+aos::FlatbufferDetachedBuffer<DoubleSubstation> RelativeSubstationForTag(
+ const DoubleSubstation *absolute_substation,
+ const frc971::vision::TargetMap *target_map, uint64_t tag) {
+ const Eigen::Affine3d tag_pose = AprilTagPoses(target_map).at(tag);
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.Finish(
+ RelativeSubstationForTagAndAbsolute(tag_pose, absolute_substation, &fbb));
+ return fbb.Release();
+}
+
+} // namespace y2023::localizer
diff --git a/y2023/localizer/map_expander_lib.h b/y2023/localizer/map_expander_lib.h
new file mode 100644
index 0000000..773a769
--- /dev/null
+++ b/y2023/localizer/map_expander_lib.h
@@ -0,0 +1,49 @@
+#ifndef Y2023_LOCALIZER_MAP_EXPANDER_LIB_H_
+#define Y2023_LOCALIZER_MAP_EXPANDER_LIB_H_
+
+#include <Eigen/Dense>
+#include <map>
+
+#include "aos/flatbuffers.h"
+#include "aos/json_to_flatbuffer.h"
+#include "frc971/vision/target_map_generated.h"
+#include "y2023/localizer/relative_scoring_map_generated.h"
+#include "y2023/localizer/scoring_map_generated.h"
+
+namespace y2023::localizer {
+
+// This function takes a RelativeScoringMap plus a TargetMap with april tag
+// locations and uses it to generate a map of the various scoring locations on
+// the field.
+// This allows us to use a condensed JSON file (the RelativeScoringMap) to
+// generate the more verbose ScoringMap, making it so that humans don't have to
+// modify every single location in the verbose ScoringMap should something
+// change. However, we do still want to have the full ScoringMap available
+// for modifications should we discover that, e.g., an individual field has
+// one individual pole or april tag or something out of aligment.
+aos::FlatbufferDetachedBuffer<ScoringMap> ExpandMap(
+ const RelativeScoringMap *relative_map,
+ const frc971::vision::TargetMap *target_map);
+
+inline aos::FlatbufferDetachedBuffer<ScoringMap> ExpandMap(
+ std::string_view relative_map_json, std::string_view target_map_json) {
+ aos::FlatbufferDetachedBuffer<RelativeScoringMap> relative =
+ aos::JsonToFlatbuffer<RelativeScoringMap>(relative_map_json);
+ aos::FlatbufferDetachedBuffer<frc971::vision::TargetMap> target_map =
+ aos::JsonToFlatbuffer<frc971::vision::TargetMap>(target_map_json);
+ return ExpandMap(&relative.message(), &target_map.message());
+}
+
+aos::FlatbufferDetachedBuffer<ScoringGrid> RelativeGridForTag(
+ const ScoringGrid *absolute_grid,
+ const frc971::vision::TargetMap *target_map, uint64_t tag);
+
+aos::FlatbufferDetachedBuffer<DoubleSubstation> RelativeSubstationForTag(
+ const DoubleSubstation *absolute_substation,
+ const frc971::vision::TargetMap *target_map, uint64_t tag);
+
+std::map<uint64_t, Eigen::Affine3d> AprilTagPoses(
+ const frc971::vision::TargetMap *map);
+
+} // namespace y2023::localizer
+#endif // Y2023_LOCALIZER_MAP_EXPANDER_LIB_H_
diff --git a/y2023/localizer/map_expander_lib_test.cc b/y2023/localizer/map_expander_lib_test.cc
new file mode 100644
index 0000000..5b353a1
--- /dev/null
+++ b/y2023/localizer/map_expander_lib_test.cc
@@ -0,0 +1,54 @@
+#include "y2023/localizer/map_expander_lib.h"
+
+#include "gtest/gtest.h"
+#include "aos/testing/flatbuffer_eq.h"
+
+namespace y2023::localizer::testing {
+class MapExpanderTest : public ::testing::Test {
+ protected:
+ MapExpanderTest()
+ : relative_map_(aos::JsonFileToFlatbuffer<RelativeScoringMap>(
+ "y2023/constants/relative_scoring_map.json")),
+ target_map_(aos::JsonFileToFlatbuffer<frc971::vision::TargetMap>(
+ "y2023/vision/maps/target_map.json")),
+ absolute_map_(
+ ExpandMap(&relative_map_.message(), &target_map_.message())) {}
+ const aos::FlatbufferDetachedBuffer<RelativeScoringMap> relative_map_;
+ const aos::FlatbufferDetachedBuffer<frc971::vision::TargetMap> target_map_;
+ const aos::FlatbufferDetachedBuffer<ScoringMap> absolute_map_;
+};
+TEST_F(MapExpanderTest, BackAndForthConsistent) {
+ // Use FlatbufferToJson instead of FlatbufferEq because we don't want
+ // equivalent but different encoded floating point numbers to get
+ // evaluated differently.
+#define CHECK_REVERSE(color, grid) \
+ { \
+ ASSERT_TRUE(absolute_map_.message().has_##color()); \
+ auto half = absolute_map_.message().color(); \
+ ASSERT_TRUE(half->has_##grid##_grid()); \
+ auto grid = half->grid##_grid(); \
+ auto relative_grid = \
+ RelativeGridForTag(grid, &target_map_.message(), \
+ relative_map_.message().color()->grid()); \
+ EXPECT_EQ(aos::FlatbufferToJson(relative_map_.message().nominal_grid()), \
+ aos::FlatbufferToJson(&relative_grid.message())); \
+ }
+ CHECK_REVERSE(blue, left);
+ CHECK_REVERSE(blue, middle);
+ CHECK_REVERSE(blue, right);
+ CHECK_REVERSE(red, left);
+ CHECK_REVERSE(red, middle);
+ CHECK_REVERSE(red, right);
+}
+
+// Test that the currently checked-in map is consistent with the results of
+// ExpandMap.
+TEST_F(MapExpanderTest, ExpandMap) {
+ const std::string stored_map =
+ aos::util::ReadFileToStringOrDie("y2023/constants/scoring_map.json");
+ // TODO: Provide coherent error messages so that changes can be accommodated.
+ EXPECT_EQ(aos::FlatbufferToJson(absolute_map_,
+ {.multi_line = true, .max_multi_line = true}),
+ stored_map);
+}
+} // namespace y2023::localizer::testing
diff --git a/y2023/localizer/relative_scoring_map.fbs b/y2023/localizer/relative_scoring_map.fbs
new file mode 100644
index 0000000..9d1c979
--- /dev/null
+++ b/y2023/localizer/relative_scoring_map.fbs
@@ -0,0 +1,38 @@
+include "y2023/localizer/scoring_map.fbs";
+
+namespace y2023.localizer;
+
+// This file provides a schema for a more stripped-down version of the
+// ScoringMap. In this version, we try to deduplicate as much information
+// as is reasonable so that it is easier for humans to modify and verify the
+// numbers.
+// In particular, we:
+// * Assume that each batch of 9 scoring grid is identical with respect to
+// the april tag for that scoring grid.
+// * Provide all coordinates relative to the april tags in question.
+//
+// Note that when we say "relative to the april tags," that does mean in the
+// april tag frame. As such, the Z of the relative position will be how far
+// behind the april tag the scoring position is (i.e., the X offset), the
+// relative X will be to the right of the target, and the Y will be how far
+// below the center of the target the scoring position is.
+
+// The april tag ids for each field component on one half of the field.
+table RelativeHalfField {
+ substation:uint64 (id: 0);
+ left:uint64 (id: 1);
+ middle:uint64 (id: 2);
+ right:uint64 (id: 3);
+}
+
+table RelativeScoringMap {
+ // Substation numbers are relative to the april tag.
+ substation:DoubleSubstation (id: 0);
+ // The nominal grid will provide positions that are all relative to the
+ // coordinates of the april tag for that grid.
+ nominal_grid:ScoringGrid (id: 1);
+ red:RelativeHalfField (id: 2);
+ blue:RelativeHalfField (id: 3);
+}
+
+root_type RelativeScoringMap;
diff --git a/y2023/localizer/scoring_map.fbs b/y2023/localizer/scoring_map.fbs
new file mode 100644
index 0000000..92b48ca
--- /dev/null
+++ b/y2023/localizer/scoring_map.fbs
@@ -0,0 +1,44 @@
+include "frc971/vision/target_map.fbs";
+
+namespace y2023.localizer;
+
+// "left" and "right" in this file are taken from the perspective of a robot
+// on the field.
+
+// A row of three scoring positions, where the cube scoring locations will be
+// in the middle.
+table ScoringRow {
+ left_cone:frc971.vision.Position (id: 0);
+ cube:frc971.vision.Position (id: 1);
+ right_cone:frc971.vision.Position (id: 2);
+}
+
+// A batch of 9 scoring locations, anchored by a single april tag target.
+table ScoringGrid {
+ // Either game piece type can be scored in all the locations on the bottom
+ // row.
+ bottom:ScoringRow (id: 0);
+ middle:ScoringRow (id: 1);
+ top:ScoringRow (id: 2);
+}
+
+// Location where we can retrieve game pieces from the human player.
+table DoubleSubstation {
+ left:frc971.vision.Position (id: 0);
+ right:frc971.vision.Position (id: 1);
+}
+
+table HalfField {
+ // The substation will be across the field from the grids.
+ substation:DoubleSubstation (id: 0);
+ left_grid:ScoringGrid (id: 1);
+ middle_grid:ScoringGrid (id: 2);
+ right_grid:ScoringGrid (id: 3);
+}
+
+table ScoringMap {
+ red:HalfField (id: 0);
+ blue:HalfField (id: 1);
+}
+
+root_type ScoringMap;
diff --git a/y2023/localizer/utils.cc b/y2023/localizer/utils.cc
new file mode 100644
index 0000000..7faca1f
--- /dev/null
+++ b/y2023/localizer/utils.cc
@@ -0,0 +1,14 @@
+#include "y2023/localizer/utils.h"
+
+namespace y2023::localizer {
+Eigen::Matrix<double, 4, 4> PoseToTransform(
+ const frc971::vision::TargetPoseFbs *pose) {
+ const frc971::vision::Position *position = pose->position();
+ const frc971::vision::Quaternion *quaternion = pose->orientation();
+ return (Eigen::Translation3d(
+ Eigen::Vector3d(position->x(), position->y(), position->z())) *
+ Eigen::Quaterniond(quaternion->w(), quaternion->x(), quaternion->y(),
+ quaternion->z()))
+ .matrix();
+}
+} // namespace y2023::localizer
diff --git a/y2023/localizer/utils.h b/y2023/localizer/utils.h
new file mode 100644
index 0000000..8241cf8
--- /dev/null
+++ b/y2023/localizer/utils.h
@@ -0,0 +1,14 @@
+#ifndef Y2023_LOCALIZER_UTILS_H_
+#define Y2023_LOCALIZER_UTILS_H_
+
+#include <Eigen/Dense>
+
+#include "frc971/vision/target_map_generated.h"
+
+namespace y2023::localizer {
+// Converts a TargetPoseFbs into a transformation matrix.
+Eigen::Matrix<double, 4, 4> PoseToTransform(
+ const frc971::vision::TargetPoseFbs *pose);
+} // namespace y2023::localizer
+
+#endif // Y2023_LOCALIZER_UTILS_H_
diff --git a/y2023/vision/maps/target_map.json b/y2023/vision/maps/target_map.json
index 8d971f3..07d3464 100644
--- a/y2023/vision/maps/target_map.json
+++ b/y2023/vision/maps/target_map.json
@@ -1,4 +1,8 @@
{
+/* 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.*/
"target_poses": [
{
"id": 1,