Add field name to TargetMap flatbuffer
We'll use this to know which map we're using in code once we load it on
the robot.
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: Id54b82e7b7f07b767a84e266d47468973cbe319e
diff --git a/frc971/vision/target_map.fbs b/frc971/vision/target_map.fbs
index 8adaaaa..50a9d7d 100644
--- a/frc971/vision/target_map.fbs
+++ b/frc971/vision/target_map.fbs
@@ -21,4 +21,7 @@
// This would be solved for by TargetMapper
table TargetMap {
target_poses:[TargetPoseFbs] (id: 0);
+
+ // Unique name of the field
+ field_name:string (id: 1);
}
\ No newline at end of file
diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index 3342670..ccaa805 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -314,7 +314,8 @@
return summary.IsSolutionUsable();
}
-void TargetMapper::Solve(std::optional<std::string_view> output_path) {
+void TargetMapper::Solve(std::string_view field_name,
+ std::optional<std::string_view> output_dir) {
ceres::Problem problem;
BuildOptimizationProblem(&target_poses_, target_constraints_, &problem);
@@ -323,15 +324,18 @@
// TODO(milind): add origin to first target offset to all poses
- auto map_json = MapToJson();
+ auto map_json = MapToJson(field_name);
VLOG(1) << "Solved target poses: " << map_json;
- if (output_path.has_value()) {
- LOG(INFO) << "Writing map to file: " << output_path.value();
- aos::util::WriteStringToFileOrDie(output_path.value(), map_json);
+
+ if (output_dir.has_value()) {
+ std::string output_path =
+ absl::StrCat(output_dir.value(), "/", field_name, ".json");
+ LOG(INFO) << "Writing map to file: " << output_path;
+ aos::util::WriteStringToFileOrDie(output_path, map_json);
}
}
-std::string TargetMapper::MapToJson() const {
+std::string TargetMapper::MapToJson(std::string_view field_name) const {
flatbuffers::FlatBufferBuilder fbb;
// Convert poses to flatbuffers
@@ -346,8 +350,9 @@
target_poses_fbs.emplace_back(target_pose_builder.Finish());
}
- flatbuffers::Offset<TargetMap> target_map_offset =
- CreateTargetMap(fbb, fbb.CreateVector(target_poses_fbs));
+ const auto field_name_offset = fbb.CreateString(field_name);
+ flatbuffers::Offset<TargetMap> target_map_offset = CreateTargetMap(
+ fbb, fbb.CreateVector(target_poses_fbs), field_name_offset);
return aos::FlatbufferToJson(
flatbuffers::GetMutableTemporaryPointer(fbb, target_map_offset),
diff --git a/frc971/vision/target_mapper.h b/frc971/vision/target_mapper.h
index df9f5b4..4dbe52b 100644
--- a/frc971/vision/target_mapper.h
+++ b/frc971/vision/target_mapper.h
@@ -35,11 +35,13 @@
TargetMapper(std::map<TargetId, ceres::examples::Pose2d> target_poses,
std::vector<ceres::examples::Constraint2d> target_constraints);
- // If output_path is set, the map will be saved to that file as a json
- void Solve(std::optional<std::string_view> output_path = std::nullopt);
+ // Solves for the target map. If output_dir is set, the map will be saved to
+ // output_dir/field_name.json
+ void Solve(std::string_view field_name,
+ std::optional<std::string_view> output_dir = std::nullopt);
// Prints target poses into a TargetMap flatbuffer json
- std::string MapToJson() const;
+ std::string MapToJson(std::string_view field_name) const;
static std::optional<TargetPose> GetTargetPoseById(
std::vector<TargetPose> target_poses, TargetId target_id);
diff --git a/frc971/vision/target_mapper_test.cc b/frc971/vision/target_mapper_test.cc
index 37b037b..f56cd8d 100644
--- a/frc971/vision/target_mapper_test.cc
+++ b/frc971/vision/target_mapper_test.cc
@@ -12,6 +12,7 @@
namespace {
constexpr double kToleranceMeters = 0.05;
constexpr double kToleranceRadians = 0.05;
+constexpr std::string_view kFieldName = "test";
} // namespace
#define EXPECT_POSE_NEAR(pose1, pose2) \
@@ -219,7 +220,7 @@
.first;
frc971::vision::TargetMapper mapper(target_poses, target_constraints);
- mapper.Solve();
+ mapper.Solve(kFieldName);
ASSERT_EQ(mapper.target_poses().size(), 2);
EXPECT_POSE_NEAR(mapper.target_poses()[0], MakePose(5.0, 0.0, M_PI));
@@ -253,7 +254,7 @@
.first;
frc971::vision::TargetMapper mapper(target_poses, target_constraints);
- mapper.Solve();
+ mapper.Solve(kFieldName);
ASSERT_EQ(mapper.target_poses().size(), 2);
EXPECT_POSE_NEAR(mapper.target_poses()[0], MakePose(5.0, 0.0, M_PI));
@@ -284,7 +285,7 @@
.first;
frc971::vision::TargetMapper mapper(target_poses, target_constraints);
- mapper.Solve();
+ mapper.Solve(kFieldName);
ASSERT_EQ(mapper.target_poses().size(), 2);
EXPECT_POSE_NEAR(mapper.target_poses()[0], MakePose(5.0, 0.0, M_PI));
@@ -375,7 +376,7 @@
timestamped_target_detections)
.first;
frc971::vision::TargetMapper mapper(target_poses, target_constraints);
- mapper.Solve();
+ mapper.Solve(kFieldName);
for (auto [target_pose_id, mapper_target_pose] : mapper.target_poses()) {
TargetMapper::TargetPose actual_target_pose =
@@ -399,7 +400,7 @@
frc971::vision::TargetMapper mapper_bad_poses(target_poses,
target_constraints);
- mapper_bad_poses.Solve();
+ mapper_bad_poses.Solve(kFieldName);
for (auto [target_pose_id, mapper_target_pose] :
mapper_bad_poses.target_poses()) {