Add all flags and remap channels in target mapping
Added everything needed to have target mapping working from input to
output with the engineering room log.
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: Ifedb3b4cd3d0cede8f1f9599e3bdb0b43e795f74
diff --git a/y2023/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
index b41d7d5..a941923 100644
--- a/y2023/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -12,12 +12,20 @@
#include "opencv2/highgui.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc.hpp"
+#include "y2023/constants/simulated_constants_sender.h"
#include "y2023/vision/aprilrobotics.h"
#include "y2023/vision/vision_util.h"
-DEFINE_string(json_path, "target_map.json",
+DEFINE_string(json_path, "y2023/vision/maps/target_map.json",
"Specify path for json with initial pose guesses.");
-DECLARE_int32(team_number);
+DEFINE_string(constants_path, "y2023/constants/constants.json",
+ "Path to the constant file");
+DEFINE_string(output_dir, "y2023/vision/maps",
+ "Directory to write solved target map to");
+DEFINE_string(field_name, "charged_up",
+ "Field name, for the output json filename and flatbuffer field");
+DEFINE_int32(team_number, 7971,
+ "Use the calibration for a node with this team number");
namespace y2023 {
namespace vision {
@@ -112,10 +120,19 @@
for (size_t i = 1; i <= kNumPis; i++) {
reader.RemapLoggedChannel(absl::StrFormat("/pi%u/camera", i),
"frc971.vision.TargetMap");
+ reader.RemapLoggedChannel(absl::StrFormat("/pi%u/camera", i),
+ "foxglove.ImageAnnotations");
+ reader.RemapLoggedChannel(absl::StrFormat("/pi%u/constants", i),
+ "y2023.Constants");
}
+ reader.RemapLoggedChannel("/imu/constants", "y2023.Constants");
+
reader.Register();
+ SendSimulationConstants(reader.event_loop_factory(), FLAGS_team_number,
+ FLAGS_constants_path);
+
std::vector<std::unique_ptr<AprilRoboticsDetector>> detectors;
const aos::Node *pi1 =
@@ -160,7 +177,7 @@
DataAdapter::MatchTargetDetections(timestamped_target_detections);
frc971::vision::TargetMapper mapper(FLAGS_json_path, target_constraints);
- mapper.Solve("charged_up");
+ mapper.Solve(FLAGS_field_name, FLAGS_output_dir);
// Clean up all the pointers
for (auto &detector_ptr : detectors) {