Move year unspecific files to frc971
Change-Id: Iebecaafe3e791368eeba7f20c6f06e9a3c407b86
Signed-off-by: Yash Maheshwari <yashmahe2018@gmail.com>
diff --git a/frc971/vision/BUILD b/frc971/vision/BUILD
index 01a6d1a..7b632fc 100644
--- a/frc971/vision/BUILD
+++ b/frc971/vision/BUILD
@@ -3,6 +3,72 @@
load("//aos:config.bzl", "aos_config")
load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
+cc_binary(
+ name = "modify_extrinsics",
+ srcs = [
+ "modify_extrinsics.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos:configuration",
+ "//aos:init",
+ "//aos/events:event_loop",
+ "//frc971/vision:calibration_fbs",
+ "//frc971/vision:vision_util_lib",
+ "@com_google_absl//absl/strings:str_format",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_binary(
+ name = "image_replay",
+ srcs = [
+ "image_replay.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos:configuration",
+ "//aos:init",
+ "//aos/events:simulated_event_loop",
+ "//aos/events/logging:log_reader",
+ "//frc971/vision:vision_fbs",
+ "//third_party:opencv",
+ ],
+)
+
+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",
+ "//aos/util:filesystem_fbs",
+ "//frc971/input:joystick_state_fbs",
+ "@com_github_gflags_gflags//:gflags",
+ "@com_github_google_glog//:glog",
+ ],
+)
+
+cc_binary(
+ name = "foxglove_image_converter",
+ srcs = ["foxglove_image_converter.cc"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//frc971/vision:foxglove_image_converter_lib",
+ ],
+)
+
static_flatbuffer(
name = "vision_fbs",
srcs = ["vision.fbs"],
@@ -348,6 +414,7 @@
"//y2022:__subpackages__",
"//y2023:__subpackages__",
"//y2024:__subpackages__",
+ "//y2024_swerve:__subpackages__",
],
deps = [
":intrinsics_calibration_lib",
diff --git a/frc971/vision/foxglove_image_converter.cc b/frc971/vision/foxglove_image_converter.cc
new file mode 100644
index 0000000..c004ac0
--- /dev/null
+++ b/frc971/vision/foxglove_image_converter.cc
@@ -0,0 +1,21 @@
+#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.");
+DEFINE_string(channel, "/camera", "Input/Output channel 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, FLAGS_channel, FLAGS_channel,
+ frc971::vision::ImageCompression::kJpeg);
+
+ event_loop.Run();
+}
diff --git a/frc971/vision/image_logger.cc b/frc971/vision/image_logger.cc
new file mode 100644
index 0000000..2984824
--- /dev/null
+++ b/frc971/vision/image_logger.cc
@@ -0,0 +1,137 @@
+#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 "aos/util/filesystem_generated.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("image_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());
+
+ aos::Fetcher<aos::util::FilesystemStatus> filesystem_status =
+ event_loop.MakeFetcher<aos::util::FilesystemStatus>("/aos");
+
+ bool logging = false;
+ bool enabled = false;
+ aos::monotonic_clock::time_point last_disable_time =
+ aos::monotonic_clock::min_time;
+ 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;
+ filesystem_status.Fetch();
+
+ // Store the last time we got disabled
+ if (enabled && !joystick_state.enabled()) {
+ last_disable_time = timestamp;
+ }
+ enabled = joystick_state.enabled();
+
+ bool enough_space = true;
+
+ if (filesystem_status.get() != nullptr) {
+ enough_space = false;
+ for (const aos::util::Filesystem *fs :
+ *filesystem_status->filesystems()) {
+ CHECK(fs->has_path());
+ if (fs->path()->string_view() == "/") {
+ if (fs->free_space() > 50ull * 1024ull * 1024ull * 1024ull) {
+ enough_space = true;
+ }
+ }
+ }
+ }
+
+ const bool should_be_logging =
+ (enabled ||
+ timestamp < last_disable_time + std::chrono::duration<double>(
+ FLAGS_disabled_time)) &&
+ enough_space;
+
+ if (!logging && should_be_logging) {
+ 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 && !should_be_logging) {
+ // 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/frc971/vision/image_replay.cc b/frc971/vision/image_replay.cc
new file mode 100644
index 0000000..3b75c92
--- /dev/null
+++ b/frc971/vision/image_replay.cc
@@ -0,0 +1,47 @@
+#include "gflags/gflags.h"
+#include "opencv2/highgui.hpp"
+#include "opencv2/imgproc.hpp"
+
+#include "aos/events/logging/log_reader.h"
+#include "aos/events/logging/log_writer.h"
+#include "aos/events/simulated_event_loop.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/logging/log_message_generated.h"
+#include "frc971/vision/vision_generated.h"
+
+DEFINE_string(node, "orin1", "The node to view the log from");
+DEFINE_string(channel, "/camera0", "The channel to view the log from");
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+
+ // open logfiles
+ aos::logger::LogReader reader(
+ aos::logger::SortParts(aos::logger::FindLogs(argc, argv)));
+
+ aos::SimulatedEventLoopFactory factory(reader.configuration());
+ reader.Register(&factory);
+
+ aos::NodeEventLoopFactory *node = factory.GetNodeEventLoopFactory(FLAGS_node);
+
+ std::unique_ptr<aos::EventLoop> image_loop = node->MakeEventLoop("image");
+ image_loop->MakeWatcher(
+ "/" + FLAGS_node + "/" + FLAGS_channel,
+ [](const frc971::vision::CameraImage &msg) {
+ cv::Mat color_image(cv::Size(msg.cols(), msg.rows()), CV_8UC2,
+ (void *)msg.data()->data());
+
+ cv::Mat bgr(color_image.size(), CV_8UC3);
+ cv::cvtColor(color_image, bgr, cv::COLOR_YUV2BGR_YUYV);
+
+ cv::imshow("Replay", bgr);
+ cv::waitKey(1);
+ });
+
+ factory.Run();
+
+ reader.Deregister();
+
+ return 0;
+}
diff --git a/frc971/vision/modify_extrinsics.cc b/frc971/vision/modify_extrinsics.cc
new file mode 100644
index 0000000..d021744
--- /dev/null
+++ b/frc971/vision/modify_extrinsics.cc
@@ -0,0 +1,188 @@
+#include <cmath>
+#include <filesystem>
+#include <regex>
+
+#include "Eigen/Dense"
+#include "Eigen/Geometry"
+#include "absl/strings/str_format.h"
+
+#include "aos/configuration.h"
+#include "aos/events/event_loop.h"
+#include "aos/flatbuffer_merge.h"
+#include "aos/init.h"
+#include "aos/network/team_number.h"
+#include "aos/time/time.h"
+#include "aos/util/file.h"
+#include "frc971/vision/calibration_generated.h"
+#include "frc971/vision/vision_util_lib.h"
+
+// This is a helper program to build and rename calibration files
+// You can:
+// (1) pass it in a new set of orin #, team #, camera #, to rename the file
+// (2) Pass in extrinsics to set the extrinsics
+// By default, writes to /tmp, but will write to calib_files folder if
+// full path is given and calibration_folder is blank
+
+DEFINE_string(orig_calib_file, "",
+ "Intrinsics to use for estimating board pose prior to solving "
+ "for the new intrinsics.");
+DEFINE_string(calibration_folder, "/tmp", "Folder to place calibration files.");
+DEFINE_string(node_name, "",
+ "Node name to use, e.g. orin1, imu; unchanged if blank");
+DEFINE_int32(team_number, -1, "Team number to use; unchanged if -1");
+DEFINE_int32(camera_number, -1, "Camera number to use; unchanged if -1");
+
+DEFINE_bool(set_extrinsics, true, "Set to false to ignore extrinsic data");
+DEFINE_bool(use_inches, true,
+ "Whether to use inches as units (meters if false)");
+DEFINE_bool(use_degrees, true,
+ "Whether to use degrees as units (radians if false)");
+DEFINE_double(camera_x, 0.0, "x location of camera");
+DEFINE_double(camera_y, 0.0, "y location of camera");
+DEFINE_double(camera_z, 0.0, "z location of camera");
+// Don't currently allow for roll of cameras
+DEFINE_double(camera_yaw, 0.0, "yaw of camera about robot z axis");
+DEFINE_double(camera_pitch, 0.0, "pitch of camera relative to robot y axis");
+// TODO: This could be done by setting the pixel size and using the intrinsics
+DEFINE_double(focal_length, 0.002, "Focal length in meters");
+
+namespace frc971::vision {
+namespace {
+
+// TODO: Put this in vision_util_lib? Except, it depends on Eigen
+std::vector<float> MatrixToVector(const Eigen::Matrix<double, 4, 4> &H) {
+ std::vector<float> data;
+ for (int row = 0; row < 4; ++row) {
+ for (int col = 0; col < 4; ++col) {
+ data.push_back(H(row, col));
+ }
+ }
+ return data;
+}
+
+// Merge the original calibration file with all its changes
+aos::FlatbufferDetachedBuffer<calibration::CameraCalibration> BuildCalibration(
+ const frc971::vision::calibration::CameraCalibration *calibration,
+ std::string node_name, uint16_t camera_number, uint16_t team_number) {
+ aos::FlatbufferDetachedBuffer<frc971::vision::calibration::CameraCalibration>
+ cal_copy = aos::RecursiveCopyFlatBuffer(calibration);
+
+ flatbuffers::FlatBufferBuilder fbb;
+ flatbuffers::Offset<flatbuffers::String> node_name_offset =
+ fbb.CreateString(absl::StrFormat("%s", node_name.c_str()));
+
+ // If we're told to set the extrinsics, clear old and add in new
+ flatbuffers::Offset<calibration::TransformationMatrix>
+ fixed_extrinsics_offset;
+ if (FLAGS_set_extrinsics) {
+ cal_copy.mutable_message()->clear_fixed_extrinsics();
+ Eigen::Affine3d extrinsic_matrix;
+ // Convert to metric
+ double translation_scale = (FLAGS_use_inches ? 0.0254 : 1.0);
+ Eigen::Translation3d translation(FLAGS_camera_x * translation_scale,
+ FLAGS_camera_y * translation_scale,
+ FLAGS_camera_z * translation_scale);
+
+ // convert to radians
+ double angle_scale = (FLAGS_use_degrees ? M_PI / 180.0 : 1.0);
+ // The rotation that takes robot coordinates (x forward, z up) to camera
+ // coordiantes (z forward, x right)
+ Eigen::Quaterniond R_robo_cam =
+ Eigen::AngleAxisd(-M_PI / 2.0, Eigen::Vector3d::UnitZ()) *
+ Eigen::AngleAxisd(-M_PI / 2.0, Eigen::Vector3d::UnitX());
+ Eigen::AngleAxisd pitchAngle(FLAGS_camera_pitch * angle_scale,
+ Eigen::Vector3d::UnitY());
+ Eigen::AngleAxisd yawAngle(FLAGS_camera_yaw * angle_scale,
+ Eigen::Vector3d::UnitZ());
+
+ Eigen::Quaterniond rotation = yawAngle * pitchAngle * R_robo_cam;
+ Eigen::Vector3d focal_length_offset =
+ (rotation * Eigen::Translation3d(0.0, 0.0, FLAGS_focal_length))
+ .translation();
+ translation = translation * Eigen::Translation3d(focal_length_offset);
+
+ extrinsic_matrix = translation * rotation;
+ flatbuffers::Offset<flatbuffers::Vector<float>> data_offset =
+ fbb.CreateVector(
+ frc971::vision::MatrixToVector(extrinsic_matrix.matrix()));
+ calibration::TransformationMatrix::Builder matrix_builder(fbb);
+ matrix_builder.add_data(data_offset);
+ fixed_extrinsics_offset = matrix_builder.Finish();
+ }
+
+ calibration::CameraCalibration::Builder camera_calibration_builder(fbb);
+ camera_calibration_builder.add_node_name(node_name_offset);
+ camera_calibration_builder.add_team_number(team_number);
+ if (FLAGS_set_extrinsics) {
+ camera_calibration_builder.add_fixed_extrinsics(fixed_extrinsics_offset);
+ }
+ camera_calibration_builder.add_camera_number(camera_number);
+ fbb.Finish(camera_calibration_builder.Finish());
+
+ aos::FlatbufferDetachedBuffer<calibration::CameraCalibration> updated_cal =
+ fbb.Release();
+
+ aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
+ merged_calibration =
+ aos::MergeFlatBuffers(&cal_copy.message(), &updated_cal.message());
+ return merged_calibration;
+}
+
+void Main(std::string orig_calib_filename) {
+ LOG(INFO) << "Reading from file: " << orig_calib_filename;
+ aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
+ base_calibration =
+ aos::JsonFileToFlatbuffer<calibration::CameraCalibration>(
+ orig_calib_filename);
+
+ // Populate the new variables from command-line or from base_calibration
+ std::string node_name =
+ (FLAGS_node_name == "" ? base_calibration.message().node_name()->str()
+ : FLAGS_node_name);
+ int team_number =
+ (FLAGS_team_number == -1 ? base_calibration.message().team_number()
+ : FLAGS_team_number);
+ int camera_number =
+ (FLAGS_camera_number == -1 ? base_calibration.message().camera_number()
+ : FLAGS_camera_number);
+ aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
+ new_calibration = BuildCalibration(&base_calibration.message(), node_name,
+ camera_number, team_number);
+
+ // Set a new timestamp on the file, but leave calibration_timestamp unchanged
+ const aos::realtime_clock::time_point realtime_now =
+ aos::realtime_clock::now();
+ std::stringstream time_ss;
+ time_ss << realtime_now;
+ // Use the camera_id that is set in the json file (not the filename)
+ std::string camera_id = base_calibration.message().camera_id()->str();
+
+ const std::string dirname =
+ (FLAGS_calibration_folder == ""
+ ? std::filesystem::path(orig_calib_filename).parent_path().string()
+ : FLAGS_calibration_folder);
+ const std::string new_calib_filename = frc971::vision::CalibrationFilename(
+ dirname, node_name.c_str(), team_number, camera_number, camera_id.c_str(),
+ time_ss.str());
+
+ VLOG(1) << "From: " << orig_calib_filename << " -> "
+ << aos::FlatbufferToJson(base_calibration, {.multi_line = true});
+
+ VLOG(1) << "Writing: " << new_calib_filename << " -> "
+ << aos::FlatbufferToJson(new_calibration, {.multi_line = true});
+
+ LOG(INFO) << "Writing to file: " << new_calib_filename;
+ aos::util::WriteStringToFileOrDie(
+ new_calib_filename,
+ aos::FlatbufferToJson(new_calibration, {.multi_line = true}));
+}
+
+} // namespace
+} // namespace frc971::vision
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ CHECK(argc == 2) << "Must supply a starting calibration filename";
+ std::string filename = argv[1];
+ frc971::vision::Main(filename);
+}
diff --git a/frc971/vision/rename_calibration_file.sh b/frc971/vision/rename_calibration_file.sh
new file mode 100755
index 0000000..dcdf2f2
--- /dev/null
+++ b/frc971/vision/rename_calibration_file.sh
@@ -0,0 +1,86 @@
+#!/bin/bash
+
+# Helper script to rename the camera calibration file when moving to new robot
+
+# grep isn't happy with set
+# set -e
+
+
+usage_and_exit () {
+ echo
+ echo "Usage:"
+ echo "$0 ORIG_FILENAME NEW_TEAM_NUMBER NEW_ORIN_NUMBER NEW_CAMERA_NUMBER"
+ echo
+ exit 2
+}
+
+if [[ $# -ne 4 ]]; then
+ echo "ERROR: Requires 4 parameters"
+ usage_and_exit
+fi
+
+ORIG_FILENAME=$1
+NEW_TEAM_NUMBER=$2
+NEW_ORIN_NUMBER=$3
+NEW_CAMERA_NUMBER=$4
+
+if [[ ! -x ${ORIG_FILENAME} ]]; then
+ echo "${ORIG_FILENAME} does not exist"
+ usage_and_exit
+fi
+
+check_971=`echo "${NEW_TEAM_NUMBER}" | grep "971"`
+if [[ ${check_971} == "" ]]; then
+ echo "NEW_TEAM_NUMBER (${NEW_TEAM_NUMBER}) does not contain '971'"
+ usage_and_exit
+fi
+
+if [[ ${NEW_ORIN_NUMBER} != 1 && ${NEW_ORIN_NUMBER} != 2 ]]; then
+ echo "NEW_ORIN_NUMBER (${NEW_ORIN_NUMBER}) must be either 1 or 2"
+ usage_and_exit
+fi
+
+if [[ ${NEW_CAMERA_NUMBER} != 0 && ${NEW_CAMERA_NUMBER} != 1 ]]; then
+ echo "NEW_CAMERA_NUMBER (${NEW_CAMERA_NUMBER}) must be either 0 or 1"
+ usage_and_exit
+fi
+
+# Extract parts of the filename, based on just the basename
+# This assumes filenames of the form:
+# calibration_orin-971-1-0_cam-24-01_2024-02-07_20-11-35.566609408.json
+IFS='_' read -r -a name_parts <<< `basename "${ORIG_FILENAME}"`
+
+echo "For ${ORIG_FILENAME}:"
+for element in "${name_parts[@]}"
+do
+ echo "$element"
+done
+
+# Rename file based on this new info (be sure to handle paths properly)
+NEW_FILENAME=`dirname ${ORIG_FILENAME}`"/${name_parts[0]}_orin-${NEW_TEAM_NUMBER}-${NEW_ORIN_NUMBER}-${NEW_CAMERA_NUMBER}_${name_parts[2]}_${name_parts[3]}_${name_parts[4]}"
+
+echo
+echo "For camera id: ${name_parts[2]}"
+echo "Renaming from:"
+echo "${ORIG_FILENAME} to: "
+echo "${NEW_FILENAME}"
+echo
+echo "and changing from "
+echo "${name_parts[1]} to: "
+echo "orin-${NEW_TEAM_NUMBER}-${NEW_ORIN_NUMBER}-${NEW_CAMERA_NUMBER}"
+echo
+
+mv ${ORIG_FILENAME} ${NEW_FILENAME}
+
+
+echo "REPLACING ORIN_NUMBER"
+sed -i s/orin./orin${NEW_ORIN_NUMBER}/ ${NEW_FILENAME}
+
+echo "Replacing TEAM NUMBER"
+sed -i s/\"team_number\"\:\ [1-9]*\,/\"team_number\"\:\ ${NEW_TEAM_NUMBER},/ ${NEW_FILENAME}
+
+echo "REPLACING CAMERA_NUMBER"
+sed -i s/\"camera_number\"\:\ [0-9]/\"camera_number\"\:\ ${NEW_CAMERA_NUMBER}/ ${NEW_FILENAME}
+
+
+