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/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_