Add a tool for checking localizations
Allows us to put a non-field april tag on the field and see where the
robot thinks it is relative to the field origin.
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I194c2ca988500b5073f4f375d5e6a8f91be892bd
diff --git a/y2023/BUILD b/y2023/BUILD
index dc5e708..29e9322 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -41,6 +41,7 @@
"//aos/starter:irq_affinity",
"//aos/util:foxglove_websocket",
"//y2023/vision:viewer",
+ "//y2023/vision:localization_verifier",
"//y2023/vision:aprilrobotics",
"//y2023/localizer:localizer_main",
"//y2023/constants:constants_sender",
diff --git a/y2023/localizer/BUILD b/y2023/localizer/BUILD
index 8f1237c..e137e9e 100644
--- a/y2023/localizer/BUILD
+++ b/y2023/localizer/BUILD
@@ -51,6 +51,7 @@
name = "localizer",
srcs = ["localizer.cc"],
hdrs = ["localizer.h"],
+ visibility = ["//visibility:public"],
deps = [
":status_fbs",
":visualization_fbs",
diff --git a/y2023/vision/BUILD b/y2023/vision/BUILD
index 724bdc0..bb83489 100644
--- a/y2023/vision/BUILD
+++ b/y2023/vision/BUILD
@@ -37,6 +37,25 @@
)
cc_binary(
+ name = "localization_verifier",
+ srcs = [
+ "localization_verifier.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//y2023:__subpackages__"],
+ deps = [
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//frc971/constants:constants_sender_lib",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
+ "//frc971/vision:vision_fbs",
+ "//y2023/localizer",
+ "//y2023/vision:vision_util",
+ "@com_google_absl//absl/strings",
+ ],
+)
+
+cc_binary(
name = "target_mapping",
srcs = [
"target_mapping.cc",
diff --git a/y2023/vision/localization_verifier.cc b/y2023/vision/localization_verifier.cc
new file mode 100644
index 0000000..7a5f792
--- /dev/null
+++ b/y2023/vision/localization_verifier.cc
@@ -0,0 +1,103 @@
+#include "aos/init.h"
+#include "frc971/constants/constants_sender_lib.h"
+#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
+#include "frc971/vision/vision_generated.h"
+#include "glog/logging.h"
+#include "y2023/localizer/localizer.h"
+
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+
+DEFINE_uint64(min_april_id, 1,
+ "Minimum april id to consider as part of the field and ignore");
+DEFINE_uint64(max_april_id, 8,
+ "Maximum april id to consider as part of the field and ignore");
+
+// This binary allows us to place extra april tags on the field and verify
+// that we can compute their field pose correctly
+
+namespace y2023::vision {
+
+using localizer::Localizer;
+
+Localizer::Transform LocalizerOutputToTransform(
+ const frc971::controls::LocalizerOutput &localizer) {
+ const auto T_field_robot =
+ Eigen::Translation3d(localizer.x(), localizer.y(), 0.0);
+
+ Eigen::AngleAxisd robot_yaw_angle(localizer.theta(),
+ Eigen::Vector3d::UnitZ());
+ const auto R_field_robot = Eigen::Quaterniond(robot_yaw_angle);
+ const Localizer::Transform H_field_robot =
+ (T_field_robot * R_field_robot).matrix();
+ return H_field_robot;
+}
+
+void HandleDetections(
+ const frc971::vision::TargetMap &detections,
+ const Localizer::Transform &H_robot_camera,
+ aos::Fetcher<frc971::controls::LocalizerOutput> *localizer_fetcher) {
+ localizer_fetcher->Fetch();
+ CHECK(localizer_fetcher->get());
+
+ for (const auto *target_pose : *detections.target_poses()) {
+ // Only look at april tags not part of the field
+ if (target_pose->id() >= FLAGS_min_april_id &&
+ target_pose->id() <= FLAGS_max_april_id) {
+ continue;
+ }
+
+ const Localizer::Transform H_camera_target =
+ localizer::PoseToTransform(target_pose);
+ const Localizer::Transform H_field_robot =
+ LocalizerOutputToTransform(*localizer_fetcher->get());
+
+ // Get the field-based target pose using the detection, extrinsics, and
+ // localizer output
+ const Localizer::Transform H_field_target =
+ H_field_robot * H_robot_camera * H_camera_target;
+
+ LOG(INFO) << "Field to target " << target_pose->id();
+ LOG(INFO) << "Transform: " << H_field_target;
+ LOG(INFO) << "Translation: "
+ << Eigen::Affine3d(H_field_target).translation();
+ LOG(INFO);
+ }
+}
+
+void LocalizationVerifierMain() {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ frc971::constants::WaitForConstants<Constants>(&config.message());
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ frc971::constants::ConstantsFetcher<Constants> constants_fetcher(&event_loop);
+
+ aos::Fetcher<frc971::controls::LocalizerOutput> localizer_fetcher =
+ event_loop.MakeFetcher<frc971::controls::LocalizerOutput>("/localizer");
+
+ for (const auto *camera : *constants_fetcher.constants().cameras()) {
+ CHECK(camera->has_calibration());
+ Localizer::Transform H_robot_camera =
+ frc971::control_loops::drivetrain::FlatbufferToTransformationMatrix(
+ *camera->calibration()->fixed_extrinsics());
+
+ const std::string_view pi_name =
+ camera->calibration()->node_name()->string_view();
+ event_loop.MakeWatcher(absl::StrCat("/", pi_name, "/camera"),
+ [&](const frc971::vision::TargetMap &target_map) {
+ HandleDetections(target_map, H_robot_camera,
+ &localizer_fetcher);
+ });
+
+ event_loop.Run();
+ }
+}
+
+} // namespace y2023::vision
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ y2023::vision::LocalizationVerifierMain();
+}