Add y2024 folder
2023 bot specific code was removed.
Signed-off-by: Nathan Leong <100028864@mvla.net>
Change-Id: I88fc4a4b5e6bc883ea327cc306efa4e20035908b
diff --git a/y2024/vision/BUILD b/y2024/vision/BUILD
new file mode 100644
index 0000000..98b5197
--- /dev/null
+++ b/y2024/vision/BUILD
@@ -0,0 +1,24 @@
+filegroup(
+ name = "image_streamer_start",
+ srcs = ["image_streamer_start.sh"],
+ visibility = ["//visibility:public"],
+)
+
+cc_binary(
+ name = "image_logger",
+ srcs = [
+ "image_logger.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos:configuration",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//aos/events/logging:log_writer",
+ "//aos/logging:log_namer",
+ "//frc971/input:joystick_state_fbs",
+ "@com_github_gflags_gflags//:gflags",
+ "@com_github_google_glog//:glog",
+ ],
+)
diff --git a/y2024/vision/README.md b/y2024/vision/README.md
new file mode 100644
index 0000000..c81581f
--- /dev/null
+++ b/y2024/vision/README.md
@@ -0,0 +1,25 @@
+How to use the extrinsic calibration for camera-to-camera calibration
+
+This all assumes you have cameras that have been intrinsically
+calibrated, and that pi1 has a valid extrinsic calibration (from robot
+origin / IMU to the pi1 camera).
+
+It by default will compute the extrinsics for each of the other cameras (pi2,
+pi3, pi4) relative to the robot origin (IMU origin).
+
+Steps:
+* Place two Aruco Diamond markers about 1 meter apart
+ (center-to-center) at a height so that they will be in view of the
+ cameras when the robot is about 1-2m away
+* Start with the robot in a position that both markers are fully in
+ view by one camera
+* Enable logging for all cameras
+* Rotate the robot in place through a full circle, so that each camera sees both tags, and at times just one tag.
+* Stop the logging and copy the files to your laptop
+* Run the calibration code on the resulting files, e.g.,
+ * `bazel run -c opt //y2023/vision:calibrate_multi_cameras -- /data/PATH_TO_LOGS --team_number 971 --target_type charuco_diamond
+ * Can add "--visualize" flag to see the camera views and marker detections
+ * I've sometimes found it necessary to add the "--skip_missing_forwarding_entries" flag-- as guided by the logging messages
+* From the output, copy the calculated ("Updated") extrinsic into the
+ corresponding calibration file for the right robot and camera in the
+ calib_files directory
diff --git a/y2024/vision/foxglove_image_converter.cc b/y2024/vision/foxglove_image_converter.cc
new file mode 100644
index 0000000..bfaafc8
--- /dev/null
+++ b/y2024/vision/foxglove_image_converter.cc
@@ -0,0 +1,20 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "frc971/vision/foxglove_image_converter_lib.h"
+
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+
+int main(int argc, char *argv[]) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ frc971::vision::FoxgloveImageConverter converter(
+ &event_loop, "/camera", "/camera",
+ frc971::vision::ImageCompression::kJpeg);
+
+ event_loop.Run();
+}
diff --git a/y2024/vision/image_logger.cc b/y2024/vision/image_logger.cc
new file mode 100644
index 0000000..45e25f6
--- /dev/null
+++ b/y2024/vision/image_logger.cc
@@ -0,0 +1,112 @@
+#include <sys/resource.h>
+#include <sys/time.h>
+
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+
+#include "aos/configuration.h"
+#include "aos/events/logging/log_writer.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/logging/log_namer.h"
+#include "frc971/input/joystick_state_generated.h"
+
+DEFINE_string(config, "aos_config.json", "Config file to use.");
+
+DEFINE_double(rotate_every, 0.0,
+ "If set, rotate the logger after this many seconds");
+DECLARE_int32(flush_size);
+DEFINE_double(disabled_time, 5.0,
+ "Continue logging if disabled for this amount of time or less");
+DEFINE_bool(direct, false,
+ "If true, write using O_DIRECT and write 512 byte aligned blocks "
+ "whenever possible.");
+
+std::unique_ptr<aos::logger::MultiNodeFilesLogNamer> MakeLogNamer(
+ aos::EventLoop *event_loop) {
+ std::optional<std::string> log_name =
+ aos::logging::MaybeGetLogName("fbs_log");
+
+ if (!log_name.has_value()) {
+ return nullptr;
+ }
+
+ return std::make_unique<aos::logger::MultiNodeFilesLogNamer>(
+ event_loop, std::make_unique<aos::logger::RenamableFileBackend>(
+ absl::StrCat(log_name.value(), "/"), FLAGS_direct));
+}
+
+int main(int argc, char *argv[]) {
+ gflags::SetUsageMessage(
+ "This program provides a simple logger binary that logs all SHMEM data "
+ "directly to a file specified at the command line when the robot is "
+ "enabled and for a bit of time after.");
+ aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ bool logging = false;
+ bool enabled = false;
+ aos::monotonic_clock::time_point last_disable_time =
+ event_loop.monotonic_now();
+ aos::monotonic_clock::time_point last_rotation_time =
+ event_loop.monotonic_now();
+ aos::logger::Logger logger(&event_loop);
+
+ if (FLAGS_rotate_every != 0.0) {
+ logger.set_on_logged_period([&](aos::monotonic_clock::time_point) {
+ const auto now = event_loop.monotonic_now();
+ if (logging && now > last_rotation_time + std::chrono::duration<double>(
+ FLAGS_rotate_every)) {
+ logger.Rotate();
+ last_rotation_time = now;
+ }
+ });
+ }
+
+ event_loop.OnRun([]() {
+ errno = 0;
+ setpriority(PRIO_PROCESS, 0, -20);
+ PCHECK(errno == 0) << ": Renicing to -20 failed.";
+ });
+
+ event_loop.MakeWatcher(
+ "/imu/aos", [&](const aos::JoystickState &joystick_state) {
+ const auto timestamp = event_loop.context().monotonic_event_time;
+ // Store the last time we got disabled
+ if (enabled && !joystick_state.enabled()) {
+ last_disable_time = timestamp;
+ }
+ enabled = joystick_state.enabled();
+
+ if (!logging && enabled) {
+ auto log_namer = MakeLogNamer(&event_loop);
+ if (log_namer == nullptr) {
+ return;
+ }
+
+ // Start logging if we just got enabled
+ LOG(INFO) << "Starting logging";
+ logger.StartLogging(std::move(log_namer));
+ logging = true;
+ last_rotation_time = event_loop.monotonic_now();
+ } else if (logging && !enabled &&
+ (timestamp - last_disable_time) >
+ std::chrono::duration<double>(FLAGS_disabled_time)) {
+ // Stop logging if we've been disabled for a non-negligible amount of
+ // time
+ LOG(INFO) << "Stopping logging";
+ logger.StopLogging(event_loop.monotonic_now());
+ logging = false;
+ }
+ });
+
+ event_loop.Run();
+
+ LOG(INFO) << "Shutting down";
+
+ return 0;
+}
diff --git a/y2024/vision/image_streamer_start.sh b/y2024/vision/image_streamer_start.sh
new file mode 100755
index 0000000..48d9da7
--- /dev/null
+++ b/y2024/vision/image_streamer_start.sh
@@ -0,0 +1,22 @@
+#!/bin/bash
+
+# Some configurations to avoid dropping frames
+# 640x480@30fps, 400x300@60fps.
+# Bitrate 500000-1500000
+WIDTH=640
+HEIGHT=480
+BITRATE=1500000
+LISTEN_ON="/camera/downsized"
+# Don't interfere with field webpage
+STREAMING_PORT=1181
+
+# Handle weirdness with openssl and gstreamer
+export OPENSSL_CONF=""
+
+# Enable for verbose logging
+#export GST_DEBUG=4
+
+export LD_LIBRARY_PATH=/usr/lib/aarch64-linux-gnu/gstreamer-1.0
+
+exec ./image_streamer --width=$WIDTH --height=$HEIGHT --bitrate=$BITRATE --listen_on=$LISTEN_ON --config=aos_config.json --streaming_port=$STREAMING_PORT
+
diff --git a/y2024/vision/localization_verifier.cc b/y2024/vision/localization_verifier.cc
new file mode 100644
index 0000000..c16f874
--- /dev/null
+++ b/y2024/vision/localization_verifier.cc
@@ -0,0 +1,108 @@
+#include "glog/logging.h"
+
+#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 "y2023/localizer/localizer.h"
+#include "y2023/localizer/utils.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) << "H_field_robot " << H_field_robot;
+ LOG(INFO) << "H_robot_camera " << H_robot_camera;
+ LOG(INFO) << "H_camera_target " << H_camera_target;
+ 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"),
+ [H_robot_camera,
+ &localizer_fetcher](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();
+}
diff --git a/y2024/vision/maps/BUILD b/y2024/vision/maps/BUILD
new file mode 100644
index 0000000..38191a4
--- /dev/null
+++ b/y2024/vision/maps/BUILD
@@ -0,0 +1,7 @@
+filegroup(
+ name = "maps",
+ srcs = glob([
+ "*.json",
+ ]),
+ visibility = ["//visibility:public"],
+)