Merge changes If70577c6,Id131f3b3
* changes:
Add a driver pistol grip fire button
Recalibrate robot after climber upgrade
diff --git a/frc971/vision/intrinsics_calibration.cc b/frc971/vision/intrinsics_calibration.cc
index 16a53a7..d3ddba7 100644
--- a/frc971/vision/intrinsics_calibration.cc
+++ b/frc971/vision/intrinsics_calibration.cc
@@ -43,7 +43,19 @@
<< "Need a base intrinsics json to use to auto-capture images when the "
"camera moves.";
std::unique_ptr<aos::ExitHandle> exit_handle = event_loop.MakeExitHandle();
- IntrinsicsCalibration extractor(&event_loop, hostname, FLAGS_channel,
+
+ std::string camera_name = absl::StrCat(
+ "/", aos::network::ParsePiOrOrin(hostname).value(),
+ std::to_string(aos::network::ParsePiOrOrinNumber(hostname).value()),
+ FLAGS_channel);
+ // THIS IS A HACK FOR 2024, since we call Orin2 "Imu"
+ if (aos::network::ParsePiOrOrin(hostname).value() == "orin" &&
+ aos::network::ParsePiOrOrinNumber(hostname).value() == 2) {
+ LOG(INFO) << "\nHACK for 2024: Renaming orin2 to imu\n";
+ camera_name = absl::StrCat("/imu", FLAGS_channel);
+ }
+
+ IntrinsicsCalibration extractor(&event_loop, hostname, camera_name,
FLAGS_camera_id, FLAGS_base_intrinsics,
FLAGS_display_undistorted,
FLAGS_calibration_folder, exit_handle.get());
diff --git a/frc971/vision/intrinsics_calibration_lib.cc b/frc971/vision/intrinsics_calibration_lib.cc
index 59a45ac..5584ed7 100644
--- a/frc971/vision/intrinsics_calibration_lib.cc
+++ b/frc971/vision/intrinsics_calibration_lib.cc
@@ -36,17 +36,27 @@
rvecs_eigen, tvecs_eigen);
}),
image_callback_(
- event_loop,
- absl::StrCat("/", aos::network::ParsePiOrOrin(hostname_).value(),
- std::to_string(cpu_number_.value()), camera_channel_),
+ event_loop, camera_channel_,
[this](cv::Mat rgb_image,
const aos::monotonic_clock::time_point eof) {
+ if (exit_collection_) {
+ return;
+ }
charuco_extractor_.HandleImage(rgb_image, eof);
},
kMaxImageAge),
display_undistorted_(display_undistorted),
calibration_folder_(calibration_folder),
- exit_handle_(exit_handle) {
+ exit_handle_(exit_handle),
+ exit_collection_(false) {
+ if (!FLAGS_visualize) {
+ // The only way to exit into the calibration routines is by hitting "q"
+ // while visualization is running. The event_loop doesn't pause enough
+ // to handle ctrl-c exit requests
+ LOG(INFO) << "Setting visualize to true, since currently the intrinsics "
+ "only works this way";
+ FLAGS_visualize = true;
+ }
LOG(INFO) << "Hostname is: " << hostname_ << " and camera channel is "
<< camera_channel_;
@@ -81,7 +91,11 @@
}
int keystroke = cv::waitKey(1);
-
+ if ((keystroke & 0xFF) == static_cast<int>('q')) {
+ LOG(INFO) << "Going to exit";
+ exit_collection_ = true;
+ exit_handle_->Exit();
+ }
// If we haven't got a valid pose estimate, don't use these points
if (!valid) {
LOG(INFO) << "Skipping because pose is not valid";
@@ -161,9 +175,6 @@
<< kDeltaTThreshold;
}
}
-
- } else if ((keystroke & 0xFF) == static_cast<int>('q')) {
- exit_handle_->Exit();
}
}
@@ -175,8 +186,14 @@
std::string_view camera_id, uint16_t team_number,
double reprojection_error) {
flatbuffers::FlatBufferBuilder fbb;
+ // THIS IS A HACK FOR 2024, since we call Orin2 "Imu"
+ std::string cpu_name = absl::StrFormat("%s%d", cpu_type, cpu_number);
+ if (cpu_type == "orin" && cpu_number == 2) {
+ LOG(INFO) << "Renaming orin2 to imu";
+ cpu_name = "imu";
+ }
flatbuffers::Offset<flatbuffers::String> name_offset =
- fbb.CreateString(absl::StrFormat("%s%d", cpu_type, cpu_number));
+ fbb.CreateString(cpu_name.c_str());
flatbuffers::Offset<flatbuffers::String> camera_id_offset =
fbb.CreateString(camera_id);
flatbuffers::Offset<flatbuffers::Vector<float>> intrinsics_offset =
diff --git a/frc971/vision/intrinsics_calibration_lib.h b/frc971/vision/intrinsics_calibration_lib.h
index 605741f..faa82e9 100644
--- a/frc971/vision/intrinsics_calibration_lib.h
+++ b/frc971/vision/intrinsics_calibration_lib.h
@@ -77,6 +77,8 @@
const bool display_undistorted_;
const std::string calibration_folder_;
aos::ExitHandle *exit_handle_;
+
+ bool exit_collection_;
};
} // namespace frc971::vision
diff --git a/y2024/vision/BUILD b/y2024/vision/BUILD
index 4904554..bd4fe76 100644
--- a/y2024/vision/BUILD
+++ b/y2024/vision/BUILD
@@ -167,3 +167,20 @@
"@org_tuxfamily_eigen//:eigen",
],
)
+
+cc_binary(
+ name = "image_replay",
+ srcs = [
+ "image_replay.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//y2024:__subpackages__"],
+ deps = [
+ "//aos:configuration",
+ "//aos:init",
+ "//aos/events:simulated_event_loop",
+ "//aos/events/logging:log_reader",
+ "//frc971/vision:vision_fbs",
+ "//third_party:opencv",
+ ],
+)
diff --git a/y2024/vision/image_replay.cc b/y2024/vision/image_replay.cc
new file mode 100644
index 0000000..f03bcf1
--- /dev/null
+++ b/y2024/vision/image_replay.cc
@@ -0,0 +1,47 @@
+#include "gflags/gflags.h"
+#include "opencv2/imgproc.hpp"
+#include <opencv2/highgui.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;
+}